From 85c00bdb04659f0fbe0d2ea72b21635e36251ba2 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 17 Mar 2024 15:23:44 +0300
Subject: [PATCH 01/64] Prerefactor commit
Add arch directory with initial archictury after refactoring
Use black to format files
Other minor fixes
---
README.md | 2 +-
arch/game_controller_seq.puml | 41 ++++
arch/mocked_communicator.puml | 0
arch/mocked_robot.puml | 0
arch/robot_controller.puml | 106 +++++++++
arch/server_components.puml | 15 ++
arch/system.puml | 18 ++
arch/test_scenarious.puml | 0
install_deps.sh | 2 +-
run_container.sh | 4 +-
src/server_main/server_main/server_node.py | 114 +++++-----
src/server_main/server_main/strategy.py | 248 ++++++++++++++++++++-
12 files changed, 485 insertions(+), 65 deletions(-)
create mode 100644 arch/game_controller_seq.puml
create mode 100644 arch/mocked_communicator.puml
create mode 100644 arch/mocked_robot.puml
create mode 100644 arch/robot_controller.puml
create mode 100644 arch/server_components.puml
create mode 100644 arch/system.puml
create mode 100644 arch/test_scenarious.puml
diff --git a/README.md b/README.md
index 71fc5a9..b2ec9eb 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
# androsot_server
[TODO]
-sudo apt-get install ros-humble-usb-cam
+./install_deps.h
Here working aruco detect wrap for ros2:
https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco/tree/main
ros2 run ros2_aruco aruco_node --ros-args -r /camera/image_raw:=/image_raw -r /camera/camera_info:=/camera_info -p camera_frame:=camera
diff --git a/arch/game_controller_seq.puml b/arch/game_controller_seq.puml
new file mode 100644
index 0000000..cdb6225
--- /dev/null
+++ b/arch/game_controller_seq.puml
@@ -0,0 +1,41 @@
+@startuml
+
+!include https://raw.githubusercontent.com/plantuml-stdlib/C4-PlantUML/master/C4_Sequence.puml
+
+
+participant GameController #66B2FF
+
+participant StrategyService #66B2FF
+
+participant LocalizationcService #66B2FF
+
+
+participant Communicator
+
+participant CmdConverter
+
+== Initialization ==
+
+Rel(GameController, StrategyService, "Get robots command")
+
+Rel(StrategyService, LocalizationcService, "Get robots localization", "get_loc")
+
+Rel(LocalizationcService, StrategyService, "Send robots localization")
+
+Rel(StrategyService, StrategyService, "Recalculate strategy")
+
+Rel(StrategyService, LocalizationcService, "async Update localization request", "upd_loc")
+
+Rel(LocalizationcService, Communicator, "async Get robot imu", "upd_loc")
+
+Rel(Communicator, LocalizationcService, "Send robot imu", "upd_loc")
+
+Rel(StrategyService, GameController, "Send commands", "set robots states and etc")
+
+Rel(GameController, CmdConverter, "Convert commands")
+
+Rel(CmdConverter, GameController, "Converted commands")
+
+Rel(GameController, Communicator, "Send commands")
+
+@enduml
\ No newline at end of file
diff --git a/arch/mocked_communicator.puml b/arch/mocked_communicator.puml
new file mode 100644
index 0000000..e69de29
diff --git a/arch/mocked_robot.puml b/arch/mocked_robot.puml
new file mode 100644
index 0000000..e69de29
diff --git a/arch/robot_controller.puml b/arch/robot_controller.puml
new file mode 100644
index 0000000..3a9444f
--- /dev/null
+++ b/arch/robot_controller.puml
@@ -0,0 +1,106 @@
+@startuml robot_ctrl
+
+struct RobotPose
+{
+ double x
+ double y
+ double yaw
+}
+
+struct BallPose
+{
+ double x
+ double y
+ double yaw
+}
+
+class Robot {
+ RobotPose pose
+ +void updateLocalization()
+ +void getState()
+ +void setState()
+}
+
+class StrategyService {
+ Robot[] robots
+ +void startGame()
+ +void stopGame()
+ +void pauseGame()
+ +void resumeGame()
+}
+StrategyService *-- Robot
+StrategyService *-- Approach
+Robot *-- RobotPose
+
+interface Filter
+{
+ +void update()
+ +void step()
+}
+
+class AlphaFilter
+{
+ Params etc
+}
+
+AlphaFilter <|-- Filter
+
+class LocalizationService {
+ StateEstimator[] robots_pos_est
+ StateEstimator ball_filter
+ +RobotPose getRobotPose (int robot_id)
+ +BallPose getBallPose()
+}
+class EstimatorFabrica
+{
+ StateEstimator create(params...)
+}
+LocalizationService *-- EstimatorFabrica
+class StateEstimator
+{
+ Filter filter
+ -async updSensorsData()
+}
+LocalizationService *-- StateEstimator
+StateEstimator *-- Filter
+interface Approach
+{
+ some api
+}
+note top of Approach : Need interface \n that in future can be facade \n of teb_local_planer ros component
+
+class GameController {
+ StrategyService strategy_service
+ LocalizationService localization_service
+ RobotsCommunicator robots_communicator
+ +void Run()
+}
+
+GameController o-- StrategyService
+GameController o-- LocalizationService
+GameController o-- RobotsCommunicator
+
+interface RobotsCommunicator
+{
+ ...
+}
+
+class WifiCommunicator
+{
+
+}
+
+class BLECommunicator
+{
+
+}
+
+BLECommunicator <|-- RobotsCommunicator
+WifiCommunicator <|-- RobotsCommunicator
+class CmdConverter
+{
+
+}
+@enduml
+
+
diff --git a/arch/server_components.puml b/arch/server_components.puml
new file mode 100644
index 0000000..13011a0
--- /dev/null
+++ b/arch/server_components.puml
@@ -0,0 +1,15 @@
+@startuml C4_Elements
+!include https://raw.githubusercontent.com/plantuml-stdlib/C4-PlantUML/master/C4_Component.puml
+
+Container_Boundary(c2, "Server") {
+ Component(VideoCapture, "Video capture node", "usb_cam", "ros-humble-usb-cam")
+ Component(ArucoProcessor, "Aruco processor node", "ros2_aruco", "https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco")
+ Component(BallDetector, "Ball detector node", "yolo v8", "https://wavegit.mipt.ru/starkit/yolo_openvino_ros2")
+ Component(RobotsController, "Robots controller", "python3)", "God component. Do localization, strategy and communicate with robots.")
+}
+Rel(VideoCapture, ArucoProcessor, "Image", "Some format...")
+Rel(VideoCapture, BallDetector, "Image", "Some format...")
+Rel(ArucoProcessor, RobotsController, "Aruco markers pose", "Ros2 tf")
+Rel(BallDetector, RobotsController, "Ball point", "soccer_vision_2d_msgs.msg.BallArray")
+
+@enduml
\ No newline at end of file
diff --git a/arch/system.puml b/arch/system.puml
new file mode 100644
index 0000000..75b199b
--- /dev/null
+++ b/arch/system.puml
@@ -0,0 +1,18 @@
+@startuml C4_Elements
+!include https://raw.githubusercontent.com/plantuml-stdlib/C4-PlantUML/master/C4_Container.puml
+
+Person(personAlias, "Label", "Optional Description")
+
+System_Boundary(c1, "Androsot server") {
+ Container(server, "Server", "ROS2", "Main entity")
+ Container(VideoCapture, "Video capture node", "Technology", "Optional Description")
+ Container(ArucoProcessor, "Aruco processor", "Technology", "Optional Description")
+ Container(VideoCapture, "Video capture node", "Technology", "Optional Description")
+
+}
+
+note top of c1: https://medium.com/software-architecture-foundations/robot-operating-system-2-ros-2-architecture-731ef1867776
+System(systemAlias, "Label", "Optional Description")
+
+Rel(personAlias, containerAlias, "Label", "Optional Technology")
+@enduml
\ No newline at end of file
diff --git a/arch/test_scenarious.puml b/arch/test_scenarious.puml
new file mode 100644
index 0000000..e69de29
diff --git a/install_deps.sh b/install_deps.sh
index ff40a44..b3663a2 100755
--- a/install_deps.sh
+++ b/install_deps.sh
@@ -4,6 +4,6 @@
pip3 install openvino
pip install scikit-spatial
sudo apt install ros-humble-usb-cam ros-humble-tf-transformations ros-humble-soccer-vision-2d-msgs -y
- pip3 install paho-mqtt pyquaternion
+ pip3 install paho-mqtt==1.5.1 pyquaternion
apt install mosquitto -y
diff --git a/run_container.sh b/run_container.sh
index 2a027d0..c2b74f5 100755
--- a/run_container.sh
+++ b/run_container.sh
@@ -56,10 +56,10 @@ fi
if [ "$launch_on_kondo" = "y" ]
then
-sudo docker run --rm -it --name $container_name $camera_devs --ipc="host" --network $network_name -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
+sudo docker run --rm -it --name $container_name --privileged $camera_devs --ipc="host" --network $network_name -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace $image_tag
elif [ "$launch_on_kondo" = "n" ]
then
-sudo docker run --rm -it --name $container_name --network $network_name --ipc="host" -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
+sudo docker run --rm -it --name $container_name --network $network_name --ipc="host" -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace $image_tag
else
echo "khm: y or n. goodbye."
exit
diff --git a/src/server_main/server_main/server_node.py b/src/server_main/server_main/server_node.py
index d42107e..cba30f8 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -13,47 +13,48 @@ import json
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
+from tf2_geometry_msgs import do_transform_point
import tf_transformations as tf_t
import math
from skspatial.objects import Line, Plane
from pyquaternion import Quaternion
import paho.mqtt.client as mqtt
from tf2_ros import TransformBroadcaster
-
+import numpy as np
from collections import deque
-class AlphaFilter():
+
+class AlphaFilter:
def __init__(self):
pass
self.buffer = deque()
- self.buffer_size = 10
- self.alpha = 0.70
+ self.buffer_size = 10
+ self.alpha = 0.70
def update(self, value):
- if (self.buffer_size == len(self.buffer)):
+ if self.buffer_size == len(self.buffer):
self.buffer.popleft()
-
- self.buffer.append(value)
+ self.buffer.append(value)
def get(self):
weight_sum = 1
## find yaw mid
- mid = Pose() # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! lost quaternion
+ mid = Pose() # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! lost quaternion
for i, elem in enumerate(self.buffer):
mid.position.x += elem.position.x * self.alpha ** (len(self.buffer) - i)
mid.position.y += elem.position.y * self.alpha ** (len(self.buffer) - i)
mid.position.z += elem.position.z * self.alpha ** (len(self.buffer) - i)
- weight_sum += self.alpha ** (len(self.buffer) - i )
-
+ weight_sum += self.alpha ** (len(self.buffer) - i)
+
mid.position.x /= weight_sum
mid.position.y /= weight_sum
mid.position.z /= weight_sum
return mid
-class AlphaFilterBall():
+
+class AlphaFilterBall:
def __init__(self):
pass
self.buffer = deque()
@@ -61,20 +62,21 @@ class AlphaFilterBall():
self.alpha = 0.70
def update(self, value):
- if (self.buffer_size == len(self.buffer)):
+ if self.buffer_size == len(self.buffer):
self.buffer.popleft()
- mid = value
+ mid = value
weight_sum = 1
for i, elem in enumerate(self.buffer):
mid += elem * self.alpha ** (len(self.buffer) - i)
- weight_sum += self.alpha ** (len(self.buffer) - i )
+ weight_sum += self.alpha ** (len(self.buffer) - i)
mid /= weight_sum
-
- self.buffer.append(mid)
+
+ self.buffer.append(mid)
def get(self):
return self.buffer[-1]
+
class ServerNode(Node):
def __init__(self):
super().__init__("ServerNode")
@@ -107,7 +109,9 @@ class ServerNode(Node):
)
self.robots_pub = {}
for robot_id in self.robot_ids:
- self.robots_pub[robot_id] = self.create_publisher(PoseStamped, "/robot_pose" + str(robot_id), 10)
+ self.robots_pub[robot_id] = self.create_publisher(
+ PoseStamped, "/robot_pose" + str(robot_id), 10
+ )
self.loged_data = []
self.file_to_save = "/home/root/workspace/a.out"
self.create_publisher(PoseStamped, "/robot0_pose", 10)
@@ -131,11 +135,9 @@ class ServerNode(Node):
self.world_robots_poses = {}
self.init_mqqt()
self.declare_parameter("publish_ball2centre", True)
-
self.tf_broadcaster = TransformBroadcaster(self)
-
def on_connect(mqttc, obj, flags, rc):
print("rc: " + str(rc))
@@ -156,7 +158,9 @@ class ServerNode(Node):
self.ball_topic = self.team_name + "/" + "ball"
self.robots_topic = {}
for robot_id in self.robot_ids:
- self.robots_topic[robot_id] = self.team_name + "/" + str(robot_id) + "/" + "robot"
+ self.robots_topic[robot_id] = (
+ self.team_name + "/" + str(robot_id) + "/" + "robot"
+ )
self.mqttc = mqtt.Client()
self.mqttc.on_message = ServerNode.on_message
self.mqttc.on_connect = ServerNode.on_connect
@@ -202,6 +206,7 @@ class ServerNode(Node):
)
except TransformException as ex:
import time
+
time.sleep(0.1)
self.get_logger().info(f"Could not transform world to camera: {ex}")
# self.get_camera_to_world_frame()
@@ -223,8 +228,8 @@ class ServerNode(Node):
shifted_pose.pose.orientation = pose.pose.orientation
shift_point = PointStamped()
- #shift_point.point.x = 0.0 # 0.035
- shift_point.point.z = -0.035# 0.045
+ # shift_point.point.x = 0.0 # 0.035
+ shift_point.point.z = -0.035 # 0.045
transform = TransformStamped()
transform.header = pose.header
transform.child_frame_id = "aruco" + str(id)
@@ -240,14 +245,14 @@ class ServerNode(Node):
inverse_transform = buffer_core.lookup_transform_core(
transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
)
- #self.tf_broadcaster.sendTransform(inverse_transform)
+ # self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
quat = rot.x, rot.y, rot.z, rot.w
euler = tf_t.euler_from_quaternion(quat)
- #print("TF RPY: ", euler )
- #print("Ibverssed TF: ", inverse_transform.transform.translation)
- #import time
- #time.sleep(0.1) # also remove shift
+ # print("TF RPY: ", euler )
+ # print("Ibverssed TF: ", inverse_transform.transform.translation)
+ # import time
+ # time.sleep(0.1) # also remove shift
shifted_pose.pose.position = do_transform_point(shift_point, transform).point
return shifted_pose
@@ -281,16 +286,16 @@ class ServerNode(Node):
shifted_pose = self.shift_pose_to_robot_centre(pose, id)
rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
return rotated_pose
-
+
@staticmethod
def quatWAvg(Q):
- '''
+ """
Averaging Quaternions.
Arguments:
Q(ndarray): an Mx4 ndarray of quaternions.
weights(list): an M elements list, a weight for each quaternion.
- '''
+ """
# Form the symmetric accumulator matrix
A = np.zeros((4, 4))
@@ -298,7 +303,7 @@ class ServerNode(Node):
for i in range(M):
q = Q[:, i]
- A += np.outer(q, q)# rank 1 update
+ A += np.outer(q, q) # rank 1 update
# Get the eigenvector corresponding to largest eigen value
return np.linalg.eigh(A)[1][:, -1]
@@ -321,8 +326,8 @@ class ServerNode(Node):
mid_pose.position.y /= len(poses)
mid_pose.position.z /= len(poses)
- mid_quat = self.quatWAvg(Q_array)
- print('MID_quat')
+ mid_quat = self.quatWAvg(Q_array)
+ print("MID_quat")
print(mid_quat)
mid_pose.orientation.x = mid_quat[0]
mid_pose.orientation.y = mid_quat[1]
@@ -351,8 +356,8 @@ class ServerNode(Node):
if robot_id in robots_poses:
if not robot_id in self.world_robots_poses:
self.world_robots_poses[robot_id] = AlphaFilter()
- self.world_robots_poses[robot_id].update(self.find_mid_pose(
- robots_poses[robot_id])
+ self.world_robots_poses[robot_id].update(
+ self.find_mid_pose(robots_poses[robot_id])
) # transform aruco to forward
def update_ball_pose(self, msg):
@@ -362,7 +367,9 @@ class ServerNode(Node):
msg.header.frame_id in self.camera_frames
and self.camera_models[msg.header.frame_id] == None
):
- print(f"No frame from yolo ball coord.self.camera_models[msg.header.frame_id] == None. Frame: {msg.header.frame_id}")
+ print(
+ f"No frame from yolo ball coord.self.camera_models[msg.header.frame_id] == None. Frame: {msg.header.frame_id}"
+ )
return
if not msg.header.frame_id in self.camera_frames:
@@ -410,7 +417,6 @@ class ServerNode(Node):
self.ball_coordinate_world[0].update(ball_coord[0])
self.ball_coordinate_world[1].update(ball_coord[1])
self.ball_coordinate_world[2].update(ball_coord[2])
-
def get_yaw_world_rotation(self, pose: Pose) -> float:
x_ax = [0, 0, 1]
@@ -435,15 +441,23 @@ class ServerNode(Node):
msg.pose = self.world_robots_poses[id].get()
self.robots_pub[id].publish(msg)
# self.get_logger().info("send_command_to_goalkeeper")
- if (self.team_name=='starkit1'):
+ if self.team_name == "starkit1":
robot_msg = f"{-msg.pose.position.x} {-msg.pose.position.y} {-self.get_yaw_world_rotation(msg.pose)}"
else:
robot_msg = f"{msg.pose.position.x} {msg.pose.position.y} {self.get_yaw_world_rotation(msg.pose)}"
-
- if (0):
- self.loged_data.append([self.get_clock().now().nanoseconds, msg.pose.position.x, msg.pose.position.y,self.get_yaw_world_rotation(msg.pose)])
- with open(self.file_to_save, 'w') as file:
+
+ if 0:
+ self.loged_data.append(
+ [
+ self.get_clock().now().nanoseconds,
+ msg.pose.position.x,
+ msg.pose.position.y,
+ self.get_yaw_world_rotation(msg.pose),
+ ]
+ )
+ with open(self.file_to_save, "w") as file:
import csv
+
writer = csv.writer(file)
writer.writerows(self.loged_data)
self.mqttc.publish(self.robots_topic[id], robot_msg, 1)
@@ -459,17 +473,13 @@ class ServerNode(Node):
ball_msg.point.y = self.ball_coordinate_world[1].get()
ball_msg.point.z = self.ball_coordinate_world[2].get()
self.ball_pub.publish(ball_msg)
-
- if (self.team_name == 'starkit1'):
- ball_msg = (
- f"{-self.ball_coordinate_world[0].get()} {-self.ball_coordinate_world[1].get()}"
- )
+
+ if self.team_name == "starkit1":
+ ball_msg = f"{-self.ball_coordinate_world[0].get()} {-self.ball_coordinate_world[1].get()}"
else:
- ball_msg = (
- f"{self.ball_coordinate_world[0].get()} {self.ball_coordinate_world[1].get()}"
- )
+ ball_msg = f"{self.ball_coordinate_world[0].get()} {self.ball_coordinate_world[1].get()}"
print("publish ball")
- if (self.get_parameter('publish_ball2centre').value):
+ if self.get_parameter("publish_ball2centre").value:
self.mqttc.publish(self.ball_topic, "0.0 0.0", 1)
else:
self.mqttc.publish(self.ball_topic, ball_msg, 1)
diff --git a/src/server_main/server_main/strategy.py b/src/server_main/server_main/strategy.py
index 18118a7..13557ec 100644
--- a/src/server_main/server_main/strategy.py
+++ b/src/server_main/server_main/strategy.py
@@ -1,13 +1,243 @@
+# from ros2_aruco_interfaces.msg import ArucoMarkers
-
-class Strategy:
- def __init__(self) -> None:
+# from tf2_msgs.msg import TFMessage
+# from std_msgs.msg import Header
+# from geometry_msgs.msg import Pose, PointStamped, PoseStamped
+# from sensor_msgs.msg import CameraInfo
+# from soccer_vision_2d_msgs.msg import BallArray, Ball
+
+# import random
+
+
+import rclpy
+from rclpy.node import Node
+
+
+from visualization_msgs.msg import Marker
+import paho.mqtt.client as mqtt
+# https://github.com/eclipse/paho.mqtt.python
+import tf_transformations
+import math
+from ble_communicator import BLECommunicator
+from collections import namedtuple
+
+Pose = namedtuple('Pose', ['x y yaw'], defaults=[0, 0, 0])
+class RobotInfo:
+ def __init__(self):
+ self.state = None
+ self.role = None
+ self.pose = Pose()
+ # self.valid = False invalid State
+ self.last_update_timestamp = None
+ self.connected = False
+ self.goal = {}
+class StrategyService(Node):
+ def __init__(self):
+ super().__init__('MqttKondoDummy')
+ # get from azer project
+
+ self.field_params = None
+ self.init_publisher()
+ self.init_mqqt()
+ self.init_robots()
+ #self.timer = self.create_timer(0.0001, self.forward_main_cycle)
+ self.robots_commucator = BLECommunicator()
+
+ def init_mqqt(self):
+ self.declare_parameter('mqtt_broker', 'localhost')
+ self.broker = self.get_parameter('mqtt_broker').value
+ self.declare_parameter('mqtt_port', 1900)
+ self.port = self.get_parameter('mqtt_port').value
+ self.team_name = "starkit1"
+ self.ball_topic = self.team_name + "/ball"
+ self.robots_topic = { f"{self.team_name}/{robot_id}/robot" for robot_id in range(0, 3)}
+
+ self.mqttc = mqtt.Client()
+ self.mqttc.on_message = self.on_message
+ self.mqttc.on_connect = StrategyService.on_connect
+ self.mqttc.on_publish = StrategyService.on_publish
+ self.mqttc.on_subscribe = StrategyService.on_subscribe
+ self.mqttc.connect(self.broker, self.port, 60)
+ self.mqttc.subscribe(self.ball_topic, 0)
+ for topic in self.robots_topic:
+ self.mqttc.subscribe(topic, 0)
+
+ def on_connect(mqttc, obj, flags, rc):
+ print("rc: " + str(rc))
+
+ def on_message(self, mqttc, obj, msg):
+ print("get msg")
+
+ if (msg.topic == self.ball_topic):
+ self.ball_coord = [float(i) for i in msg.payload.split(b" ")]
+ print(self.ball_coord)
+ elif (msg.topic in self.robots_topic):
+ pf_coord = [float(i) for i in msg.payload.split(b" ")]
+ self.robots[msg.topic].connected = True
+ self.robots[msg.topic].pose.x = pf_coord[0]
+ self.robots[msg.topic].pose.y = pf_coord[1]
+ self.robots[msg.topic].pose.yaw = pf_coord[2]
+ # [TODO] update timesatmp
+ else:
+ print("Warning: Unrecognized topic")
+
+
+ def on_publish(mqttc, obj, mid):
+ print("mid: " + str(mid))
+
+ def on_subscribe(mqttc, obj, mid, granted_qos):
+ print("Subscribed: " + str(mid) + " " + str(granted_qos))
+
+ def init_robots(self):
+ self.robots = dict()
+ for robot_topic in self.robots_topic:
+ self.robots[robot_topic] = RobotInfo()
+
pass
- self.robot_pfs = None
- self.robot_roles = None
- def set_pfs(self):
+
+ def startegy_loop(self):
+
+ while(True):
+ self.mqttc.loop()
+ self.update_robot_info()
+ self.update_strategy()
+ self.update_goals()
+ self.send_commands()
+
+ def update_robot_info(self):
+ for robot in self.robots:
+ robot.role = "forward"
+ self.robots[self.robots_topic[0]] = "goalkeeper"
+ def update_strategy(self):
pass
- def send_msgs(self):
+ def update_goals(self):
+ for robot in self.robots:
+ if (robot.role == "forward"):
+ pass
+ elif (robot.role == "goalkeeper"):
+ pass
+
+ def send_commands(self, commands):
pass
- def strategy_loop(self):
- pass
\ No newline at end of file
+ def init_publisher(self):
+ self.robot_pub = self.create_publisher(
+ Marker,
+ "/robot_dummy",
+ 10
+ )
+ self.ball_pub = self.create_publisher(
+ Marker,
+ "/ball_dummmy",
+ 10
+ )
+ self.robot_pub2 = self.create_publisher(
+ Marker,
+ "/robot_dummy2",
+ 10
+ )
+ self.ball_pub2 = self.create_publisher(
+ Marker,
+ "/ball_dummm2",
+ 10
+ )
+
+ def publish_marker(self):
+ if (self.pf_coord is not None):
+ marker = Marker()
+ marker.header.frame_id = 'world'
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord[0]
+ marker.pose.position.y = self.pf_coord[1]
+ quaternion = tf_transformations.quaternion_about_axis(self.pf_coord[2], (0,0,1))
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = .02
+ marker.scale.y = .02
+ marker.scale.z = .02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 0.2
+ self.robot_pub.publish(marker)
+ print("robot:", marker)
+ if (self.ball_coord is not None):
+ marker = Marker()
+ marker.header.frame_id = 'world'
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord[0]
+ marker.pose.position.y = self.ball_coord[1]
+ marker.scale.x = .05
+ marker.scale.y = .05
+ marker.scale.z = .05
+ marker.color.a = 1.0
+ marker.color.r = 1.0
+ marker.color.g = 0.4
+ marker.color.b = 0.1
+ self.ball_pub.publish(marker)
+ print("ball " ,marker)
+ if (self.pf_coord2 is not None):
+ marker = Marker()
+ marker.header.frame_id = 'world'
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord2[0]
+ marker.pose.position.y = self.pf_coord2[1]
+ quaternion = tf_transformations.quaternion_about_axis(self.pf_coord2[2], (0,0,1))
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = .02
+ marker.scale.y = .02
+ marker.scale.z = .02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 1.0
+ self.robot_pub2.publish(marker)
+ print("robot:", marker)
+ if (self.ball_coord2 is not None):
+ marker = Marker()
+ marker.header.frame_id = 'world'
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord2[0]
+ marker.pose.position.y = self.ball_coord2[1]
+ marker.scale.x = .05
+ marker.scale.y = .05
+ marker.scale.z = .05
+ marker.color.a = 1.0
+ marker.color.r = 0.3
+ marker.color.g = 0.4
+ marker.color.b = 1.0
+ self.ball_pub2.publish(marker)
+
+
+
+def main(args=None):
+ # rclpy.init(args=args)
+
+ server_node = StrategyService()
+ server_node.startegy_loop()
+
+ # rclpy.spin(server_node)
+
+ # # Destroy the node explicitly
+ # # (optional - otherwise it will be done automatically
+ # # when the garbage collector destroys the node object)
+ # server_node.destroy_node()
+ # rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
--
GitLab
From ea122c36d13b8b646988118ebebc011959a2deda Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 30 Mar 2024 10:56:06 +0300
Subject: [PATCH 02/64] PyQt version
---
.../calibration_node/__init__.py | 145 +++++++++++++++++
.../calibration_node/calibration.py | 148 ++++++++++++++++++
.../calibration_node/gui_window.py | 106 +++++++++++++
.../calibration_node/window_interface.py | 21 +++
4 files changed, 420 insertions(+)
create mode 100644 src/calibration_node/calibration_node/__init__.py
create mode 100644 src/calibration_node/calibration_node/calibration.py
create mode 100644 src/calibration_node/calibration_node/gui_window.py
create mode 100644 src/calibration_node/calibration_node/window_interface.py
diff --git a/src/calibration_node/calibration_node/__init__.py b/src/calibration_node/calibration_node/__init__.py
new file mode 100644
index 0000000..0bb9339
--- /dev/null
+++ b/src/calibration_node/calibration_node/__init__.py
@@ -0,0 +1,145 @@
+from ros2_aruco_interfaces.msg import ArucoMarkers
+
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
+from sensor_msgs.msg import CameraInfo
+from soccer_vision_2d_msgs.msg import BallArray, Ball
+import tf2_ros
+
+from image_geometry import PinholeCameraModel
+
+import rclpy
+from rclpy.node import Node
+import json
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point
+import tf_transformations as tf_t
+import math
+from skspatial.objects import Line, Plane
+from pyquaternion import Quaternion
+import paho.mqtt.client as mqtt
+from tf2_ros import TransformBroadcaster
+import numpy as np
+
+from collections import deque
+
+class CalibrationNode(Node):
+ def __init__(self):
+ super().__init__("CalibrationNode")
+ # to launch file
+ # self.declare_parameter("aruco_size_mm", "starkit1")
+ # self.aruco_size_mm = self.get_parameter("aruco_size_mm").value
+
+ # self.declare_parameter("aruco_s", "starkit1")
+ # self.aruco_id = self.get_parameter("aruco_size_mm").value
+
+
+ # !!aruco topic
+ self.declare_parameter("aruco_markers_topic", "/aruco_markers")
+ topic_name = self.get_parameter("aruco_markers_topic").value
+
+ self.declare_parameter("aruco_size", 0.170) #
+ self.aruco_size = self.get_parameter("aruco_size").value # m
+
+ self.aruco_subscription = self.create_subscription(
+ ArucoMarkers, topic_name, self.get_aruco_pose, 10
+ )
+ self.current_aruco_poses = {} # {if: x, y}
+
+ self.declare_parameter("aruco_markers_topic", "/aruco_markers")
+ topic_name = self.get_parameter("aruco_markers_topic").value
+
+ self.declare_parameter("position_config")
+ topic_name = self.get_parameter("position_config").value
+
+ def read_config(path:str):
+ self.position_list
+ pass
+
+ def get_aruco_pose(self, msg):
+ self.current_aruco_poses.clear()
+ for id, pose in zip(msg.marker_ids, msg.poses):
+ pose_stamped = PoseStamped()
+ pose_stamped.pose = pose
+ pose_stamped.header = msg.header
+ self.current_aruco_poses[id] = pose_stamped
+
+ def get_yaw_world_rotation(self, pose: Pose) -> float:
+ x_ax = [0, 0, 1]
+ quat_arr = [
+ pose.orientation.x,
+ pose.orientation.y,
+ pose.orientation.z,
+ pose.orientation.w,
+ ]
+ quat_conj = tf_t.quaternion_conjugate(quat_arr)
+ # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
+ rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
+ return math.atan2(
+ rotated_x[1], rotated_x[0]
+ ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
+
+
+ def get_yaw_world_rotation(self, pose: Pose) -> float:
+ x_ax = [0, 0, 1]
+ quat_arr = [
+ pose.orientation.x,
+ pose.orientation.y,
+ pose.orientation.z,
+ pose.orientation.w,
+ ]
+ quat_conj = tf_t.quaternion_conjugate(quat_arr)
+ # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
+ rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
+ return math.atan2(
+ rotated_x[1], rotated_x[0]
+ ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
+
+ def dump(self, id):
+ if id in self.world_robots_poses:
+ msg = PoseStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "world"
+ msg.pose = self.world_robots_poses[id].get()
+ self.robots_pub[id].publish(msg)
+ # self.get_logger().info("send_command_to_goalkeeper")
+ if self.team_name == "starkit1":
+ robot_msg = f"{-msg.pose.position.x} {-msg.pose.position.y} {-self.get_yaw_world_rotation(msg.pose)}"
+ else:
+ robot_msg = f"{msg.pose.position.x} {msg.pose.position.y} {self.get_yaw_world_rotation(msg.pose)}"
+
+ if 0: # calibraiotn log ???
+ self.loged_data.append(
+ [
+ self.get_clock().now().nanoseconds,
+ msg.pose.position.x,
+ msg.pose.position.y,
+ self.get_yaw_world_rotation(msg.pose),
+ ]
+ )
+ with open(self.file_to_save, "w") as file:
+ import csv
+
+ writer = csv.writer(file)
+ writer.writerows(self.loged_data)
+ self.mqttc.publish(self.robots_topic[id], robot_msg, 1)
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ server_node = CalibrationNode()
+
+ rclpy.spin(server_node)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ server_node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
new file mode 100644
index 0000000..3471d1f
--- /dev/null
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -0,0 +1,148 @@
+import rclpy
+import time
+import sys
+from threading import Thread
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_msgs.msg import String
+from PyQt5.QtWidgets import QApplication, QLabel, QMainWindow, QWidget, QPushButton
+from ros2_aruco_interfaces.msg import ArucoMarkers
+
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
+from sensor_msgs.msg import CameraInfo
+from soccer_vision_2d_msgs.msg import BallArray, Ball
+import tf2_ros
+
+from image_geometry import PinholeCameraModel
+
+import rclpy
+from rclpy.node import Node
+import json
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point
+import tf_transformations as tf_t
+import math
+from skspatial.objects import Line, Plane
+from pyquaternion import Quaternion
+import paho.mqtt.client as mqtt
+from tf2_ros import TransformBroadcaster
+import numpy as np
+
+from collections import deque
+
+from .gui_window import GuiWindow
+from threading import Thread
+
+from dataclasses import dataclass
+from PyQt5.QtCore import QThread
+@dataclass
+class MatchPose:
+ """Class for keeping track of an item in inventory."""
+ tf_ui: TransformStamped
+ tf_camera: TransformStamped
+
+class CalibrationNode(Node):
+ def __init__(self):
+ super().__init__("calibration_node")
+ self.declare_parameter("world_tf_topic", "/tf_static")
+ world_topic = self.get_parameter("world_tf_topic").value
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.tf_from_camera_to_world = dict()
+
+ self.declare_parameter("aruco_markers_topic", "/aruco_markers")
+ topic_name = self.get_parameter("aruco_markers_topic").value
+ self.aruco_subscription = self.create_subscription(
+ ArucoMarkers, topic_name, self.get_aruco_pose, 1
+ )
+ self.declare_parameter("aruco_size", 0.170) #
+ self.aruco_size = self.get_parameter("aruco_size").value # m
+ self.aruco_pose = None
+ self.current_aruco_tf = dict()
+ self.match_pose_arr = list()
+
+ self.declare_parameter("camera_frames", ["camera1", "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
+ self.camera_frames = self.get_parameter("camera_frames").value
+ self.camera_models = {} # {"frame_id": PinholeCameraModel}
+
+ self.ui_window = GuiWindow(self.get_logger())
+ self.gui_thread = QThread()#self.ui_window)
+ # self.worker = self.ui_window.run
+ #self.gui_thread.start()
+ self.ui_window.moveToThread(self.gui_thread)
+ self.gui_thread.run()
+ self.get_logger().info("create window")
+ self.timer = self.create_timer(100, self.calib_cb)
+ # self.future = self.create_future()
+# def get_future(self):
+# return self.future
+ def get_aruco_pose(self, msg):
+ self.current_aruco_tf.clear()
+ for id, pose in zip(msg.marker_ids, msg.poses):
+ tf_stamped = TransformStamped()
+ tf_stamped.transform.translation.x = pose.position.x
+ tf_stamped.transform.translation.y = pose.position.y
+ tf_stamped.transform.translation.z = pose.position.z
+ tf_stamped.rotation = pose.orientation
+ tf_stamped.header = msg.header
+ self.tf.header.frame_id = "aruco" + str(id)
+ self.current_aruco_tf[id] = tf_stamped
+ self.aruco_pose = tf_stamped
+
+ def calib_cb(self):
+ pose_ui = self.ui_window.try_get_position()
+ print("inside callback")
+ self.get_logger().info("Timer callback called")
+ # Huetta!
+ if (pose_ui is not None):
+ pose_ui.header.stamp = self.get_clock().now().to_msg()
+ pose_aruco = self.current_aruco_tf
+ self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
+ if (self.ui_window.is_calibrate_ready()):
+ self.calibrate()
+ del self.ui_window
+ self.gui_thread.join()
+ self.calibrate()
+
+ def calibrate(self):
+ # use only last
+ for match_pose in self.match_pose_arr:
+ # transform = TransformStamped()
+ # transform.header = pose.header
+ # transform.child_frame_id = "aruco" + str(id)
+ # (
+ # transform.transform.translation.x,
+ # transform.transform.translation.y,
+ # transform.transform.translation.z,
+ # ) = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
+ # transform.transform.rotation = pose.pose.orientation
+
+ buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_core.set_transform(match_pose.tf_aruco, "default_authority")
+ buffer_core.set_transform(match_pose.tf_ui, "default_authority")
+ inverse_transform = buffer_core.lookup_transform_core(
+ match_pose.tf_ui.header.frame_id, match_pose.tf_aruco.header.frame_id
+ # Opposite i think
+ #transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
+ )
+ self.tf_broadcaster.sendTransform(inverse_transform)
+ rot = inverse_transform.transform.rotation
+ quat = rot.x, rot.y, rot.z, rot.w
+ euler = tf_t.euler_from_quaternion(quat)
+ print("TF RPY: ", euler )
+ print("Ibverssed TF: ", inverse_transform.transform.translation)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ minimal_node = CalibrationNode()
+# future = minimal_node.get_future()
+# rclpy.spin_until_future_complete(minimal_node, future)
+ rclpy.spin(minimal_node)
+ minimal_node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
new file mode 100644
index 0000000..f545633
--- /dev/null
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -0,0 +1,106 @@
+import sys
+from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QVBoxLayout, QPushButton, QLineEdit, QLabel
+from .window_interface import WindowInterface
+from queue import Queue, Full, Empty
+
+import time
+from typing import Optional
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
+import tf_transformations as tf_t
+
+class GuiWindow(QMainWindow, WindowInterface):
+ def __init__(self, logger):
+ self.logger = logger
+ self.app = QApplication(["node", "1"])
+
+ super().__init__()
+ self.setWindowTitle("Pose Input")
+ self.setGeometry(100, 100, 300, 200)
+
+ # Create a central widget
+ central_widget = QWidget()
+ self.setCentralWidget(central_widget)
+
+ # Create a vertical layout
+ layout = QVBoxLayout()
+
+ # Create labels and input fields for pose data
+ self.pose_labels = ["Id:", "X:", "Y:", "Z:", "Roll:", "Pitch:", "Yaw:"]
+ self.pose_inputs = [QLineEdit() for _ in range(len(self.pose_labels))]
+ for label, input_field in zip(self.pose_labels, self.pose_inputs):
+ layout.addWidget(QLabel(label))
+ layout.addWidget(input_field)
+
+ # Create "Calibrate" and "Next" buttons
+ calibrate_button = QPushButton("Calibrate")
+ next_button = QPushButton("Save Position")
+ layout.addWidget(calibrate_button)
+ layout.addWidget(next_button)
+
+ # Connect buttons to their respective functions
+ calibrate_button.clicked.connect(self.calibrate)
+ next_button.clicked.connect(self.save_position)
+
+ # Set the layout on the central widget
+ central_widget.setLayout(layout)
+ self.tf_queue = Queue(32)# AtomicQueue(256, wait_strategy=BUSY_SPIN_WAIT_STRATEGY)
+ self.calibration_ready = False
+
+ def calibrate(self) -> None:
+ self.calibration_ready = True
+ print("Calibrating...")
+
+ def save_position(self) -> None:
+ try:
+ # Why ros specific in ui???
+ self.tf = TransformStamped()
+ quat = tf_t.quaternion_from_euler(*[float(x.text())for x in self.pose_inputs[4:7]])
+ self.tf.transform.translation.x = float(self.pose_inputs[1].text())
+ self.tf.transform.translation.y = float(self.pose_inputs[2].text())
+ self.tf.transform.translation.z = float(self.pose_inputs[3].text())
+ self.tf.transform.rotation.x = quat[0]
+ self.tf.transform.rotation.y = quat[1]
+ self.tf.transform.rotation.z = quat[2]
+ self.tf.transform.rotation.z = quat[3]
+ self.tf.header.frame_id = "world"
+ self.tf.child_frame_id = "aruco" + self.pose_inputs[0].text()
+ except Exception as e:
+ print(e)
+ try:
+ self.tf_queue.put(self.tf)
+ except Full:
+ print("Queue is full")
+ finally:
+ "IDK what happend"
+ print("Next...")
+
+ def is_calibrate_ready(self) -> bool:
+ return self.calibration_ready
+
+ def try_get_position(self) -> Optional[Pose]:
+ try:
+ return self.tf_queue.get_nowait()
+ except Empty:
+ pass # debug log
+ finally:
+ print("IDK what happend")
+ return None
+
+ def run(self):
+ self.show()
+ self.app.exec_()
+
+ def __del__(self):
+ self.app.quit()
+
+
+
+def main():
+ app = QApplication(sys.argv)
+ window = GuiWindow()
+ window.show()
+ sys.exit(app.exec_())
+
+if __name__ == "__main__":
+ main()
+
diff --git a/src/calibration_node/calibration_node/window_interface.py b/src/calibration_node/calibration_node/window_interface.py
new file mode 100644
index 0000000..b3ed2f0
--- /dev/null
+++ b/src/calibration_node/calibration_node/window_interface.py
@@ -0,0 +1,21 @@
+import abc
+
+class WindowInterface():
+ @abc.abstractmethod
+ def is_calibrate_ready(self):
+ pass
+
+ @abc.abstractmethod
+ def try_get_position(self):
+ pass
+
+ @abc.abstractmethod
+ def run(self):
+ pass
+
+ @abc.abstractmethod
+ def destroy(self):
+ pass
+
+
+
--
GitLab
From f8931a3e63e707ce5c27c74cc9a85c26f36c58b9 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 30 Mar 2024 13:14:23 +0300
Subject: [PATCH 03/64] Try to use PyQt in not main thread
This is not work because PyQt and other legacy gui framework cannot work
not in main thread:(
---
.../calibration_node/calibration.py | 4 +--
src/calibration_node/package.xml | 18 +++++++++++++
.../resource/calibration_node | 0
src/calibration_node/setup.cfg | 4 +++
src/calibration_node/setup.py | 26 +++++++++++++++++++
src/calibration_node/test/test_copyright.py | 25 ++++++++++++++++++
src/calibration_node/test/test_flake8.py | 25 ++++++++++++++++++
src/calibration_node/test/test_pep257.py | 23 ++++++++++++++++
8 files changed, 123 insertions(+), 2 deletions(-)
create mode 100644 src/calibration_node/package.xml
create mode 100644 src/calibration_node/resource/calibration_node
create mode 100644 src/calibration_node/setup.cfg
create mode 100644 src/calibration_node/setup.py
create mode 100644 src/calibration_node/test/test_copyright.py
create mode 100644 src/calibration_node/test/test_flake8.py
create mode 100644 src/calibration_node/test/test_pep257.py
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 3471d1f..e4e174e 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -68,9 +68,9 @@ class CalibrationNode(Node):
self.camera_models = {} # {"frame_id": PinholeCameraModel}
self.ui_window = GuiWindow(self.get_logger())
- self.gui_thread = QThread()#self.ui_window)
+ self.gui_thread = Thread(target=self.ui_window.run)
# self.worker = self.ui_window.run
- #self.gui_thread.start()
+ self.gui_thread.start()
self.ui_window.moveToThread(self.gui_thread)
self.gui_thread.run()
self.get_logger().info("create window")
diff --git a/src/calibration_node/package.xml b/src/calibration_node/package.xml
new file mode 100644
index 0000000..8b2220d
--- /dev/null
+++ b/src/calibration_node/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>calibration_node</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="xatc120@gmail.com">nakuznetsov</maintainer>
+ <license>TODO: License declaration</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/calibration_node/resource/calibration_node b/src/calibration_node/resource/calibration_node
new file mode 100644
index 0000000..e69de29
diff --git a/src/calibration_node/setup.cfg b/src/calibration_node/setup.cfg
new file mode 100644
index 0000000..da20ed2
--- /dev/null
+++ b/src/calibration_node/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/calibration_node
+[install]
+install_scripts=$base/lib/calibration_node
diff --git a/src/calibration_node/setup.py b/src/calibration_node/setup.py
new file mode 100644
index 0000000..06cae11
--- /dev/null
+++ b/src/calibration_node/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'calibration_node'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='nakuznetsov',
+ maintainer_email='status',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'calibration_node = calibration_node.calibration:main'
+ ],
+ },
+)
diff --git a/src/calibration_node/test/test_copyright.py b/src/calibration_node/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/calibration_node/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/calibration_node/test/test_flake8.py b/src/calibration_node/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/calibration_node/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/calibration_node/test/test_pep257.py b/src/calibration_node/test/test_pep257.py
new file mode 100644
index 0000000..f78afc8
--- /dev/null
+++ b/src/calibration_node/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test']) # want other codestyle
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From d5ed90b72502033654569e3a4d761c04423df7f2 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 30 Mar 2024 16:31:23 +0300
Subject: [PATCH 04/64] Moved spin to PyQt in different thread
Works with bugs
---
.../calibration_node/calibration.py | 53 ++++++++-------
.../calibration_node/gui_window.py | 31 +++++++--
.../single_camera_calibration.launch.py | 64 +++++++++++++++++++
src/calibration_node/setup.py | 5 +-
4 files changed, 123 insertions(+), 30 deletions(-)
create mode 100644 src/calibration_node/launch/single_camera_calibration.launch.py
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index e4e174e..5fc17e3 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -60,35 +60,41 @@ class CalibrationNode(Node):
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
self.aruco_pose = None
- self.current_aruco_tf = dict()
+ # self.current_aruco_tf = dict()
self.match_pose_arr = list()
-
- self.declare_parameter("camera_frames", ["camera1", "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
+ self.tf_broadcaster = TransformBroadcaster(self)
+ self.current_aruco_tf = None
+ self.declare_parameter("camera_frames", ["camera1"])#, "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
self.camera_models = {} # {"frame_id": PinholeCameraModel}
- self.ui_window = GuiWindow(self.get_logger())
- self.gui_thread = Thread(target=self.ui_window.run)
- # self.worker = self.ui_window.run
- self.gui_thread.start()
- self.ui_window.moveToThread(self.gui_thread)
- self.gui_thread.run()
- self.get_logger().info("create window")
- self.timer = self.create_timer(100, self.calib_cb)
+ self.ui_window = GuiWindow(self)
+ # I tried(
+ # self.gui_thread = Thread(target=self.ui_window.run)
+ # # self.worker = self.ui_window.run
+ # self.gui_thread.start()
+ # self.ui_window.moveToThread(self.gui_thread)
+ # self.gui_thread.run()
+ self.get_logger().info("\n\n\ncreate window\n\n\n")
+ self.timer = self.create_timer(1, self.calib_cb)
# self.future = self.create_future()
# def get_future(self):
# return self.future
+
+ def run(self):
+ self.ui_window.run() # write ti interface
+
def get_aruco_pose(self, msg):
- self.current_aruco_tf.clear()
+ # self.current_aruco_tf.clear()
for id, pose in zip(msg.marker_ids, msg.poses):
tf_stamped = TransformStamped()
tf_stamped.transform.translation.x = pose.position.x
tf_stamped.transform.translation.y = pose.position.y
tf_stamped.transform.translation.z = pose.position.z
- tf_stamped.rotation = pose.orientation
+ tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- self.tf.header.frame_id = "aruco" + str(id)
- self.current_aruco_tf[id] = tf_stamped
+ tf_stamped.header.frame_id = "aruco" + str(id)
+ self.current_aruco_tf = tf_stamped
self.aruco_pose = tf_stamped
def calib_cb(self):
@@ -96,14 +102,13 @@ class CalibrationNode(Node):
print("inside callback")
self.get_logger().info("Timer callback called")
# Huetta!
- if (pose_ui is not None):
+ if (pose_ui is not None and self.current_aruco_tf is not None):
pose_ui.header.stamp = self.get_clock().now().to_msg()
pose_aruco = self.current_aruco_tf
self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
if (self.ui_window.is_calibrate_ready()):
self.calibrate()
del self.ui_window
- self.gui_thread.join()
self.calibrate()
def calibrate(self):
@@ -120,10 +125,11 @@ class CalibrationNode(Node):
# transform.transform.rotation = pose.pose.orientation
buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
- buffer_core.set_transform(match_pose.tf_aruco, "default_authority")
+ print (type(match_pose.tf_camera))
+ buffer_core.set_transform(match_pose.tf_camera, "default_authority")
buffer_core.set_transform(match_pose.tf_ui, "default_authority")
inverse_transform = buffer_core.lookup_transform_core(
- match_pose.tf_ui.header.frame_id, match_pose.tf_aruco.header.frame_id
+ match_pose.tf_ui.header.frame_id, match_pose.tf_camera.header.frame_id, rclpy.time.Time()
# Opposite i think
#transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
)
@@ -136,13 +142,12 @@ class CalibrationNode(Node):
def main(args=None):
- rclpy.init(args=args)
- minimal_node = CalibrationNode()
+ rclpy.init()
+ minimal_node = CalibrationNode() # add start function
+ minimal_node.run()
# future = minimal_node.get_future()
# rclpy.spin_until_future_complete(minimal_node, future)
- rclpy.spin(minimal_node)
- minimal_node.destroy_node()
- rclpy.shutdown()
+
if __name__ == '__main__':
main()
\ No newline at end of file
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index f545633..6ccbc32 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -1,5 +1,6 @@
import sys
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QVBoxLayout, QPushButton, QLineEdit, QLabel
+from PyQt5.QtCore import QTimer
from .window_interface import WindowInterface
from queue import Queue, Full, Empty
@@ -8,12 +9,23 @@ from typing import Optional
from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
import tf_transformations as tf_t
+import rclpy
+
+from threading import Thread
+
class GuiWindow(QMainWindow, WindowInterface):
- def __init__(self, logger):
- self.logger = logger
+ def __init__(self, node ):
self.app = QApplication(["node", "1"])
+ self.logger = node.get_logger()
+ self.node = node
super().__init__()
+ # Not working
+ # self.node_spin_timer = QTimer()
+ # self.node_spin_timer.start(1000) # mmm magic_number
+ # self.node_spin_timer.timeout.connect(self._handler_ros_node)
+ self.ros_thread = Thread(target=self._handler_ros_node)
+
self.setWindowTitle("Pose Input")
self.setGeometry(100, 100, 300, 200)
@@ -46,6 +58,10 @@ class GuiWindow(QMainWindow, WindowInterface):
self.tf_queue = Queue(32)# AtomicQueue(256, wait_strategy=BUSY_SPIN_WAIT_STRATEGY)
self.calibration_ready = False
+
+ def _handler_ros_node(self)->None:
+ rclpy.spin(self.node)
+
def calibrate(self) -> None:
self.calibration_ready = True
print("Calibrating...")
@@ -82,16 +98,21 @@ class GuiWindow(QMainWindow, WindowInterface):
return self.tf_queue.get_nowait()
except Empty:
pass # debug log
- finally:
- print("IDK what happend")
- return None
+ except Exception as e:
+ print(e)
+ return None
def run(self):
+ self.ros_thread.start()
self.show()
self.app.exec_()
def __del__(self):
self.app.quit()
+ self.node.destroy_node()
+ self.ros_thread.join()
+ rclpy.shutdown()
+
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
new file mode 100644
index 0000000..062b0fd
--- /dev/null
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -0,0 +1,64 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ camera_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params.yaml")
+ robot_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "calib_conf.json")
+ return LaunchDescription([
+
+# Node(
+# package='tf2_ros',
+# executable='static_transform_publisher',
+# # args = [1,2,0,0,0,0, 'world', 'camera']
+# # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+# # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+# # ]
+# # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+# arguments=["-2.2984534584977494", "0.03178341624420522", "2.2612959861952744", "-1.5959610015041537", "-0.001000506359060393", " -2.5601661670824227", "world", "camera"]
+
+
+# ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.170} # 0.07
+ ],
+ remappings=[
+ # ('/camera/image_raw', '/image_raw'),
+ # ('/camera/camera_info', '/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config],
+ output='screen',
+ namespace='camera',
+ emulate_tty=True
+ ),
+ # world frame publisher
+
+ Node(
+ package='calibration_node',
+ executable='calibration_node',
+# parameters=[
+# {'robots_params': robot_config},
+# {'camera_frames': ["camera"]}
+# ],
+ output='screen',
+ emulate_tty=True
+ )
+ ])
diff --git a/src/calibration_node/setup.py b/src/calibration_node/setup.py
index 06cae11..4261ad8 100644
--- a/src/calibration_node/setup.py
+++ b/src/calibration_node/setup.py
@@ -1,5 +1,6 @@
from setuptools import find_packages, setup
-
+import os
+from glob import glob
package_name = 'calibration_node'
setup(
@@ -9,6 +10,8 @@ setup(
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
+ (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
--
GitLab
From 33212f98cf311700db6c23772dc16b14ab686e5c Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 30 Mar 2024 21:41:57 +0300
Subject: [PATCH 05/64] WIP
Need some debug
---
.../calibration_node/calibration.py | 105 ++++++++++--------
.../calibration_node/gui_window.py | 6 +-
2 files changed, 63 insertions(+), 48 deletions(-)
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 5fc17e3..4414d7e 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -37,6 +37,7 @@ from threading import Thread
from dataclasses import dataclass
from PyQt5.QtCore import QThread
+import yaml
@dataclass
class MatchPose:
"""Class for keeping track of an item in inventory."""
@@ -44,13 +45,23 @@ class MatchPose:
tf_camera: TransformStamped
class CalibrationNode(Node):
+
def __init__(self):
super().__init__("calibration_node")
+ self._init_params()
+
+ self.current_aruco_tf = dict()
+ self.match_pose_arr = list()
+ self.tf_broadcaster = TransformBroadcaster(self)
+ self.timer = self.create_timer(1, self.calib_cb)
+ self.ui_window = GuiWindow(self)
+
+
+ def _init_params(self):
self.declare_parameter("world_tf_topic", "/tf_static")
world_topic = self.get_parameter("world_tf_topic").value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
- self.tf_from_camera_to_world = dict()
self.declare_parameter("aruco_markers_topic", "/aruco_markers")
topic_name = self.get_parameter("aruco_markers_topic").value
@@ -60,32 +71,13 @@ class CalibrationNode(Node):
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
self.aruco_pose = None
- # self.current_aruco_tf = dict()
- self.match_pose_arr = list()
- self.tf_broadcaster = TransformBroadcaster(self)
- self.current_aruco_tf = None
self.declare_parameter("camera_frames", ["camera1"])#, "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
- self.camera_models = {} # {"frame_id": PinholeCameraModel}
-
- self.ui_window = GuiWindow(self)
- # I tried(
- # self.gui_thread = Thread(target=self.ui_window.run)
- # # self.worker = self.ui_window.run
- # self.gui_thread.start()
- # self.ui_window.moveToThread(self.gui_thread)
- # self.gui_thread.run()
- self.get_logger().info("\n\n\ncreate window\n\n\n")
- self.timer = self.create_timer(1, self.calib_cb)
- # self.future = self.create_future()
-# def get_future(self):
-# return self.future
+ self.path_to_save = self.declare_parameter("path_to_save", "./config.yaml")
- def run(self):
- self.ui_window.run() # write ti interface
def get_aruco_pose(self, msg):
- # self.current_aruco_tf.clear()
+ self.current_aruco_tf.clear()
for id, pose in zip(msg.marker_ids, msg.poses):
tf_stamped = TransformStamped()
tf_stamped.transform.translation.x = pose.position.x
@@ -93,61 +85,84 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- tf_stamped.header.frame_id = "aruco" + str(id)
- self.current_aruco_tf = tf_stamped
+ tf_stamped.header.frame_id = str(id)
+ self.current_aruco_tf[id] = tf_stamped
self.aruco_pose = tf_stamped
def calib_cb(self):
pose_ui = self.ui_window.try_get_position()
- print("inside callback")
self.get_logger().info("Timer callback called")
+
# Huetta!
- if (pose_ui is not None and self.current_aruco_tf is not None):
+ if (pose_ui is not None):
pose_ui.header.stamp = self.get_clock().now().to_msg()
- pose_aruco = self.current_aruco_tf
- self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
+ if pose_ui.child_frame_id in self.current_aruco_tf:
+ pose_aruco = self.current_aruco_tf[pose_ui.child_frame_id]
+ self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
+
if (self.ui_window.is_calibrate_ready()):
self.calibrate()
del self.ui_window
- self.calibrate()
def calibrate(self):
# use only last
+ translations_xyz = []
+ orientations_rpy = []
for match_pose in self.match_pose_arr:
- # transform = TransformStamped()
- # transform.header = pose.header
- # transform.child_frame_id = "aruco" + str(id)
- # (
- # transform.transform.translation.x,
- # transform.transform.translation.y,
- # transform.transform.translation.z,
- # ) = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
- # transform.transform.rotation = pose.pose.orientation
-
buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
- print (type(match_pose.tf_camera))
buffer_core.set_transform(match_pose.tf_camera, "default_authority")
buffer_core.set_transform(match_pose.tf_ui, "default_authority")
inverse_transform = buffer_core.lookup_transform_core(
match_pose.tf_ui.header.frame_id, match_pose.tf_camera.header.frame_id, rclpy.time.Time()
# Opposite i think
- #transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
)
self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
+ trans = inverse_transform.transform.translation
+ translation_arr = [trans.x, trans.y, trans.z]
quat = rot.x, rot.y, rot.z, rot.w
euler = tf_t.euler_from_quaternion(quat)
+ translations_xyz.append(translation_arr)
+ orientations_rpy.append(euler) # Not true
print("TF RPY: ", euler )
print("Ibverssed TF: ", inverse_transform.transform.translation)
-
+
+ mid_translation = np.matrix(translations_xyz).mean(axis=1)
+ mid_orientation_rpy = np.matrix(orientations_rpy).mean(axis=1)
+ print ("Current camera position:")
+ print(f"Orientation RPY:{mid_orientation_rpy}")
+ quat = tf_t.quaternion_from_euler(mid_orientation_rpy)
+ print(f"Orientation xyzw:{quat}")
+ print(f"Translation: {mid_translation}")
+ self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
+
+ def _save_transform_to_yaml(self, translation, quaternion, frame_id, child_frame_id):
+ # Prepare the data structure
+ data = {
+ 'static_transform_publisher': {
+ 'x': translation[0],
+ 'y': translation[1],
+ 'z': translation[2],
+ 'roll': quaternion[0],
+ 'pitch': quaternion[1],
+ 'yaw': quaternion[2],
+ 'frame_id': frame_id,
+ 'child_frame_id': child_frame_id
+ }
+ }
+
+ # Save the data to a YAML file
+ with open(self.path_to_save, 'w') as file:
+ yaml.dump(data, file, default_flow_style=False)
+
+
+ def run(self):
+ self.ui_window.run() # write ti interface
def main(args=None):
rclpy.init()
minimal_node = CalibrationNode() # add start function
minimal_node.run()
-# future = minimal_node.get_future()
-# rclpy.spin_until_future_complete(minimal_node, future)
-
if __name__ == '__main__':
main()
\ No newline at end of file
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 6ccbc32..1096a8b 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -79,15 +79,15 @@ class GuiWindow(QMainWindow, WindowInterface):
self.tf.transform.rotation.z = quat[2]
self.tf.transform.rotation.z = quat[3]
self.tf.header.frame_id = "world"
- self.tf.child_frame_id = "aruco" + self.pose_inputs[0].text()
+ self.tf.child_frame_id = self.pose_inputs[0].text()
except Exception as e:
print(e)
try:
self.tf_queue.put(self.tf)
except Full:
print("Queue is full")
- finally:
- "IDK what happend"
+ except Exception as e:
+ print("IDK what happend:", e)
print("Next...")
def is_calibrate_ready(self) -> bool:
--
GitLab
From 5191fd15a0de01f38b2384cda6d452b1eb0011b9 Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Tue, 2 Apr 2024 19:42:35 +0300
Subject: [PATCH 06/64] copied pf from robotics course
---
src/localization/ParticleFilter.py | 262 +++++++++++++++++++++++
src/localization/field.py | 8 +
src/localization/landmarks.json | 8 +
src/localization/parfield.json | 20 ++
src/localization/particle.py | 38 ++++
src/localization/pf_constants.json | 20 ++
src/localization/pf_visualization.py | 80 +++++++
src/localization/robot.py | 47 ++++
src/notebooks/Seminar-Localisation.ipynb | 187 ++++++++++++++++
9 files changed, 670 insertions(+)
create mode 100644 src/localization/ParticleFilter.py
create mode 100644 src/localization/field.py
create mode 100644 src/localization/landmarks.json
create mode 100644 src/localization/parfield.json
create mode 100644 src/localization/particle.py
create mode 100644 src/localization/pf_constants.json
create mode 100644 src/localization/pf_visualization.py
create mode 100644 src/localization/robot.py
create mode 100644 src/notebooks/Seminar-Localisation.ipynb
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
new file mode 100644
index 0000000..1e4e942
--- /dev/null
+++ b/src/localization/ParticleFilter.py
@@ -0,0 +1,262 @@
+import math
+import json
+import time
+
+import random
+from particle import Particle
+from robot import Robot
+
+
+class ParticleFilter():
+ def __init__(self, myrobot, field, landmarks, n=100, sense_noise = 0.4, apr = True):
+ self.n = n
+ self.myrobot = myrobot
+ self.landmarks = landmarks
+ self.count = 0
+ self.p = []
+
+ with open('../localization/pf_constants.json', 'r') as constants:
+ constants = json.load(constants)
+
+ self.forward_noise = constants['noise']['forward_noise']
+ self.turn_noise = constants['noise']['turn_noise']
+ self.sense_noise = constants['noise']['sense_noise']
+ self.gauss_noise = constants['noise']['gauss_noise']
+ self.yaw_noise = constants['noise']['yaw_noise']
+
+ self.number_of_res = constants['consistency']['number_of_res']
+ self.consistency = constants['consistency']['consistency']
+ self.goodObsGain = constants['consistency']['goodObsGain']
+ self.badObsCost = constants['consistency']['badObsCost']
+ self.stepCost = constants['consistency']['stepCost']
+ self.dist_threshold = constants['consistency']['dist_threshold']
+ self.con_threshold = constants['consistency']['con_threshold']
+ self.spec_threshold = constants['consistency']['spec_threshold']
+
+ if apr:
+ self.gen_particles()
+ else:
+ for i in range(self.n):
+ x = Robot((random.random()-0.5)*field.w_length, (random.random()-0.5)*field.w_width, random.random()*math.pi*2)
+ self.p.append([x,0])
+
+
+ def return_coord(self):
+ return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
+
+ def gen_particles(self):
+ self.p = []
+ for i in range(self.n):
+ x_coord = self.myrobot.x + random.gauss(0, self.sense_noise)
+ y_coord = self.myrobot.y + random.gauss(0, self.sense_noise)
+ yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
+ if yaw < 0:
+ yaw = 2*math.pi + yaw
+ if yaw > 2*math.pi:
+ yaw %= (2 * math.pi*180/math.pi) #TODO
+ self.p.append([Particle(x_coord, y_coord, yaw), 0])
+ self.count += 1
+
+ def gen_n_particles_robot(self, n):
+ p = []
+ for i in range(n):
+ x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
+ y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
+ yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
+ if yaw < 0:
+ yaw = 2*math.pi + yaw
+ if yaw > 2*math.pi:
+ yaw %= (2 * math.pi)
+ p.append([Particle(x_coord, y_coord, yaw), 0])
+ return p
+
+ def uniform_reset(self):
+ self.p = []
+ for i in range(self.n):
+ x = Robot((random()-0.5)*field.w_length, (random()-0.5)
+ * field.w_width, random()*math.pi*2)
+ #x.set_noise(forward_noise, turn_noise, 0)
+ self.p.append([x, 0])
+ self.myrobot.update_coord(self.p)
+
+ def update_consistency(self, observations):
+ #prob = self.myrobot.observation_score(observations)
+ stepConsistency = 0
+ for color_landmarks in observations:
+ if (color_landmarks not in self.landmarks):
+ continue
+
+ if len(observations[color_landmarks]) != 0:
+ for observation in observations[color_landmarks]:
+ dists = []
+ for landmark in self.landmarks[color_landmarks]:
+
+ # calc posts coords in field for every mesurement
+ x_posts = (self.myrobot.x + observation[0]*math.cos(self.myrobot.yaw)
+ - observation[1]*math.sin(self.myrobot.yaw))
+ y_posts = (self.myrobot.y + observation[0]*math.sin(self.myrobot.yaw)
+ + observation[1]*math.cos(self.myrobot.yaw))
+ #print('x_posts, y_posts', x_posts, y_posts)
+ dist = math.sqrt(
+ (x_posts - landmark[0])**2 + (y_posts - landmark[1])**2)
+ dists.append(dist)
+ #print('dist, len =', dist, len(dists))
+ if min(dists) < self.dist_threshold:
+ stepConsistency += self.goodObsGain
+
+ #print('good step', stepConsistency)
+ else:
+ stepConsistency -= self.badObsCost
+ #print('bad step', stepConsistency)
+ # else:
+ #stepConsistency -= self.stepCost
+ #print('step cons', stepConsistency)
+ self.consistency += stepConsistency
+ if math.fabs(self.consistency) > self.spec_threshold:
+ self.consistency = math.copysign(
+ self.spec_threshold, self.consistency)
+ #print('consistency', self.consistency)
+
+ def particles_move(self, coord):
+ self.myrobot.move(coord['shift_x'],
+ coord['shift_y'], coord['shift_yaw'])
+ # now we simulate a robot motion for each of
+ # these particles
+ for partic in self.p:
+ partic[0].move(coord['shift_x'], coord['shift_y'],
+ coord['shift_yaw'])
+ self.count += 1
+
+ def gen_n_particles_robot(self, n):
+ p = []
+ for i in range(n):
+ x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
+ y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
+ yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
+ if yaw < 0:
+ yaw = 2*math.pi + yaw
+ if yaw > 2*math.pi:
+ yaw %= (2 * math.pi)
+ p.append([Particle(x_coord, y_coord, yaw), 0])
+ return p
+
+ def gen_n_particles(self, n):
+ tmp = []
+ for i in range(n):
+ x = Robot((random()-0.5)*field.w_width, (random()-0.5)
+ * field.w_length, random()*math.pi*2)
+ tmp.append([x, 0])
+ return tmp
+
+ def observation_to_predict(self, observations):
+ predicts = []
+ for color_landmarks in observations:
+ if (color_landmarks not in self.landmarks):
+ continue
+
+ for landmark in self.landmarks[color_landmarks]:
+ if len(observations[color_landmarks]) != 0:
+ for obs in observations[color_landmarks]:
+ y_posts = self.myrobot.x + \
+ obs[0]*math.sin(self.myrobot.yaw) + \
+ obs[1]*math.cos(self.myrobot.yaw)
+ x_posts = self.myrobot.y + \
+ obs[0]*math.cos(self.myrobot.yaw) - \
+ obs[1]*math.sin(self.myrobot.yaw)
+ predicts.append([x_posts, y_posts])
+ return predicts
+
+ def resampling_wheel(self, weights, p_tmp):
+ new_particles = {}
+ index = int(random.random() * self.n)
+ beta = 0.0
+ mw = max(weights)
+ # print(mw)
+ for i in range(self.n):
+ beta += random.random() * 2.0 * mw
+ while beta > weights[index]:
+ beta -= weights[index]
+ index = (index + 1) % self.n
+ if index in new_particles.keys():
+ new_particles[index] += 1
+ else:
+ new_particles[index] = 1
+ # p_tmp.append([self.p[index][0],w[index]])
+ for el in new_particles:
+ p_tmp.append([self.p[el][0], weights[el]*new_particles[el]])
+ return p_tmp
+
+ def resampling(self, observations):
+ p_tmp = []
+ w = []
+ S = 0
+ for i in range(self.n):
+ w.append(self.p[i][0].observation_score(
+ observations, self.landmarks, self.sense_noise))
+ S += (w[i])
+ for i in range(self.n):
+ w[i] = w[i]/S
+ self.resampling_wheel(w, p_tmp)
+ S = 0
+ for i in range(len(p_tmp)):
+ S += p_tmp[i][1]
+ for i in range(len(p_tmp)):
+ p_tmp[i][1] /= S
+ # if p_tmp[i][1] > 0.02:
+ #print("x y yaw ", p_tmp[i][0].x, p_tmp[i][0].y, p_tmp[i][0].yaw*180/math.pi)
+ self.update_coord(p_tmp)
+ self.update_consistency(observations)
+ # def rationing(weights, sum):
+ # for i in range(len(weights))
+ new_particles = self.gen_n_particles_robot(self.n - len(p_tmp))
+
+ p_tmp.extend(new_particles)
+ self.p = p_tmp
+ self.count += 1
+ self.update_consistency(observations)
+
+
+ def custom_reset(self, x, y, yaw):
+ self.myrobot.x = x
+ self.myrobot.y = y
+ self.myrobot.yaw = yaw
+ self.p = self.gen_n_particles_robot(self.n)
+
+ def fall_reset(self, observations):
+ self.custom_reset(self.myrobot.x + random.gauss(0, self.sense_noise),
+ self.myrobot.y + random.gauss(0, self.sense_noise),
+ self.myrobot.yaw + random.gauss(0, self.yaw_noise))
+ self.resampling(observations)
+ self.update_consistency(observations)
+
+ def update_coord(self, particles):
+ x = 0.0
+ y = 0.0
+ orientation = 0.0
+ adding = 0.0
+ if (self.myrobot.yaw < math.pi/2) or (self.myrobot.yaw > math.pi*3/2):
+ adding = math.pi*2
+ for particle in particles:
+ x += particle[0].x * particle[1]
+ y += particle[0].y * particle[1]
+ if (particle[0].yaw < math.pi):
+ culc_yaw = particle[0].yaw + adding
+ else:
+ culc_yaw = particle[0].yaw
+ orientation += culc_yaw * particle[1]
+ self.myrobot.x = x
+ self.myrobot.y = y
+ self.myrobot.yaw = orientation % (2*math.pi)
+
+ def return_coord(self):
+ return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
+
+
+def updatePF(pf, measurement):
+ k = pf.number_of_res
+ if pf.consistency < pf.con_threshold:
+ k += 1
+ for i in range(2):
+ pf.resampling(measurement)
+ print('eto coord', pf.return_coord(), pf.myrobot.yaw*180/math.pi)
+ return pf.return_coord()
diff --git a/src/localization/field.py b/src/localization/field.py
new file mode 100644
index 0000000..b2fa6a7
--- /dev/null
+++ b/src/localization/field.py
@@ -0,0 +1,8 @@
+import json
+
+class Field:
+ def __init__(self, path):
+ with open(path, "r") as f:
+ self.field = json.loads(f.read())
+ self.w_width = self.field['main_rectangle'][0][0]
+ self.w_length = self.field['main_rectangle'][0][1]
diff --git a/src/localization/landmarks.json b/src/localization/landmarks.json
new file mode 100644
index 0000000..d06b2f8
--- /dev/null
+++ b/src/localization/landmarks.json
@@ -0,0 +1,8 @@
+{
+ "blue_posts": [
+ [ -4.5, -1.0], [ -4.5, 1.0]
+ ],
+ "yellow_posts":[
+ [ 4.5, -1.0], [ 4.5, 1.0]
+ ]
+}
diff --git a/src/localization/parfield.json b/src/localization/parfield.json
new file mode 100644
index 0000000..e6e8b9f
--- /dev/null
+++ b/src/localization/parfield.json
@@ -0,0 +1,20 @@
+{
+ "circles": [
+ [0.0, 0.0, 1.0]
+ ],
+ "lines": [
+ [[-3.0, 3.0], [0.0, 0.0]]
+ ],
+ "points": [
+ [0, 0]
+ ],
+ "main_rectangle": [
+ [6.0, 9.0]
+ ],
+ "red_posts":[
+ [-0.75, -1.8], [0.75, -1.8]
+ ],
+ "rectangles": [
+ [[-1, -4.5], 2, 1],[[-1,3.5],2,1]
+ ]
+}
diff --git a/src/localization/particle.py b/src/localization/particle.py
new file mode 100644
index 0000000..0617dd6
--- /dev/null
+++ b/src/localization/particle.py
@@ -0,0 +1,38 @@
+import math
+import sys
+
+import random
+from robot import Robot
+
+
+def gaussian(x, sigma):
+ # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
+ return math.exp(-(x ** 2) / 2*(sigma ** 2)) / math.sqrt(2.0 * math.pi * (sigma ** 2))
+
+class Particle(Robot):
+
+ def observation_score(self, observations, landmarks, sense_noise):
+ # particle weight calculation
+ prob = 1.0
+ for color_landmarks in observations:
+ if (color_landmarks not in landmarks):
+ continue
+
+ for landmark in landmarks[color_landmarks]:
+ dists = []
+ if observations[color_landmarks]:
+
+ for observation in observations[color_landmarks]:
+ # calc posts coords in field for every mesurement
+ x_posts = self.x + \
+ observation[0]*math.cos(self.yaw) - \
+ observation[1]*math.sin(self.yaw)
+ y_posts = self.y + \
+ observation[0]*math.sin(self.yaw) + \
+ observation[1]*math.cos(self.yaw)
+ dist = math.sqrt(
+ (x_posts - landmark[0])**2 + (y_posts - landmark[1])**2)
+ dists.append(dist)
+ if (dists != []):
+ prob *= gaussian(min(dists), sense_noise)
+ return prob
diff --git a/src/localization/pf_constants.json b/src/localization/pf_constants.json
new file mode 100644
index 0000000..d84f822
--- /dev/null
+++ b/src/localization/pf_constants.json
@@ -0,0 +1,20 @@
+{
+ "noise": {
+ "forward_noise": 0.025,
+ "turn_noise": 0.1,
+ "sense_noise": 0.4,
+ "gauss_noise": 0.4,
+ "yaw_noise": 0.1
+ },
+
+ "consistency":{
+ "number_of_res":3,
+ "consistency":0.0,
+ "goodObsGain":0.2,
+ "badObsCost":0.1,
+ "stepCost":0.1,
+ "dist_threshold":0.3,
+ "con_threshold": 0.0,
+ "spec_threshold":1.5
+ }
+}
diff --git a/src/localization/pf_visualization.py b/src/localization/pf_visualization.py
new file mode 100644
index 0000000..eca8cbc
--- /dev/null
+++ b/src/localization/pf_visualization.py
@@ -0,0 +1,80 @@
+import matplotlib.pylab as plt
+from field import Field
+import math
+field = Field('../localization/parfield.json')
+def visualization(robot, pr, factor = 7 ):
+
+ plt.figure("Robot in the world",figsize=(field.w_width, field.w_length))
+ plt.title('Particle filter')
+
+ # draw coordinate grid for plotting
+ grid = [-field.w_width/2.0, field.w_width/2.0, -field.w_length/2.0, field.w_length/2.0]
+ ax = plt.axis(grid)
+
+ landmarks = {
+ "blue_posts": [
+ [ -1.0, -4.5], [ 1.0, -4.5]
+ ],
+ "yellow_posts":[[ -1.0, 4.5], [1.0, 4.5]]}
+
+ for el in field.field:
+ if el == 'circles':
+ for circle in field.field['circles']:
+ plot_circle = plt.Circle((circle[0], circle[1]), circle[2],linewidth=2, fill=False, edgecolor='#330000')
+ plt.gca().add_patch(plot_circle)
+ if el == 'lines' :
+ for line in field.field['lines']:
+ plot_line = plt.Line2D(line[0], line[1], linewidth=2, linestyle="-", color='#330000')
+ plt.gca().add_line(plot_line)
+ if el == 'rectangles' :
+ for rectangle in field.field['rectangles']:
+ rect = plt.Rectangle(rectangle[0], rectangle[1], rectangle[2], linewidth=2, linestyle="-", fill = False, edgecolor='#330000')
+ plt.gca().add_patch(rect)
+ '''
+ # draw particles
+ for ind in range(len(p)):
+
+ # particle
+ circle = plt.Circle((p[ind][0].x, p[ind][0].y), 1./factor/2, facecolor='#ffb266', edgecolor='#994c00', alpha=0.5)
+ plt.gca().add_patch(circle)
+
+ # particle's orientation
+ arrow = plt.Arrow(p[ind][0].x, p[ind][0].y, 2*math.cos(p[ind][0].yaw)/factor, 2*math.sin(p[ind][0].yaw)/factor, width=1/factor, alpha=1., facecolor='#994c00', edgecolor='#994c00')
+ plt.gca().add_patch(arrow)
+ '''
+ # draw resampled particles
+ for ind in range(len(pr)):
+
+ # particle
+ circle = plt.Circle((pr[ind][0].y, pr[ind][0].x), 1./factor/2, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
+ plt.gca().add_patch(circle)
+
+ # particle's orientation
+ arrow = plt.Arrow(pr[ind][0].y, pr[ind][0].x, 2*math.sin(pr[ind][0].yaw)/factor, math.cos(pr[ind][0].yaw)/factor,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
+ plt.gca().add_patch(arrow)
+
+
+ # robot's location
+ circle = plt.Circle((robot.y, robot.x), 1./factor, facecolor='#FF66E9', edgecolor='#FF66E9')
+ plt.gca().add_patch(circle)
+
+ # robot's orientation
+ arrow = plt.Arrow(robot.y, robot.x, 3*math.sin(robot.yaw)/factor, 3*math.cos(robot.yaw)/factor, width=1.0/factor, alpha=0.5, facecolor='#000000', edgecolor='#000000')
+ plt.gca().add_patch(arrow)
+
+
+ #fixed landmarks of known locations2
+
+ for lm in landmarks:
+ for lms in landmarks[lm]:
+ if lm == "yellow_posts":
+ circle = plt.Circle(((lms[0], lms[1])),
+ 1./factor, facecolor = '#ffff00', edgecolor='#330000')
+ plt.gca().add_patch(circle)
+ else:
+ circle = plt.Circle(((lms[0], lms[1])),
+ 1./factor, facecolor = '#060C73', edgecolor='#330000')
+ plt.gca().add_patch(circle)
+
+
+ #plt.close()
\ No newline at end of file
diff --git a/src/localization/robot.py b/src/localization/robot.py
new file mode 100644
index 0000000..dc03c9e
--- /dev/null
+++ b/src/localization/robot.py
@@ -0,0 +1,47 @@
+import math
+
+
+class Robot:
+ def __init__(self, x=0, y=0, yaw=0):
+ self.x = x # robot's x coordinate
+ self.y = y # robot's y coordinate
+ self.yaw = yaw # robot's angle
+
+ def set_coord(self, new_x, new_y, new_orientation):
+ self.x = float(new_x)
+ self.y = float(new_y)
+ self.yaw = float(new_orientation)
+
+ def sense(self, landmarks):
+ z = []
+ for i in range(len(landmarks)):
+ dist = math.sqrt((self.x - landmarks[i][0]) ** 2
+ + (self.y - landmarks[i][1]) ** 2)
+ z.append(dist)
+ return z
+
+ def move(self, x, y, yaw):
+ # turn, and add randomomness to the turning command
+ orientation = self.yaw + float(yaw)
+ if orientation < 0:
+ orientation += (math.pi*2)
+ orientation %= (2 * math.pi)
+ self.x += x*math.cos(self.yaw)
+ self.y += x*math.sin(self.yaw)
+ self.yaw = orientation
+
+ def observation_to_predict(self, observations, landmarks):
+ predicts = []
+ for color_landmarks in landmarks:
+ if (color_landmarks not in landmarks):
+ continue
+
+ for landmark in landmarks[color_landmarks]:
+ x_posts = self.x - \
+ observation[0]*math.sin(-self.yaw) + \
+ observation[1]*math.cos(-self.yaw)
+ y_posts = self.y + \
+ observation[0]*math.cos(-self.yaw) - \
+ observation[1]*math.sin(-self.yaw)
+ predicts.append([x_posts, y_posts])
+ return predicts
\ No newline at end of file
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
new file mode 100644
index 0000000..733d31d
--- /dev/null
+++ b/src/notebooks/Seminar-Localisation.ipynb
@@ -0,0 +1,187 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# КолеÑо отÑева "
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import random\n",
+ "\n",
+ "def resampling_wheel(weights, N, particleList):\n",
+ " new_particles = []\n",
+ " index = int(random.randint(0, N))\n",
+ " beta = 0.0\n",
+ " max_weight = max(weights)\n",
+ " for i in range(N):\n",
+ " beta += random.uniform(0, 2.0 * max_weight)\n",
+ " while beta > weights[index]:\n",
+ " beta -= weights[index]\n",
+ " index = (index + 1) % N\n",
+ " new_particles.append(particleList[index])\n",
+ " print(new_particles)\n",
+ " particleList = new_particles\n",
+ " "
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "weights = [0.12, 0.16, 0.14, 0.1, 0.1, 0.38]\n",
+ "particleList = ['particle_1', 'particle_2', 'particle_3', 'particle_4', 'particle_5', 'particle_6']"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "resampling_wheel(weights, 6, particleList)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Фильтр чаÑтиц"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "!pip3 install matplotlib"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import sys\n",
+ "import json\n",
+ "import math\n",
+ "\n",
+ "sys.path.append(\"../localization/\")\n",
+ "\n",
+ "from ParticleFilter import ParticleFilter\n",
+ "from robot import Robot\n",
+ "from field import Field\n",
+ "\n",
+ "with open(\"../localization/landmarks.json\", \"r\") as f:\n",
+ " landmarks = json.loads(f.read())\n",
+ "from pf_visualization import visualization"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), landmarks, n = 500, apr = True)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 3,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "shift = {'shift_x':1.0, 'shift_y':0.01, 'shift_yaw':0}\n",
+ "observations = {\"yellow_posts\":[[4.3, -0.9],[4.2, 1.1]], \"blue_posts\":[]}"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf.particles_move(shift)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf.resampling(observations)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf.fall_reset(observations)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 4,
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "image/png": "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\n",
+ "text/plain": [
+ "<Figure size 432x648 with 1 Axes>"
+ ]
+ },
+ "metadata": {
+ "needs_background": "light"
+ },
+ "output_type": "display_data"
+ }
+ ],
+ "source": [
+ "visualization(pf.myrobot, pf.p, factor = 7)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": []
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 3",
+ "language": "python",
+ "name": "python3"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.7.3"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}
--
GitLab
From 2c8cda354f64bc78e4c9c22d7e76732598cbc1de Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Wed, 3 Apr 2024 11:18:46 +0300
Subject: [PATCH 07/64] corrected particles_move
---
src/localization/ParticleFilter.py | 57 +++++-------------------------
src/localization/particle.py | 25 ++-----------
src/localization/robot.py | 4 +--
3 files changed, 14 insertions(+), 72 deletions(-)
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index 1e4e942..5e928f0 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -8,10 +8,9 @@ from robot import Robot
class ParticleFilter():
- def __init__(self, myrobot, field, landmarks, n=100, sense_noise = 0.4, apr = True):
+ def __init__(self, myrobot, field, n=100, sense_noise = 0.4, apr = True):
self.n = n
self.myrobot = myrobot
- self.landmarks = landmarks
self.count = 0
self.p = []
@@ -80,34 +79,14 @@ class ParticleFilter():
self.myrobot.update_coord(self.p)
def update_consistency(self, observations):
- #prob = self.myrobot.observation_score(observations)
stepConsistency = 0
- for color_landmarks in observations:
- if (color_landmarks not in self.landmarks):
- continue
-
- if len(observations[color_landmarks]) != 0:
- for observation in observations[color_landmarks]:
- dists = []
- for landmark in self.landmarks[color_landmarks]:
-
- # calc posts coords in field for every mesurement
- x_posts = (self.myrobot.x + observation[0]*math.cos(self.myrobot.yaw)
- - observation[1]*math.sin(self.myrobot.yaw))
- y_posts = (self.myrobot.y + observation[0]*math.sin(self.myrobot.yaw)
- + observation[1]*math.cos(self.myrobot.yaw))
- #print('x_posts, y_posts', x_posts, y_posts)
- dist = math.sqrt(
- (x_posts - landmark[0])**2 + (y_posts - landmark[1])**2)
- dists.append(dist)
- #print('dist, len =', dist, len(dists))
- if min(dists) < self.dist_threshold:
- stepConsistency += self.goodObsGain
-
- #print('good step', stepConsistency)
- else:
- stepConsistency -= self.badObsCost
- #print('bad step', stepConsistency)
+ dist = math.sqrt((self.myrobot.x - observations[0])**2 + (self.myrobot.y - observations[1])**2)
+ if dist < self.dist_threshold:
+ stepConsistency += self.goodObsGain
+ #print('good step', stepConsistency)
+ else:
+ stepConsistency -= self.badObsCost
+ #print('bad step', stepConsistency)
# else:
#stepConsistency -= self.stepCost
#print('step cons', stepConsistency)
@@ -148,24 +127,6 @@ class ParticleFilter():
tmp.append([x, 0])
return tmp
- def observation_to_predict(self, observations):
- predicts = []
- for color_landmarks in observations:
- if (color_landmarks not in self.landmarks):
- continue
-
- for landmark in self.landmarks[color_landmarks]:
- if len(observations[color_landmarks]) != 0:
- for obs in observations[color_landmarks]:
- y_posts = self.myrobot.x + \
- obs[0]*math.sin(self.myrobot.yaw) + \
- obs[1]*math.cos(self.myrobot.yaw)
- x_posts = self.myrobot.y + \
- obs[0]*math.cos(self.myrobot.yaw) - \
- obs[1]*math.sin(self.myrobot.yaw)
- predicts.append([x_posts, y_posts])
- return predicts
-
def resampling_wheel(self, weights, p_tmp):
new_particles = {}
index = int(random.random() * self.n)
@@ -192,7 +153,7 @@ class ParticleFilter():
S = 0
for i in range(self.n):
w.append(self.p[i][0].observation_score(
- observations, self.landmarks, self.sense_noise))
+ observations, self.sense_noise))
S += (w[i])
for i in range(self.n):
w[i] = w[i]/S
diff --git a/src/localization/particle.py b/src/localization/particle.py
index 0617dd6..317a87d 100644
--- a/src/localization/particle.py
+++ b/src/localization/particle.py
@@ -11,28 +11,9 @@ def gaussian(x, sigma):
class Particle(Robot):
- def observation_score(self, observations, landmarks, sense_noise):
+ def observation_score(self, observations, sense_noise):
# particle weight calculation
prob = 1.0
- for color_landmarks in observations:
- if (color_landmarks not in landmarks):
- continue
-
- for landmark in landmarks[color_landmarks]:
- dists = []
- if observations[color_landmarks]:
-
- for observation in observations[color_landmarks]:
- # calc posts coords in field for every mesurement
- x_posts = self.x + \
- observation[0]*math.cos(self.yaw) - \
- observation[1]*math.sin(self.yaw)
- y_posts = self.y + \
- observation[0]*math.sin(self.yaw) + \
- observation[1]*math.cos(self.yaw)
- dist = math.sqrt(
- (x_posts - landmark[0])**2 + (y_posts - landmark[1])**2)
- dists.append(dist)
- if (dists != []):
- prob *= gaussian(min(dists), sense_noise)
+ dist = math.sqrt((self.x - observations[0])**2 + (self.y - observations[1])**2)
+ prob *= gaussian(dist, sense_noise)
return prob
diff --git a/src/localization/robot.py b/src/localization/robot.py
index dc03c9e..798e488 100644
--- a/src/localization/robot.py
+++ b/src/localization/robot.py
@@ -26,8 +26,8 @@ class Robot:
if orientation < 0:
orientation += (math.pi*2)
orientation %= (2 * math.pi)
- self.x += x*math.cos(self.yaw)
- self.y += x*math.sin(self.yaw)
+ self.x += x*math.cos(self.yaw) - y * math.sin(self.yaw)
+ self.y += x*math.sin(self.yaw) + y * math.cos(self.yaw)
self.yaw = orientation
def observation_to_predict(self, observations, landmarks):
--
GitLab
From 6e3a32aea1919bb079e4d3590575ce117c00f031 Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Wed, 3 Apr 2024 11:52:58 +0300
Subject: [PATCH 08/64] fixed uniform_reset
---
src/localization/ParticleFilter.py | 15 +++++++++------
src/localization/particle.py | 4 +++-
2 files changed, 12 insertions(+), 7 deletions(-)
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index 5e928f0..526169f 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -13,6 +13,7 @@ class ParticleFilter():
self.myrobot = myrobot
self.count = 0
self.p = []
+ self.field = field
with open('../localization/pf_constants.json', 'r') as constants:
constants = json.load(constants)
@@ -72,15 +73,17 @@ class ParticleFilter():
def uniform_reset(self):
self.p = []
for i in range(self.n):
- x = Robot((random()-0.5)*field.w_length, (random()-0.5)
- * field.w_width, random()*math.pi*2)
+ x = Particle(x=(random.random()-0.5)*self.field.w_length, y=(random.random()-0.5)
+ * self.field.w_width, yaw=random.random()*math.pi*2)
#x.set_noise(forward_noise, turn_noise, 0)
self.p.append([x, 0])
- self.myrobot.update_coord(self.p)
+ self.update_coord(self.p)
def update_consistency(self, observations):
stepConsistency = 0
- dist = math.sqrt((self.myrobot.x - observations[0])**2 + (self.myrobot.y - observations[1])**2)
+ dist = math.sqrt((self.myrobot.x - observations[0])**2 +\
+ (self.myrobot.y - observations[1])**2 +\
+ (self.myrobot.yaw - observations[2])**2)
if dist < self.dist_threshold:
stepConsistency += self.goodObsGain
#print('good step', stepConsistency)
@@ -122,8 +125,8 @@ class ParticleFilter():
def gen_n_particles(self, n):
tmp = []
for i in range(n):
- x = Robot((random()-0.5)*field.w_width, (random()-0.5)
- * field.w_length, random()*math.pi*2)
+ x = Robot((random()-0.5)*self.field.w_width, (random()-0.5)
+ * self.field.w_length, random()*math.pi*2)
tmp.append([x, 0])
return tmp
diff --git a/src/localization/particle.py b/src/localization/particle.py
index 317a87d..a982d4a 100644
--- a/src/localization/particle.py
+++ b/src/localization/particle.py
@@ -14,6 +14,8 @@ class Particle(Robot):
def observation_score(self, observations, sense_noise):
# particle weight calculation
prob = 1.0
- dist = math.sqrt((self.x - observations[0])**2 + (self.y - observations[1])**2)
+ dist = math.sqrt((self.x - observations[0])**2 + \
+ (self.y - observations[1])**2 + \
+ (self.yaw - observations[2])**2)
prob *= gaussian(dist, sense_noise)
return prob
--
GitLab
From d90e2ef83a4eda94befe91b668589b3ccc0df11a Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Wed, 3 Apr 2024 15:31:21 +0300
Subject: [PATCH 09/64] changed half of the code to numpy
---
src/localization/ParticleFilter.py | 154 +++++++++++++--------------
src/localization/pf_visualization.py | 6 +-
2 files changed, 78 insertions(+), 82 deletions(-)
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index 526169f..a244d1f 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -2,17 +2,18 @@ import math
import json
import time
-import random
+import numpy as np
+from numpy import random
from particle import Particle
from robot import Robot
class ParticleFilter():
- def __init__(self, myrobot, field, n=100, sense_noise = 0.4, apr = True):
+ def __init__(self, myrobot, field, n=100, apr = True):
self.n = n
self.myrobot = myrobot
self.count = 0
- self.p = []
+ self.p = np.zeros((n, 4))
self.field = field
with open('../localization/pf_constants.json', 'r') as constants:
@@ -45,16 +46,12 @@ class ParticleFilter():
return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
def gen_particles(self):
- self.p = []
- for i in range(self.n):
- x_coord = self.myrobot.x + random.gauss(0, self.sense_noise)
- y_coord = self.myrobot.y + random.gauss(0, self.sense_noise)
- yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi*180/math.pi) #TODO
- self.p.append([Particle(x_coord, y_coord, yaw), 0])
+ self.p [:, :2]= np.array([[self.myrobot.x, self.myrobot.y]]) + \
+ random.normal(scale=self.sense_noise, size=(self.n, 2))
+ self.p [:, 2]= self.myrobot.yaw + \
+ random.normal(scale=self.yaw_noise, size=(self.n, ))
+ self.p [:, 2]= self.p[:, 2] % (2 * np.pi)
+
self.count += 1
def gen_n_particles_robot(self, n):
@@ -104,22 +101,28 @@ class ParticleFilter():
coord['shift_y'], coord['shift_yaw'])
# now we simulate a robot motion for each of
# these particles
- for partic in self.p:
- partic[0].move(coord['shift_x'], coord['shift_y'],
- coord['shift_yaw'])
+ self.p += np.array([[coord['shift_x'], coord['shift_y'],
+ coord['shift_yaw'], 0.0]])
self.count += 1
def gen_n_particles_robot(self, n):
- p = []
- for i in range(n):
- x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
- y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
- yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi)
- p.append([Particle(x_coord, y_coord, yaw), 0])
+ p = np.zeros((n, 4))
+ p[:, :2]= np.array([[self.myrobot.x, self.myrobot.y]]) + \
+ random.normal(scale=self.sense_noise, size=(n, 2))
+ p[:, 2]= self.myrobot.yaw + \
+ random.normal(scale=self.yaw_noise, size=(n, ))
+ p[:, 2]= p[:, 2] % (2 * np.pi)
+
+ self.count += 1
+ # for i in range(n):
+ # x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
+ # y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
+ # yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
+ # if yaw < 0:
+ # yaw = 2*math.pi + yaw
+ # if yaw > 2*math.pi:
+ # yaw %= (2 * math.pi)
+ # p.append([Particle(x_coord, y_coord, yaw), 0])
return p
def gen_n_particles(self, n):
@@ -129,13 +132,18 @@ class ParticleFilter():
* self.field.w_length, random()*math.pi*2)
tmp.append([x, 0])
return tmp
+
+ def compute_weights(self, observation, sigma):
+ dist = np.sqrt(np.square(self.p[:, :3] - observation).sum(axis=-1)) # TODO correct comp for yaw(+- pi)
+ prob = np.exp(-(dist ** 2) / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
+ return prob
+
- def resampling_wheel(self, weights, p_tmp):
+ def resampling_wheel(self, weights):
new_particles = {}
index = int(random.random() * self.n)
beta = 0.0
- mw = max(weights)
- # print(mw)
+ mw = np.max(weights)
for i in range(self.n):
beta += random.random() * 2.0 * mw
while beta > weights[index]:
@@ -145,40 +153,26 @@ class ParticleFilter():
new_particles[index] += 1
else:
new_particles[index] = 1
- # p_tmp.append([self.p[index][0],w[index]])
- for el in new_particles:
- p_tmp.append([self.p[el][0], weights[el]*new_particles[el]])
- return p_tmp
-
- def resampling(self, observations):
- p_tmp = []
- w = []
- S = 0
- for i in range(self.n):
- w.append(self.p[i][0].observation_score(
- observations, self.sense_noise))
- S += (w[i])
- for i in range(self.n):
- w[i] = w[i]/S
- self.resampling_wheel(w, p_tmp)
- S = 0
- for i in range(len(p_tmp)):
- S += p_tmp[i][1]
- for i in range(len(p_tmp)):
- p_tmp[i][1] /= S
- # if p_tmp[i][1] > 0.02:
- #print("x y yaw ", p_tmp[i][0].x, p_tmp[i][0].y, p_tmp[i][0].yaw*180/math.pi)
- self.update_coord(p_tmp)
- self.update_consistency(observations)
- # def rationing(weights, sum):
- # for i in range(len(weights))
- new_particles = self.gen_n_particles_robot(self.n - len(p_tmp))
-
- p_tmp.extend(new_particles)
- self.p = p_tmp
+ new_ids = np.fromiter(new_particles.keys(), dtype=int)
+ p_tmp = self.p[new_ids, :3]
+ counts = np.fromiter(new_particles.values(), dtype=int)
+ new_weights = weights[new_ids] * counts
+ return p_tmp, new_weights
+
+ def resampling(self, observation):
+ w = self.compute_weights(observation, self.sense_noise)
+ S = w.sum()
+ w = w / S
+ p_tmp, new_weights = self.resampling_wheel(w)
+ S = new_weights.sum()
+ new_weights /= S
+ p_with_weights_tmp = np.hstack((p_tmp, new_weights.reshape(-1, 1)))
+ self.update_coord(p_with_weights_tmp)
+ self.update_consistency(observation)
+ generated_particles = self.gen_n_particles_robot(self.n - p_with_weights_tmp.shape[0])
+ self.p = np.vstack((p_with_weights_tmp, generated_particles))
self.count += 1
- self.update_consistency(observations)
-
+ self.update_consistency(observation)
def custom_reset(self, x, y, yaw):
self.myrobot.x = x
@@ -194,23 +188,25 @@ class ParticleFilter():
self.update_consistency(observations)
def update_coord(self, particles):
- x = 0.0
- y = 0.0
- orientation = 0.0
- adding = 0.0
- if (self.myrobot.yaw < math.pi/2) or (self.myrobot.yaw > math.pi*3/2):
- adding = math.pi*2
- for particle in particles:
- x += particle[0].x * particle[1]
- y += particle[0].y * particle[1]
- if (particle[0].yaw < math.pi):
- culc_yaw = particle[0].yaw + adding
- else:
- culc_yaw = particle[0].yaw
- orientation += culc_yaw * particle[1]
- self.myrobot.x = x
- self.myrobot.y = y
- self.myrobot.yaw = orientation % (2*math.pi)
+ # x = 0.0
+ # y = 0.0
+ # orientation = 0.0
+ # adding = 0.0
+ # if (self.myrobot.yaw < math.pi/2) or (self.myrobot.yaw > math.pi*3/2):
+ # adding = math.pi*2
+
+ # for particle in particles:
+ # x += particle[0].x * particle[1]
+ # y += particle[0].y * particle[1]
+ # if (particle[0].yaw < math.pi):
+ # culc_yaw = particle[0].yaw + adding
+ # else:
+ # culc_yaw = particle[0].yaw
+ # orientation += culc_yaw * particle[1]
+ position = (particles[:, :3] * particles[:, 3].reshape(-1, 1)).sum(axis=0)
+ self.myrobot.x = position[0]
+ self.myrobot.y = position[1]
+ self.myrobot.yaw = position[2] % (2*np.pi)
def return_coord(self):
return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
diff --git a/src/localization/pf_visualization.py b/src/localization/pf_visualization.py
index eca8cbc..24af7c1 100644
--- a/src/localization/pf_visualization.py
+++ b/src/localization/pf_visualization.py
@@ -43,14 +43,14 @@ def visualization(robot, pr, factor = 7 ):
plt.gca().add_patch(arrow)
'''
# draw resampled particles
- for ind in range(len(pr)):
+ for ind in range(pr.shape[0]):
# particle
- circle = plt.Circle((pr[ind][0].y, pr[ind][0].x), 1./factor/2, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
+ circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
plt.gca().add_patch(circle)
# particle's orientation
- arrow = plt.Arrow(pr[ind][0].y, pr[ind][0].x, 2*math.sin(pr[ind][0].yaw)/factor, math.cos(pr[ind][0].yaw)/factor,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
+ arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor, math.cos(pr[ind, 2])/factor,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
plt.gca().add_patch(arrow)
--
GitLab
From 3e91fbee39be3985bfc3c83365550385afe59645 Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Thu, 4 Apr 2024 18:48:25 +0300
Subject: [PATCH 10/64] changed the way of computing weights
---
src/localization/ParticleFilter.py | 44 ++++--------------------------
src/localization/pf_constants.json | 2 +-
2 files changed, 7 insertions(+), 39 deletions(-)
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index a244d1f..3c8db2b 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -54,19 +54,6 @@ class ParticleFilter():
self.count += 1
- def gen_n_particles_robot(self, n):
- p = []
- for i in range(n):
- x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
- y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
- yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi)
- p.append([Particle(x_coord, y_coord, yaw), 0])
- return p
-
def uniform_reset(self):
self.p = []
for i in range(self.n):
@@ -114,15 +101,6 @@ class ParticleFilter():
p[:, 2]= p[:, 2] % (2 * np.pi)
self.count += 1
- # for i in range(n):
- # x_coord = self.myrobot.x + random.gauss(0, self.sense_noise*3)
- # y_coord = self.myrobot.y + random.gauss(0, self.sense_noise*3)
- # yaw = self.myrobot.yaw + random.gauss(0, self.yaw_noise)*math.pi
- # if yaw < 0:
- # yaw = 2*math.pi + yaw
- # if yaw > 2*math.pi:
- # yaw %= (2 * math.pi)
- # p.append([Particle(x_coord, y_coord, yaw), 0])
return p
def gen_n_particles(self, n):
@@ -134,7 +112,12 @@ class ParticleFilter():
return tmp
def compute_weights(self, observation, sigma):
- dist = np.sqrt(np.square(self.p[:, :3] - observation).sum(axis=-1)) # TODO correct comp for yaw(+- pi)
+ delta_x = self.p[:, 0] - observation[0]
+ delta_y = self.p[:, 1] - observation[1]
+ delta_yaw = np.abs(self.p[:, 2] - observation[2])
+ delta_yaw = np.where(delta_yaw<=np.pi, delta_yaw, 2*np.pi - delta_yaw)
+ # print(delta_yaw)
+ dist = np.sqrt(np.square(delta_x) + np.square(delta_y) + np.square(delta_yaw)) # TODO add diff weights to x, y and yaw
prob = np.exp(-(dist ** 2) / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
return prob
@@ -188,21 +171,6 @@ class ParticleFilter():
self.update_consistency(observations)
def update_coord(self, particles):
- # x = 0.0
- # y = 0.0
- # orientation = 0.0
- # adding = 0.0
- # if (self.myrobot.yaw < math.pi/2) or (self.myrobot.yaw > math.pi*3/2):
- # adding = math.pi*2
-
- # for particle in particles:
- # x += particle[0].x * particle[1]
- # y += particle[0].y * particle[1]
- # if (particle[0].yaw < math.pi):
- # culc_yaw = particle[0].yaw + adding
- # else:
- # culc_yaw = particle[0].yaw
- # orientation += culc_yaw * particle[1]
position = (particles[:, :3] * particles[:, 3].reshape(-1, 1)).sum(axis=0)
self.myrobot.x = position[0]
self.myrobot.y = position[1]
diff --git a/src/localization/pf_constants.json b/src/localization/pf_constants.json
index d84f822..d0a3a4d 100644
--- a/src/localization/pf_constants.json
+++ b/src/localization/pf_constants.json
@@ -4,7 +4,7 @@
"turn_noise": 0.1,
"sense_noise": 0.4,
"gauss_noise": 0.4,
- "yaw_noise": 0.1
+ "yaw_noise": 0.3
},
"consistency":{
--
GitLab
From cae5f12ea91516cc704294ce0e806eebc60b9920 Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Thu, 4 Apr 2024 19:12:42 +0300
Subject: [PATCH 11/64] sizes of visualised particles are proportional to their
weights
---
src/localization/pf_visualization.py | 10 +++++++---
1 file changed, 7 insertions(+), 3 deletions(-)
diff --git a/src/localization/pf_visualization.py b/src/localization/pf_visualization.py
index 24af7c1..31e0b38 100644
--- a/src/localization/pf_visualization.py
+++ b/src/localization/pf_visualization.py
@@ -1,6 +1,7 @@
import matplotlib.pylab as plt
from field import Field
import math
+import numpy as np
field = Field('../localization/parfield.json')
def visualization(robot, pr, factor = 7 ):
@@ -42,15 +43,18 @@ def visualization(robot, pr, factor = 7 ):
arrow = plt.Arrow(p[ind][0].x, p[ind][0].y, 2*math.cos(p[ind][0].yaw)/factor, 2*math.sin(p[ind][0].yaw)/factor, width=1/factor, alpha=1., facecolor='#994c00', edgecolor='#994c00')
plt.gca().add_patch(arrow)
'''
+ # max_size = np.exp(pr.shape[0] * np.max(pr[:, 3])) - 0.99
+ max_size = np.max(pr[:, 3]) + 1e-6
# draw resampled particles
for ind in range(pr.shape[0]):
-
+ # size = (np.exp(pr.shape[0] * pr[ind, 3]) - 0.99) / max_size * 5
+ size = pr[ind, 3] / max_size * 5
# particle
- circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
+ circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2 * size, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
plt.gca().add_patch(circle)
# particle's orientation
- arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor, math.cos(pr[ind, 2])/factor,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
+ arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor * size, math.cos(pr[ind, 2])/factor * size,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
plt.gca().add_patch(arrow)
--
GitLab
From fd87cb6f892d0787397080368bc05bb0290c612b Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Thu, 4 Apr 2024 19:30:23 +0300
Subject: [PATCH 12/64] added min ratio to show only particles with large
enough weight
---
src/localization/pf_visualization.py | 20 ++++----
src/notebooks/Seminar-Localisation.ipynb | 58 ++++++++++++------------
2 files changed, 39 insertions(+), 39 deletions(-)
diff --git a/src/localization/pf_visualization.py b/src/localization/pf_visualization.py
index 31e0b38..0d73406 100644
--- a/src/localization/pf_visualization.py
+++ b/src/localization/pf_visualization.py
@@ -3,7 +3,7 @@ from field import Field
import math
import numpy as np
field = Field('../localization/parfield.json')
-def visualization(robot, pr, factor = 7 ):
+def visualization(robot, pr, factor = 7 , min_ratio=10, max_vis_size=5):
plt.figure("Robot in the world",figsize=(field.w_width, field.w_length))
plt.title('Particle filter')
@@ -48,14 +48,16 @@ def visualization(robot, pr, factor = 7 ):
# draw resampled particles
for ind in range(pr.shape[0]):
# size = (np.exp(pr.shape[0] * pr[ind, 3]) - 0.99) / max_size * 5
- size = pr[ind, 3] / max_size * 5
- # particle
- circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2 * size, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
- plt.gca().add_patch(circle)
-
- # particle's orientation
- arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor * size, math.cos(pr[ind, 2])/factor * size,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
- plt.gca().add_patch(arrow)
+ size = pr[ind, 3]
+ if size >= ((max_size - 1e-6)/ min_ratio):
+ size *= max_vis_size / max_size
+ # particle
+ circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2 * size, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
+ plt.gca().add_patch(circle)
+
+ # particle's orientation
+ arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor * size, math.cos(pr[ind, 2])/factor * size,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
+ plt.gca().add_patch(arrow)
# robot's location
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
index 733d31d..33c7f29 100644
--- a/src/notebooks/Seminar-Localisation.ipynb
+++ b/src/notebooks/Seminar-Localisation.ipynb
@@ -68,13 +68,13 @@
},
{
"cell_type": "code",
- "execution_count": 1,
+ "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import sys\n",
"import json\n",
- "import math\n",
+ "import numpy as np\n",
"\n",
"sys.path.append(\"../localization/\")\n",
"\n",
@@ -82,37 +82,48 @@
"from robot import Robot\n",
"from field import Field\n",
"\n",
- "with open(\"../localization/landmarks.json\", \"r\") as f:\n",
- " landmarks = json.loads(f.read())\n",
"from pf_visualization import visualization"
]
},
{
"cell_type": "code",
- "execution_count": 2,
+ "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
- "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), landmarks, n = 500, apr = True)"
+ "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 1000, apr = True)\n",
+ "# pf.uniform_reset()"
]
},
{
"cell_type": "code",
- "execution_count": 3,
+ "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
- "shift = {'shift_x':1.0, 'shift_y':0.01, 'shift_yaw':0}\n",
- "observations = {\"yellow_posts\":[[4.3, -0.9],[4.2, 1.1]], \"blue_posts\":[]}"
+ "observation = np.array([0.0, 0.0, 6.28-1.57])\n",
+ "for _ in range(200):\n",
+ " pf.resampling(observation)"
]
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 39,
"metadata": {},
- "outputs": [],
+ "outputs": [
+ {
+ "data": {
+ "image/png": "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",
+ "text/plain": [
+ "<Figure size 600x900 with 1 Axes>"
+ ]
+ },
+ "metadata": {},
+ "output_type": "display_data"
+ }
+ ],
"source": [
- "pf.particles_move(shift)"
+ "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=3)"
]
},
{
@@ -121,7 +132,7 @@
"metadata": {},
"outputs": [],
"source": [
- "pf.resampling(observations)"
+ "visualization(pf.myrobot, pf.p, factor = 7)"
]
},
{
@@ -130,27 +141,14 @@
"metadata": {},
"outputs": [],
"source": [
- "pf.fall_reset(observations)"
+ "pf.uniform_reset()"
]
},
{
"cell_type": "code",
- "execution_count": 4,
+ "execution_count": null,
"metadata": {},
- "outputs": [
- {
- "data": {
- "image/png": "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\n",
- "text/plain": [
- "<Figure size 432x648 with 1 Axes>"
- ]
- },
- "metadata": {
- "needs_background": "light"
- },
- "output_type": "display_data"
- }
- ],
+ "outputs": [],
"source": [
"visualization(pf.myrobot, pf.p, factor = 7)"
]
@@ -179,7 +177,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
- "version": "3.7.3"
+ "version": "3.11.7"
}
},
"nbformat": 4,
--
GitLab
From 3f9c35009375d9910efcb80dce07ce1429ecea95 Mon Sep 17 00:00:00 2001
From: "i.shargin" <shargin.ia@phystech.edu>
Date: Fri, 5 Apr 2024 21:41:24 +0300
Subject: [PATCH 13/64] deleted some cells in notebook
---
src/notebooks/Seminar-Localisation.ipynb | 59 ------------------------
1 file changed, 59 deletions(-)
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
index 33c7f29..9325b88 100644
--- a/src/notebooks/Seminar-Localisation.ipynb
+++ b/src/notebooks/Seminar-Localisation.ipynb
@@ -1,55 +1,5 @@
{
"cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# КолеÑо отÑева "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "import random\n",
- "\n",
- "def resampling_wheel(weights, N, particleList):\n",
- " new_particles = []\n",
- " index = int(random.randint(0, N))\n",
- " beta = 0.0\n",
- " max_weight = max(weights)\n",
- " for i in range(N):\n",
- " beta += random.uniform(0, 2.0 * max_weight)\n",
- " while beta > weights[index]:\n",
- " beta -= weights[index]\n",
- " index = (index + 1) % N\n",
- " new_particles.append(particleList[index])\n",
- " print(new_particles)\n",
- " particleList = new_particles\n",
- " "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "weights = [0.12, 0.16, 0.14, 0.1, 0.1, 0.38]\n",
- "particleList = ['particle_1', 'particle_2', 'particle_3', 'particle_4', 'particle_5', 'particle_6']"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "resampling_wheel(weights, 6, particleList)"
- ]
- },
{
"cell_type": "markdown",
"metadata": {},
@@ -57,15 +7,6 @@
"# Фильтр чаÑтиц"
]
},
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "!pip3 install matplotlib"
- ]
- },
{
"cell_type": "code",
"execution_count": null,
--
GitLab
From 3a85485a2c0070a499188c838ece4f2de701bb54 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 5 Apr 2024 22:22:48 +0300
Subject: [PATCH 14/64] fixed dev by zero, wrong mean of yaw and added
yaw_importance
---
src/localization/ParticleFilter.py | 20 +++++++++++++++-----
src/localization/pf_constants.json | 6 +++++-
src/notebooks/Seminar-Localisation.ipynb | 24 +++++++++++++-----------
3 files changed, 33 insertions(+), 17 deletions(-)
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index 3c8db2b..43927f8 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -34,6 +34,8 @@ class ParticleFilter():
self.con_threshold = constants['consistency']['con_threshold']
self.spec_threshold = constants['consistency']['spec_threshold']
+ self.yaw_importance = constants['weights']['yaw_importance']
+
if apr:
self.gen_particles()
else:
@@ -115,9 +117,12 @@ class ParticleFilter():
delta_x = self.p[:, 0] - observation[0]
delta_y = self.p[:, 1] - observation[1]
delta_yaw = np.abs(self.p[:, 2] - observation[2])
+ # print(delta_yaw)
delta_yaw = np.where(delta_yaw<=np.pi, delta_yaw, 2*np.pi - delta_yaw)
# print(delta_yaw)
- dist = np.sqrt(np.square(delta_x) + np.square(delta_y) + np.square(delta_yaw)) # TODO add diff weights to x, y and yaw
+ # print(delta_yaw)
+ dist = np.sqrt(np.square(delta_x) + np.square(delta_y) + \
+ np.square(delta_yaw * self.yaw_importance)) # TODO add diff weights to x, y and yaw
prob = np.exp(-(dist ** 2) / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
return prob
@@ -145,10 +150,12 @@ class ParticleFilter():
def resampling(self, observation):
w = self.compute_weights(observation, self.sense_noise)
S = w.sum()
- w = w / S
+ if S > 0:
+ w = w / S
p_tmp, new_weights = self.resampling_wheel(w)
S = new_weights.sum()
- new_weights /= S
+ if S > 0:
+ new_weights /= S
p_with_weights_tmp = np.hstack((p_tmp, new_weights.reshape(-1, 1)))
self.update_coord(p_with_weights_tmp)
self.update_consistency(observation)
@@ -171,10 +178,13 @@ class ParticleFilter():
self.update_consistency(observations)
def update_coord(self, particles):
- position = (particles[:, :3] * particles[:, 3].reshape(-1, 1)).sum(axis=0)
+ position = (particles[:, :2] * particles[:, 3].reshape(-1, 1)).sum(axis=0)
self.myrobot.x = position[0]
self.myrobot.y = position[1]
- self.myrobot.yaw = position[2] % (2*np.pi)
+ sin = (np.sin(particles[:, 2]) * particles[:, 3].reshape(-1, 1)).sum()
+ cos = (np.cos(particles[:, 2]) * particles[:, 3].reshape(-1, 1)).sum()
+ angle = np.angle(cos + 1j*sin)
+ self.myrobot.yaw = angle % (2*np.pi)
def return_coord(self):
return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
diff --git a/src/localization/pf_constants.json b/src/localization/pf_constants.json
index d0a3a4d..b05bc72 100644
--- a/src/localization/pf_constants.json
+++ b/src/localization/pf_constants.json
@@ -16,5 +16,9 @@
"dist_threshold":0.3,
"con_threshold": 0.0,
"spec_threshold":1.5
- }
+ },
+
+ "weights":{
+ "yaw_importance":2
+ }
}
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
index 9325b88..880df9d 100644
--- a/src/notebooks/Seminar-Localisation.ipynb
+++ b/src/notebooks/Seminar-Localisation.ipynb
@@ -9,7 +9,7 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@@ -28,7 +28,7 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
@@ -38,33 +38,35 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 14,
"metadata": {},
"outputs": [],
"source": [
- "observation = np.array([0.0, 0.0, 6.28-1.57])\n",
- "for _ in range(200):\n",
+ "observation = np.array([1.5, 2.0, 1.57/2])\n",
+ "for _ in range(50):\n",
" pf.resampling(observation)"
]
},
{
"cell_type": "code",
- "execution_count": 39,
+ "execution_count": 15,
"metadata": {},
"outputs": [
{
"data": {
- "image/png": "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",
+ "image/png": "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\n",
"text/plain": [
- "<Figure size 600x900 with 1 Axes>"
+ "<Figure size 432x648 with 1 Axes>"
]
},
- "metadata": {},
+ "metadata": {
+ "needs_background": "light"
+ },
"output_type": "display_data"
}
],
"source": [
- "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=3)"
+ "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=5)"
]
},
{
@@ -118,7 +120,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
- "version": "3.11.7"
+ "version": "3.8.10"
}
},
"nbformat": 4,
--
GitLab
From 265ccbb5e00922f385f9abbbb328cf5fd7715c91 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 5 Apr 2024 22:33:21 +0300
Subject: [PATCH 15/64] now area, not radius of visualised particle is
proportional to weight
---
src/localization/pf_visualization.py | 9 +++++++--
src/notebooks/Seminar-Localisation.ipynb | 14 +++++++-------
2 files changed, 14 insertions(+), 9 deletions(-)
diff --git a/src/localization/pf_visualization.py b/src/localization/pf_visualization.py
index 0d73406..9296d83 100644
--- a/src/localization/pf_visualization.py
+++ b/src/localization/pf_visualization.py
@@ -52,11 +52,16 @@ def visualization(robot, pr, factor = 7 , min_ratio=10, max_vis_size=5):
if size >= ((max_size - 1e-6)/ min_ratio):
size *= max_vis_size / max_size
# particle
- circle = plt.Circle((pr[ind, 1], pr[ind, 0]), 1./factor/2 * size, facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
+ circle = plt.Circle((pr[ind, 1], pr[ind, 0]),
+ 1./factor/2 * np.sqrt(max_vis_size) * np.sqrt(size),
+ facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
plt.gca().add_patch(circle)
# particle's orientation
- arrow = plt.Arrow(pr[ind, 1], pr[ind, 0], 2*math.sin(pr[ind, 2])/factor * size, math.cos(pr[ind, 2])/factor * size,width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
+ arrow = plt.Arrow(pr[ind, 1], pr[ind, 0],
+ math.sin(pr[ind, 2])/factor * size,
+ math.cos(pr[ind, 2])/factor * size,
+ width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
plt.gca().add_patch(arrow)
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
index 880df9d..5016d27 100644
--- a/src/notebooks/Seminar-Localisation.ipynb
+++ b/src/notebooks/Seminar-Localisation.ipynb
@@ -32,29 +32,29 @@
"metadata": {},
"outputs": [],
"source": [
- "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 1000, apr = True)\n",
+ "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 100, apr = True)\n",
"# pf.uniform_reset()"
]
},
{
"cell_type": "code",
- "execution_count": 14,
+ "execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
- "observation = np.array([1.5, 2.0, 1.57/2])\n",
- "for _ in range(50):\n",
+ "observation = np.array([-3.0, -1.0, 6.28 - 1.57/2.0])\n",
+ "for _ in range(100):\n",
" pf.resampling(observation)"
]
},
{
"cell_type": "code",
- "execution_count": 15,
+ "execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
- "image/png": "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\n",
+ "image/png": "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\n",
"text/plain": [
"<Figure size 432x648 with 1 Axes>"
]
@@ -66,7 +66,7 @@
}
],
"source": [
- "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=5)"
+ "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=2)"
]
},
{
--
GitLab
From 574867a4278de3e9c728a10938fcb2a9976c4bc6 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 7 Apr 2024 14:57:40 +0300
Subject: [PATCH 16/64] Need cleaning
Can detect aruco and inverse it transfrom to get camera position
---
.../calibration_node/calibration.py | 87 ++++++++++++-------
.../calibration_node/gui_window.py | 2 +-
.../config/static_transforms.yaml | 13 +++
src/calibration_node/launch/tf.launch.py | 41 +++++++++
src/calibration_node/setup.py | 2 +-
5 files changed, 110 insertions(+), 35 deletions(-)
create mode 100644 src/calibration_node/config/static_transforms.yaml
create mode 100644 src/calibration_node/launch/tf.launch.py
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 4414d7e..2dd17e4 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -38,6 +38,8 @@ from threading import Thread
from dataclasses import dataclass
from PyQt5.QtCore import QThread
import yaml
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+
@dataclass
class MatchPose:
"""Class for keeping track of an item in inventory."""
@@ -73,7 +75,9 @@ class CalibrationNode(Node):
self.aruco_pose = None
self.declare_parameter("camera_frames", ["camera1"])#, "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
- self.path_to_save = self.declare_parameter("path_to_save", "./config.yaml")
+ self.declare_parameter("path_to_save", "./static_transforms.yaml")
+ self.path_to_save = self.get_parameter("path_to_save").value
+ self.tf_static_broadcaster = StaticTransformBroadcaster(self)
def get_aruco_pose(self, msg):
@@ -85,8 +89,9 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
+ tf_stamped.child_frame_id = self.camera_frames[0] # add add seconde camera
tf_stamped.header.frame_id = str(id)
- self.current_aruco_tf[id] = tf_stamped
+ self.current_aruco_tf[str(id)] = tf_stamped
self.aruco_pose = tf_stamped
def calib_cb(self):
@@ -96,26 +101,36 @@ class CalibrationNode(Node):
# Huetta!
if (pose_ui is not None):
pose_ui.header.stamp = self.get_clock().now().to_msg()
- if pose_ui.child_frame_id in self.current_aruco_tf:
- pose_aruco = self.current_aruco_tf[pose_ui.child_frame_id]
+ self.get_logger().debug(f"self.current_aruco_tf: {self.current_aruco_tf}, pose_ui.child_frame_id: {pose_ui.child_frame_id}")
+ if str(pose_ui.child_frame_id) in self.current_aruco_tf: # [TODO] pose_ui.child_frame_id - has int type????
+ pose_aruco = self.current_aruco_tf[str(pose_ui.child_frame_id)]
self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
if (self.ui_window.is_calibrate_ready()):
self.calibrate()
- del self.ui_window
+ # del self.ui_window
def calibrate(self):
# use only last
translations_xyz = []
orientations_rpy = []
+ last_used_quaternion = None
for match_pose in self.match_pose_arr:
buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ # rot ebal pythona
+ time_now = self.get_clock().now()
+ match_pose.tf_ui.header.stamp = time_now.to_msg()# rclpy.time.Time()
+ match_pose.tf_camera.header.stamp = time_now.to_msg()# rclpy.time.Time()
buffer_core.set_transform(match_pose.tf_camera, "default_authority")
buffer_core.set_transform(match_pose.tf_ui, "default_authority")
+ self.get_logger().debug(f"tf_ui is {match_pose.tf_ui}\n, tf_camera is {match_pose.tf_camera} ")
+ #
inverse_transform = buffer_core.lookup_transform_core(
- match_pose.tf_ui.header.frame_id, match_pose.tf_camera.header.frame_id, rclpy.time.Time()
+ match_pose.tf_camera.child_frame_id, match_pose.tf_ui.header.frame_id, time_now
# Opposite i think
)
+ inverse_transform.child_frame_id = "camera"
+ inverse_transform.header.frame_id = "world"
self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
trans = inverse_transform.transform.translation
@@ -124,45 +139,51 @@ class CalibrationNode(Node):
euler = tf_t.euler_from_quaternion(quat)
translations_xyz.append(translation_arr)
orientations_rpy.append(euler) # Not true
- print("TF RPY: ", euler )
- print("Ibverssed TF: ", inverse_transform.transform.translation)
-
- mid_translation = np.matrix(translations_xyz).mean(axis=1)
- mid_orientation_rpy = np.matrix(orientations_rpy).mean(axis=1)
- print ("Current camera position:")
- print(f"Orientation RPY:{mid_orientation_rpy}")
- quat = tf_t.quaternion_from_euler(mid_orientation_rpy)
- print(f"Orientation xyzw:{quat}")
- print(f"Translation: {mid_translation}")
- self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
-
+ last_used_quaternion = quat
+
+ # for multi aruco calibration
+ #if translations_xyz != [] and orientations_rpy != []:
+ # mid_translation = np.array(translations_xyz).mean(axis=0)
+ # mid_orientation_rpy = np.array(orientations_rpy).mean(axis=0)
+ # print ("Current camera position:")
+ # print(f"Orientation RPY:{mid_orientation_rpy}")
+ # quat = tf_t.quaternion_from_euler(*mid_orientation_rpy.asarray())
+ # print(f"Orientation xyzw:{quat}")
+ # print(f"Translation: {mid_translation}")
+ # self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
+
+ if last_used_quaternion is not None:
+ self._save_transform_to_yaml(translations_xyz[-1], last_used_quaternion, "world" ,self.camera_frames[0]) # [TODO] HARDCODE camera frame
+ self.get_logger().debug("Command for tf static:\n ros2 run tf2_ros static_transform_publisher "
+ f"{translations_xyz[-1][0]} {translations_xyz[-1][1]} {translations_xyz[-1][2]} "
+ f"{rot.x} {rot.y} {rot.z} {rot.w}"
+ " camera world" )
def _save_transform_to_yaml(self, translation, quaternion, frame_id, child_frame_id):
# Prepare the data structure
data = {
- 'static_transform_publisher': {
- 'x': translation[0],
- 'y': translation[1],
- 'z': translation[2],
- 'roll': quaternion[0],
- 'pitch': quaternion[1],
- 'yaw': quaternion[2],
- 'frame_id': frame_id,
- 'child_frame_id': child_frame_id
- }
+ 'static_transforms': [
+ {
+ 'name': "base_link_2_radar",
+ 'translation': translation,
+ 'rotation': quaternion,
+ 'parent_frame_id': frame_id,
+ 'child_frame_id': child_frame_id
+ }
+ ]
}
-
# Save the data to a YAML file
with open(self.path_to_save, 'w') as file:
yaml.dump(data, file, default_flow_style=False)
-
+ self.get_logger().debug(f"save config to {self.path_to_save}")
def run(self):
- self.ui_window.run() # write ti interface
+ self.ui_window.run() # write it interface
def main(args=None):
rclpy.init()
- minimal_node = CalibrationNode() # add start function
+ minimal_node = CalibrationNode()
minimal_node.run()
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
+
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 1096a8b..590ace5 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -77,7 +77,7 @@ class GuiWindow(QMainWindow, WindowInterface):
self.tf.transform.rotation.x = quat[0]
self.tf.transform.rotation.y = quat[1]
self.tf.transform.rotation.z = quat[2]
- self.tf.transform.rotation.z = quat[3]
+ self.tf.transform.rotation.w = quat[3]
self.tf.header.frame_id = "world"
self.tf.child_frame_id = self.pose_inputs[0].text()
except Exception as e:
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
new file mode 100644
index 0000000..b030f53
--- /dev/null
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -0,0 +1,13 @@
+static_transforms:
+- child_frame_id: camera1
+ name: base_link_2_radar
+ parent_frame_id: world
+ rotation:
+ - 0.92861703627208
+ - 0.013534026051733207
+ - -0.004905194687194856
+ - -0.3707602583195375
+ translation:
+ - -0.08637496015533369
+ - -0.7362665187573849
+ - 1.337591814410169
diff --git a/src/calibration_node/launch/tf.launch.py b/src/calibration_node/launch/tf.launch.py
new file mode 100644
index 0000000..eaa5d95
--- /dev/null
+++ b/src/calibration_node/launch/tf.launch.py
@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+import yaml
+
+def load_yaml_file(file_path):
+ with open(file_path, 'r') as file:
+ return yaml.safe_load(file)
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ # Get the path to the YAML file
+ config_path = os.path.join(
+ get_package_share_directory('calibration_node'),
+ 'config',
+ 'static_transforms.yaml'
+ )
+
+ static_transforms = load_yaml_file(config_path)
+
+ for transform in static_transforms['static_transforms']:
+ node = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ name=transform['child_frame_id'],
+ arguments=[
+ str(transform['translation'][0]),
+ str(transform['translation'][1]),
+ str(transform['translation'][2]),
+ str(transform['rotation'][0]),
+ str(transform['rotation'][1]),
+ str(transform['rotation'][2]),
+ str(transform['rotation'][3]),
+ transform['parent_frame_id'],
+ transform['child_frame_id']
+ ]
+ )
+ ld.add_action(node)
+ return ld
\ No newline at end of file
diff --git a/src/calibration_node/setup.py b/src/calibration_node/setup.py
index 4261ad8..7ad9a95 100644
--- a/src/calibration_node/setup.py
+++ b/src/calibration_node/setup.py
@@ -11,7 +11,7 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
-
+ (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.[yaml]*'))),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
--
GitLab
From 2e3b1e774aa7c70b8220a3939882ff9a6d8e2b11 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 7 Apr 2024 17:37:24 +0300
Subject: [PATCH 17/64] added localisation_node
---
src/localization/ParticleFilter.py | 6 +-
src/localization/Seminar-Localisation.ipynb | 128 ++++++++
src/localization/localisation_node.py | 326 ++++++++++++++++++++
src/server_main/server_main/server_node.py | 7 +-
4 files changed, 461 insertions(+), 6 deletions(-)
create mode 100644 src/localization/Seminar-Localisation.ipynb
create mode 100644 src/localization/localisation_node.py
diff --git a/src/localization/ParticleFilter.py b/src/localization/ParticleFilter.py
index 43927f8..aecb57b 100644
--- a/src/localization/ParticleFilter.py
+++ b/src/localization/ParticleFilter.py
@@ -121,9 +121,9 @@ class ParticleFilter():
delta_yaw = np.where(delta_yaw<=np.pi, delta_yaw, 2*np.pi - delta_yaw)
# print(delta_yaw)
# print(delta_yaw)
- dist = np.sqrt(np.square(delta_x) + np.square(delta_y) + \
- np.square(delta_yaw * self.yaw_importance)) # TODO add diff weights to x, y and yaw
- prob = np.exp(-(dist ** 2) / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
+ dist = np.square(delta_x) + np.square(delta_y) + \
+ np.square(delta_yaw * self.yaw_importance)
+ prob = np.exp(-dist / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
return prob
diff --git a/src/localization/Seminar-Localisation.ipynb b/src/localization/Seminar-Localisation.ipynb
new file mode 100644
index 0000000..5016d27
--- /dev/null
+++ b/src/localization/Seminar-Localisation.ipynb
@@ -0,0 +1,128 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Фильтр чаÑтиц"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import sys\n",
+ "import json\n",
+ "import numpy as np\n",
+ "\n",
+ "sys.path.append(\"../localization/\")\n",
+ "\n",
+ "from ParticleFilter import ParticleFilter\n",
+ "from robot import Robot\n",
+ "from field import Field\n",
+ "\n",
+ "from pf_visualization import visualization"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 100, apr = True)\n",
+ "# pf.uniform_reset()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 4,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "observation = np.array([-3.0, -1.0, 6.28 - 1.57/2.0])\n",
+ "for _ in range(100):\n",
+ " pf.resampling(observation)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 8,
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "image/png": "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\n",
+ "text/plain": [
+ "<Figure size 432x648 with 1 Axes>"
+ ]
+ },
+ "metadata": {
+ "needs_background": "light"
+ },
+ "output_type": "display_data"
+ }
+ ],
+ "source": [
+ "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=2)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "visualization(pf.myrobot, pf.p, factor = 7)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pf.uniform_reset()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "visualization(pf.myrobot, pf.p, factor = 7)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": []
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 3",
+ "language": "python",
+ "name": "python3"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.8.10"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}
diff --git a/src/localization/localisation_node.py b/src/localization/localisation_node.py
new file mode 100644
index 0000000..cc6d35f
--- /dev/null
+++ b/src/localization/localisation_node.py
@@ -0,0 +1,326 @@
+from ros2_aruco_interfaces.msg import ArucoMarkers
+
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D
+import tf2_ros
+
+import rclpy
+from rclpy.node import Node
+import json
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point
+import tf_transformations as tf_t
+import math
+from pyquaternion import Quaternion
+from tf2_ros import TransformBroadcaster
+
+from ParticleFilter import ParticleFilter
+
+
+from collections import deque
+
+class AlphaFilter():
+ def __init__(self):
+ pass
+ self.buffer = deque()
+ self.buffer_size = 10
+ self.alpha = 0.70
+
+ def update(self, value):
+ if (self.buffer_size == len(self.buffer)):
+ self.buffer.popleft()
+
+
+ self.buffer.append(value)
+
+ def get(self):
+ weight_sum = 1
+ ## find yaw mid
+ mid = Pose() # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! lost quaternion
+ for i, elem in enumerate(self.buffer):
+ mid.position.x += elem.position.x * self.alpha ** (len(self.buffer) - i)
+ mid.position.y += elem.position.y * self.alpha ** (len(self.buffer) - i)
+ mid.position.z += elem.position.z * self.alpha ** (len(self.buffer) - i)
+ weight_sum += self.alpha ** (len(self.buffer) - i )
+
+ mid.position.x /= weight_sum
+ mid.position.y /= weight_sum
+ mid.position.z /= weight_sum
+ return mid
+
+class LocalisationNode(Node):
+ def __init__(self):
+ super().__init__("LocalisationNode")
+ self.robot_ids = [0, 1, 2]
+
+ self.declare_parameter("aruco_markers_topic", "/aruco_markers")
+ topic_name = self.get_parameter("aruco_markers_topic").value
+ # 0 - front, 1 - right, 2 - back, 3 - left, 4 - up
+ self.declare_parameter("robots_params", rclpy.Parameter.Type.STRING)
+ self.aruco_rotation_map = {}
+ robots_params_path = str(self.get_parameter("robots_params").value)
+ robots_params = None
+ with open(robots_params_path) as f:
+ robots_params = json.load(f)
+ self.robots = robots_params["robots"]
+ self.create_aruco_rotation_map()
+ self.init_aruco_ids()
+ self.declare_parameter("aruco_size", 0.050) #TODO
+ self.aruco_size = self.get_parameter("aruco_size").value # m
+
+ self.declare_parameter("world_tf_topic", "/tf_static")
+ world_topic = self.get_parameter("world_tf_topic").value
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.tf_from_camera_to_world = dict()
+ self.aruco_subscription = self.create_subscription(
+ ArucoMarkers, topic_name, self.get_aruco_pose, 10
+ )
+ self.robots_pub = {}
+ for robot_id in self.robot_ids:
+ self.robots_pub[robot_id] = self.create_publisher(PoseStamped, "/robot_pose" + str(robot_id), 10)
+ self.loged_data = []
+ self.file_to_save = "/home/root/workspace/a.out"
+ self.create_publisher(PoseStamped, "/robot0_pose", 10)
+ self.current_aruco_poses = {} # {if: x, y}
+ self.current_robot_poses = {}
+ self.world_robots_poses = {}
+
+ self.tf_broadcaster = TransformBroadcaster(self)
+
+ def create_aruco_rotation_map(self):
+ for robot_id, param in self.robots.items():
+ aruco_ids = param["aruco_ids"]
+ aruco_transforms = param["aruco_transforms"]
+ for aruco_id, aruco_tf in zip(aruco_ids, aruco_transforms):
+ print(f"RPY: {aruco_tf[0]}, {aruco_tf[1]}, {aruco_tf[2]}")
+ self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler( #TODO get rid of euler angles
+ aruco_tf[0], aruco_tf[1], aruco_tf[2]
+ )
+
+ def init_aruco_ids(self):
+ self.aruco_ids = {}
+ for robot_id, param in self.robots.items():
+ self.aruco_ids[robot_id] = param["aruco_ids"]
+
+ def get_robot_id(self, id: int) -> int:
+ for robot_id, ids in self.aruco_ids.items():
+ if id in ids:
+ return int(robot_id)
+ print(self.aruco_ids)
+ print(f"Bad aruco id:{id}")
+ return -1
+ # raise IOError("Bad aruco ids")
+
+ def get_camera_to_world_frame(self):
+ try:
+ for frame in self.camera_frames:
+ self.tf_from_camera_to_world[
+ frame
+ ] = self.tf_buffer.lookup_transform( # check inversion not allwed
+ "world", frame, rclpy.time.Time()
+ )
+ except TransformException as ex:
+ import time
+ time.sleep(0.1)
+ self.get_logger().info(f"Could not transform world to camera: {ex}")
+ # self.get_camera_to_world_frame()
+
+ def get_aruco_pose(self, msg):
+ self.current_aruco_poses.clear()
+ for id, pose in zip(msg.marker_ids, msg.poses):
+ pose_stamped = PoseStamped()
+ pose_stamped.pose = pose
+ pose_stamped.header = msg.header
+ self.current_aruco_poses[id] = pose_stamped
+
+ def shift_pose_to_robot_centre(
+ self, pose, id
+ ): # Now all arruco has same offset of centre
+ # https://answers.ros.org/question/289323/simple-tf-transform-without-broadcasterlistener/
+ shifted_pose = PoseStamped()
+ shifted_pose.header = pose.header
+ shifted_pose.pose.orientation = pose.pose.orientation
+
+ shift_point = PointStamped()
+ #shift_point.point.x = 0.0 # 0.035
+ shift_point.point.z = -0.035# 0.045
+ transform = TransformStamped()
+ transform.header = pose.header
+ transform.child_frame_id = "aruco" + str(id)
+ (
+ transform.transform.translation.x,
+ transform.transform.translation.y,
+ transform.transform.translation.z,
+ ) = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
+ transform.transform.rotation = pose.pose.orientation
+
+ buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_core.set_transform(transform, "default_authority")
+ inverse_transform = buffer_core.lookup_transform_core(
+ transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
+ )
+ #self.tf_broadcaster.sendTransform(inverse_transform)
+ rot = inverse_transform.transform.rotation
+ quat = rot.x, rot.y, rot.z, rot.w
+ euler = tf_t.euler_from_quaternion(quat)
+ #print("TF RPY: ", euler )
+ #print("Ibverssed TF: ", inverse_transform.transform.translation)
+ #import time
+ #time.sleep(0.1) # also remove shift
+ shifted_pose.pose.position = do_transform_point(shift_point, transform).point
+
+ return shifted_pose
+
+ def rotate_pose_to_forward(self, pose: PoseStamped, id):
+ rotated_pose = PoseStamped()
+ rotated_pose.pose.position = pose.pose.position
+
+ quaternion_iter = [
+ pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w,
+ ]
+ transformed_quate_iter = tf_t.quaternion_multiply(
+ quaternion_iter, self.aruco_rotation_map[id]
+ )
+ (
+ rotated_pose.pose.orientation.x,
+ rotated_pose.pose.orientation.y,
+ rotated_pose.pose.orientation.z,
+ rotated_pose.pose.orientation.w,
+ ) = transformed_quate_iter
+ # rotated_pose.header.stamp = self.get_clock().now().to_msg()
+ rotated_pose.header.frame_id = (
+ pose.header.frame_id
+ ) # "camera" ########## Choose frame id
+ return rotated_pose
+
+ def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
+ shifted_pose = self.shift_pose_to_robot_centre(pose, id)
+ rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
+ return rotated_pose
+
+ @staticmethod
+ def quatWAvg(Q):
+ '''
+ Averaging Quaternions.
+
+ Arguments:
+ Q(ndarray): an Mx4 ndarray of quaternions.
+ weights(list): an M elements list, a weight for each quaternion.
+ '''
+
+ # Form the symmetric accumulator matrix
+ A = np.zeros((4, 4))
+ M = Q.shape[1]
+
+ for i in range(M):
+ q = Q[:, i]
+ A += np.outer(q, q)# rank 1 update
+
+ # Get the eigenvector corresponding to largest eigen value
+ return np.linalg.eigh(A)[1][:, -1]
+
+ def find_mid_pose(self, poses) -> Pose:
+ mid_pose = Pose()
+ Q_array = np.zeros((4, len(poses)))
+ # find mid of orientation and coordinates
+ for count, pose in enumerate(poses):
+ mid_pose.position.x += pose.pose.position.x
+ mid_pose.position.y += pose.pose.position.y
+ mid_pose.position.z += pose.pose.position.z
+
+ Q_array[0, count] = pose.pose.orientation.x
+ Q_array[1, count] = pose.pose.orientation.y
+ Q_array[2, count] = pose.pose.orientation.z
+ Q_array[3, count] = pose.pose.orientation.w
+
+ mid_pose.position.x /= len(poses)
+ mid_pose.position.y /= len(poses)
+ mid_pose.position.z /= len(poses)
+
+ mid_quat = self.quatWAvg(Q_array)
+ mid_pose.orientation.x = mid_quat[0]
+ mid_pose.orientation.y = mid_quat[1]
+ mid_pose.orientation.z = mid_quat[2]
+ mid_pose.orientation.w = mid_quat[3]
+
+ return mid_pose
+
+ def count_robots_poses(self):
+ robots_poses = {}
+ for id, pose in self.current_aruco_poses.items():
+ robot_id = self.get_robot_id(id)
+ self.get_logger().info(f"robot_id: {robot_id}")
+ if robot_id == -1:
+ continue
+ # forward_pose = self.get_forward_pose(pose, id) def rotate_pose_to_forward
+ rotated_to_forward_pose = self.transform_to_robot_centre(pose, id)
+
+ world_pos = self.tf_buffer.transform(rotated_to_forward_pose, "world")
+ if not (robot_id in robots_poses):
+ robots_poses[robot_id] = [world_pos]
+ else:
+ robots_poses[robot_id].append(world_pos)
+ # find mid of x,y
+ for robot_id in self.robot_ids:
+ if robot_id in robots_poses:
+ if not robot_id in self.world_robots_poses:
+ self.world_robots_poses[robot_id] = AlphaFilter()
+ self.world_robots_poses[robot_id].update(self.find_mid_pose(
+ robots_poses[robot_id])
+ ) # transform aruco to forward
+
+
+
+ def get_yaw_world_rotation(self, pose: Pose) -> float:
+ x_ax = [0, 0, 1]
+ quat_arr = [
+ pose.orientation.x,
+ pose.orientation.y,
+ pose.orientation.z,
+ pose.orientation.w,
+ ]
+ quat_conj = tf_t.quaternion_conjugate(quat_arr) #TODO
+ # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
+ rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
+ return math.atan2(
+ rotated_x[1], rotated_x[0]
+ ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
+
+ def compute_observation(self, id):
+ if id in self.world_robots_poses:
+ msg = PoseStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "world"
+ pose = self.world_robots_poses[id].get()
+ msg.pose = pose
+ self.robots_pub[id].publish(msg)
+ yaw = self.get_yaw_world_rotation(msg.pose)
+
+ # if (0):
+ # self.loged_data.append([self.get_clock().now().nanoseconds, msg.pose.position.x, msg.pose.position.y,self.get_yaw_world_rotation(msg.pose)])
+ # with open(self.file_to_save, 'w') as file:
+ # import csv
+ # writer = csv.writer(file)
+ # writer.writerows(self.loged_data)
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ localisation_node = LocalisationNode()
+
+ rclpy.spin(localisation_node)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ localisation_node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/server_main/server_main/server_node.py b/src/server_main/server_main/server_node.py
index d42107e..5c71277 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -94,7 +94,7 @@ class ServerNode(Node):
self.robots = robots_params["robots"]
self.create_aruco_rotation_map()
self.init_aruco_ids()
- self.declare_parameter("aruco_size", 0.050) #
+ self.declare_parameter("aruco_size", 0.050) #TODO
self.aruco_size = self.get_parameter("aruco_size").value # m
self.declare_parameter("world_tf_topic", "/tf_static")
@@ -157,7 +157,7 @@ class ServerNode(Node):
self.robots_topic = {}
for robot_id in self.robot_ids:
self.robots_topic[robot_id] = self.team_name + "/" + str(robot_id) + "/" + "robot"
- self.mqttc = mqtt.Client()
+ self.mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1)
self.mqttc.on_message = ServerNode.on_message
self.mqttc.on_connect = ServerNode.on_connect
self.mqttc.on_publish = ServerNode.on_publish
@@ -174,7 +174,8 @@ class ServerNode(Node):
aruco_ids = param["aruco_ids"]
aruco_transforms = param["aruco_transforms"]
for aruco_id, aruco_tf in zip(aruco_ids, aruco_transforms):
- self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler(
+ print(f"RPY: {aruco_tf[0]}, {aruco_tf[1]}, {aruco_tf[2]}")
+ self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler( #TODO get rid of euler angles
aruco_tf[0], aruco_tf[1], aruco_tf[2]
)
--
GitLab
From 122f647132393b51ddf6b39216025b385f29384f Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 7 Apr 2024 17:47:42 +0300
Subject: [PATCH 18/64] Black refactor and remove useless imports
---
.../calibration_node/calibration.py | 138 +++++++++---------
.../calibration_node/gui_window.py | 36 +++--
.../calibration_node/window_interface.py | 6 +-
3 files changed, 93 insertions(+), 87 deletions(-)
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 2dd17e4..441df84 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -1,53 +1,35 @@
import rclpy
-import time
-import sys
-from threading import Thread
-from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
-from std_msgs.msg import String
-from PyQt5.QtWidgets import QApplication, QLabel, QMainWindow, QWidget, QPushButton
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
-from sensor_msgs.msg import CameraInfo
-from soccer_vision_2d_msgs.msg import BallArray, Ball
+from geometry_msgs.msg import TransformStamped
import tf2_ros
-from image_geometry import PinholeCameraModel
import rclpy
from rclpy.node import Node
-import json
-from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
import tf_transformations as tf_t
-import math
-from skspatial.objects import Line, Plane
-from pyquaternion import Quaternion
-import paho.mqtt.client as mqtt
from tf2_ros import TransformBroadcaster
-import numpy as np
-from collections import deque
from .gui_window import GuiWindow
-from threading import Thread
from dataclasses import dataclass
-from PyQt5.QtCore import QThread
import yaml
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+
@dataclass
class MatchPose:
"""Class for keeping track of an item in inventory."""
+
tf_ui: TransformStamped
tf_camera: TransformStamped
-class CalibrationNode(Node):
+class CalibrationNode(Node):
def __init__(self):
super().__init__("calibration_node")
self._init_params()
@@ -58,7 +40,6 @@ class CalibrationNode(Node):
self.timer = self.create_timer(1, self.calib_cb)
self.ui_window = GuiWindow(self)
-
def _init_params(self):
self.declare_parameter("world_tf_topic", "/tf_static")
world_topic = self.get_parameter("world_tf_topic").value
@@ -73,13 +54,14 @@ class CalibrationNode(Node):
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
self.aruco_pose = None
- self.declare_parameter("camera_frames", ["camera1"])#, "camera2"])#rclpy.Parameter.Type.STRING_ARRAY)
+ self.declare_parameter(
+ "camera_frames", ["camera1" "camera2"]
+ ) # rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
self.declare_parameter("path_to_save", "./static_transforms.yaml")
self.path_to_save = self.get_parameter("path_to_save").value
self.tf_static_broadcaster = StaticTransformBroadcaster(self)
-
def get_aruco_pose(self, msg):
self.current_aruco_tf.clear()
for id, pose in zip(msg.marker_ids, msg.poses):
@@ -89,29 +71,33 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- tf_stamped.child_frame_id = self.camera_frames[0] # add add seconde camera
- tf_stamped.header.frame_id = str(id)
+ tf_stamped.child_frame_id = self.camera_frames[0] # add add seconde camera
+ tf_stamped.header.frame_id = str(id)
self.current_aruco_tf[str(id)] = tf_stamped
self.aruco_pose = tf_stamped
def calib_cb(self):
pose_ui = self.ui_window.try_get_position()
self.get_logger().info("Timer callback called")
-
+
# Huetta!
- if (pose_ui is not None):
+ if pose_ui is not None:
pose_ui.header.stamp = self.get_clock().now().to_msg()
- self.get_logger().debug(f"self.current_aruco_tf: {self.current_aruco_tf}, pose_ui.child_frame_id: {pose_ui.child_frame_id}")
- if str(pose_ui.child_frame_id) in self.current_aruco_tf: # [TODO] pose_ui.child_frame_id - has int type????
+ self.get_logger().debug(
+ f"self.current_aruco_tf: {self.current_aruco_tf}, pose_ui.child_frame_id: {pose_ui.child_frame_id}"
+ )
+ if (
+ str(pose_ui.child_frame_id) in self.current_aruco_tf
+ ): # [TODO] pose_ui.child_frame_id - has int type????
pose_aruco = self.current_aruco_tf[str(pose_ui.child_frame_id)]
self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
- if (self.ui_window.is_calibrate_ready()):
+ if self.ui_window.is_calibrate_ready():
self.calibrate()
# del self.ui_window
def calibrate(self):
- # use only last
+ # use only last
translations_xyz = []
orientations_rpy = []
last_used_quaternion = None
@@ -119,15 +105,18 @@ class CalibrationNode(Node):
buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
# rot ebal pythona
time_now = self.get_clock().now()
- match_pose.tf_ui.header.stamp = time_now.to_msg()# rclpy.time.Time()
- match_pose.tf_camera.header.stamp = time_now.to_msg()# rclpy.time.Time()
+ match_pose.tf_ui.header.stamp = time_now.to_msg()
+ match_pose.tf_camera.header.stamp = time_now.to_msg()
buffer_core.set_transform(match_pose.tf_camera, "default_authority")
buffer_core.set_transform(match_pose.tf_ui, "default_authority")
- self.get_logger().debug(f"tf_ui is {match_pose.tf_ui}\n, tf_camera is {match_pose.tf_camera} ")
- #
+ self.get_logger().debug(
+ f"tf_ui is {match_pose.tf_ui}\n, tf_camera is {match_pose.tf_camera} "
+ )
+
inverse_transform = buffer_core.lookup_transform_core(
- match_pose.tf_camera.child_frame_id, match_pose.tf_ui.header.frame_id, time_now
- # Opposite i think
+ match_pose.tf_camera.child_frame_id,
+ match_pose.tf_ui.header.frame_id,
+ time_now,
)
inverse_transform.child_frame_id = "camera"
inverse_transform.header.frame_id = "world"
@@ -138,52 +127,61 @@ class CalibrationNode(Node):
quat = rot.x, rot.y, rot.z, rot.w
euler = tf_t.euler_from_quaternion(quat)
translations_xyz.append(translation_arr)
- orientations_rpy.append(euler) # Not true
+ orientations_rpy.append(euler) # Not true
last_used_quaternion = quat
-
- # for multi aruco calibration
- #if translations_xyz != [] and orientations_rpy != []:
- # mid_translation = np.array(translations_xyz).mean(axis=0)
- # mid_orientation_rpy = np.array(orientations_rpy).mean(axis=0)
- # print ("Current camera position:")
- # print(f"Orientation RPY:{mid_orientation_rpy}")
- # quat = tf_t.quaternion_from_euler(*mid_orientation_rpy.asarray())
- # print(f"Orientation xyzw:{quat}")
- # print(f"Translation: {mid_translation}")
- # self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
+
+ # for multi aruco calibration
+ # if translations_xyz != [] and orientations_rpy != []:
+ # mid_translation = np.array(translations_xyz).mean(axis=0)
+ # mid_orientation_rpy = np.array(orientations_rpy).mean(axis=0)
+ # print ("Current camera position:")
+ # print(f"Orientation RPY:{mid_orientation_rpy}")
+ # quat = tf_t.quaternion_from_euler(*mid_orientation_rpy.asarray())
+ # print(f"Orientation xyzw:{quat}")
+ # print(f"Translation: {mid_translation}")
+ # self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
if last_used_quaternion is not None:
- self._save_transform_to_yaml(translations_xyz[-1], last_used_quaternion, "world" ,self.camera_frames[0]) # [TODO] HARDCODE camera frame
- self.get_logger().debug("Command for tf static:\n ros2 run tf2_ros static_transform_publisher "
- f"{translations_xyz[-1][0]} {translations_xyz[-1][1]} {translations_xyz[-1][2]} "
- f"{rot.x} {rot.y} {rot.z} {rot.w}"
- " camera world" )
- def _save_transform_to_yaml(self, translation, quaternion, frame_id, child_frame_id):
- # Prepare the data structure
+ self._save_transform_to_yaml(
+ translations_xyz[-1],
+ last_used_quaternion,
+ "world",
+ self.camera_frames[0],
+ ) # [TODO] HARDCODE camera frame
+ self.get_logger().debug(
+ "Command for tf static:\n ros2 run tf2_ros static_transform_publisher "
+ f"{translations_xyz[-1][0]} {translations_xyz[-1][1]} {translations_xyz[-1][2]} "
+ f"{rot.x} {rot.y} {rot.z} {rot.w}"
+ " camera world"
+ )
+
+ def _save_transform_to_yaml(
+ self, translation, quaternion, frame_id, child_frame_id
+ ):
data = {
- 'static_transforms': [
+ "static_transforms": [
{
- 'name': "base_link_2_radar",
- 'translation': translation,
- 'rotation': quaternion,
- 'parent_frame_id': frame_id,
- 'child_frame_id': child_frame_id
+ "name": "base_link_2_radar",
+ "translation": translation,
+ "rotation": quaternion,
+ "parent_frame_id": frame_id,
+ "child_frame_id": child_frame_id,
}
]
}
- # Save the data to a YAML file
- with open(self.path_to_save, 'w') as file:
+ with open(self.path_to_save, "w") as file:
yaml.dump(data, file, default_flow_style=False)
self.get_logger().debug(f"save config to {self.path_to_save}")
def run(self):
- self.ui_window.run() # write it interface
+ self.ui_window.run() # [TODO] write it to interface
+
def main(args=None):
rclpy.init()
- minimal_node = CalibrationNode()
+ minimal_node = CalibrationNode()
minimal_node.run()
-if __name__ == '__main__':
- main()
+if __name__ == "__main__":
+ main()
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 590ace5..353d502 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -1,20 +1,29 @@
import sys
-from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QVBoxLayout, QPushButton, QLineEdit, QLabel
+from PyQt5.QtWidgets import (
+ QApplication,
+ QMainWindow,
+ QWidget,
+ QVBoxLayout,
+ QPushButton,
+ QLineEdit,
+ QLabel,
+)
from PyQt5.QtCore import QTimer
from .window_interface import WindowInterface
from queue import Queue, Full, Empty
import time
from typing import Optional
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
+from geometry_msgs.msg import Pose, TransformStamped
import tf_transformations as tf_t
import rclpy
from threading import Thread
+
class GuiWindow(QMainWindow, WindowInterface):
- def __init__(self, node ):
+ def __init__(self, node):
self.app = QApplication(["node", "1"])
self.logger = node.get_logger()
self.node = node
@@ -55,13 +64,14 @@ class GuiWindow(QMainWindow, WindowInterface):
# Set the layout on the central widget
central_widget.setLayout(layout)
- self.tf_queue = Queue(32)# AtomicQueue(256, wait_strategy=BUSY_SPIN_WAIT_STRATEGY)
+ self.tf_queue = Queue(
+ 32
+ ) # AtomicQueue(256, wait_strategy=BUSY_SPIN_WAIT_STRATEGY)
self.calibration_ready = False
-
- def _handler_ros_node(self)->None:
+ def _handler_ros_node(self) -> None:
rclpy.spin(self.node)
-
+
def calibrate(self) -> None:
self.calibration_ready = True
print("Calibrating...")
@@ -69,8 +79,10 @@ class GuiWindow(QMainWindow, WindowInterface):
def save_position(self) -> None:
try:
# Why ros specific in ui???
- self.tf = TransformStamped()
- quat = tf_t.quaternion_from_euler(*[float(x.text())for x in self.pose_inputs[4:7]])
+ self.tf = TransformStamped()
+ quat = tf_t.quaternion_from_euler(
+ *[float(x.text()) for x in self.pose_inputs[4:7]]
+ )
self.tf.transform.translation.x = float(self.pose_inputs[1].text())
self.tf.transform.translation.y = float(self.pose_inputs[2].text())
self.tf.transform.translation.z = float(self.pose_inputs[3].text())
@@ -97,7 +109,7 @@ class GuiWindow(QMainWindow, WindowInterface):
try:
return self.tf_queue.get_nowait()
except Empty:
- pass # debug log
+ pass # debug log
except Exception as e:
print(e)
return None
@@ -114,14 +126,12 @@ class GuiWindow(QMainWindow, WindowInterface):
rclpy.shutdown()
-
-
def main():
app = QApplication(sys.argv)
window = GuiWindow()
window.show()
sys.exit(app.exec_())
+
if __name__ == "__main__":
main()
-
diff --git a/src/calibration_node/calibration_node/window_interface.py b/src/calibration_node/calibration_node/window_interface.py
index b3ed2f0..3e5f6a8 100644
--- a/src/calibration_node/calibration_node/window_interface.py
+++ b/src/calibration_node/calibration_node/window_interface.py
@@ -1,6 +1,7 @@
import abc
-class WindowInterface():
+
+class WindowInterface:
@abc.abstractmethod
def is_calibrate_ready(self):
pass
@@ -16,6 +17,3 @@ class WindowInterface():
@abc.abstractmethod
def destroy(self):
pass
-
-
-
--
GitLab
From cf7b22d2e521cf981b3d4aeebca9d1de7a3c9fab Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 7 Apr 2024 17:58:21 +0300
Subject: [PATCH 19/64] =?UTF-8?q?=D0=9D=D0=B0=D1=80=D0=B0=D0=B1=D0=BE?=
=?UTF-8?q?=D1=82=D0=BA=D0=B8=20=D0=BF=D1=80=D0=BE=D1=85=D0=BE=D1=80=D0=B0?=
=?UTF-8?q?=20=D0=B4=D0=BB=D1=8F=20=D1=81=D0=B2=D1=8F=D0=B7=D0=B8=20=D1=81?=
=?UTF-8?q?=20=D0=BF=D1=83=D0=BB=D1=8C=D1=82=D0=BE=D0=BC?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Комментарий прохора: Вот кÑтати Ð¾Ð±Ð½Ð¾Ð²Ð»ÐµÐ½Ð¸Ñ Ð½Ð¾Ð´Ñ‹. Вроде Ñделал получше, добавил обработку ошибок, напиÑал нормальный код, запуÑкающий ноду, Ñделал больше Ð»Ð¾Ð³Ð³Ð¸Ñ€Ð¾Ð²Ð°Ð½Ð¸Ñ Ð¸ добавил роÑовÑкие параметры.
ТеÑтил, вÑÑ‘ нормально работает. Ðе знаю, еÑли чеÑтно, что ещё Ñ Ð½ÐµÐ¹ можно Ñделать. КажетÑÑ, что пока Ñтоит отложить прокÑи до тех пор, пока не поÑвитÑÑ Ñ‚Ð¾, Ñ Ñ‡ÐµÐ¼ её Ñоединить можно (чтобы уже какой-то Ñигнал более-менее адекватный был. Ещё о формате входного Ð´Ð»Ñ Ð¿Ñ€Ð¾ÐºÑи ÑÐ¾Ð¾Ð±Ñ‰ÐµÐ½Ð¸Ñ Ð¼Ð¾Ð¶Ð½Ð¾ подумать)
---
src/command/CMakeLists.txt | 32 +++++
src/command/LICENSE | 202 +++++++++++++++++++++++++++++++
src/command/msg/Command.msg | 6 +
src/command/msg/Trajector.msg | 0
src/command/package.xml | 24 ++++
src/proxy/LICENSE | 202 +++++++++++++++++++++++++++++++
src/proxy/package.xml | 22 ++++
src/proxy/proxy/CommandSender.py | 49 ++++++++
src/proxy/proxy/Proxy.py | 98 +++++++++++++++
src/proxy/proxy/__init__.py | 0
src/proxy/resource/proxy | 0
src/proxy/setup.cfg | 4 +
src/proxy/setup.py | 27 +++++
src/proxy/test/test_copyright.py | 25 ++++
src/proxy/test/test_flake8.py | 25 ++++
src/proxy/test/test_pep257.py | 23 ++++
16 files changed, 739 insertions(+)
create mode 100644 src/command/CMakeLists.txt
create mode 100644 src/command/LICENSE
create mode 100644 src/command/msg/Command.msg
create mode 100644 src/command/msg/Trajector.msg
create mode 100644 src/command/package.xml
create mode 100644 src/proxy/LICENSE
create mode 100644 src/proxy/package.xml
create mode 100644 src/proxy/proxy/CommandSender.py
create mode 100644 src/proxy/proxy/Proxy.py
create mode 100644 src/proxy/proxy/__init__.py
create mode 100644 src/proxy/resource/proxy
create mode 100644 src/proxy/setup.cfg
create mode 100644 src/proxy/setup.py
create mode 100644 src/proxy/test/test_copyright.py
create mode 100644 src/proxy/test/test_flake8.py
create mode 100644 src/proxy/test/test_pep257.py
diff --git a/src/command/CMakeLists.txt b/src/command/CMakeLists.txt
new file mode 100644
index 0000000..3013140
--- /dev/null
+++ b/src/command/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.8)
+project(command)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Command.msg"
+)
+
+ament_package()
diff --git a/src/command/LICENSE b/src/command/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/src/command/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/command/msg/Command.msg b/src/command/msg/Command.msg
new file mode 100644
index 0000000..2763d8d
--- /dev/null
+++ b/src/command/msg/Command.msg
@@ -0,0 +1,6 @@
+string robot_name
+uint8 command
+uint8 step_count
+uint8 step_gain
+int8 turn_gain
+int8 lateral_gain
\ No newline at end of file
diff --git a/src/command/msg/Trajector.msg b/src/command/msg/Trajector.msg
new file mode 100644
index 0000000..e69de29
diff --git a/src/command/package.xml b/src/command/package.xml
new file mode 100644
index 0000000..7fd2563
--- /dev/null
+++ b/src/command/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>command</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="iashin.pa@phystech.edu">funny-ded</maintainer>
+ <license>Apache-2.0</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+ <exec_depend>rosidl_default_runtime</exec_depend>
+
+ <member_of_group>rosidl_interface_packages</member_of_group>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/src/proxy/LICENSE b/src/proxy/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/src/proxy/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/proxy/package.xml b/src/proxy/package.xml
new file mode 100644
index 0000000..877e19c
--- /dev/null
+++ b/src/proxy/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>proxy</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="iashin.pa@phystech.edu">funny-ded</maintainer>
+ <license>Apache-2.0</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <exec_depend>rclpy</exec_depend>
+
+ <exec_depend>command</exec_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/proxy/proxy/CommandSender.py b/src/proxy/proxy/CommandSender.py
new file mode 100644
index 0000000..a129cd8
--- /dev/null
+++ b/src/proxy/proxy/CommandSender.py
@@ -0,0 +1,49 @@
+import rclpy
+from rclpy.node import Node
+
+from command.msg import Command
+
+
+class CommandSender(Node):
+
+ def __init__(self):
+ super().__init__('androsot_command_sender')
+ self.publisher_ = self.create_publisher(Command, 'androsot', 10)
+
+ timer_period = 10 # seconds
+
+ self.timer = self.create_timer(timer_period, self.timer_callback)
+
+ def timer_callback(self):
+ msg = Command()
+
+ # Hardcoded params.
+ # Actually this can be done by any other node with countings
+ msg.robot_name = 'ROKI-0060'
+ msg.command = 1
+ msg.step_count = 1
+ msg.step_gain = 1
+ msg.turn_gain = 1
+ msg.lateral_gain = 1
+
+ self.publisher_.publish(msg)
+
+ self.get_logger().info(f'Publishing: {msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}')
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ minimal_publisher = CommandSender()
+
+ rclpy.spin(minimal_publisher)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ minimal_publisher.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
new file mode 100644
index 0000000..cded5ee
--- /dev/null
+++ b/src/proxy/proxy/Proxy.py
@@ -0,0 +1,98 @@
+import asyncio
+import socket
+import threading
+
+import rclpy
+from rclpy.node import Node
+
+from command.msg import Command
+
+class Proxy(Node):
+
+ def __init__(self, host: str="localhost", port: int=1969,
+ connection_atempts: int=10, connection_delay: float=2.5):
+ super().__init__('androsot_proxy')
+
+ self.subscription = self.create_subscription(
+ Command,
+ 'androsot',
+ self.listener_callback,
+ 10
+ )
+
+ self.host = host
+ self.port = port
+
+ self.n_conn_attempts = connection_atempts
+ self.conn_delay = connection_delay
+
+ self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.writer = None
+
+ self.subscription
+
+ async def connect(self):
+ for _ in range(self.n_conn_attempts):
+ try:
+ await asyncio.to_thread(self.connection.connect, (self.host, self.port))
+ _, self.writer = await asyncio.open_connection(sock=self.connection)
+ break
+
+ except ConnectionError as err:
+ self.get_logger().warning(f"{self.host}:{self.port}: {err.args[1]}")
+ await asyncio.sleep(self.conn_delay)
+
+ if self.writer is None or self.writer.is_closing():
+ raise ConnectionError(f"Connection refused after {self.conn_delay * self.n_conn_attempts} [sec]")
+
+ async def listener_callback(self, msg):
+ self.writer.write(f"{msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}\n".encode())
+ await self.writer.drain()
+ self.get_logger().info(f'I heard: {msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}')
+
+ async def disconnect(self):
+ if self.writer is not None:
+ self.writer.close()
+ await self.writer.wait_closed()
+ self.connection.close()
+
+
+
+async def spin(node: Node):
+ cancel = node.create_guard_condition(lambda: None)
+
+ def _spin(node: Node,
+ future: asyncio.Future,
+ event_loop: asyncio.AbstractEventLoop):
+ while not future.cancelled():
+ rclpy.spin_once(node)
+ if not future.cancelled():
+ event_loop.call_soon_threadsafe(future.set_result, None)
+
+ event_loop = asyncio.get_event_loop()
+ spin_task = event_loop.create_future()
+ spin_thread = threading.Thread(target=_spin, args=(node, spin_task, event_loop))
+ spin_thread.start()
+ try:
+ await spin_task
+ except asyncio.CancelledError:
+ cancel.trigger()
+ spin_thread.join()
+ node.destroy_guard_condition(cancel)
+
+async def main(args=None):
+ proxy = Proxy()
+ await proxy.connect()
+
+ await spin(proxy)
+
+ proxy.destroy_node()
+
+ return
+
+print(__name__)
+
+if __name__ == "proxy.Proxy":
+ rclpy.init()
+ asyncio.run(main())
+ rclpy.shutdown()
\ No newline at end of file
diff --git a/src/proxy/proxy/__init__.py b/src/proxy/proxy/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/proxy/resource/proxy b/src/proxy/resource/proxy
new file mode 100644
index 0000000..e69de29
diff --git a/src/proxy/setup.cfg b/src/proxy/setup.cfg
new file mode 100644
index 0000000..12a060f
--- /dev/null
+++ b/src/proxy/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/proxy
+[install]
+install_scripts=$base/lib/proxy
diff --git a/src/proxy/setup.py b/src/proxy/setup.py
new file mode 100644
index 0000000..da4aa91
--- /dev/null
+++ b/src/proxy/setup.py
@@ -0,0 +1,27 @@
+from setuptools import find_packages, setup
+
+package_name = 'proxy'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='funny-ded',
+ maintainer_email='iashin.pa@phystech.edu',
+ description='TODO: Package description',
+ license='Apache-2.0',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'command = proxy.CommandSender:main',
+ 'proxy = proxy.Proxy:main',
+ ],
+},
+)
diff --git a/src/proxy/test/test_copyright.py b/src/proxy/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/proxy/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/proxy/test/test_flake8.py b/src/proxy/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/proxy/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/proxy/test/test_pep257.py b/src/proxy/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/proxy/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From e1517c8362530c9adf802e04acb62ee15684e0c7 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Mon, 8 Apr 2024 00:42:13 +0300
Subject: [PATCH 20/64] Add second camera to calibration
---
.../calibration_node/calibration.py | 64 +++++++++++--------
.../single_camera_calibration.launch.py | 17 +----
2 files changed, 38 insertions(+), 43 deletions(-)
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 441df84..acd4161 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -35,14 +35,16 @@ class CalibrationNode(Node):
self._init_params()
self.current_aruco_tf = dict()
+ for camera in self.camera_frames:
+ self.current_aruco_tf[camera] = dict()
self.match_pose_arr = list()
self.tf_broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(1, self.calib_cb)
self.ui_window = GuiWindow(self)
def _init_params(self):
- self.declare_parameter("world_tf_topic", "/tf_static")
- world_topic = self.get_parameter("world_tf_topic").value
+ self.declare_parameter("world_tf_topic", "world")
+ world_name = self.get_parameter("world_tf_topic").value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
@@ -53,7 +55,6 @@ class CalibrationNode(Node):
)
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
- self.aruco_pose = None
self.declare_parameter(
"camera_frames", ["camera1" "camera2"]
) # rclpy.Parameter.Type.STRING_ARRAY)
@@ -61,20 +62,21 @@ class CalibrationNode(Node):
self.declare_parameter("path_to_save", "./static_transforms.yaml")
self.path_to_save = self.get_parameter("path_to_save").value
self.tf_static_broadcaster = StaticTransformBroadcaster(self)
+ self.dump_tf = list()
def get_aruco_pose(self, msg):
- self.current_aruco_tf.clear()
for id, pose in zip(msg.marker_ids, msg.poses):
+ if msg.header.frame_id not in self.current_aruco_tf:
+ continue
tf_stamped = TransformStamped()
tf_stamped.transform.translation.x = pose.position.x
tf_stamped.transform.translation.y = pose.position.y
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- tf_stamped.child_frame_id = self.camera_frames[0] # add add seconde camera
+ tf_stamped.child_fra me_id = self.camera_frames[0] # add add seconde camera
tf_stamped.header.frame_id = str(id)
- self.current_aruco_tf[str(id)] = tf_stamped
- self.aruco_pose = tf_stamped
+ self.current_aruco_tf[msg.header.frame_id][str(id)] = tf_stamped
def calib_cb(self):
pose_ui = self.ui_window.try_get_position()
@@ -86,11 +88,13 @@ class CalibrationNode(Node):
self.get_logger().debug(
f"self.current_aruco_tf: {self.current_aruco_tf}, pose_ui.child_frame_id: {pose_ui.child_frame_id}"
)
- if (
- str(pose_ui.child_frame_id) in self.current_aruco_tf
- ): # [TODO] pose_ui.child_frame_id - has int type????
- pose_aruco = self.current_aruco_tf[str(pose_ui.child_frame_id)]
- self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
+ for camera_frame in self.current_aruco_tf.keys():
+ if (
+ str(pose_ui.child_frame_id) in self.current_aruco_tf[camera_frame]
+ ): # [TODO] pose_ui.child_frame_id - has int type????child_frame_id id is number aruco
+
+ pose_aruco = self.current_aruco_tf[camera_frame][str(pose_ui.child_frame_id)]
+ self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
if self.ui_window.is_calibrate_ready():
self.calibrate()
@@ -101,8 +105,9 @@ class CalibrationNode(Node):
translations_xyz = []
orientations_rpy = []
last_used_quaternion = None
+ buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+
for match_pose in self.match_pose_arr:
- buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
# rot ebal pythona
time_now = self.get_clock().now()
match_pose.tf_ui.header.stamp = time_now.to_msg()
@@ -113,12 +118,14 @@ class CalibrationNode(Node):
f"tf_ui is {match_pose.tf_ui}\n, tf_camera is {match_pose.tf_camera} "
)
+ for camera_frame in self.camera_frames:
+
inverse_transform = buffer_core.lookup_transform_core(
- match_pose.tf_camera.child_frame_id,
- match_pose.tf_ui.header.frame_id,
+ match_pose.tf_camera.child_frame_id, # camera
+ match_pose.tf_ui.header.frame_id, # world
time_now,
)
- inverse_transform.child_frame_id = "camera"
+ inverse_transform.child_frame_id = camera_frame
inverse_transform.header.frame_id = "world"
self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
@@ -142,7 +149,7 @@ class CalibrationNode(Node):
# self._save_transform_to_yaml(mid_translation.asarray(), quat, self.camera_frames[0], "world") # [TODO] HARDCODE camera frame
if last_used_quaternion is not None:
- self._save_transform_to_yaml(
+ self._add_transform_to_yaml(
translations_xyz[-1],
last_used_quaternion,
"world",
@@ -154,20 +161,23 @@ class CalibrationNode(Node):
f"{rot.x} {rot.y} {rot.z} {rot.w}"
" camera world"
)
+ self._save_transform_to_yaml()
+ def _add_transform_to_yaml(self, translation,
+ quaternion, frame_id, child_frame_id):
+ self.dump_tf.append(
+ {
+ "name": child_frame_id,
+ "translation": translation,
+ "rotation": quaternion,
+ "parent_frame_id": frame_id,
+ "child_frame_id": child_frame_id,
+ })
def _save_transform_to_yaml(
- self, translation, quaternion, frame_id, child_frame_id
+ self
):
data = {
- "static_transforms": [
- {
- "name": "base_link_2_radar",
- "translation": translation,
- "rotation": quaternion,
- "parent_frame_id": frame_id,
- "child_frame_id": child_frame_id,
- }
- ]
+ "static_transforms": self.dump_tf
}
with open(self.path_to_save, "w") as file:
yaml.dump(data, file, default_flow_style=False)
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
index 062b0fd..711acc8 100644
--- a/src/calibration_node/launch/single_camera_calibration.launch.py
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -8,23 +8,8 @@ def generate_launch_description():
camera_config = os.path.join(get_package_share_directory('server_main'),
'config',
"camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "calib_conf.json")
return LaunchDescription([
-# Node(
-# package='tf2_ros',
-# executable='static_transform_publisher',
-# # args = [1,2,0,0,0,0, 'world', 'camera']
-# # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
-# # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
-# # ]
-# # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
-# arguments=["-2.2984534584977494", "0.03178341624420522", "2.2612959861952744", "-1.5959610015041537", "-0.001000506359060393", " -2.5601661670824227", "world", "camera"]
-
-
-# ),
Node(
package='ros2_aruco',
executable='aruco_node',
@@ -41,7 +26,7 @@ def generate_launch_description():
output='screen',
emulate_tty=True
),
- Node(
+ Node(
package='usb_cam',
executable='usb_cam_node_exe',
parameters=[camera_config],
--
GitLab
From 44a7c26ad37799a20389746656b9c394c00d27fc Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 12 Apr 2024 19:59:21 +0300
Subject: [PATCH 21/64] created localisation package
---
src/localisation/config/calib_conf.json | 14 ++
.../config/calibration_matrix.yaml | 16 +++
src/localisation/config/camera_info.yaml | 21 +++
src/localisation/config/camera_info2.yaml | 20 +++
src/localisation/config/camera_params.yaml | 22 +++
src/localisation/config/camera_params2.yaml | 22 +++
src/localisation/config/marker_rotation.yaml | 6 +
src/localisation/config/mosquitto.conf | 21 +++
src/localisation/config/robots_config.json | 36 +++++
src/localisation/config/robots_config2.json | 14 ++
.../Untitled-checkpoint.ipynb | 6 +
.../localisation}/ParticleFilter.py | 0
.../localisation}/Seminar-Localisation.ipynb | 0
src/localisation/localisation/Untitled.ipynb | 109 +++++++++++++++
src/localisation/localisation/__init__.py | 0
src/localisation/localisation/alpha_filter.py | 0
.../localisation}/field.py | 0
.../localisation}/landmarks.json | 0
.../localisation}/localisation_node.py | 36 +----
.../localisation}/parfield.json | 0
.../localisation}/particle.py | 0
.../localisation}/pf_constants.json | 0
.../localisation}/pf_visualization.py | 0
.../localisation}/robot.py | 0
src/localisation/package.xml | 24 ++++
src/localisation/setup.cfg | 4 +
src/localisation/setup.py | 30 ++++
src/localisation/test/test_copyright.py | 25 ++++
src/localisation/test/test_flake8.py | 25 ++++
src/localisation/test/test_pep257.py | 23 ++++
src/notebooks/Seminar-Localisation.ipynb | 128 ------------------
31 files changed, 441 insertions(+), 161 deletions(-)
create mode 100644 src/localisation/config/calib_conf.json
create mode 100644 src/localisation/config/calibration_matrix.yaml
create mode 100644 src/localisation/config/camera_info.yaml
create mode 100644 src/localisation/config/camera_info2.yaml
create mode 100644 src/localisation/config/camera_params.yaml
create mode 100644 src/localisation/config/camera_params2.yaml
create mode 100644 src/localisation/config/marker_rotation.yaml
create mode 100644 src/localisation/config/mosquitto.conf
create mode 100644 src/localisation/config/robots_config.json
create mode 100644 src/localisation/config/robots_config2.json
create mode 100644 src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
rename src/{localization => localisation/localisation}/ParticleFilter.py (100%)
rename src/{localization => localisation/localisation}/Seminar-Localisation.ipynb (100%)
create mode 100644 src/localisation/localisation/Untitled.ipynb
create mode 100644 src/localisation/localisation/__init__.py
create mode 100644 src/localisation/localisation/alpha_filter.py
rename src/{localization => localisation/localisation}/field.py (100%)
rename src/{localization => localisation/localisation}/landmarks.json (100%)
rename src/{localization => localisation/localisation}/localisation_node.py (91%)
rename src/{localization => localisation/localisation}/parfield.json (100%)
rename src/{localization => localisation/localisation}/particle.py (100%)
rename src/{localization => localisation/localisation}/pf_constants.json (100%)
rename src/{localization => localisation/localisation}/pf_visualization.py (100%)
rename src/{localization => localisation/localisation}/robot.py (100%)
create mode 100644 src/localisation/package.xml
create mode 100644 src/localisation/setup.cfg
create mode 100644 src/localisation/setup.py
create mode 100644 src/localisation/test/test_copyright.py
create mode 100644 src/localisation/test/test_flake8.py
create mode 100644 src/localisation/test/test_pep257.py
delete mode 100644 src/notebooks/Seminar-Localisation.ipynb
diff --git a/src/localisation/config/calib_conf.json b/src/localisation/config/calib_conf.json
new file mode 100644
index 0000000..7bac7d2
--- /dev/null
+++ b/src/localisation/config/calib_conf.json
@@ -0,0 +1,14 @@
+{
+ "robots": {
+ "0": {
+ "aruco_ids": [0, 1, 2, 3, 4],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0],
+ [0, 0, 0]
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/localisation/config/calibration_matrix.yaml b/src/localisation/config/calibration_matrix.yaml
new file mode 100644
index 0000000..56218b1
--- /dev/null
+++ b/src/localisation/config/calibration_matrix.yaml
@@ -0,0 +1,16 @@
+camera_matrix:
+- - 692.5604995244659
+ - 0.0
+ - 336.582371405584
+- - 0.0
+ - 692.4058785229926
+ - 207.3964967153443
+- - 0.0
+ - 0.0
+ - 1.0
+dist_coeff:
+- - -0.019834435039636538
+ - 1.6377918003108394
+ - -0.008073747599334049
+ - 0.008472730984545531
+ - -6.105157535323345
diff --git a/src/localisation/config/camera_info.yaml b/src/localisation/config/camera_info.yaml
new file mode 100644
index 0000000..f732a6c
--- /dev/null
+++ b/src/localisation/config/camera_info.yaml
@@ -0,0 +1,21 @@
+image_width: 1920
+image_height: 1080
+camera_name: camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1454.6684146024668, 0.0, 999.049079061686, 0.0, 1439.4606636404403, 513.6532631258082, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.043097070537003644, -0.10599336336465601, 0.0012429129258369842, 0.004094904502998846, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1,0,0, 0,1,0, 0,0,1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1450.0821533203125, 0.0, 1008.4757212882178, 0.0, 0.0, 1448.0181884765625, 514.0421690974472, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/localisation/config/camera_info2.yaml b/src/localisation/config/camera_info2.yaml
new file mode 100644
index 0000000..3c89cd5
--- /dev/null
+++ b/src/localisation/config/camera_info2.yaml
@@ -0,0 +1,20 @@
+image_width: 1920
+image_height: 1080
+camera_name: camera2
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1419.4187950618846, 0.0, 974.9896663237244, 0.0, 1414.0655832766458, 495.6548379114915, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.00920295872972792, -0.0695572098352804, 0.0018571515990045589, 0.0001661032848335393, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1,0,0, 0,1,0, 0,0,1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1403.2264404296875, 0.0, 975.6681712616119, 0.0, 0.0, 1412.9404296875, 496.1955598228087, 0.0, 0.0, 0.0, 1.0, 0.0]
diff --git a/src/localisation/config/camera_params.yaml b/src/localisation/config/camera_params.yaml
new file mode 100644
index 0000000..f34094b
--- /dev/null
+++ b/src/localisation/config/camera_params.yaml
@@ -0,0 +1,22 @@
+/**:
+ ros__parameters:
+ video_device: "/dev/video2"
+ framerate: 30.0
+ io_method: "mmap"
+ frame_id: "camera"
+ pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
+ image_width: 1920
+ image_height: 1080
+ camera_name: "camera"
+ camera_info_url: "package://server_main/config/camera_info.yaml"
+ brightness: -1
+ contrast: -1
+ saturation: -1
+ sharpness: -1
+ gain: -1
+ auto_white_balance: true
+ white_balance: 4000
+ autoexposure: true
+ exposure: 100
+ autofocus: false
+ focus: -1
\ No newline at end of file
diff --git a/src/localisation/config/camera_params2.yaml b/src/localisation/config/camera_params2.yaml
new file mode 100644
index 0000000..e2e1c67
--- /dev/null
+++ b/src/localisation/config/camera_params2.yaml
@@ -0,0 +1,22 @@
+/**:
+ ros__parameters:
+ video_device: "/dev/video4"
+ framerate: 30.0
+ io_method: "mmap"
+ frame_id: "camera2"
+ pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
+ image_width: 1920
+ image_height: 1080
+ camera_name: "camera2"
+ camera_info_url: "package://server_main/config/camera_info2.yaml"
+ brightness: -1
+ contrast: -1
+ saturation: -1
+ sharpness: -1
+ gain: -1
+ auto_white_balance: true
+ white_balance: 4000
+ autoexposure: true
+ exposure: 100
+ autofocus: false
+ focus: -1
\ No newline at end of file
diff --git a/src/localisation/config/marker_rotation.yaml b/src/localisation/config/marker_rotation.yaml
new file mode 100644
index 0000000..8b8deb7
--- /dev/null
+++ b/src/localisation/config/marker_rotation.yaml
@@ -0,0 +1,6 @@
+robots:
+ 1:
+ aruco_ids: [0, 1, 2, 3, 4]
+ # eyler angles to fowrward
+ aruco_transforms: [[0,0,0], [0,1.57,0],
+ [0,-3.14,0], [0,-1.57,0], [0,1.57,1.57]]
diff --git a/src/localisation/config/mosquitto.conf b/src/localisation/config/mosquitto.conf
new file mode 100644
index 0000000..eba98d4
--- /dev/null
+++ b/src/localisation/config/mosquitto.conf
@@ -0,0 +1,21 @@
+listener 1900
+max_queued_messages 1
+queue_qos0_messages 1
+max_queued_messages 1
+
+listener 1901
+max_queued_messages 1
+queue_qos0_messages 1
+max_queued_messages 1
+
+listener 1902
+max_queued_messages 1
+queue_qos0_messages 1
+max_queued_messages 1
+
+listener 1903
+max_queued_messages 1
+queue_qos0_messages 1
+max_queued_messages 1
+
+allow_anonymous true
diff --git a/src/localisation/config/robots_config.json b/src/localisation/config/robots_config.json
new file mode 100644
index 0000000..0c6a8b4
--- /dev/null
+++ b/src/localisation/config/robots_config.json
@@ -0,0 +1,36 @@
+{
+ "robots": {
+ "0": {
+ "aruco_ids": [0, 1, 2, 3, 4],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [0, 1.57, 1.57]
+ ]
+ },
+
+ "1": {
+ "aruco_ids": [5,6,7,8,9],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [0, 1.57, 1.57]
+ ]
+ },
+
+ "2": {
+ "aruco_ids": [10,11,12,13,14],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [0, 1.57, 1.57]
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/localisation/config/robots_config2.json b/src/localisation/config/robots_config2.json
new file mode 100644
index 0000000..818a916
--- /dev/null
+++ b/src/localisation/config/robots_config2.json
@@ -0,0 +1,14 @@
+{
+ "robots": {
+ "1": {
+ "aruco_ids": [15, 16, 17, 18, 19],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [0, 1.57, 1.57]
+ ]
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb b/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
new file mode 100644
index 0000000..7fec515
--- /dev/null
+++ b/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
@@ -0,0 +1,6 @@
+{
+ "cells": [],
+ "metadata": {},
+ "nbformat": 4,
+ "nbformat_minor": 4
+}
diff --git a/src/localization/ParticleFilter.py b/src/localisation/localisation/ParticleFilter.py
similarity index 100%
rename from src/localization/ParticleFilter.py
rename to src/localisation/localisation/ParticleFilter.py
diff --git a/src/localization/Seminar-Localisation.ipynb b/src/localisation/localisation/Seminar-Localisation.ipynb
similarity index 100%
rename from src/localization/Seminar-Localisation.ipynb
rename to src/localisation/localisation/Seminar-Localisation.ipynb
diff --git a/src/localisation/localisation/Untitled.ipynb b/src/localisation/localisation/Untitled.ipynb
new file mode 100644
index 0000000..80b5756
--- /dev/null
+++ b/src/localisation/localisation/Untitled.ipynb
@@ -0,0 +1,109 @@
+{
+ "cells": [
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import math\n",
+ "import numpy as np\n",
+ "import matplotlib.pyplot as plt"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 92,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "angle = np.linspace(0, 4 * np.pi, 100)\n",
+ "sin = np.sin(angle)\n",
+ "cos = np.cos(angle)\n",
+ "# computed_angle = map(math.atan2, sin, cos)\n",
+ "computed_angle = []\n",
+ "for i in range(angle.size):\n",
+ " computed_angle.append(math.atan2(sin[i], cos[i]))"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 89,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "(100,)\n"
+ ]
+ }
+ ],
+ "source": [
+ "print(angle.shape)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 87,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# computed_angle = list(computed_angle)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 93,
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "[<matplotlib.lines.Line2D at 0x7f86fd9eb730>]"
+ ]
+ },
+ "execution_count": 93,
+ "metadata": {},
+ "output_type": "execute_result"
+ },
+ {
+ "data": {
+ "image/png": "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\n",
+ "text/plain": [
+ "<Figure size 432x288 with 1 Axes>"
+ ]
+ },
+ "metadata": {
+ "needs_background": "light"
+ },
+ "output_type": "display_data"
+ }
+ ],
+ "source": [
+ "plt.plot(computed_angle)"
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 3",
+ "language": "python",
+ "name": "python3"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.8.10"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}
diff --git a/src/localisation/localisation/__init__.py b/src/localisation/localisation/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/localisation/localisation/alpha_filter.py b/src/localisation/localisation/alpha_filter.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/localization/field.py b/src/localisation/localisation/field.py
similarity index 100%
rename from src/localization/field.py
rename to src/localisation/localisation/field.py
diff --git a/src/localization/landmarks.json b/src/localisation/localisation/landmarks.json
similarity index 100%
rename from src/localization/landmarks.json
rename to src/localisation/localisation/landmarks.json
diff --git a/src/localization/localisation_node.py b/src/localisation/localisation/localisation_node.py
similarity index 91%
rename from src/localization/localisation_node.py
rename to src/localisation/localisation/localisation_node.py
index cc6d35f..a00278d 100644
--- a/src/localization/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -15,39 +15,9 @@ import math
from pyquaternion import Quaternion
from tf2_ros import TransformBroadcaster
-from ParticleFilter import ParticleFilter
+# from ParticleFilter import ParticleFilter
-
-from collections import deque
-
-class AlphaFilter():
- def __init__(self):
- pass
- self.buffer = deque()
- self.buffer_size = 10
- self.alpha = 0.70
-
- def update(self, value):
- if (self.buffer_size == len(self.buffer)):
- self.buffer.popleft()
-
-
- self.buffer.append(value)
-
- def get(self):
- weight_sum = 1
- ## find yaw mid
- mid = Pose() # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!! lost quaternion
- for i, elem in enumerate(self.buffer):
- mid.position.x += elem.position.x * self.alpha ** (len(self.buffer) - i)
- mid.position.y += elem.position.y * self.alpha ** (len(self.buffer) - i)
- mid.position.z += elem.position.z * self.alpha ** (len(self.buffer) - i)
- weight_sum += self.alpha ** (len(self.buffer) - i )
-
- mid.position.x /= weight_sum
- mid.position.y /= weight_sum
- mid.position.z /= weight_sum
- return mid
+# from alpha_filter import AlphaFilter
class LocalisationNode(Node):
def __init__(self):
@@ -57,7 +27,7 @@ class LocalisationNode(Node):
self.declare_parameter("aruco_markers_topic", "/aruco_markers")
topic_name = self.get_parameter("aruco_markers_topic").value
# 0 - front, 1 - right, 2 - back, 3 - left, 4 - up
- self.declare_parameter("robots_params", rclpy.Parameter.Type.STRING)
+ self.declare_parameter("robots_params", "/home/root/workspace/install/localisation/share/localisation/config/robots_config.json")
self.aruco_rotation_map = {}
robots_params_path = str(self.get_parameter("robots_params").value)
robots_params = None
diff --git a/src/localization/parfield.json b/src/localisation/localisation/parfield.json
similarity index 100%
rename from src/localization/parfield.json
rename to src/localisation/localisation/parfield.json
diff --git a/src/localization/particle.py b/src/localisation/localisation/particle.py
similarity index 100%
rename from src/localization/particle.py
rename to src/localisation/localisation/particle.py
diff --git a/src/localization/pf_constants.json b/src/localisation/localisation/pf_constants.json
similarity index 100%
rename from src/localization/pf_constants.json
rename to src/localisation/localisation/pf_constants.json
diff --git a/src/localization/pf_visualization.py b/src/localisation/localisation/pf_visualization.py
similarity index 100%
rename from src/localization/pf_visualization.py
rename to src/localisation/localisation/pf_visualization.py
diff --git a/src/localization/robot.py b/src/localisation/localisation/robot.py
similarity index 100%
rename from src/localization/robot.py
rename to src/localisation/localisation/robot.py
diff --git a/src/localisation/package.xml b/src/localisation/package.xml
new file mode 100644
index 0000000..011e107
--- /dev/null
+++ b/src/localisation/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>localisation</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>TODO: License declaration</license>
+
+ <depend>ros2_aruco_interfaces</depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>tf2_geometry_msgs</exec_depend>
+ <exec_depend>tf2_ros</exec_depend>
+ <exec_depend>tf_transformations</exec_depend>
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/localisation/setup.cfg b/src/localisation/setup.cfg
new file mode 100644
index 0000000..1084e65
--- /dev/null
+++ b/src/localisation/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/localisation
+[install]
+install_scripts=$base/lib/localisation
diff --git a/src/localisation/setup.py b/src/localisation/setup.py
new file mode 100644
index 0000000..9728d5b
--- /dev/null
+++ b/src/localisation/setup.py
@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'localisation'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+ (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='root',
+ maintainer_email='root@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'localisation_node = localisation.localisation_node:main'
+ ],
+ },
+)
diff --git a/src/localisation/test/test_copyright.py b/src/localisation/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/localisation/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/localisation/test/test_flake8.py b/src/localisation/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/localisation/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/localisation/test/test_pep257.py b/src/localisation/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/localisation/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/notebooks/Seminar-Localisation.ipynb b/src/notebooks/Seminar-Localisation.ipynb
deleted file mode 100644
index 5016d27..0000000
--- a/src/notebooks/Seminar-Localisation.ipynb
+++ /dev/null
@@ -1,128 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Фильтр чаÑтиц"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 1,
- "metadata": {},
- "outputs": [],
- "source": [
- "import sys\n",
- "import json\n",
- "import numpy as np\n",
- "\n",
- "sys.path.append(\"../localization/\")\n",
- "\n",
- "from ParticleFilter import ParticleFilter\n",
- "from robot import Robot\n",
- "from field import Field\n",
- "\n",
- "from pf_visualization import visualization"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 2,
- "metadata": {},
- "outputs": [],
- "source": [
- "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 100, apr = True)\n",
- "# pf.uniform_reset()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 4,
- "metadata": {},
- "outputs": [],
- "source": [
- "observation = np.array([-3.0, -1.0, 6.28 - 1.57/2.0])\n",
- "for _ in range(100):\n",
- " pf.resampling(observation)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 8,
- "metadata": {},
- "outputs": [
- {
- "data": {
- "image/png": "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\n",
- "text/plain": [
- "<Figure size 432x648 with 1 Axes>"
- ]
- },
- "metadata": {
- "needs_background": "light"
- },
- "output_type": "display_data"
- }
- ],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=2)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "pf.uniform_reset()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": []
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3",
- "language": "python",
- "name": "python3"
- },
- "language_info": {
- "codemirror_mode": {
- "name": "ipython",
- "version": 3
- },
- "file_extension": ".py",
- "mimetype": "text/x-python",
- "name": "python",
- "nbconvert_exporter": "python",
- "pygments_lexer": "ipython3",
- "version": "3.8.10"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}
--
GitLab
From 00d54b76f13ade83a10da8eb9b87f6c882646b6e Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 13 Apr 2024 01:12:15 +0300
Subject: [PATCH 22/64] It works and it enough
---
.../calibration_node/__init__.py | 146 +-----------------
.../calibration_node/calibration.py | 54 ++++---
.../calibration_node/gui_window.py | 4 +-
.../launch/multi_camera_calibration.launch.py | 78 ++++++++++
.../single_camera_calibration.launch.py | 2 +-
5 files changed, 117 insertions(+), 167 deletions(-)
create mode 100644 src/calibration_node/launch/multi_camera_calibration.launch.py
diff --git a/src/calibration_node/calibration_node/__init__.py b/src/calibration_node/calibration_node/__init__.py
index 0bb9339..a67dff3 100644
--- a/src/calibration_node/calibration_node/__init__.py
+++ b/src/calibration_node/calibration_node/__init__.py
@@ -1,145 +1 @@
-from ros2_aruco_interfaces.msg import ArucoMarkers
-
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
-from sensor_msgs.msg import CameraInfo
-from soccer_vision_2d_msgs.msg import BallArray, Ball
-import tf2_ros
-
-from image_geometry import PinholeCameraModel
-
-import rclpy
-from rclpy.node import Node
-import json
-from tf2_ros import TransformException
-from tf2_ros.buffer import Buffer
-from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
-import tf_transformations as tf_t
-import math
-from skspatial.objects import Line, Plane
-from pyquaternion import Quaternion
-import paho.mqtt.client as mqtt
-from tf2_ros import TransformBroadcaster
-import numpy as np
-
-from collections import deque
-
-class CalibrationNode(Node):
- def __init__(self):
- super().__init__("CalibrationNode")
- # to launch file
- # self.declare_parameter("aruco_size_mm", "starkit1")
- # self.aruco_size_mm = self.get_parameter("aruco_size_mm").value
-
- # self.declare_parameter("aruco_s", "starkit1")
- # self.aruco_id = self.get_parameter("aruco_size_mm").value
-
-
- # !!aruco topic
- self.declare_parameter("aruco_markers_topic", "/aruco_markers")
- topic_name = self.get_parameter("aruco_markers_topic").value
-
- self.declare_parameter("aruco_size", 0.170) #
- self.aruco_size = self.get_parameter("aruco_size").value # m
-
- self.aruco_subscription = self.create_subscription(
- ArucoMarkers, topic_name, self.get_aruco_pose, 10
- )
- self.current_aruco_poses = {} # {if: x, y}
-
- self.declare_parameter("aruco_markers_topic", "/aruco_markers")
- topic_name = self.get_parameter("aruco_markers_topic").value
-
- self.declare_parameter("position_config")
- topic_name = self.get_parameter("position_config").value
-
- def read_config(path:str):
- self.position_list
- pass
-
- def get_aruco_pose(self, msg):
- self.current_aruco_poses.clear()
- for id, pose in zip(msg.marker_ids, msg.poses):
- pose_stamped = PoseStamped()
- pose_stamped.pose = pose
- pose_stamped.header = msg.header
- self.current_aruco_poses[id] = pose_stamped
-
- def get_yaw_world_rotation(self, pose: Pose) -> float:
- x_ax = [0, 0, 1]
- quat_arr = [
- pose.orientation.x,
- pose.orientation.y,
- pose.orientation.z,
- pose.orientation.w,
- ]
- quat_conj = tf_t.quaternion_conjugate(quat_arr)
- # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
- rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
- return math.atan2(
- rotated_x[1], rotated_x[0]
- ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
-
-
- def get_yaw_world_rotation(self, pose: Pose) -> float:
- x_ax = [0, 0, 1]
- quat_arr = [
- pose.orientation.x,
- pose.orientation.y,
- pose.orientation.z,
- pose.orientation.w,
- ]
- quat_conj = tf_t.quaternion_conjugate(quat_arr)
- # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
- rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
- return math.atan2(
- rotated_x[1], rotated_x[0]
- ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
-
- def dump(self, id):
- if id in self.world_robots_poses:
- msg = PoseStamped()
- msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
- msg.pose = self.world_robots_poses[id].get()
- self.robots_pub[id].publish(msg)
- # self.get_logger().info("send_command_to_goalkeeper")
- if self.team_name == "starkit1":
- robot_msg = f"{-msg.pose.position.x} {-msg.pose.position.y} {-self.get_yaw_world_rotation(msg.pose)}"
- else:
- robot_msg = f"{msg.pose.position.x} {msg.pose.position.y} {self.get_yaw_world_rotation(msg.pose)}"
-
- if 0: # calibraiotn log ???
- self.loged_data.append(
- [
- self.get_clock().now().nanoseconds,
- msg.pose.position.x,
- msg.pose.position.y,
- self.get_yaw_world_rotation(msg.pose),
- ]
- )
- with open(self.file_to_save, "w") as file:
- import csv
-
- writer = csv.writer(file)
- writer.writerows(self.loged_data)
- self.mqttc.publish(self.robots_topic[id], robot_msg, 1)
-
-
-
-def main(args=None):
- rclpy.init(args=args)
-
- server_node = CalibrationNode()
-
- rclpy.spin(server_node)
-
- # Destroy the node explicitly
- # (optional - otherwise it will be done automatically
- # when the garbage collector destroys the node object)
- server_node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == "__main__":
- main()
+print("init`")
\ No newline at end of file
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index acd4161..9f808be 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -56,7 +56,7 @@ class CalibrationNode(Node):
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
self.declare_parameter(
- "camera_frames", ["camera1" "camera2"]
+ "camera_frames", ["camera1", "camera2"]
) # rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
self.declare_parameter("path_to_save", "./static_transforms.yaml")
@@ -74,8 +74,9 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- tf_stamped.child_fra me_id = self.camera_frames[0] # add add seconde camera
- tf_stamped.header.frame_id = str(id)
+ #print (f"!!!!!!!!!!\n Header is {msg.header}")
+ tf_stamped.child_frame_id = str(id) #self.camera_frames[0] # add add seconde camera
+ # tf_stamped.header.frame_id =
self.current_aruco_tf[msg.header.frame_id][str(id)] = tf_stamped
def calib_cb(self):
@@ -85,15 +86,15 @@ class CalibrationNode(Node):
# Huetta!
if pose_ui is not None:
pose_ui.header.stamp = self.get_clock().now().to_msg()
- self.get_logger().debug(
+ self.get_logger().info(
f"self.current_aruco_tf: {self.current_aruco_tf}, pose_ui.child_frame_id: {pose_ui.child_frame_id}"
)
for camera_frame in self.current_aruco_tf.keys():
if (
- str(pose_ui.child_frame_id) in self.current_aruco_tf[camera_frame]
+ str(pose_ui.header.frame_id) in self.current_aruco_tf[camera_frame]
): # [TODO] pose_ui.child_frame_id - has int type????child_frame_id id is number aruco
- pose_aruco = self.current_aruco_tf[camera_frame][str(pose_ui.child_frame_id)]
+ pose_aruco = self.current_aruco_tf[camera_frame][str(pose_ui.header.frame_id)]
self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
if self.ui_window.is_calibrate_ready():
@@ -105,26 +106,41 @@ class CalibrationNode(Node):
translations_xyz = []
orientations_rpy = []
last_used_quaternion = None
- buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_cores = dict()
+ for camera in self.camera_frames:
+ buffer_cores[camera] = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ # buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ time_now = self.get_clock().now()
for match_pose in self.match_pose_arr:
# rot ebal pythona
- time_now = self.get_clock().now()
+ buffer_core = buffer_cores[match_pose.tf_camera.header.frame_id]
match_pose.tf_ui.header.stamp = time_now.to_msg()
match_pose.tf_camera.header.stamp = time_now.to_msg()
- buffer_core.set_transform(match_pose.tf_camera, "default_authority")
+ self.get_logger().info(
+ f"self.current_aruco_tf: {match_pose.tf_camera}, pose_ui.child_frame_id: {match_pose.tf_ui}"
+ )
buffer_core.set_transform(match_pose.tf_ui, "default_authority")
- self.get_logger().debug(
+ buffer_core.set_transform(match_pose.tf_camera, "default_authority")
+ self.get_logger().info(
f"tf_ui is {match_pose.tf_ui}\n, tf_camera is {match_pose.tf_camera} "
)
for camera_frame in self.camera_frames:
-
- inverse_transform = buffer_core.lookup_transform_core(
- match_pose.tf_camera.child_frame_id, # camera
- match_pose.tf_ui.header.frame_id, # world
- time_now,
- )
+ try:
+ # buffer_core.set_transform(
+ # buffer_core.lookup_transform_core("aruco", camera_frame, time_now), "default_authority"
+ # )
+ # print(dir(buffer_core))
+ # print(buffer_core.all_frames_as_string())
+ inverse_transform = buffer_cores[camera_frame].lookup_transform_core(
+ "world",#match_pose.tf_ui.header.frame_id, # world
+ camera_frame,#match_pose.tf_camera.header.frame_id, # camera
+ time_now,
+ )
+ except Exception as e:
+ print(e)
+ continue
inverse_transform.child_frame_id = camera_frame
inverse_transform.header.frame_id = "world"
self.tf_broadcaster.sendTransform(inverse_transform)
@@ -153,13 +169,13 @@ class CalibrationNode(Node):
translations_xyz[-1],
last_used_quaternion,
"world",
- self.camera_frames[0],
+ camera_frame,
) # [TODO] HARDCODE camera frame
- self.get_logger().debug(
+ self.get_logger().info(
"Command for tf static:\n ros2 run tf2_ros static_transform_publisher "
f"{translations_xyz[-1][0]} {translations_xyz[-1][1]} {translations_xyz[-1][2]} "
f"{rot.x} {rot.y} {rot.z} {rot.w}"
- " camera world"
+ f" camera {camera_frame}"
)
self._save_transform_to_yaml()
def _add_transform_to_yaml(self, translation,
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 353d502..0c59463 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -90,8 +90,8 @@ class GuiWindow(QMainWindow, WindowInterface):
self.tf.transform.rotation.y = quat[1]
self.tf.transform.rotation.z = quat[2]
self.tf.transform.rotation.w = quat[3]
- self.tf.header.frame_id = "world"
- self.tf.child_frame_id = self.pose_inputs[0].text()
+ self.tf.header.frame_id = self.pose_inputs[0].text()
+ self.tf.child_frame_id = "world" #
except Exception as e:
print(e)
try:
diff --git a/src/calibration_node/launch/multi_camera_calibration.launch.py b/src/calibration_node/launch/multi_camera_calibration.launch.py
new file mode 100644
index 0000000..63a3381
--- /dev/null
+++ b/src/calibration_node/launch/multi_camera_calibration.launch.py
@@ -0,0 +1,78 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ camera_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params.yaml")
+ camera_config2 = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params2.yaml")
+
+ return LaunchDescription([
+
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config],
+ output='screen',
+ namespace='camera1',
+ emulate_tty=True
+ ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera1'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.170} # 0.07
+ ],
+ remappings=[
+ ('/camera/image_raw', '/camera1/image_raw'),
+ ('/camera/camera_info', '/camera1/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config2],
+ output='screen',
+ namespace='camera2',
+ emulate_tty=True
+ ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera2'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.170} # 0.07
+ ],
+ remappings=[
+ ('/camera/image_raw', '/camera2/image_raw'),
+ ('/camera/camera_info', '/camera2/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+
+ # world frame publisher
+
+ Node(
+ package='calibration_node',
+ executable='calibration_node',
+# parameters=[
+# {'robots_params': robot_config},
+# {'camera_frames': ["camera"]}
+# ],
+ output='screen',
+ emulate_tty=True
+ )
+ ])
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
index 711acc8..7389688 100644
--- a/src/calibration_node/launch/single_camera_calibration.launch.py
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -14,7 +14,7 @@ def generate_launch_description():
package='ros2_aruco',
executable='aruco_node',
parameters=[
- {'camera_frame': 'camera'},
+ {'camera_frame': 'camera1'},
{'aruco_dictionary_id': 'DICT_4X4_100'},
{'marker_size':0.170} # 0.07
],
--
GitLab
From ab4387b65898deadc628db65c14b3094eab0b532 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 13 Apr 2024 01:13:57 +0300
Subject: [PATCH 23/64] Config fixes for laptop with two cameras
---
src/server_main/config/camera_info.yaml | 2 +-
src/server_main/config/camera_params.yaml | 6 +++---
src/server_main/config/camera_params2.yaml | 2 +-
3 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/server_main/config/camera_info.yaml b/src/server_main/config/camera_info.yaml
index f732a6c..4f26561 100644
--- a/src/server_main/config/camera_info.yaml
+++ b/src/server_main/config/camera_info.yaml
@@ -1,6 +1,6 @@
image_width: 1920
image_height: 1080
-camera_name: camera
+camera_name: camera1
camera_matrix:
rows: 3
cols: 3
diff --git a/src/server_main/config/camera_params.yaml b/src/server_main/config/camera_params.yaml
index f34094b..814104e 100644
--- a/src/server_main/config/camera_params.yaml
+++ b/src/server_main/config/camera_params.yaml
@@ -1,13 +1,13 @@
/**:
ros__parameters:
- video_device: "/dev/video2"
+ video_device: "/dev/video4"
framerate: 30.0
io_method: "mmap"
- frame_id: "camera"
+ frame_id: "camera1"
pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
image_width: 1920
image_height: 1080
- camera_name: "camera"
+ camera_name: "camera1"
camera_info_url: "package://server_main/config/camera_info.yaml"
brightness: -1
contrast: -1
diff --git a/src/server_main/config/camera_params2.yaml b/src/server_main/config/camera_params2.yaml
index e2e1c67..837502b 100644
--- a/src/server_main/config/camera_params2.yaml
+++ b/src/server_main/config/camera_params2.yaml
@@ -1,6 +1,6 @@
/**:
ros__parameters:
- video_device: "/dev/video4"
+ video_device: "/dev/video6"
framerate: 30.0
io_method: "mmap"
frame_id: "camera2"
--
GitLab
From baf8b93874da39f392dad0a9da29a125c0112c64 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 13 Apr 2024 01:17:46 +0300
Subject: [PATCH 24/64] create ros2 project
---
.../localization_preprocessor/__init__.py | 0
.../aruco_preprocessor_node.py | 6 +++++
src/localization_preprocessor/package.xml | 18 +++++++++++++
.../resource/localization_preprocessor | 0
src/localization_preprocessor/setup.cfg | 4 +++
src/localization_preprocessor/setup.py | 26 +++++++++++++++++++
.../test/test_copyright.py | 25 ++++++++++++++++++
.../test/test_flake8.py | 25 ++++++++++++++++++
.../test/test_pep257.py | 23 ++++++++++++++++
9 files changed, 127 insertions(+)
create mode 100644 src/localization_preprocessor/localization_preprocessor/__init__.py
create mode 100644 src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
create mode 100644 src/localization_preprocessor/package.xml
create mode 100644 src/localization_preprocessor/resource/localization_preprocessor
create mode 100644 src/localization_preprocessor/setup.cfg
create mode 100644 src/localization_preprocessor/setup.py
create mode 100644 src/localization_preprocessor/test/test_copyright.py
create mode 100644 src/localization_preprocessor/test/test_flake8.py
create mode 100644 src/localization_preprocessor/test/test_pep257.py
diff --git a/src/localization_preprocessor/localization_preprocessor/__init__.py b/src/localization_preprocessor/localization_preprocessor/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
new file mode 100644
index 0000000..fa0bd00
--- /dev/null
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -0,0 +1,6 @@
+def main():
+ print('Hi from localization_preprocessor.')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/localization_preprocessor/package.xml b/src/localization_preprocessor/package.xml
new file mode 100644
index 0000000..b817a19
--- /dev/null
+++ b/src/localization_preprocessor/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>localization_preprocessor</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>TODO: License declaration</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/localization_preprocessor/resource/localization_preprocessor b/src/localization_preprocessor/resource/localization_preprocessor
new file mode 100644
index 0000000..e69de29
diff --git a/src/localization_preprocessor/setup.cfg b/src/localization_preprocessor/setup.cfg
new file mode 100644
index 0000000..1b13798
--- /dev/null
+++ b/src/localization_preprocessor/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/localization_preprocessor
+[install]
+install_scripts=$base/lib/localization_preprocessor
diff --git a/src/localization_preprocessor/setup.py b/src/localization_preprocessor/setup.py
new file mode 100644
index 0000000..d2e4362
--- /dev/null
+++ b/src/localization_preprocessor/setup.py
@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'localization_preprocessor'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='root',
+ maintainer_email='root@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'aruco_preprocessor_node = localization_preprocessor.aruco_preprocessor_node:main'
+ ],
+ },
+)
diff --git a/src/localization_preprocessor/test/test_copyright.py b/src/localization_preprocessor/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/localization_preprocessor/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/localization_preprocessor/test/test_flake8.py b/src/localization_preprocessor/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/localization_preprocessor/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/localization_preprocessor/test/test_pep257.py b/src/localization_preprocessor/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/localization_preprocessor/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From 109539ba01339ec746818bd60fb261cfc265641a Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 13 Apr 2024 03:38:46 +0300
Subject: [PATCH 25/64] [WIP] Add FieldPose msg and add some logic to
preprocessor
---
src/androsot_msgs/CMakeLists.txt | 31 +++
src/androsot_msgs/msg/FieldPose.msg | 3 +
src/androsot_msgs/package.xml | 22 +++
.../multi_camera_aruco_preprocessor.launch.py | 0
.../solo_camera_aruco_preprocessor.launch.py | 52 +++++
.../aruco_preprocessor_node.py | 182 +++++++++++++++++-
src/localization_preprocessor/package.xml | 2 +-
src/localization_preprocessor/setup.py | 5 +-
8 files changed, 293 insertions(+), 4 deletions(-)
create mode 100644 src/androsot_msgs/CMakeLists.txt
create mode 100644 src/androsot_msgs/msg/FieldPose.msg
create mode 100644 src/androsot_msgs/package.xml
create mode 100644 src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
create mode 100644 src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
mode change 100644 => 100755 src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
diff --git a/src/androsot_msgs/CMakeLists.txt b/src/androsot_msgs/CMakeLists.txt
new file mode 100644
index 0000000..c21e919
--- /dev/null
+++ b/src/androsot_msgs/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(androsot_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/FieldPose.msg"
+)
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/androsot_msgs/msg/FieldPose.msg b/src/androsot_msgs/msg/FieldPose.msg
new file mode 100644
index 0000000..cd03a56
--- /dev/null
+++ b/src/androsot_msgs/msg/FieldPose.msg
@@ -0,0 +1,3 @@
+float64 x
+float64 y
+float64 yaw
\ No newline at end of file
diff --git a/src/androsot_msgs/package.xml b/src/androsot_msgs/package.xml
new file mode 100644
index 0000000..2e66974
--- /dev/null
+++ b/src/androsot_msgs/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>androsot_msgs</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="xatc120@gmail.com">nakuznetsov</maintainer>
+ <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+<build_depend>rosidl_default_generators</build_depend>
+
+<exec_depend>rosidl_default_runtime</exec_depend>
+
+<member_of_group>rosidl_interface_packages</member_of_group>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
new file mode 100644
index 0000000..3f867dd
--- /dev/null
+++ b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
@@ -0,0 +1,52 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ camera_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params.yaml")
+ robot_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "robots_config.json")
+
+ return LaunchDescription([
+
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config],
+ output='screen',
+ namespace='camera1',
+ emulate_tty=True
+ ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera1'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.0696} # 0.07
+ ],
+ remappings=[
+ ('/camera/image_raw', '/camera1/image_raw'),
+ ('/camera/camera_info', '/camera1/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+
+ Node(
+ package='localization_preprocessor',
+ executable='aruco_preprocessor_node',
+ parameters=[
+ {'robots_params': robot_config},
+ {'camera_frames': ["camera"]}
+ ],
+ output='screen',
+ emulate_tty=True
+ )
+ ])
\ No newline at end of file
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
old mode 100644
new mode 100755
index fa0bd00..c353b96
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -1,6 +1,184 @@
-def main():
- print('Hi from localization_preprocessor.')
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Header
+from geometry_msgs.msg import Pose, PoseStamped, TransformStamped, PointStamped
+from androsot_msgs.msg import FieldPose # Assuming you've defined this
+from tf2_ros import TransformException, TransformListener, Buffer
+import tf2_ros
+from tf2_geometry_msgs import do_transform_point
+from ros2_aruco_interfaces.msg import ArucoMarkers
+import tf_transformations as tf_t
+import json
+class ArucoPreprocessorNode(Node):
+
+ def __init__(self):
+ super().__init__('aruco_preprocessor_node')
+ self.declare_parameter("aruco_markers_topic", "/aruco_markers")
+ topic_name = self.get_parameter("aruco_markers_topic").value
+
+ self.subscription = self.create_subscription(
+ 'aruco_msgs/MarkerArray', # Assuming this is the correct topic and message type
+ topic_name,
+ self.aruco_cb,
+ 10)
+ self.init_robot_params()
+
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.tf_from_camera_to_world = dict()
+
+
+
+
+ self.publisher_ = self.create_publisher(FieldPose, 'aruco_field_pose', 10)
+
+ def init_robot_params(self):
+ self.declare_parameter("robots_params", rclpy.Parameter.Type.STRING)
+ self.aruco_rotation_map = {}
+ robots_params_path = str(self.get_parameter("robots_params").value)
+ robots_params = None
+ with open(robots_params_path) as f:
+ robots_params = json.load(f)
+ self.robots = robots_params["robots"]
+ self.create_aruco_rotation_map()
+ self.init_aruco_ids()
+
+
+ def create_aruco_rotation_map(self):
+ for robot_id, param in self.robots.items():
+ aruco_ids = param["aruco_ids"]
+ aruco_transforms = param["aruco_transforms"]
+ for aruco_id, aruco_tf in zip(aruco_ids, aruco_transforms):
+ self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler(
+ aruco_tf[0], aruco_tf[1], aruco_tf[2]
+ )
+
+ def init_aruco_ids(self):
+ self.aruco_ids = {}
+ for robot_id, param in self.robots.items():
+ self.aruco_ids[robot_id] = param["aruco_ids"]
+
+ def get_robot_id(self, id: int) -> int:
+ for robot_id, ids in self.aruco_ids.items():
+ if id in ids:
+ return int(robot_id)
+ print(self.aruco_ids)
+ print(f"Bad aruco id:{id}")
+ return -1
+ # raise IOError("Bad aruco ids")
+
+ def get_camera_to_world_frame(self):
+ try:
+ for frame in self.camera_frames:
+ self.tf_from_camera_to_world[
+ frame
+ ] = self.tf_buffer.lookup_transform( # check inversion not allwed
+ "world", frame, rclpy.time.Time()
+ )
+ except TransformException as ex:
+ import time
+ time.sleep(0.1)
+ self.get_logger().info(f"Could not transform world to camera: {ex}")
+ # self.get_camera_to_world_frame()
+
+ def aruco_cb(self, msg):
+ # Process the incoming message
+ # For simplicity, let's assume we're interested in the first marker
+ if len(msg.markers) > 0:
+ marker = msg.markers[0]
+ pose = marker.pose.pose
+ # Convert the pose to x, y, yaw
+ # This is a placeholder conversion, you'll need to implement the actual conversion
+ x = pose.position.x
+ y = pose.position.y
+ yaw = pose.orientation.z # Assuming z-axis for yaw, adjust as necessary
+
+ # Create and publish the custom message
+ custom_msg = FieldPose()
+ custom_msg.header = marker.header
+ custom_msg.x = x
+ custom_msg.y = y
+ custom_msg.yaw = yaw
+ self.publisher_.publish(custom_msg)
+
+ def shift_pose_to_robot_centre(
+ self, pose, id
+ ): # Now all arruco has same offset of centre
+ # https://answers.ros.org/question/289323/simple-tf-transform-without-broadcasterlistener/
+ shifted_pose = PoseStamped()
+ shifted_pose.header = pose.header
+ shifted_pose.pose.orientation = pose.pose.orientation
+
+ shift_point = PointStamped()
+ #shift_point.point.x = 0.0 # 0.035
+ shift_point.point.z = -0.035# 0.045
+ transform = TransformStamped()
+ transform.header = pose.header
+ transform.child_frame_id = "aruco" + str(id)
+ (
+ transform.transform.translation.x,
+ transform.transform.translation.y,
+ transform.transform.translation.z,
+ ) = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
+ transform.transform.rotation = pose.pose.orientation
+
+ buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_core.set_transform(transform, "default_authority")
+ inverse_transform = buffer_core.lookup_transform_core(
+ transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
+ )
+ #self.tf_broadcaster.sendTransform(inverse_transform)
+ rot = inverse_transform.transform.rotation
+ quat = rot.x, rot.y, rot.z, rot.w
+ euler = tf_t.euler_from_quaternion(quat)
+
+ shifted_pose.pose.position = do_transform_point(shift_point, transform).point
+
+ return shifted_pose
+
+ def rotate_pose_to_forward(self, pose: PoseStamped, id):
+ rotated_pose = PoseStamped()
+ rotated_pose.pose.position = pose.pose.position
+
+ quaternion_iter = [
+ pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w,
+ ]
+ transformed_quate_iter = tf_t.quaternion_multiply(
+ quaternion_iter, self.aruco_rotation_map[id]
+ )
+ (
+ rotated_pose.pose.orientation.x,
+ rotated_pose.pose.orientation.y,
+ rotated_pose.pose.orientation.z,
+ rotated_pose.pose.orientation.w,
+ ) = transformed_quate_iter
+ # rotated_pose.header.stamp = self.get_clock().now().to_msg()
+ rotated_pose.header.frame_id = (
+ pose.header.frame_id
+ ) # "camera" ########## Choose frame id
+ return rotated_pose
+
+ def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
+ shifted_pose = self.shift_pose_to_robot_centre(pose, id)
+ rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
+ return rotated_pose
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ aruco_subscriber = ArucoPreprocessorNode()
+
+ rclpy.spin(aruco_subscriber)
+
+ # Destroy the node explicitly
+ aruco_subscriber.destroy_node()
+ rclpy.shutdown()
+
if __name__ == '__main__':
main()
diff --git a/src/localization_preprocessor/package.xml b/src/localization_preprocessor/package.xml
index b817a19..eb443b7 100644
--- a/src/localization_preprocessor/package.xml
+++ b/src/localization_preprocessor/package.xml
@@ -11,7 +11,7 @@
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
-
+ <exec_depend>androsot_msgs</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
diff --git a/src/localization_preprocessor/setup.py b/src/localization_preprocessor/setup.py
index d2e4362..cd1b158 100644
--- a/src/localization_preprocessor/setup.py
+++ b/src/localization_preprocessor/setup.py
@@ -1,5 +1,6 @@
from setuptools import setup
-
+import os
+from glob import glob
package_name = 'localization_preprocessor'
setup(
@@ -9,6 +10,8 @@ setup(
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
+ (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+ (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.[yaml]*'))),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
--
GitLab
From d129a34ecb8226f506491c447933ec1862b4a718 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 13 Apr 2024 14:55:25 +0300
Subject: [PATCH 26/64] Fix androsot messages
Add not tested code to preprocessor
---
src/androsot_msgs/CMakeLists.txt | 3 +-
src/androsot_msgs/msg/FieldPose.msg | 2 +
.../aruco_preprocessor_node.py | 75 +++++++++++++------
3 files changed, 58 insertions(+), 22 deletions(-)
diff --git a/src/androsot_msgs/CMakeLists.txt b/src/androsot_msgs/CMakeLists.txt
index c21e919..738cc18 100644
--- a/src/androsot_msgs/CMakeLists.txt
+++ b/src/androsot_msgs/CMakeLists.txt
@@ -12,9 +12,10 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
find_package(rosidl_default_generators REQUIRED)
-
+find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/FieldPose.msg"
+ DEPENDENCIES std_msgs
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
diff --git a/src/androsot_msgs/msg/FieldPose.msg b/src/androsot_msgs/msg/FieldPose.msg
index cd03a56..b4f478c 100644
--- a/src/androsot_msgs/msg/FieldPose.msg
+++ b/src/androsot_msgs/msg/FieldPose.msg
@@ -1,3 +1,5 @@
+std_msgs/Header header
+
float64 x
float64 y
float64 yaw
\ No newline at end of file
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index c353b96..2e210fc 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -19,7 +19,7 @@ class ArucoPreprocessorNode(Node):
topic_name = self.get_parameter("aruco_markers_topic").value
self.subscription = self.create_subscription(
- 'aruco_msgs/MarkerArray', # Assuming this is the correct topic and message type
+ ArucoMarkers,
topic_name,
self.aruco_cb,
10)
@@ -83,26 +83,6 @@ class ArucoPreprocessorNode(Node):
self.get_logger().info(f"Could not transform world to camera: {ex}")
# self.get_camera_to_world_frame()
- def aruco_cb(self, msg):
- # Process the incoming message
- # For simplicity, let's assume we're interested in the first marker
- if len(msg.markers) > 0:
- marker = msg.markers[0]
- pose = marker.pose.pose
- # Convert the pose to x, y, yaw
- # This is a placeholder conversion, you'll need to implement the actual conversion
- x = pose.position.x
- y = pose.position.y
- yaw = pose.orientation.z # Assuming z-axis for yaw, adjust as necessary
-
- # Create and publish the custom message
- custom_msg = FieldPose()
- custom_msg.header = marker.header
- custom_msg.x = x
- custom_msg.y = y
- custom_msg.yaw = yaw
- self.publisher_.publish(custom_msg)
-
def shift_pose_to_robot_centre(
self, pose, id
): # Now all arruco has same offset of centre
@@ -169,6 +149,59 @@ class ArucoPreprocessorNode(Node):
return rotated_pose
+ def aruco_cb(self, msg):
+
+ for id, pose in zip(msg.marker_ids, msg.poses):
+ pose_stamped = PoseStamped()
+ pose_stamped.pose = pose
+ pose_stamped.header = msg.header
+ self.current_aruco_poses[id] = pose_stamped
+
+
+ field_pose = FieldPose()
+ field_pose.header = marker.header
+ field_pose.x = x
+ field_pose.y = y
+ field_pose.yaw = yaw
+ self.publisher_.publish(field_pose)
+
+ def send_coord_to_robot(self, id):
+ if id in self.world_robots_poses:
+ msg = PoseStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "world"
+ msg.pose = self.world_robots_poses[id].get()
+ self.robots_pub[id].publish(msg)
+ # self.get_logger().info("send_command_to_goalkeeper")
+ if (self.team_name=='starkit1'):
+ robot_msg = f"{-msg.pose.position.x} {-msg.pose.position.y} {-self.get_yaw_world_rotation(msg.pose)}"
+ else:
+ robot_msg = f"{msg.pose.position.x} {msg.pose.position.y} {self.get_yaw_world_rotation(msg.pose)}"
+
+ def count_robots_poses(self):
+ robots_poses = {}
+ for id, pose in self.current_aruco_poses.items():
+ robot_id = self.get_robot_id(id)
+ self.get_logger().info(f"robot_id: {robot_id}")
+ if robot_id == -1:
+ continue
+ # forward_pose = self.get_forward_pose(pose, id) def rotate_pose_to_forward
+ rotated_to_forward_pose = self.transform_to_robot_centre(pose, id)
+
+ world_pos = self.tf_buffer.transform(rotated_to_forward_pose, "world")
+ if not (robot_id in robots_poses):
+ robots_poses[robot_id] = [world_pos]
+ else:
+ robots_poses[robot_id].append(world_pos)
+ # find mid of x,y
+ for robot_id in self.robot_ids:
+ if robot_id in robots_poses:
+ if not robot_id in self.world_robots_poses:
+ self.world_robots_poses[robot_id] = AlphaFilter()
+ self.world_robots_poses[robot_id].update(self.find_mid_pose(
+ robots_poses[robot_id])
+ ) # transform aruco to forward
+
def main(args=None):
rclpy.init(args=args)
--
GitLab
From ecb8ca62c9c97df625fda11443d39fe56e4f11e7 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sat, 13 Apr 2024 17:21:55 +0300
Subject: [PATCH 27/64] deleted unnecessary stuff
---
src/localisation/config/calib_conf.json | 14 -
.../config/calibration_matrix.yaml | 16 --
src/localisation/config/camera_info.yaml | 21 --
src/localisation/config/camera_info2.yaml | 20 --
src/localisation/config/camera_params.yaml | 22 --
src/localisation/config/camera_params2.yaml | 22 --
src/localisation/config/marker_rotation.yaml | 6 -
src/localisation/config/mosquitto.conf | 21 --
.../config/robot_to_aruco_ids.json | 15 ++
src/localisation/config/robots_config.json | 36 ---
src/localisation/config/robots_config2.json | 14 -
.../localisation/localisation_node.py | 249 ++----------------
12 files changed, 35 insertions(+), 421 deletions(-)
delete mode 100644 src/localisation/config/calib_conf.json
delete mode 100644 src/localisation/config/calibration_matrix.yaml
delete mode 100644 src/localisation/config/camera_info.yaml
delete mode 100644 src/localisation/config/camera_info2.yaml
delete mode 100644 src/localisation/config/camera_params.yaml
delete mode 100644 src/localisation/config/camera_params2.yaml
delete mode 100644 src/localisation/config/marker_rotation.yaml
delete mode 100644 src/localisation/config/mosquitto.conf
create mode 100644 src/localisation/config/robot_to_aruco_ids.json
delete mode 100644 src/localisation/config/robots_config.json
delete mode 100644 src/localisation/config/robots_config2.json
diff --git a/src/localisation/config/calib_conf.json b/src/localisation/config/calib_conf.json
deleted file mode 100644
index 7bac7d2..0000000
--- a/src/localisation/config/calib_conf.json
+++ /dev/null
@@ -1,14 +0,0 @@
-{
- "robots": {
- "0": {
- "aruco_ids": [0, 1, 2, 3, 4],
- "aruco_transforms": [
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0],
- [0, 0, 0]
- ]
- }
- }
-}
\ No newline at end of file
diff --git a/src/localisation/config/calibration_matrix.yaml b/src/localisation/config/calibration_matrix.yaml
deleted file mode 100644
index 56218b1..0000000
--- a/src/localisation/config/calibration_matrix.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-camera_matrix:
-- - 692.5604995244659
- - 0.0
- - 336.582371405584
-- - 0.0
- - 692.4058785229926
- - 207.3964967153443
-- - 0.0
- - 0.0
- - 1.0
-dist_coeff:
-- - -0.019834435039636538
- - 1.6377918003108394
- - -0.008073747599334049
- - 0.008472730984545531
- - -6.105157535323345
diff --git a/src/localisation/config/camera_info.yaml b/src/localisation/config/camera_info.yaml
deleted file mode 100644
index f732a6c..0000000
--- a/src/localisation/config/camera_info.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-image_width: 1920
-image_height: 1080
-camera_name: camera
-camera_matrix:
- rows: 3
- cols: 3
- data: [1454.6684146024668, 0.0, 999.049079061686, 0.0, 1439.4606636404403, 513.6532631258082, 0.0, 0.0, 1.0]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0.043097070537003644, -0.10599336336465601, 0.0012429129258369842, 0.004094904502998846, 0.0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1,0,0, 0,1,0, 0,0,1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1450.0821533203125, 0.0, 1008.4757212882178, 0.0, 0.0, 1448.0181884765625, 514.0421690974472, 0.0, 0.0, 0.0, 1.0, 0.0]
-
diff --git a/src/localisation/config/camera_info2.yaml b/src/localisation/config/camera_info2.yaml
deleted file mode 100644
index 3c89cd5..0000000
--- a/src/localisation/config/camera_info2.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1920
-image_height: 1080
-camera_name: camera2
-camera_matrix:
- rows: 3
- cols: 3
- data: [1419.4187950618846, 0.0, 974.9896663237244, 0.0, 1414.0655832766458, 495.6548379114915, 0.0, 0.0, 1.0]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0.00920295872972792, -0.0695572098352804, 0.0018571515990045589, 0.0001661032848335393, 0.0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1,0,0, 0,1,0, 0,0,1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1403.2264404296875, 0.0, 975.6681712616119, 0.0, 0.0, 1412.9404296875, 496.1955598228087, 0.0, 0.0, 0.0, 1.0, 0.0]
diff --git a/src/localisation/config/camera_params.yaml b/src/localisation/config/camera_params.yaml
deleted file mode 100644
index f34094b..0000000
--- a/src/localisation/config/camera_params.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-/**:
- ros__parameters:
- video_device: "/dev/video2"
- framerate: 30.0
- io_method: "mmap"
- frame_id: "camera"
- pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
- image_width: 1920
- image_height: 1080
- camera_name: "camera"
- camera_info_url: "package://server_main/config/camera_info.yaml"
- brightness: -1
- contrast: -1
- saturation: -1
- sharpness: -1
- gain: -1
- auto_white_balance: true
- white_balance: 4000
- autoexposure: true
- exposure: 100
- autofocus: false
- focus: -1
\ No newline at end of file
diff --git a/src/localisation/config/camera_params2.yaml b/src/localisation/config/camera_params2.yaml
deleted file mode 100644
index e2e1c67..0000000
--- a/src/localisation/config/camera_params2.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-/**:
- ros__parameters:
- video_device: "/dev/video4"
- framerate: 30.0
- io_method: "mmap"
- frame_id: "camera2"
- pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
- image_width: 1920
- image_height: 1080
- camera_name: "camera2"
- camera_info_url: "package://server_main/config/camera_info2.yaml"
- brightness: -1
- contrast: -1
- saturation: -1
- sharpness: -1
- gain: -1
- auto_white_balance: true
- white_balance: 4000
- autoexposure: true
- exposure: 100
- autofocus: false
- focus: -1
\ No newline at end of file
diff --git a/src/localisation/config/marker_rotation.yaml b/src/localisation/config/marker_rotation.yaml
deleted file mode 100644
index 8b8deb7..0000000
--- a/src/localisation/config/marker_rotation.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-robots:
- 1:
- aruco_ids: [0, 1, 2, 3, 4]
- # eyler angles to fowrward
- aruco_transforms: [[0,0,0], [0,1.57,0],
- [0,-3.14,0], [0,-1.57,0], [0,1.57,1.57]]
diff --git a/src/localisation/config/mosquitto.conf b/src/localisation/config/mosquitto.conf
deleted file mode 100644
index eba98d4..0000000
--- a/src/localisation/config/mosquitto.conf
+++ /dev/null
@@ -1,21 +0,0 @@
-listener 1900
-max_queued_messages 1
-queue_qos0_messages 1
-max_queued_messages 1
-
-listener 1901
-max_queued_messages 1
-queue_qos0_messages 1
-max_queued_messages 1
-
-listener 1902
-max_queued_messages 1
-queue_qos0_messages 1
-max_queued_messages 1
-
-listener 1903
-max_queued_messages 1
-queue_qos0_messages 1
-max_queued_messages 1
-
-allow_anonymous true
diff --git a/src/localisation/config/robot_to_aruco_ids.json b/src/localisation/config/robot_to_aruco_ids.json
new file mode 100644
index 0000000..b44a6ef
--- /dev/null
+++ b/src/localisation/config/robot_to_aruco_ids.json
@@ -0,0 +1,15 @@
+{
+ "robots": {
+ "0": {
+ "aruco_ids": [0, 1, 2, 3, 4]
+ },
+
+ "1": {
+ "aruco_ids": [5,6,7,8,9]
+ },
+
+ "2": {
+ "aruco_ids": [10,11,12,13,14]
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/localisation/config/robots_config.json b/src/localisation/config/robots_config.json
deleted file mode 100644
index 0c6a8b4..0000000
--- a/src/localisation/config/robots_config.json
+++ /dev/null
@@ -1,36 +0,0 @@
-{
- "robots": {
- "0": {
- "aruco_ids": [0, 1, 2, 3, 4],
- "aruco_transforms": [
- [0, 0, 0],
- [0, -1.57, 0],
- [0, 3.14, 0],
- [0, 1.57, 0],
- [0, 1.57, 1.57]
- ]
- },
-
- "1": {
- "aruco_ids": [5,6,7,8,9],
- "aruco_transforms": [
- [0, 0, 0],
- [0, -1.57, 0],
- [0, 3.14, 0],
- [0, 1.57, 0],
- [0, 1.57, 1.57]
- ]
- },
-
- "2": {
- "aruco_ids": [10,11,12,13,14],
- "aruco_transforms": [
- [0, 0, 0],
- [0, -1.57, 0],
- [0, 3.14, 0],
- [0, 1.57, 0],
- [0, 1.57, 1.57]
- ]
- }
- }
-}
\ No newline at end of file
diff --git a/src/localisation/config/robots_config2.json b/src/localisation/config/robots_config2.json
deleted file mode 100644
index 818a916..0000000
--- a/src/localisation/config/robots_config2.json
+++ /dev/null
@@ -1,14 +0,0 @@
-{
- "robots": {
- "1": {
- "aruco_ids": [15, 16, 17, 18, 19],
- "aruco_transforms": [
- [0, 0, 0],
- [0, -1.57, 0],
- [0, 3.14, 0],
- [0, 1.57, 0],
- [0, 1.57, 1.57]
- ]
- }
- }
-}
\ No newline at end of file
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index a00278d..8958fe8 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -22,262 +22,53 @@ from tf2_ros import TransformBroadcaster
class LocalisationNode(Node):
def __init__(self):
super().__init__("LocalisationNode")
- self.robot_ids = [0, 1, 2]
+ self.robot_ids = [0]
- self.declare_parameter("aruco_markers_topic", "/aruco_markers")
- topic_name = self.get_parameter("aruco_markers_topic").value
- # 0 - front, 1 - right, 2 - back, 3 - left, 4 - up
- self.declare_parameter("robots_params", "/home/root/workspace/install/localisation/share/localisation/config/robots_config.json")
- self.aruco_rotation_map = {}
- robots_params_path = str(self.get_parameter("robots_params").value)
- robots_params = None
- with open(robots_params_path) as f:
- robots_params = json.load(f)
- self.robots = robots_params["robots"]
- self.create_aruco_rotation_map()
- self.init_aruco_ids()
- self.declare_parameter("aruco_size", 0.050) #TODO
- self.aruco_size = self.get_parameter("aruco_size").value # m
+ self.declare_parameter("robot_to_aruco_ids", "/home/root/workspace/install/localisation/share/localisation/config/robot_to_aruco_ids.json")
- self.declare_parameter("world_tf_topic", "/tf_static")
- world_topic = self.get_parameter("world_tf_topic").value
- self.tf_buffer = Buffer()
- self.tf_listener = TransformListener(self.tf_buffer, self)
- self.tf_from_camera_to_world = dict()
- self.aruco_subscription = self.create_subscription(
- ArucoMarkers, topic_name, self.get_aruco_pose, 10
- )
+ robot_to_aruco_ids_path = str(self.get_parameter("robot_to_aruco_ids").value)
+ robot_to_aruco_ids = None
+ with open(robot_to_aruco_ids_path) as f:
+ robot_to_aruco_ids = json.load(f)
+ self.robots_to_aruco_ids = robot_to_aruco_ids["robots"]
+
+ self.init_aruco_ids()
+
self.robots_pub = {}
for robot_id in self.robot_ids:
- self.robots_pub[robot_id] = self.create_publisher(PoseStamped, "/robot_pose" + str(robot_id), 10)
+ self.robots_pub[robot_id] = self.create_publisher(Pose2D, "/robot_pose" + str(robot_id), 10)
self.loged_data = []
self.file_to_save = "/home/root/workspace/a.out"
- self.create_publisher(PoseStamped, "/robot0_pose", 10)
self.current_aruco_poses = {} # {if: x, y}
self.current_robot_poses = {}
- self.world_robots_poses = {}
-
- self.tf_broadcaster = TransformBroadcaster(self)
-
- def create_aruco_rotation_map(self):
- for robot_id, param in self.robots.items():
- aruco_ids = param["aruco_ids"]
- aruco_transforms = param["aruco_transforms"]
- for aruco_id, aruco_tf in zip(aruco_ids, aruco_transforms):
- print(f"RPY: {aruco_tf[0]}, {aruco_tf[1]}, {aruco_tf[2]}")
- self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler( #TODO get rid of euler angles
- aruco_tf[0], aruco_tf[1], aruco_tf[2]
- )
+ self.aruco_observed_poses = {}
def init_aruco_ids(self):
self.aruco_ids = {}
- for robot_id, param in self.robots.items():
+ for robot_id, param in self.robots_to_aruco_ids.items():
self.aruco_ids[robot_id] = param["aruco_ids"]
- def get_robot_id(self, id: int) -> int:
+ def get_robot_id_from_aruco_id(self, id: int) -> int:
for robot_id, ids in self.aruco_ids.items():
if id in ids:
return int(robot_id)
print(self.aruco_ids)
print(f"Bad aruco id:{id}")
return -1
- # raise IOError("Bad aruco ids")
-
- def get_camera_to_world_frame(self):
- try:
- for frame in self.camera_frames:
- self.tf_from_camera_to_world[
- frame
- ] = self.tf_buffer.lookup_transform( # check inversion not allwed
- "world", frame, rclpy.time.Time()
- )
- except TransformException as ex:
- import time
- time.sleep(0.1)
- self.get_logger().info(f"Could not transform world to camera: {ex}")
- # self.get_camera_to_world_frame()
-
- def get_aruco_pose(self, msg):
- self.current_aruco_poses.clear()
- for id, pose in zip(msg.marker_ids, msg.poses):
- pose_stamped = PoseStamped()
- pose_stamped.pose = pose
- pose_stamped.header = msg.header
- self.current_aruco_poses[id] = pose_stamped
-
- def shift_pose_to_robot_centre(
- self, pose, id
- ): # Now all arruco has same offset of centre
- # https://answers.ros.org/question/289323/simple-tf-transform-without-broadcasterlistener/
- shifted_pose = PoseStamped()
- shifted_pose.header = pose.header
- shifted_pose.pose.orientation = pose.pose.orientation
-
- shift_point = PointStamped()
- #shift_point.point.x = 0.0 # 0.035
- shift_point.point.z = -0.035# 0.045
- transform = TransformStamped()
- transform.header = pose.header
- transform.child_frame_id = "aruco" + str(id)
- (
- transform.transform.translation.x,
- transform.transform.translation.y,
- transform.transform.translation.z,
- ) = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
- transform.transform.rotation = pose.pose.orientation
-
- buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
- buffer_core.set_transform(transform, "default_authority")
- inverse_transform = buffer_core.lookup_transform_core(
- transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
- )
- #self.tf_broadcaster.sendTransform(inverse_transform)
- rot = inverse_transform.transform.rotation
- quat = rot.x, rot.y, rot.z, rot.w
- euler = tf_t.euler_from_quaternion(quat)
- #print("TF RPY: ", euler )
- #print("Ibverssed TF: ", inverse_transform.transform.translation)
- #import time
- #time.sleep(0.1) # also remove shift
- shifted_pose.pose.position = do_transform_point(shift_point, transform).point
-
- return shifted_pose
-
- def rotate_pose_to_forward(self, pose: PoseStamped, id):
- rotated_pose = PoseStamped()
- rotated_pose.pose.position = pose.pose.position
-
- quaternion_iter = [
- pose.pose.orientation.x,
- pose.pose.orientation.y,
- pose.pose.orientation.z,
- pose.pose.orientation.w,
- ]
- transformed_quate_iter = tf_t.quaternion_multiply(
- quaternion_iter, self.aruco_rotation_map[id]
- )
- (
- rotated_pose.pose.orientation.x,
- rotated_pose.pose.orientation.y,
- rotated_pose.pose.orientation.z,
- rotated_pose.pose.orientation.w,
- ) = transformed_quate_iter
- # rotated_pose.header.stamp = self.get_clock().now().to_msg()
- rotated_pose.header.frame_id = (
- pose.header.frame_id
- ) # "camera" ########## Choose frame id
- return rotated_pose
-
- def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
- shifted_pose = self.shift_pose_to_robot_centre(pose, id)
- rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
- return rotated_pose
-
- @staticmethod
- def quatWAvg(Q):
- '''
- Averaging Quaternions.
-
- Arguments:
- Q(ndarray): an Mx4 ndarray of quaternions.
- weights(list): an M elements list, a weight for each quaternion.
- '''
-
- # Form the symmetric accumulator matrix
- A = np.zeros((4, 4))
- M = Q.shape[1]
-
- for i in range(M):
- q = Q[:, i]
- A += np.outer(q, q)# rank 1 update
-
- # Get the eigenvector corresponding to largest eigen value
- return np.linalg.eigh(A)[1][:, -1]
-
- def find_mid_pose(self, poses) -> Pose:
- mid_pose = Pose()
- Q_array = np.zeros((4, len(poses)))
- # find mid of orientation and coordinates
- for count, pose in enumerate(poses):
- mid_pose.position.x += pose.pose.position.x
- mid_pose.position.y += pose.pose.position.y
- mid_pose.position.z += pose.pose.position.z
-
- Q_array[0, count] = pose.pose.orientation.x
- Q_array[1, count] = pose.pose.orientation.y
- Q_array[2, count] = pose.pose.orientation.z
- Q_array[3, count] = pose.pose.orientation.w
-
- mid_pose.position.x /= len(poses)
- mid_pose.position.y /= len(poses)
- mid_pose.position.z /= len(poses)
-
- mid_quat = self.quatWAvg(Q_array)
- mid_pose.orientation.x = mid_quat[0]
- mid_pose.orientation.y = mid_quat[1]
- mid_pose.orientation.z = mid_quat[2]
- mid_pose.orientation.w = mid_quat[3]
-
- return mid_pose
+ # raise IOError("Bad aruco ids")
def count_robots_poses(self):
- robots_poses = {}
for id, pose in self.current_aruco_poses.items():
- robot_id = self.get_robot_id(id)
+ robot_id = self.get_robot_id_from_aruco_id(id)
self.get_logger().info(f"robot_id: {robot_id}")
if robot_id == -1:
continue
- # forward_pose = self.get_forward_pose(pose, id) def rotate_pose_to_forward
- rotated_to_forward_pose = self.transform_to_robot_centre(pose, id)
-
- world_pos = self.tf_buffer.transform(rotated_to_forward_pose, "world")
- if not (robot_id in robots_poses):
- robots_poses[robot_id] = [world_pos]
+
+ if not (robot_id in self.aruco_observed_poses):
+ self.aruco_observed_poses[robot_id] = [pose]
else:
- robots_poses[robot_id].append(world_pos)
- # find mid of x,y
- for robot_id in self.robot_ids:
- if robot_id in robots_poses:
- if not robot_id in self.world_robots_poses:
- self.world_robots_poses[robot_id] = AlphaFilter()
- self.world_robots_poses[robot_id].update(self.find_mid_pose(
- robots_poses[robot_id])
- ) # transform aruco to forward
-
-
-
- def get_yaw_world_rotation(self, pose: Pose) -> float:
- x_ax = [0, 0, 1]
- quat_arr = [
- pose.orientation.x,
- pose.orientation.y,
- pose.orientation.z,
- pose.orientation.w,
- ]
- quat_conj = tf_t.quaternion_conjugate(quat_arr) #TODO
- # rotation_matrix = tf_t.quaternion_matrix(quat_conj)
- rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
- return math.atan2(
- rotated_x[1], rotated_x[0]
- ) # ?? https://math.stackexchange.com/questions/4191786/getting-the-rotation-angle-along-a-single-local-axis-from-a-quaternion
-
- def compute_observation(self, id):
- if id in self.world_robots_poses:
- msg = PoseStamped()
- msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
- pose = self.world_robots_poses[id].get()
- msg.pose = pose
- self.robots_pub[id].publish(msg)
- yaw = self.get_yaw_world_rotation(msg.pose)
+ self.aruco_observed_poses[robot_id].append(pose)
- # if (0):
- # self.loged_data.append([self.get_clock().now().nanoseconds, msg.pose.position.x, msg.pose.position.y,self.get_yaw_world_rotation(msg.pose)])
- # with open(self.file_to_save, 'w') as file:
- # import csv
- # writer = csv.writer(file)
- # writer.writerows(self.loged_data)
-
def main(args=None):
rclpy.init(args=args)
--
GitLab
From edd10a0aa0c1031067b72b0c4055bd04648e14eb Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sat, 13 Apr 2024 22:50:44 +0300
Subject: [PATCH 28/64] started to make dump localisation
---
src/localisation/launch/ekf_filter.launch.py | 14 ++++++
.../localisation/localisation_node.py | 48 ++++---------------
2 files changed, 22 insertions(+), 40 deletions(-)
create mode 100644 src/localisation/launch/ekf_filter.launch.py
diff --git a/src/localisation/launch/ekf_filter.launch.py b/src/localisation/launch/ekf_filter.launch.py
new file mode 100644
index 0000000..9c8dcef
--- /dev/null
+++ b/src/localisation/launch/ekf_filter.launch.py
@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ ekf_node = Node(
+ package="robot_localization",
+ executable="ekf_node",
+ )
+
+ ld.add_action(ekf_node)
+
+ return ld
\ No newline at end of file
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 8958fe8..b86cd4c 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -22,52 +22,20 @@ from tf2_ros import TransformBroadcaster
class LocalisationNode(Node):
def __init__(self):
super().__init__("LocalisationNode")
- self.robot_ids = [0]
-
- self.declare_parameter("robot_to_aruco_ids", "/home/root/workspace/install/localisation/share/localisation/config/robot_to_aruco_ids.json")
-
- robot_to_aruco_ids_path = str(self.get_parameter("robot_to_aruco_ids").value)
- robot_to_aruco_ids = None
- with open(robot_to_aruco_ids_path) as f:
- robot_to_aruco_ids = json.load(f)
- self.robots_to_aruco_ids = robot_to_aruco_ids["robots"]
-
- self.init_aruco_ids()
-
- self.robots_pub = {}
- for robot_id in self.robot_ids:
- self.robots_pub[robot_id] = self.create_publisher(Pose2D, "/robot_pose" + str(robot_id), 10)
+ self.robot_id = 0
+ self.create_subscription(Pose2D, 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
+ self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose_robot_" + str(self.robot_id), 10)
self.loged_data = []
- self.file_to_save = "/home/root/workspace/a.out"
- self.current_aruco_poses = {} # {if: x, y}
+ self.current_aruco_poses = {}
self.current_robot_poses = {}
self.aruco_observed_poses = {}
- def init_aruco_ids(self):
- self.aruco_ids = {}
- for robot_id, param in self.robots_to_aruco_ids.items():
- self.aruco_ids[robot_id] = param["aruco_ids"]
-
- def get_robot_id_from_aruco_id(self, id: int) -> int:
- for robot_id, ids in self.aruco_ids.items():
- if id in ids:
- return int(robot_id)
- print(self.aruco_ids)
- print(f"Bad aruco id:{id}")
- return -1
- # raise IOError("Bad aruco ids")
+ def aruco_observation_callback(self, msg):
+ pose = {'x':msg.x, 'y'=msg.y, 'yaw' =msg.theta}
+
def count_robots_poses(self):
- for id, pose in self.current_aruco_poses.items():
- robot_id = self.get_robot_id_from_aruco_id(id)
- self.get_logger().info(f"robot_id: {robot_id}")
- if robot_id == -1:
- continue
-
- if not (robot_id in self.aruco_observed_poses):
- self.aruco_observed_poses[robot_id] = [pose]
- else:
- self.aruco_observed_poses[robot_id].append(pose)
+ pass
def main(args=None):
rclpy.init(args=args)
--
GitLab
From 3416c6c309696e7880630da71a81f1a63bb8c162 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 13:16:13 +0300
Subject: [PATCH 29/64] brought to life version with alpha filter
---
.../localisation/dump_aruco_publisher.py | 77 ++++++++++++++
.../localisation/localisation_node.py | 100 ++++++++++++++++--
src/localisation/setup.py | 3 +-
3 files changed, 173 insertions(+), 7 deletions(-)
create mode 100644 src/localisation/localisation/dump_aruco_publisher.py
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
new file mode 100644
index 0000000..e73a5a5
--- /dev/null
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -0,0 +1,77 @@
+from ros2_aruco_interfaces.msg import ArucoMarkers
+
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D
+import tf2_ros
+
+import rclpy
+from rclpy.node import Node
+import json
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point
+import tf_transformations as tf_t
+from pyquaternion import Quaternion
+from tf2_ros import TransformBroadcaster
+import numpy as np
+import math
+
+
+class DumpArucoPubNode(Node):
+ def __init__(self):
+ super().__init__("dump_aruco_pub_node")
+ self.robot_id = 0
+ self.pose_pub= self.create_publisher(Pose, 'aruco_pose_robot_' + str(self.robot_id), 10)
+ timer_period = 0.01 # seconds
+ self.timer = self.create_timer(timer_period, self.timer_callback)
+ self.counter = 0
+
+ def timer_callback(self):
+ self.counter += 1
+ msg = Pose()
+ if self.counter % 10 == 0 and False:
+ # yaw = -110 degrees
+ x = 3.0
+ y = 4.0
+ z = 0.2
+ q_x = 0.0
+ q_y = -0.819
+ q_z = 0.0
+ q_w = 0.574
+
+ else:
+ # yaw = 45 degrees
+ x = 1.0
+ y = -2.0
+ z = 0.2
+ q_x = -0.0
+ q_y = 0.0
+ q_z = 0.383
+ q_w = 0.924
+
+ msg.position.x = x
+ msg.position.y = y
+ msg.position.z = z
+ msg.orientation.x = q_x
+ msg.orientation.y = q_y
+ msg.orientation.z = q_z
+ msg.orientation.w = q_w
+
+ self.pose_pub.publish(msg)
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ node = DumpArucoPubNode()
+
+ rclpy.spin(node)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index b86cd4c..883dc7f 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -11,31 +11,119 @@ from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tf2_geometry_msgs import do_transform_point
import tf_transformations as tf_t
-import math
from pyquaternion import Quaternion
from tf2_ros import TransformBroadcaster
+import numpy as np
+import math
# from ParticleFilter import ParticleFilter
# from alpha_filter import AlphaFilter
+from collections import deque
+
+class PoseSinCos():
+ def __init__(self, x, y, sin, cos):
+ self.x = x
+ self.y = y
+ self.sin = sin
+ self.cos = cos
+
+class AlphaFilter():
+ def __init__(self):
+ pass
+ self.buffer = deque()
+ self.buffer_size = 10
+ self.alpha = 0.70
+
+ def update(self, value):
+ if (self.buffer_size == len(self.buffer)):
+ self.buffer.popleft()
+
+
+ self.buffer.append(value)
+
+ def get(self):
+ weight_sum = 0.0
+ ## find yaw mid
+ mid = PoseSinCos(0.0, 0.0, 0.0, 0.0)
+ for i, elem in enumerate(self.buffer):
+ mid.x += elem.x * self.alpha ** (len(self.buffer) - i)
+ mid.y += elem.y * self.alpha ** (len(self.buffer) - i)
+ mid.sin += elem.sin * self.alpha ** (len(self.buffer) - i)
+ mid.cos += elem.cos * self.alpha ** (len(self.buffer) - i)
+ weight_sum += self.alpha ** (len(self.buffer) - i ) #TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
+
+ if len(self.buffer) == 0:
+ weight_sum = 1.0
+ mid.x /= weight_sum
+ mid.y /= weight_sum
+ mid.sin /= weight_sum
+ mid.cos /= weight_sum
+ return mid
+
class LocalisationNode(Node):
def __init__(self):
super().__init__("LocalisationNode")
self.robot_id = 0
- self.create_subscription(Pose2D, 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
+ self.create_subscription(Pose, 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
+ self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose_robot_" + str(self.robot_id), 10)
self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose_robot_" + str(self.robot_id), 10)
self.loged_data = []
self.current_aruco_poses = {}
self.current_robot_poses = {}
self.aruco_observed_poses = {}
+ self.robot_pose = AlphaFilter()
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.0, 0.0)
+ timer_period = 1.0 # seconds
+ self.timer = self.create_timer(timer_period, self.timer_callback)
+
+ def aruco_observation_callback(self, msg):
+ x = msg.position.x
+ y = msg.position.y
+ sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
+ # print(sin, cos)
+ pose_sin_cos = PoseSinCos(x, y, sin, cos)
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, sin, cos)
+ self.robot_pose.update(pose_sin_cos)
+
- def aruco_observation_callback(self, msg):
- pose = {'x':msg.x, 'y'=msg.y, 'yaw' =msg.theta}
+ def get_x_y_yaw_pose_filtered(self):
+ pose_sin_cos = self.robot_pose.get()
+ pose = Pose2D()
+ pose.x = pose_sin_cos.x
+ pose.y = pose_sin_cos.y
+ pose.theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
+ return pose
+
+ def get_x_y_yaw_pose_unfiltered(self):
+ pose_sin_cos = self.unfiltered_last_observed_robot_pos
+ pose = Pose2D()
+ pose.x = pose_sin_cos.x
+ pose.y = pose_sin_cos.y
+ pose.theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
+ return pose
+
+ def timer_callback(self):
+ unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
+ self.unfiltered_pose_pub.publish(unfilt_msg)
+ filt_msg = self.get_x_y_yaw_pose_filtered()
+ self.filtered_pose_pub.publish(filt_msg)
- def count_robots_poses(self):
- pass
+ def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
+ x_ax = [1.0, 0.0, 0.0]
+ quat_arr = [
+ pose.orientation.w,
+ pose.orientation.x,
+ pose.orientation.y,
+ pose.orientation.z
+ ]
+
+ rotated_x = Quaternion(quat_arr).rotate(x_ax)
+ # print(rotated_x)
+ # TODO check normalisation
+ return rotated_x[1], rotated_x[0]
def main(args=None):
rclpy.init(args=args)
diff --git a/src/localisation/setup.py b/src/localisation/setup.py
index 9728d5b..e4404b2 100644
--- a/src/localisation/setup.py
+++ b/src/localisation/setup.py
@@ -24,7 +24,8 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
- 'localisation_node = localisation.localisation_node:main'
+ 'localisation_node = localisation.localisation_node:main',
+ 'dump_aruco_pub_node = localisation.dump_aruco_publisher:main'
],
},
)
--
GitLab
From 927d4b5dce21a719d90e3d2d77c50c3ba5db9a60 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 14:20:13 +0300
Subject: [PATCH 30/64] changed type to PoseWithCovarianceStamped
---
.../launch/dummy_launch.launch.py | 20 ++++++++++++
.../localisation/dump_aruco_publisher.py | 31 ++++++++++++-------
.../localisation/localisation_node.py | 16 +++++-----
3 files changed, 48 insertions(+), 19 deletions(-)
create mode 100644 src/localisation/launch/dummy_launch.launch.py
diff --git a/src/localisation/launch/dummy_launch.launch.py b/src/localisation/launch/dummy_launch.launch.py
new file mode 100644
index 0000000..4581afb
--- /dev/null
+++ b/src/localisation/launch/dummy_launch.launch.py
@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ localisation_node = Node(
+ package="localisation",
+ executable="localisation_node",
+ )
+
+ dump_aruco_pub_node = Node(
+ package="localisation",
+ executable="dump_aruco_pub_node",
+ )
+
+ ld.add_action(localisation_node)
+ ld.add_action(dump_aruco_pub_node)
+
+ return ld
\ No newline at end of file
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
index e73a5a5..a40f0bc 100644
--- a/src/localisation/localisation/dump_aruco_publisher.py
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -1,6 +1,6 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped
import tf2_ros
import rclpy
@@ -21,15 +21,17 @@ class DumpArucoPubNode(Node):
def __init__(self):
super().__init__("dump_aruco_pub_node")
self.robot_id = 0
- self.pose_pub= self.create_publisher(Pose, 'aruco_pose_robot_' + str(self.robot_id), 10)
+ self.pose_pub= self.create_publisher(PoseWithCovarianceStamped, 'aruco_pose_robot_' + str(self.robot_id), 10)
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.counter = 0
def timer_callback(self):
self.counter += 1
- msg = Pose()
- if self.counter % 10 == 0 and False:
+ msg = PoseWithCovarianceStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "map"
+ if self.counter % 101 == 0:
# yaw = -110 degrees
x = 3.0
y = 4.0
@@ -49,13 +51,20 @@ class DumpArucoPubNode(Node):
q_z = 0.383
q_w = 0.924
- msg.position.x = x
- msg.position.y = y
- msg.position.z = z
- msg.orientation.x = q_x
- msg.orientation.y = q_y
- msg.orientation.z = q_z
- msg.orientation.w = q_w
+ msg.pose.pose.position.x = x
+ msg.pose.pose.position.y = y
+ msg.pose.pose.position.z = z
+ msg.pose.pose.orientation.x = q_x
+ msg.pose.pose.orientation.y = q_y
+ msg.pose.pose.orientation.z = q_z
+ msg.pose.pose.orientation.w = q_w
+
+ msg.pose.covariance = [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, 0.0, 0.0]
self.pose_pub.publish(msg)
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 883dc7f..239bdb9 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,6 +1,6 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped
import tf2_ros
import rclpy
@@ -66,7 +66,7 @@ class LocalisationNode(Node):
def __init__(self):
super().__init__("LocalisationNode")
self.robot_id = 0
- self.create_subscription(Pose, 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
+ self.create_subscription(PoseWithCovarianceStamped , 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose_robot_" + str(self.robot_id), 10)
self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose_robot_" + str(self.robot_id), 10)
self.loged_data = []
@@ -79,8 +79,8 @@ class LocalisationNode(Node):
self.timer = self.create_timer(timer_period, self.timer_callback)
def aruco_observation_callback(self, msg):
- x = msg.position.x
- y = msg.position.y
+ x = msg.pose.pose.position.x
+ y = msg.pose.pose.position.y
sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
# print(sin, cos)
pose_sin_cos = PoseSinCos(x, y, sin, cos)
@@ -114,10 +114,10 @@ class LocalisationNode(Node):
def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
x_ax = [1.0, 0.0, 0.0]
quat_arr = [
- pose.orientation.w,
- pose.orientation.x,
- pose.orientation.y,
- pose.orientation.z
+ pose.pose.pose.orientation.w,
+ pose.pose.pose.orientation.x,
+ pose.pose.pose.orientation.y,
+ pose.pose.pose.orientation.z
]
rotated_x = Quaternion(quat_arr).rotate(x_ax)
--
GitLab
From e2634a0b286b1b7a10f38b783ff70b1201b261fd Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 14:41:06 +0300
Subject: [PATCH 31/64] canceled changes in server_main
---
src/server_main/server_main/server_node.py | 7 +++----
1 file changed, 3 insertions(+), 4 deletions(-)
diff --git a/src/server_main/server_main/server_node.py b/src/server_main/server_main/server_node.py
index 5c71277..d42107e 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -94,7 +94,7 @@ class ServerNode(Node):
self.robots = robots_params["robots"]
self.create_aruco_rotation_map()
self.init_aruco_ids()
- self.declare_parameter("aruco_size", 0.050) #TODO
+ self.declare_parameter("aruco_size", 0.050) #
self.aruco_size = self.get_parameter("aruco_size").value # m
self.declare_parameter("world_tf_topic", "/tf_static")
@@ -157,7 +157,7 @@ class ServerNode(Node):
self.robots_topic = {}
for robot_id in self.robot_ids:
self.robots_topic[robot_id] = self.team_name + "/" + str(robot_id) + "/" + "robot"
- self.mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1)
+ self.mqttc = mqtt.Client()
self.mqttc.on_message = ServerNode.on_message
self.mqttc.on_connect = ServerNode.on_connect
self.mqttc.on_publish = ServerNode.on_publish
@@ -174,8 +174,7 @@ class ServerNode(Node):
aruco_ids = param["aruco_ids"]
aruco_transforms = param["aruco_transforms"]
for aruco_id, aruco_tf in zip(aruco_ids, aruco_transforms):
- print(f"RPY: {aruco_tf[0]}, {aruco_tf[1]}, {aruco_tf[2]}")
- self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler( #TODO get rid of euler angles
+ self.aruco_rotation_map[aruco_id] = tf_t.quaternion_from_euler(
aruco_tf[0], aruco_tf[1], aruco_tf[2]
)
--
GitLab
From 048c0ae56ae6fc43e443dd86129aba2a7dfe4558 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 15:00:51 +0300
Subject: [PATCH 32/64] deleted files for particle filter
---
.../config/robot_to_aruco_ids.json | 15 --
.../localisation/ParticleFilter.py | 200 ------------------
.../localisation/Seminar-Localisation.ipynb | 128 -----------
src/localisation/localisation/Untitled.ipynb | 109 ----------
src/localisation/localisation/alpha_filter.py | 0
src/localisation/localisation/field.py | 8 -
src/localisation/localisation/landmarks.json | 8 -
src/localisation/localisation/parfield.json | 20 --
src/localisation/localisation/particle.py | 21 --
.../localisation/pf_constants.json | 24 ---
.../localisation/pf_visualization.py | 91 --------
src/localisation/localisation/robot.py | 47 ----
12 files changed, 671 deletions(-)
delete mode 100644 src/localisation/config/robot_to_aruco_ids.json
delete mode 100644 src/localisation/localisation/ParticleFilter.py
delete mode 100644 src/localisation/localisation/Seminar-Localisation.ipynb
delete mode 100644 src/localisation/localisation/Untitled.ipynb
delete mode 100644 src/localisation/localisation/alpha_filter.py
delete mode 100644 src/localisation/localisation/field.py
delete mode 100644 src/localisation/localisation/landmarks.json
delete mode 100644 src/localisation/localisation/parfield.json
delete mode 100644 src/localisation/localisation/particle.py
delete mode 100644 src/localisation/localisation/pf_constants.json
delete mode 100644 src/localisation/localisation/pf_visualization.py
delete mode 100644 src/localisation/localisation/robot.py
diff --git a/src/localisation/config/robot_to_aruco_ids.json b/src/localisation/config/robot_to_aruco_ids.json
deleted file mode 100644
index b44a6ef..0000000
--- a/src/localisation/config/robot_to_aruco_ids.json
+++ /dev/null
@@ -1,15 +0,0 @@
-{
- "robots": {
- "0": {
- "aruco_ids": [0, 1, 2, 3, 4]
- },
-
- "1": {
- "aruco_ids": [5,6,7,8,9]
- },
-
- "2": {
- "aruco_ids": [10,11,12,13,14]
- }
- }
-}
\ No newline at end of file
diff --git a/src/localisation/localisation/ParticleFilter.py b/src/localisation/localisation/ParticleFilter.py
deleted file mode 100644
index aecb57b..0000000
--- a/src/localisation/localisation/ParticleFilter.py
+++ /dev/null
@@ -1,200 +0,0 @@
-import math
-import json
-import time
-
-import numpy as np
-from numpy import random
-from particle import Particle
-from robot import Robot
-
-
-class ParticleFilter():
- def __init__(self, myrobot, field, n=100, apr = True):
- self.n = n
- self.myrobot = myrobot
- self.count = 0
- self.p = np.zeros((n, 4))
- self.field = field
-
- with open('../localization/pf_constants.json', 'r') as constants:
- constants = json.load(constants)
-
- self.forward_noise = constants['noise']['forward_noise']
- self.turn_noise = constants['noise']['turn_noise']
- self.sense_noise = constants['noise']['sense_noise']
- self.gauss_noise = constants['noise']['gauss_noise']
- self.yaw_noise = constants['noise']['yaw_noise']
-
- self.number_of_res = constants['consistency']['number_of_res']
- self.consistency = constants['consistency']['consistency']
- self.goodObsGain = constants['consistency']['goodObsGain']
- self.badObsCost = constants['consistency']['badObsCost']
- self.stepCost = constants['consistency']['stepCost']
- self.dist_threshold = constants['consistency']['dist_threshold']
- self.con_threshold = constants['consistency']['con_threshold']
- self.spec_threshold = constants['consistency']['spec_threshold']
-
- self.yaw_importance = constants['weights']['yaw_importance']
-
- if apr:
- self.gen_particles()
- else:
- for i in range(self.n):
- x = Robot((random.random()-0.5)*field.w_length, (random.random()-0.5)*field.w_width, random.random()*math.pi*2)
- self.p.append([x,0])
-
-
- def return_coord(self):
- return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
-
- def gen_particles(self):
- self.p [:, :2]= np.array([[self.myrobot.x, self.myrobot.y]]) + \
- random.normal(scale=self.sense_noise, size=(self.n, 2))
- self.p [:, 2]= self.myrobot.yaw + \
- random.normal(scale=self.yaw_noise, size=(self.n, ))
- self.p [:, 2]= self.p[:, 2] % (2 * np.pi)
-
- self.count += 1
-
- def uniform_reset(self):
- self.p = []
- for i in range(self.n):
- x = Particle(x=(random.random()-0.5)*self.field.w_length, y=(random.random()-0.5)
- * self.field.w_width, yaw=random.random()*math.pi*2)
- #x.set_noise(forward_noise, turn_noise, 0)
- self.p.append([x, 0])
- self.update_coord(self.p)
-
- def update_consistency(self, observations):
- stepConsistency = 0
- dist = math.sqrt((self.myrobot.x - observations[0])**2 +\
- (self.myrobot.y - observations[1])**2 +\
- (self.myrobot.yaw - observations[2])**2)
- if dist < self.dist_threshold:
- stepConsistency += self.goodObsGain
- #print('good step', stepConsistency)
- else:
- stepConsistency -= self.badObsCost
- #print('bad step', stepConsistency)
- # else:
- #stepConsistency -= self.stepCost
- #print('step cons', stepConsistency)
- self.consistency += stepConsistency
- if math.fabs(self.consistency) > self.spec_threshold:
- self.consistency = math.copysign(
- self.spec_threshold, self.consistency)
- #print('consistency', self.consistency)
-
- def particles_move(self, coord):
- self.myrobot.move(coord['shift_x'],
- coord['shift_y'], coord['shift_yaw'])
- # now we simulate a robot motion for each of
- # these particles
- self.p += np.array([[coord['shift_x'], coord['shift_y'],
- coord['shift_yaw'], 0.0]])
- self.count += 1
-
- def gen_n_particles_robot(self, n):
- p = np.zeros((n, 4))
- p[:, :2]= np.array([[self.myrobot.x, self.myrobot.y]]) + \
- random.normal(scale=self.sense_noise, size=(n, 2))
- p[:, 2]= self.myrobot.yaw + \
- random.normal(scale=self.yaw_noise, size=(n, ))
- p[:, 2]= p[:, 2] % (2 * np.pi)
-
- self.count += 1
- return p
-
- def gen_n_particles(self, n):
- tmp = []
- for i in range(n):
- x = Robot((random()-0.5)*self.field.w_width, (random()-0.5)
- * self.field.w_length, random()*math.pi*2)
- tmp.append([x, 0])
- return tmp
-
- def compute_weights(self, observation, sigma):
- delta_x = self.p[:, 0] - observation[0]
- delta_y = self.p[:, 1] - observation[1]
- delta_yaw = np.abs(self.p[:, 2] - observation[2])
- # print(delta_yaw)
- delta_yaw = np.where(delta_yaw<=np.pi, delta_yaw, 2*np.pi - delta_yaw)
- # print(delta_yaw)
- # print(delta_yaw)
- dist = np.square(delta_x) + np.square(delta_y) + \
- np.square(delta_yaw * self.yaw_importance)
- prob = np.exp(-dist / 2*(sigma ** 2)) / np.sqrt(2.0 * np.pi * (sigma ** 2))
- return prob
-
-
- def resampling_wheel(self, weights):
- new_particles = {}
- index = int(random.random() * self.n)
- beta = 0.0
- mw = np.max(weights)
- for i in range(self.n):
- beta += random.random() * 2.0 * mw
- while beta > weights[index]:
- beta -= weights[index]
- index = (index + 1) % self.n
- if index in new_particles.keys():
- new_particles[index] += 1
- else:
- new_particles[index] = 1
- new_ids = np.fromiter(new_particles.keys(), dtype=int)
- p_tmp = self.p[new_ids, :3]
- counts = np.fromiter(new_particles.values(), dtype=int)
- new_weights = weights[new_ids] * counts
- return p_tmp, new_weights
-
- def resampling(self, observation):
- w = self.compute_weights(observation, self.sense_noise)
- S = w.sum()
- if S > 0:
- w = w / S
- p_tmp, new_weights = self.resampling_wheel(w)
- S = new_weights.sum()
- if S > 0:
- new_weights /= S
- p_with_weights_tmp = np.hstack((p_tmp, new_weights.reshape(-1, 1)))
- self.update_coord(p_with_weights_tmp)
- self.update_consistency(observation)
- generated_particles = self.gen_n_particles_robot(self.n - p_with_weights_tmp.shape[0])
- self.p = np.vstack((p_with_weights_tmp, generated_particles))
- self.count += 1
- self.update_consistency(observation)
-
- def custom_reset(self, x, y, yaw):
- self.myrobot.x = x
- self.myrobot.y = y
- self.myrobot.yaw = yaw
- self.p = self.gen_n_particles_robot(self.n)
-
- def fall_reset(self, observations):
- self.custom_reset(self.myrobot.x + random.gauss(0, self.sense_noise),
- self.myrobot.y + random.gauss(0, self.sense_noise),
- self.myrobot.yaw + random.gauss(0, self.yaw_noise))
- self.resampling(observations)
- self.update_consistency(observations)
-
- def update_coord(self, particles):
- position = (particles[:, :2] * particles[:, 3].reshape(-1, 1)).sum(axis=0)
- self.myrobot.x = position[0]
- self.myrobot.y = position[1]
- sin = (np.sin(particles[:, 2]) * particles[:, 3].reshape(-1, 1)).sum()
- cos = (np.cos(particles[:, 2]) * particles[:, 3].reshape(-1, 1)).sum()
- angle = np.angle(cos + 1j*sin)
- self.myrobot.yaw = angle % (2*np.pi)
-
- def return_coord(self):
- return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
-
-
-def updatePF(pf, measurement):
- k = pf.number_of_res
- if pf.consistency < pf.con_threshold:
- k += 1
- for i in range(2):
- pf.resampling(measurement)
- print('eto coord', pf.return_coord(), pf.myrobot.yaw*180/math.pi)
- return pf.return_coord()
diff --git a/src/localisation/localisation/Seminar-Localisation.ipynb b/src/localisation/localisation/Seminar-Localisation.ipynb
deleted file mode 100644
index 5016d27..0000000
--- a/src/localisation/localisation/Seminar-Localisation.ipynb
+++ /dev/null
@@ -1,128 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Фильтр чаÑтиц"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 1,
- "metadata": {},
- "outputs": [],
- "source": [
- "import sys\n",
- "import json\n",
- "import numpy as np\n",
- "\n",
- "sys.path.append(\"../localization/\")\n",
- "\n",
- "from ParticleFilter import ParticleFilter\n",
- "from robot import Robot\n",
- "from field import Field\n",
- "\n",
- "from pf_visualization import visualization"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 2,
- "metadata": {},
- "outputs": [],
- "source": [
- "pf = ParticleFilter(Robot(0.0, 0.0, 0.0), Field(\"../localization/parfield.json\"), n = 100, apr = True)\n",
- "# pf.uniform_reset()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 4,
- "metadata": {},
- "outputs": [],
- "source": [
- "observation = np.array([-3.0, -1.0, 6.28 - 1.57/2.0])\n",
- "for _ in range(100):\n",
- " pf.resampling(observation)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 8,
- "metadata": {},
- "outputs": [
- {
- "data": {
- "image/png": "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\n",
- "text/plain": [
- "<Figure size 432x648 with 1 Axes>"
- ]
- },
- "metadata": {
- "needs_background": "light"
- },
- "output_type": "display_data"
- }
- ],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7, min_ratio=2)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "pf.uniform_reset()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "visualization(pf.myrobot, pf.p, factor = 7)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": []
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3",
- "language": "python",
- "name": "python3"
- },
- "language_info": {
- "codemirror_mode": {
- "name": "ipython",
- "version": 3
- },
- "file_extension": ".py",
- "mimetype": "text/x-python",
- "name": "python",
- "nbconvert_exporter": "python",
- "pygments_lexer": "ipython3",
- "version": "3.8.10"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}
diff --git a/src/localisation/localisation/Untitled.ipynb b/src/localisation/localisation/Untitled.ipynb
deleted file mode 100644
index 80b5756..0000000
--- a/src/localisation/localisation/Untitled.ipynb
+++ /dev/null
@@ -1,109 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "code",
- "execution_count": 1,
- "metadata": {},
- "outputs": [],
- "source": [
- "import math\n",
- "import numpy as np\n",
- "import matplotlib.pyplot as plt"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 92,
- "metadata": {},
- "outputs": [],
- "source": [
- "angle = np.linspace(0, 4 * np.pi, 100)\n",
- "sin = np.sin(angle)\n",
- "cos = np.cos(angle)\n",
- "# computed_angle = map(math.atan2, sin, cos)\n",
- "computed_angle = []\n",
- "for i in range(angle.size):\n",
- " computed_angle.append(math.atan2(sin[i], cos[i]))"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 89,
- "metadata": {},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "(100,)\n"
- ]
- }
- ],
- "source": [
- "print(angle.shape)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 87,
- "metadata": {},
- "outputs": [],
- "source": [
- "# computed_angle = list(computed_angle)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 93,
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/plain": [
- "[<matplotlib.lines.Line2D at 0x7f86fd9eb730>]"
- ]
- },
- "execution_count": 93,
- "metadata": {},
- "output_type": "execute_result"
- },
- {
- "data": {
- "image/png": "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\n",
- "text/plain": [
- "<Figure size 432x288 with 1 Axes>"
- ]
- },
- "metadata": {
- "needs_background": "light"
- },
- "output_type": "display_data"
- }
- ],
- "source": [
- "plt.plot(computed_angle)"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3",
- "language": "python",
- "name": "python3"
- },
- "language_info": {
- "codemirror_mode": {
- "name": "ipython",
- "version": 3
- },
- "file_extension": ".py",
- "mimetype": "text/x-python",
- "name": "python",
- "nbconvert_exporter": "python",
- "pygments_lexer": "ipython3",
- "version": "3.8.10"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}
diff --git a/src/localisation/localisation/alpha_filter.py b/src/localisation/localisation/alpha_filter.py
deleted file mode 100644
index e69de29..0000000
diff --git a/src/localisation/localisation/field.py b/src/localisation/localisation/field.py
deleted file mode 100644
index b2fa6a7..0000000
--- a/src/localisation/localisation/field.py
+++ /dev/null
@@ -1,8 +0,0 @@
-import json
-
-class Field:
- def __init__(self, path):
- with open(path, "r") as f:
- self.field = json.loads(f.read())
- self.w_width = self.field['main_rectangle'][0][0]
- self.w_length = self.field['main_rectangle'][0][1]
diff --git a/src/localisation/localisation/landmarks.json b/src/localisation/localisation/landmarks.json
deleted file mode 100644
index d06b2f8..0000000
--- a/src/localisation/localisation/landmarks.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
- "blue_posts": [
- [ -4.5, -1.0], [ -4.5, 1.0]
- ],
- "yellow_posts":[
- [ 4.5, -1.0], [ 4.5, 1.0]
- ]
-}
diff --git a/src/localisation/localisation/parfield.json b/src/localisation/localisation/parfield.json
deleted file mode 100644
index e6e8b9f..0000000
--- a/src/localisation/localisation/parfield.json
+++ /dev/null
@@ -1,20 +0,0 @@
-{
- "circles": [
- [0.0, 0.0, 1.0]
- ],
- "lines": [
- [[-3.0, 3.0], [0.0, 0.0]]
- ],
- "points": [
- [0, 0]
- ],
- "main_rectangle": [
- [6.0, 9.0]
- ],
- "red_posts":[
- [-0.75, -1.8], [0.75, -1.8]
- ],
- "rectangles": [
- [[-1, -4.5], 2, 1],[[-1,3.5],2,1]
- ]
-}
diff --git a/src/localisation/localisation/particle.py b/src/localisation/localisation/particle.py
deleted file mode 100644
index a982d4a..0000000
--- a/src/localisation/localisation/particle.py
+++ /dev/null
@@ -1,21 +0,0 @@
-import math
-import sys
-
-import random
-from robot import Robot
-
-
-def gaussian(x, sigma):
- # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
- return math.exp(-(x ** 2) / 2*(sigma ** 2)) / math.sqrt(2.0 * math.pi * (sigma ** 2))
-
-class Particle(Robot):
-
- def observation_score(self, observations, sense_noise):
- # particle weight calculation
- prob = 1.0
- dist = math.sqrt((self.x - observations[0])**2 + \
- (self.y - observations[1])**2 + \
- (self.yaw - observations[2])**2)
- prob *= gaussian(dist, sense_noise)
- return prob
diff --git a/src/localisation/localisation/pf_constants.json b/src/localisation/localisation/pf_constants.json
deleted file mode 100644
index b05bc72..0000000
--- a/src/localisation/localisation/pf_constants.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "noise": {
- "forward_noise": 0.025,
- "turn_noise": 0.1,
- "sense_noise": 0.4,
- "gauss_noise": 0.4,
- "yaw_noise": 0.3
- },
-
- "consistency":{
- "number_of_res":3,
- "consistency":0.0,
- "goodObsGain":0.2,
- "badObsCost":0.1,
- "stepCost":0.1,
- "dist_threshold":0.3,
- "con_threshold": 0.0,
- "spec_threshold":1.5
- },
-
- "weights":{
- "yaw_importance":2
- }
-}
diff --git a/src/localisation/localisation/pf_visualization.py b/src/localisation/localisation/pf_visualization.py
deleted file mode 100644
index 9296d83..0000000
--- a/src/localisation/localisation/pf_visualization.py
+++ /dev/null
@@ -1,91 +0,0 @@
-import matplotlib.pylab as plt
-from field import Field
-import math
-import numpy as np
-field = Field('../localization/parfield.json')
-def visualization(robot, pr, factor = 7 , min_ratio=10, max_vis_size=5):
-
- plt.figure("Robot in the world",figsize=(field.w_width, field.w_length))
- plt.title('Particle filter')
-
- # draw coordinate grid for plotting
- grid = [-field.w_width/2.0, field.w_width/2.0, -field.w_length/2.0, field.w_length/2.0]
- ax = plt.axis(grid)
-
- landmarks = {
- "blue_posts": [
- [ -1.0, -4.5], [ 1.0, -4.5]
- ],
- "yellow_posts":[[ -1.0, 4.5], [1.0, 4.5]]}
-
- for el in field.field:
- if el == 'circles':
- for circle in field.field['circles']:
- plot_circle = plt.Circle((circle[0], circle[1]), circle[2],linewidth=2, fill=False, edgecolor='#330000')
- plt.gca().add_patch(plot_circle)
- if el == 'lines' :
- for line in field.field['lines']:
- plot_line = plt.Line2D(line[0], line[1], linewidth=2, linestyle="-", color='#330000')
- plt.gca().add_line(plot_line)
- if el == 'rectangles' :
- for rectangle in field.field['rectangles']:
- rect = plt.Rectangle(rectangle[0], rectangle[1], rectangle[2], linewidth=2, linestyle="-", fill = False, edgecolor='#330000')
- plt.gca().add_patch(rect)
- '''
- # draw particles
- for ind in range(len(p)):
-
- # particle
- circle = plt.Circle((p[ind][0].x, p[ind][0].y), 1./factor/2, facecolor='#ffb266', edgecolor='#994c00', alpha=0.5)
- plt.gca().add_patch(circle)
-
- # particle's orientation
- arrow = plt.Arrow(p[ind][0].x, p[ind][0].y, 2*math.cos(p[ind][0].yaw)/factor, 2*math.sin(p[ind][0].yaw)/factor, width=1/factor, alpha=1., facecolor='#994c00', edgecolor='#994c00')
- plt.gca().add_patch(arrow)
- '''
- # max_size = np.exp(pr.shape[0] * np.max(pr[:, 3])) - 0.99
- max_size = np.max(pr[:, 3]) + 1e-6
- # draw resampled particles
- for ind in range(pr.shape[0]):
- # size = (np.exp(pr.shape[0] * pr[ind, 3]) - 0.99) / max_size * 5
- size = pr[ind, 3]
- if size >= ((max_size - 1e-6)/ min_ratio):
- size *= max_vis_size / max_size
- # particle
- circle = plt.Circle((pr[ind, 1], pr[ind, 0]),
- 1./factor/2 * np.sqrt(max_vis_size) * np.sqrt(size),
- facecolor='#ffb266', edgecolor='#cc0000', alpha=0.5)
- plt.gca().add_patch(circle)
-
- # particle's orientation
- arrow = plt.Arrow(pr[ind, 1], pr[ind, 0],
- math.sin(pr[ind, 2])/factor * size,
- math.cos(pr[ind, 2])/factor * size,
- width=1/factor, alpha=1., facecolor='#006600', edgecolor='#006600')
- plt.gca().add_patch(arrow)
-
-
- # robot's location
- circle = plt.Circle((robot.y, robot.x), 1./factor, facecolor='#FF66E9', edgecolor='#FF66E9')
- plt.gca().add_patch(circle)
-
- # robot's orientation
- arrow = plt.Arrow(robot.y, robot.x, 3*math.sin(robot.yaw)/factor, 3*math.cos(robot.yaw)/factor, width=1.0/factor, alpha=0.5, facecolor='#000000', edgecolor='#000000')
- plt.gca().add_patch(arrow)
-
-
- #fixed landmarks of known locations2
-
- for lm in landmarks:
- for lms in landmarks[lm]:
- if lm == "yellow_posts":
- circle = plt.Circle(((lms[0], lms[1])),
- 1./factor, facecolor = '#ffff00', edgecolor='#330000')
- plt.gca().add_patch(circle)
- else:
- circle = plt.Circle(((lms[0], lms[1])),
- 1./factor, facecolor = '#060C73', edgecolor='#330000')
- plt.gca().add_patch(circle)
-
-
- #plt.close()
\ No newline at end of file
diff --git a/src/localisation/localisation/robot.py b/src/localisation/localisation/robot.py
deleted file mode 100644
index 798e488..0000000
--- a/src/localisation/localisation/robot.py
+++ /dev/null
@@ -1,47 +0,0 @@
-import math
-
-
-class Robot:
- def __init__(self, x=0, y=0, yaw=0):
- self.x = x # robot's x coordinate
- self.y = y # robot's y coordinate
- self.yaw = yaw # robot's angle
-
- def set_coord(self, new_x, new_y, new_orientation):
- self.x = float(new_x)
- self.y = float(new_y)
- self.yaw = float(new_orientation)
-
- def sense(self, landmarks):
- z = []
- for i in range(len(landmarks)):
- dist = math.sqrt((self.x - landmarks[i][0]) ** 2
- + (self.y - landmarks[i][1]) ** 2)
- z.append(dist)
- return z
-
- def move(self, x, y, yaw):
- # turn, and add randomomness to the turning command
- orientation = self.yaw + float(yaw)
- if orientation < 0:
- orientation += (math.pi*2)
- orientation %= (2 * math.pi)
- self.x += x*math.cos(self.yaw) - y * math.sin(self.yaw)
- self.y += x*math.sin(self.yaw) + y * math.cos(self.yaw)
- self.yaw = orientation
-
- def observation_to_predict(self, observations, landmarks):
- predicts = []
- for color_landmarks in landmarks:
- if (color_landmarks not in landmarks):
- continue
-
- for landmark in landmarks[color_landmarks]:
- x_posts = self.x - \
- observation[0]*math.sin(-self.yaw) + \
- observation[1]*math.cos(-self.yaw)
- y_posts = self.y + \
- observation[0]*math.cos(-self.yaw) - \
- observation[1]*math.sin(-self.yaw)
- predicts.append([x_posts, y_posts])
- return predicts
\ No newline at end of file
--
GitLab
From 4b3a988ce3138c29a499305d89bdae71c9ea89be Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 15:03:04 +0300
Subject: [PATCH 33/64] deleted ipynb checkpoints
---
.../.ipynb_checkpoints/Untitled-checkpoint.ipynb | 6 ------
1 file changed, 6 deletions(-)
delete mode 100644 src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
diff --git a/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb b/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
deleted file mode 100644
index 7fec515..0000000
--- a/src/localisation/localisation/.ipynb_checkpoints/Untitled-checkpoint.ipynb
+++ /dev/null
@@ -1,6 +0,0 @@
-{
- "cells": [],
- "metadata": {},
- "nbformat": 4,
- "nbformat_minor": 4
-}
--
GitLab
From 69de1515fa0c33d0d775de4cb90e90c50df99d74 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 14 Apr 2024 15:22:31 +0300
Subject: [PATCH 34/64] [WIP]
Add config file for ekf filter [todo] try ukf
Add dummy publisher const PoseWithCovarianceStamped for testing ekf (run
via python3 )
Add transform_static_publishers for odom frame and base_ling(all is 0)
Add covariance to measure of aruco position
---
.../config/static_transforms.yaml | 29 ++--
.../config/ekf_config.yaml | 130 ++++++++++++++++++
.../launch/aruco_preprocessor.launch.py | 78 +++++++++++
.../solo_camera_aruco_preprocessor.launch.py | 11 +-
.../launch/static_tf_publisher.launch.py | 49 +++++++
.../aruco_preprocessor_node.py | 81 +++++++----
.../test/dummy_pose_pub.py | 43 ++++++
src/server_main/config/robots_config.json | 2 +-
8 files changed, 387 insertions(+), 36 deletions(-)
create mode 100644 src/localization_preprocessor/config/ekf_config.yaml
create mode 100644 src/localization_preprocessor/launch/aruco_preprocessor.launch.py
create mode 100644 src/localization_preprocessor/launch/static_tf_publisher.launch.py
create mode 100644 src/localization_preprocessor/test/dummy_pose_pub.py
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index b030f53..e552204 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -1,13 +1,26 @@
static_transforms:
- child_frame_id: camera1
- name: base_link_2_radar
+ name: camera1
parent_frame_id: world
rotation:
- - 0.92861703627208
- - 0.013534026051733207
- - -0.004905194687194856
- - -0.3707602583195375
+ - -0.018750493380285973
+ - 0.9298777574050223
+ - 0.3673898379410669
+ - -0.0006944487303789727
translation:
- - -0.08637496015533369
- - -0.7362665187573849
- - 1.337591814410169
+ - -0.4298273037428295
+ - -1.3560956749502036
+ - 1.7450139352322844
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation:
+ - 0.7868225519888545
+ - -0.40404956385232993
+ - 0.2283733065203055
+ - -0.40681673331055473
+ translation:
+ - -1.1922920466395226
+ - -0.7247391349843926
+ - 1.6971053927140525
+ - 1.6971053927140525
diff --git a/src/localization_preprocessor/config/ekf_config.yaml b/src/localization_preprocessor/config/ekf_config.yaml
new file mode 100644
index 0000000..8fc3747
--- /dev/null
+++ b/src/localization_preprocessor/config/ekf_config.yaml
@@ -0,0 +1,130 @@
+### ekf config file ###
+ekf_filter_node:
+ ros__parameters:
+ # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+ # computation until it receives at least one message from one of the inputs. It will then run continuously at the
+ # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+ frequency: 30.0
+
+ # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+ # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+ # filter will generate new output. Defaults to 1 / frequency if not specified.
+ sensor_timeout: 1.0
+
+ # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+ # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+ # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+ # by, for example, an IMU. Defaults to false if unspecified.
+ two_d_mode: false
+
+ # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+ # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+ # unspecified.
+ transform_time_offset: 0.0
+
+ # Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
+ # Defaults to 0.0 if unspecified.
+ transform_timeout: 0.0
+
+ # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+ # unhappy with any settings or data.
+ print_diagnostics: true
+
+ # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+ # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+ # effects on the performance of the node. Defaults to false if unspecified.
+ debug: true
+
+ # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+ debug_out_file: ./file.txt
+
+ # Whether we'll allow old measurements to cause a re-publication of the updated state
+ permit_corrected_publication: false
+
+ # Whether to publish the acceleration state. Defaults to false if unspecified.
+ publish_acceleration: false
+
+ # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+ publish_tf: true
+
+ # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+ # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+ # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+ # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+ # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+ # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+ # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+ # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+ # Here is how to use the following settings:
+ # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+ # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+ # odom_frame.
+ # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+ # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+ # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+ # from landmark observations) then:
+ # 3a. Set your "world_frame" to your map_frame value
+ # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+ # estimation node from robot_localization! However, that instance should *not* fuse the global data.
+ # odom_frame: camera1 # Defaults to "odom" if unspecified
+ base_link_frame: camera1 # Defaults to "base_link" if unspecified
+ world_frame: world # Defaults to the value of odom_frame if unspecified
+
+ pose0: /pose
+ pose0_config: [true, true, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+ pose0_differential: true
+ pose0_relative: false
+ pose0_queue_size: 5
+ pose0_rejection_threshold: 100.0 # Note the difference in parameter name
+
+
+ # Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+ acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+
+ # Acceleration and deceleration limits are not always the same for robots.
+ deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+
+ # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+ # gains
+ acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+
+ # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+ # gains
+ deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+ # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+ # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+ # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+ # However, if users find that a given variable is slow to converge, one approach is to increase the
+ # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+ # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+ # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+ # unspecified.
+ # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support
+ # both full specification or specification of only the diagonal values.
+ process_noise_covariance: [0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.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.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.02, 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.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.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.015]
+
+ # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+ # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+ # question. Users should take care not to use large values for variables that will not be measured directly. The values
+ # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below
+ # if unspecified. In this example, we specify only the diagonal of the matrix.
+ initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]
diff --git a/src/localization_preprocessor/launch/aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/aruco_preprocessor.launch.py
new file mode 100644
index 0000000..16c71e1
--- /dev/null
+++ b/src/localization_preprocessor/launch/aruco_preprocessor.launch.py
@@ -0,0 +1,78 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ camera_config = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params.yaml")
+ camera_config2 = os.path.join(get_package_share_directory('server_main'),
+ 'config',
+ "camera_params2.yaml")
+
+ return LaunchDescription([
+
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config],
+ output='screen',
+ namespace='camera1',
+ emulate_tty=True
+ ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera1'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.170} # 0.07
+ ],
+ remappings=[
+ ('/camera/image_raw', '/camera1/image_raw'),
+ ('/camera/camera_info', '/camera1/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+ Node(
+ package='usb_cam',
+ executable='usb_cam_node_exe',
+ parameters=[camera_config2],
+ output='screen',
+ namespace='camera2',
+ emulate_tty=True
+ ),
+ Node(
+ package='ros2_aruco',
+ executable='aruco_node',
+ parameters=[
+ {'camera_frame': 'camera2'},
+ {'aruco_dictionary_id': 'DICT_4X4_100'},
+ {'marker_size':0.170} # 0.07
+ ],
+ remappings=[
+ ('/camera/image_raw', '/camera2/image_raw'),
+ ('/camera/camera_info', '/camera2/camera_info')
+
+ ],
+ output='screen',
+ emulate_tty=True
+ ),
+
+ # world frame publisher
+
+ Node(
+ package='calibration_node',
+ executable='calibration_node',
+# parameters=[
+# {'robots_params': robot_config},
+# {'camera_frames': ["camera"]}
+# ],
+ output='screen',
+ emulate_tty=True
+ )
+ ])
\ No newline at end of file
diff --git a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
index 3f867dd..046fcc3 100644
--- a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
@@ -3,6 +3,9 @@ from launch_ros.actions import Node
from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
camera_config = os.path.join(get_package_share_directory('server_main'),
@@ -13,7 +16,13 @@ def generate_launch_description():
"robots_config.json")
return LaunchDescription([
-
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([
+ FindPackageShare("calibration_node"),
+ '/launch',
+ '/tf.launch.py'
+ ])
+ ),
Node(
package='usb_cam',
executable='usb_cam_node_exe',
diff --git a/src/localization_preprocessor/launch/static_tf_publisher.launch.py b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
new file mode 100644
index 0000000..4442999
--- /dev/null
+++ b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+import yaml
+
+def load_yaml_file(file_path):
+ with open(file_path, 'r') as file:
+ return yaml.safe_load(file)
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ node_odom = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ name="tf_odom",
+ arguments=[
+ str(0.0),
+ str(0.0),
+ str(0.0),
+ str(0),
+ str(0),
+ str(0),
+ str(1),
+ "world",
+ "odom"
+ ]
+ )
+ ld.add_action(node_odom)
+ node_odom = Node(
+ package='tf2_ros',
+ executable='static_transform_publisher',
+ name="tf_base_link",
+ arguments=[
+ str(0.0),
+ str(0.0),
+ str(0.0),
+ str(0),
+ str(0),
+ str(0),
+ str(1),
+ "odom",
+ "base_link"
+ ]
+ )
+ ld.add_action(node_odom)
+
+ return ld
\ No newline at end of file
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 2e210fc..9ba53a1 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -1,16 +1,19 @@
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
-from geometry_msgs.msg import Pose, PoseStamped, TransformStamped, PointStamped
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, PoseStamped, TransformStamped, PointStamped
from androsot_msgs.msg import FieldPose # Assuming you've defined this
from tf2_ros import TransformException, TransformListener, Buffer
import tf2_ros
from tf2_geometry_msgs import do_transform_point
from ros2_aruco_interfaces.msg import ArucoMarkers
+from tf_transformations import euler_from_quaternion
import tf_transformations as tf_t
import json
+import math
+
class ArucoPreprocessorNode(Node):
def __init__(self):
@@ -28,11 +31,17 @@ class ArucoPreprocessorNode(Node):
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.tf_from_camera_to_world = dict()
+ xyz_cov = 0.03
+ rpy_cov = 0.17
+ self.covariance = [xyz_cov, 0.0, 0.0, 0.0, 0.0, 0.0,# 0 0
+ 0.0,xyz_cov, 0.0, 0.0, 0.0, 0.0,# 7
+ 0.0, 0.0, xyz_cov, 0.0, 0.0, 0.0,# 14
+ 0.0, 0.0, 0.0, rpy_cov, 0.0, 0.0,# 21
+ 0.0, 0.0, 0.0, 0.0, rpy_cov, 0.0,# 28
+ 0.0, 0.0, 0.0, 0.0, 0.0, rpy_cov]# 35
-
-
- self.publisher_ = self.create_publisher(FieldPose, 'aruco_field_pose', 10)
+ self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
def init_robot_params(self):
self.declare_parameter("robots_params", rclpy.Parameter.Type.STRING)
@@ -83,6 +92,48 @@ class ArucoPreprocessorNode(Node):
self.get_logger().info(f"Could not transform world to camera: {ex}")
# self.get_camera_to_world_frame()
+
+
+
+ def aruco_cb(self, msg):
+
+ for id, pose in zip(msg.marker_ids, msg.poses):
+ pose_stamped = PoseStamped()
+ pose_stamped.pose = pose
+ pose_stamped.header = msg.header
+ # self.current_aruco_poses[id] = pose_stamped
+ # Example quaternion (x, y, z, w)
+ r = pose_stamped.pose.orientation
+ quaternion = [r.x, r.y,r.z,r.w]
+ roll, pitch, yaw = euler_from_quaternion(quaternion)
+ rad2deg = lambda x: x / 3.14 * 180
+ # roll is near 180
+ # pitch is near 0
+ # cov is equal for all angles
+ # pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
+ # cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
+ # yaw - pohui
+ cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
+ print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
+ field_pose = self.transform_to_robot_centre(pose_stamped, id)
+ pose_with_cov = PoseWithCovarianceStamped()
+ pose_with_cov.header = field_pose.header
+ pose_with_cov.header.stamp = self.get_clock().now().to_msg()
+ pose_with_cov.pose.pose = field_pose.pose
+ # I think python links fuck me. upd numpy copy sigment fuck me. upd life fuck me
+ print (pose_with_cov)
+ self.covariance[21] = cov
+ self.covariance[28] = cov
+ self.covariance[35] = cov
+ pose_with_cov.pose.covariance = self.covariance
+ # pose_with_cov.header.frame =
+ self.publisher_.publish(pose_with_cov)
+
+ def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
+ shifted_pose = self.shift_pose_to_robot_centre(pose, id)
+ rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
+ return rotated_pose
+
def shift_pose_to_robot_centre(
self, pose, id
): # Now all arruco has same offset of centre
@@ -143,28 +194,6 @@ class ArucoPreprocessorNode(Node):
) # "camera" ########## Choose frame id
return rotated_pose
- def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
- shifted_pose = self.shift_pose_to_robot_centre(pose, id)
- rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
- return rotated_pose
-
-
- def aruco_cb(self, msg):
-
- for id, pose in zip(msg.marker_ids, msg.poses):
- pose_stamped = PoseStamped()
- pose_stamped.pose = pose
- pose_stamped.header = msg.header
- self.current_aruco_poses[id] = pose_stamped
-
-
- field_pose = FieldPose()
- field_pose.header = marker.header
- field_pose.x = x
- field_pose.y = y
- field_pose.yaw = yaw
- self.publisher_.publish(field_pose)
-
def send_coord_to_robot(self, id):
if id in self.world_robots_poses:
msg = PoseStamped()
diff --git a/src/localization_preprocessor/test/dummy_pose_pub.py b/src/localization_preprocessor/test/dummy_pose_pub.py
new file mode 100644
index 0000000..cc41b5c
--- /dev/null
+++ b/src/localization_preprocessor/test/dummy_pose_pub.py
@@ -0,0 +1,43 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from std_msgs.msg import Header
+
+class PoseWithCovarianceStampedPublisher(Node):
+ def __init__(self):
+ super().__init__('pose_with_covariance_stamped_publisher')
+ self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
+ timer_period = 0.02 # seconds
+ self.timer_ = self.create_timer(timer_period, self.timer_callback)
+
+ def timer_callback(self):
+ msg = PoseWithCovarianceStamped()
+ msg.header = Header()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = 'camera1'
+ # Example: Set the pose and covariance
+ msg.pose.pose.position.x = 1.0
+ msg.pose.pose.position.y = 2.0
+ msg.pose.pose.position.z = 0.0
+ msg.pose.pose.orientation.x = 0.0
+ msg.pose.pose.orientation.y = 0.0
+ msg.pose.pose.orientation.z = 0.7071067811865475
+ msg.pose.pose.orientation.w = 0.7071067811865476
+ msg.pose.covariance = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+ self.publisher_.publish(msg)
+ self.get_logger().info('Publishing: "%s"' % msg)
+
+def main(args=None):
+ rclpy.init(args=args)
+ pose_with_covariance_stamped_publisher = PoseWithCovarianceStampedPublisher()
+ rclpy.spin(pose_with_covariance_stamped_publisher)
+ pose_with_covariance_stamped_publisher.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/server_main/config/robots_config.json b/src/server_main/config/robots_config.json
index 0c6a8b4..f3dcf5f 100644
--- a/src/server_main/config/robots_config.json
+++ b/src/server_main/config/robots_config.json
@@ -7,7 +7,7 @@
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [0, 1.57, 1.57]
+ [-1.57, 0, -1.57]
]
},
--
GitLab
From 33a4b922cf9f5b667176ac185b9699e09f1053a1 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 14 Apr 2024 21:21:44 +0300
Subject: [PATCH 35/64] IDK what i pushed + config for static transform f0r
14.04.2022
---
.../config/static_transforms.yaml | 29 +++++++++----------
.../aruco_preprocessor_node.py | 22 +++++++++++---
2 files changed, 32 insertions(+), 19 deletions(-)
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index e552204..5a976da 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -3,24 +3,23 @@ static_transforms:
name: camera1
parent_frame_id: world
rotation:
- - -0.018750493380285973
- - 0.9298777574050223
- - 0.3673898379410669
- - -0.0006944487303789727
+ - 0.6285916431197951
+ - 0.7009228787488695
+ - -0.23376289532559236
+ - -0.24276443935500497
translation:
- - -0.4298273037428295
- - -1.3560956749502036
- - 1.7450139352322844
+ - 0.896828403155739
+ - 0.29078709084040627
+ - 1.6071356253071611
- child_frame_id: camera2
name: camera2
parent_frame_id: world
rotation:
- - 0.7868225519888545
- - -0.40404956385232993
- - 0.2283733065203055
- - -0.40681673331055473
+ - 0.7003310187541241
+ - -0.6274366713598328
+ - 0.20942611904603334
+ - -0.26832888078814915
translation:
- - -1.1922920466395226
- - -0.7247391349843926
- - 1.6971053927140525
- - 1.6971053927140525
+ - -1.1996148871091314
+ - 0.28907139176512053
+ - 1.7855842385290335
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 9ba53a1..5c6d8b6 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -9,7 +9,7 @@ from tf2_geometry_msgs import do_transform_point
from ros2_aruco_interfaces.msg import ArucoMarkers
from tf_transformations import euler_from_quaternion
-
+import tf2_geometry_msgs
import tf_transformations as tf_t
import json
import math
@@ -31,6 +31,7 @@ class ArucoPreprocessorNode(Node):
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.tf_from_camera_to_world = dict()
+ self.camera_frames = ["camera1", "camera2"]
xyz_cov = 0.03
rpy_cov = 0.17
self.covariance = [xyz_cov, 0.0, 0.0, 0.0, 0.0, 0.0,# 0 0
@@ -96,7 +97,7 @@ class ArucoPreprocessorNode(Node):
def aruco_cb(self, msg):
-
+ self.get_camera_to_world_frame()
for id, pose in zip(msg.marker_ids, msg.poses):
pose_stamped = PoseStamped()
pose_stamped.pose = pose
@@ -118,6 +119,7 @@ class ArucoPreprocessorNode(Node):
field_pose = self.transform_to_robot_centre(pose_stamped, id)
pose_with_cov = PoseWithCovarianceStamped()
pose_with_cov.header = field_pose.header
+ # pose_with_cov.header.frame_id = "odom"
pose_with_cov.header.stamp = self.get_clock().now().to_msg()
pose_with_cov.pose.pose = field_pose.pose
# I think python links fuck me. upd numpy copy sigment fuck me. upd life fuck me
@@ -125,17 +127,29 @@ class ArucoPreprocessorNode(Node):
self.covariance[21] = cov
self.covariance[28] = cov
self.covariance[35] = cov
+ tf = self.tf_buffer.lookup_transform_core("world", field_pose.header.frame_id, rclpy.time.Time())
+ world_pose = tf2_geometry_msgs.do_transform_pose(field_pose.pose, tf)
+ pose_with_cov.pose.pose = world_pose
+ pose_with_cov.header.frame_id = "world"
pose_with_cov.pose.covariance = self.covariance
# pose_with_cov.header.frame =
+
self.publisher_.publish(pose_with_cov)
def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
shifted_pose = self.shift_pose_to_robot_centre(pose, id)
rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
return rotated_pose
-
+ def tf_camera2world(self, pose):
+
+ buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_core.set_transform(transform, "default_authority")
+ inverse_transform = buffer_core.lookup_transform_core(
+ transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
+ )
+
def shift_pose_to_robot_centre(
- self, pose, id
+ self, pose:PoseStamped, id
): # Now all arruco has same offset of centre
# https://answers.ros.org/question/289323/simple-tf-transform-without-broadcasterlistener/
shifted_pose = PoseStamped()
--
GitLab
From 79de610b95a8f4a9a8db8ced20546a85048a8daa Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Sun, 14 Apr 2024 21:22:32 +0300
Subject: [PATCH 36/64] maded visualisation for debug
---
.../localisation/localisation_node.py | 81 ++++++++++++++++++-
src/localisation/package.xml | 1 +
src/localisation/resource/localisation | 0
src/rpy_msg/CMakeLists.txt | 33 ++++++++
src/rpy_msg/msg/RPY.msg | 3 +
src/rpy_msg/package.xml | 23 ++++++
src/rpy_msg/resource/rpy_msg | 0
src/rpy_msg/rpy_msg/__init__.py | 0
src/rpy_msg/setup.cfg | 4 +
src/rpy_msg/setup.py | 25 ++++++
src/rpy_msg/test/test_copyright.py | 25 ++++++
src/rpy_msg/test/test_flake8.py | 25 ++++++
src/rpy_msg/test/test_pep257.py | 23 ++++++
13 files changed, 241 insertions(+), 2 deletions(-)
create mode 100644 src/localisation/resource/localisation
create mode 100644 src/rpy_msg/CMakeLists.txt
create mode 100644 src/rpy_msg/msg/RPY.msg
create mode 100644 src/rpy_msg/package.xml
create mode 100644 src/rpy_msg/resource/rpy_msg
create mode 100644 src/rpy_msg/rpy_msg/__init__.py
create mode 100644 src/rpy_msg/setup.cfg
create mode 100644 src/rpy_msg/setup.py
create mode 100644 src/rpy_msg/test/test_copyright.py
create mode 100644 src/rpy_msg/test/test_flake8.py
create mode 100644 src/rpy_msg/test/test_pep257.py
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 239bdb9..e68e647 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,6 +1,8 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped
+from rpy_msg.msg import RPY
+from geometry_msgs.msg import Vector3
import tf2_ros
import rclpy
@@ -13,6 +15,7 @@ from tf2_geometry_msgs import do_transform_point
import tf_transformations as tf_t
from pyquaternion import Quaternion
from tf2_ros import TransformBroadcaster
+import tf_transformations
import numpy as np
import math
@@ -22,6 +25,27 @@ import math
from collections import deque
+# class Pose2DToPoseConverter():
+# def __init__(self):
+# pass
+
+# def convert(self, pose2d):
+# # Create a Pose message
+# pose = PoseStamped()
+# # Set position from Pose2D
+# pose.header.stamp = self.get_clock().now().to_msg()
+# pose.header.frame_id = "world"
+# pose.position.x = pose2d.x
+# pose.position.y = pose2d.y
+# pose.position.z = 0.0
+# # Convert theta to quaternion for orientation
+# quaternion = tf_transformations.quaternion_from_euler(0, 0, pose2d.theta)
+# pose.orientation.x = quaternion[0]
+# pose.orientation.y = quaternion[1]
+# pose.orientation.z = quaternion[2]
+# pose.orientation.w = quaternion[3]
+# return pose
+
class PoseSinCos():
def __init__(self, x, y, sin, cos):
self.x = x
@@ -66,16 +90,19 @@ class LocalisationNode(Node):
def __init__(self):
super().__init__("LocalisationNode")
self.robot_id = 0
- self.create_subscription(PoseWithCovarianceStamped , 'aruco_pose_robot_' + str(self.robot_id), self.aruco_observation_callback, 10)
+ self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose_robot_" + str(self.robot_id), 10)
self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose_robot_" + str(self.robot_id), 10)
+ self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
+ self.rpy_vector3_pub = self.create_publisher(Vector3, 'vector3_rpy' + str(self.robot_id), 10)
+ self.filtered_pose_3d_pub= self.create_publisher(PoseStamped, "/filtered_pose_3d_robot_" + str(self.robot_id), 10)
self.loged_data = []
self.current_aruco_poses = {}
self.current_robot_poses = {}
self.aruco_observed_poses = {}
self.robot_pose = AlphaFilter()
self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.0, 0.0)
- timer_period = 1.0 # seconds
+ timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def aruco_observation_callback(self, msg):
@@ -87,6 +114,21 @@ class LocalisationNode(Node):
self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, sin, cos)
self.robot_pose.update(pose_sin_cos)
+
+ # Extract quaternion from PoseStamped message
+ quaternion = msg.pose.pose.orientation
+ # Convert quaternion to RPY
+ rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
+ roll, pitch, yaw = rpy
+
+ rpy_msg = RPY()
+ rpy_msg.roll, rpy_msg.pitch, rpy_msg.yaw = roll, pitch, yaw
+ self.rpy_pub.publish(rpy_msg)
+
+ rpy_vector3_msg = Vector3()
+ rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
+ self.rpy_vector3_pub.publish(rpy_vector3_msg)
+
def get_x_y_yaw_pose_filtered(self):
pose_sin_cos = self.robot_pose.get()
@@ -109,6 +151,27 @@ class LocalisationNode(Node):
self.unfiltered_pose_pub.publish(unfilt_msg)
filt_msg = self.get_x_y_yaw_pose_filtered()
self.filtered_pose_pub.publish(filt_msg)
+
+ msg_3d = self.convert(filt_msg)
+ self.filtered_pose_3d_pub.publish(msg_3d)
+
+ def convert_to_3d(self, pose2d):
+ # Create a Pose message
+ pose = PoseStamped()
+ # Set position from Pose2D
+ pose.header.stamp = self.get_clock().now().to_msg()
+ pose.header.frame_id = "world"
+ pose.pose.position.x = pose2d.x
+ pose.pose.position.y = pose2d.y
+ pose.pose.position.z = 0.0
+ # Convert theta to quaternion for orientation
+ quaternion = tf_transformations.quaternion_from_euler(0, 0, pose2d.theta)
+ pose.pose.orientation.x = quaternion[0]
+ pose.pose.orientation.y = quaternion[1]
+ pose.pose.orientation.z = quaternion[2]
+ pose.pose.orientation.w = quaternion[3]
+ return pose
+
def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
@@ -124,6 +187,20 @@ class LocalisationNode(Node):
# print(rotated_x)
# TODO check normalisation
return rotated_x[1], rotated_x[0]
+
+ def get_rpy_from_quaternion(self, pose: Pose):
+ x_ax = [1.0, 0.0, 0.0]
+ quat_arr = [
+ pose.pose.pose.orientation.w,
+ pose.pose.pose.orientation.x,
+ pose.pose.pose.orientation.y,
+ pose.pose.pose.orientation.z
+ ]
+
+ rotated_x = Quaternion(quat_arr).rotate(x_ax)
+ # print(rotated_x)
+ # TODO check normalisation
+ return rotated_x[1], rotated_x[0]
def main(args=None):
rclpy.init(args=args)
diff --git a/src/localisation/package.xml b/src/localisation/package.xml
index 011e107..836c3e9 100644
--- a/src/localisation/package.xml
+++ b/src/localisation/package.xml
@@ -8,6 +8,7 @@
<license>TODO: License declaration</license>
<depend>ros2_aruco_interfaces</depend>
+ <depend>rpy_msg</depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>tf2_geometry_msgs</exec_depend>
diff --git a/src/localisation/resource/localisation b/src/localisation/resource/localisation
new file mode 100644
index 0000000..e69de29
diff --git a/src/rpy_msg/CMakeLists.txt b/src/rpy_msg/CMakeLists.txt
new file mode 100644
index 0000000..0151f85
--- /dev/null
+++ b/src/rpy_msg/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(rpy_msg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/RPY.msg"
+)
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
+
diff --git a/src/rpy_msg/msg/RPY.msg b/src/rpy_msg/msg/RPY.msg
new file mode 100644
index 0000000..c963632
--- /dev/null
+++ b/src/rpy_msg/msg/RPY.msg
@@ -0,0 +1,3 @@
+float64 roll
+float64 pitch
+float64 yaw
diff --git a/src/rpy_msg/package.xml b/src/rpy_msg/package.xml
new file mode 100644
index 0000000..b726a2b
--- /dev/null
+++ b/src/rpy_msg/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>rpy_msg</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="xatc120@gmail.com">nakuznetsov</maintainer>
+ <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+<build_depend>rosidl_default_generators</build_depend>
+
+<exec_depend>rosidl_default_runtime</exec_depend>
+
+<member_of_group>rosidl_interface_packages</member_of_group>
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
+
diff --git a/src/rpy_msg/resource/rpy_msg b/src/rpy_msg/resource/rpy_msg
new file mode 100644
index 0000000..e69de29
diff --git a/src/rpy_msg/rpy_msg/__init__.py b/src/rpy_msg/rpy_msg/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/rpy_msg/setup.cfg b/src/rpy_msg/setup.cfg
new file mode 100644
index 0000000..26a3c36
--- /dev/null
+++ b/src/rpy_msg/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rpy_msg
+[install]
+install_scripts=$base/lib/rpy_msg
diff --git a/src/rpy_msg/setup.py b/src/rpy_msg/setup.py
new file mode 100644
index 0000000..d19dbcb
--- /dev/null
+++ b/src/rpy_msg/setup.py
@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'rpy_msg'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='root',
+ maintainer_email='root@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ ],
+ },
+)
diff --git a/src/rpy_msg/test/test_copyright.py b/src/rpy_msg/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/rpy_msg/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/rpy_msg/test/test_flake8.py b/src/rpy_msg/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/rpy_msg/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/rpy_msg/test/test_pep257.py b/src/rpy_msg/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/rpy_msg/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From 247695bf7529f0582b31cb1c789ae9a39f933af5 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Mon, 15 Apr 2024 21:19:17 +0300
Subject: [PATCH 37/64] proxy connected with teb local planner
---
src/androsot_approach_teb/CMakeLists.txt | 46 +
.../cmake_modules/FindG2O.cmake | 97 ++
.../cmake_modules/FindSUITESPARSE.cmake | 133 ++
src/androsot_approach_teb/package.xml | 21 +
.../src/androsot_approach_teb.cpp | 398 +++++
src/command/CMakeLists.txt | 2 +
src/command/msg/Command.msg | 7 +-
src/command/package.xml | 3 +-
src/proxy/proxy/CommandSender.py | 15 +-
src/proxy/proxy/Proxy.py | 114 +-
src/teb_local_planner/CHANGELOG.rst | 407 +++++
src/teb_local_planner/CMakeLists.txt | 162 ++
src/teb_local_planner/LICENSE | 28 +
.../cfg/TebLocalPlannerReconfigure.cfg | 420 ++++++
.../cfg/rviz_test_optim.rviz | 183 +++
.../cmake_modules/FindG2O.cmake | 97 ++
.../cmake_modules/FindSUITESPARSE.cmake | 133 ++
.../teb_local_planner/distance_calculations.h | 464 ++++++
.../teb_local_planner/equivalence_relations.h | 103 ++
.../g2o_types/base_teb_edges.h | 278 ++++
.../g2o_types/edge_acceleration.h | 732 +++++++++
.../g2o_types/edge_dynamic_obstacle.h | 154 ++
.../g2o_types/edge_kinematics.h | 231 +++
.../g2o_types/edge_obstacle.h | 295 ++++
.../g2o_types/edge_prefer_rotdir.h | 115 ++
.../g2o_types/edge_shortest_path.h | 88 ++
.../g2o_types/edge_time_optimal.h | 117 ++
.../g2o_types/edge_velocity.h | 275 ++++
.../g2o_types/edge_velocity_obstacle_ratio.h | 127 ++
.../g2o_types/edge_via_point.h | 121 ++
.../teb_local_planner/g2o_types/penalties.h | 193 +++
.../teb_local_planner/g2o_types/vertex_pose.h | 229 +++
.../g2o_types/vertex_timediff.h | 145 ++
.../include/teb_local_planner/graph_search.h | 215 +++
.../include/teb_local_planner/h_signature.h | 432 ++++++
.../homotopy_class_planner.h | 591 ++++++++
.../homotopy_class_planner.hpp | 95 ++
.../include/teb_local_planner/misc.h | 210 +++
.../include/teb_local_planner/obstacles.h | 1112 ++++++++++++++
.../teb_local_planner/optimal_planner.h | 736 +++++++++
.../teb_local_planner/planner_interface.h | 218 +++
.../include/teb_local_planner/pose_se2.h | 433 ++++++
.../teb_local_planner/recovery_behaviors.h | 134 ++
.../teb_local_planner/robot_footprint_model.h | 684 +++++++++
.../include/teb_local_planner/teb_config.h | 432 ++++++
.../teb_local_planner/teb_local_planner_ros.h | 426 ++++++
.../teb_local_planner/timed_elastic_band.h | 636 ++++++++
.../teb_local_planner/timed_elastic_band.hpp | 191 +++
.../include/teb_local_planner/visualization.h | 272 ++++
.../teb_local_planner/visualization.hpp | 227 +++
src/teb_local_planner/package.xml | 53 +
src/teb_local_planner/params/teb_params.yaml | 109 ++
.../scripts/cmd_vel_to_ackermann_drive.py | 60 +
.../scripts/export_to_mat.py | 112 ++
.../scripts/export_to_svg.py | 244 +++
.../scripts/publish_dynamic_obstacle.py | 67 +
.../scripts/publish_test_obstacles.py | 76 +
.../scripts/publish_viapoints.py | 46 +
.../scripts/visualize_velocity_profile.py | 76 +
src/teb_local_planner/src/graph_search.cpp | 346 +++++
.../src/homotopy_class_planner.cpp | 854 +++++++++++
src/teb_local_planner/src/obstacles.cpp | 216 +++
src/teb_local_planner/src/optimal_planner.cpp | 1326 +++++++++++++++++
.../src/recovery_behaviors.cpp | 118 ++
src/teb_local_planner/src/teb_config.cpp | 886 +++++++++++
.../src/teb_local_planner_ros.cpp | 1122 ++++++++++++++
src/teb_local_planner/src/test_optim_node.cpp | 322 ++++
.../src/timed_elastic_band.cpp | 630 ++++++++
src/teb_local_planner/src/visualization.cpp | 563 +++++++
.../teb_local_planner_plugin.xml | 9 +
.../test/homotopy_class_planner_test.cpp | 82 +
src/teb_local_planner/test/teb_basics.cpp | 73 +
src/teb_msgs/CMakeLists.txt | 28 +
src/teb_msgs/msg/FeedbackMsg.msg | 15 +
src/teb_msgs/msg/TrajectoryMsg.msg | 6 +
src/teb_msgs/msg/TrajectoryPointMsg.msg | 21 +
src/teb_msgs/package.xml | 32 +
77 files changed, 20412 insertions(+), 57 deletions(-)
create mode 100644 src/androsot_approach_teb/CMakeLists.txt
create mode 100644 src/androsot_approach_teb/cmake_modules/FindG2O.cmake
create mode 100644 src/androsot_approach_teb/cmake_modules/FindSUITESPARSE.cmake
create mode 100644 src/androsot_approach_teb/package.xml
create mode 100644 src/androsot_approach_teb/src/androsot_approach_teb.cpp
create mode 100644 src/teb_local_planner/CHANGELOG.rst
create mode 100644 src/teb_local_planner/CMakeLists.txt
create mode 100644 src/teb_local_planner/LICENSE
create mode 100755 src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
create mode 100644 src/teb_local_planner/cfg/rviz_test_optim.rviz
create mode 100644 src/teb_local_planner/cmake_modules/FindG2O.cmake
create mode 100644 src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
create mode 100644 src/teb_local_planner/include/teb_local_planner/distance_calculations.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/graph_search.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/h_signature.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
create mode 100644 src/teb_local_planner/include/teb_local_planner/misc.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/obstacles.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/optimal_planner.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/planner_interface.h
create mode 100755 src/teb_local_planner/include/teb_local_planner/pose_se2.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/teb_config.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
create mode 100644 src/teb_local_planner/include/teb_local_planner/visualization.h
create mode 100644 src/teb_local_planner/include/teb_local_planner/visualization.hpp
create mode 100644 src/teb_local_planner/package.xml
create mode 100644 src/teb_local_planner/params/teb_params.yaml
create mode 100755 src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
create mode 100755 src/teb_local_planner/scripts/export_to_mat.py
create mode 100755 src/teb_local_planner/scripts/export_to_svg.py
create mode 100755 src/teb_local_planner/scripts/publish_dynamic_obstacle.py
create mode 100755 src/teb_local_planner/scripts/publish_test_obstacles.py
create mode 100755 src/teb_local_planner/scripts/publish_viapoints.py
create mode 100755 src/teb_local_planner/scripts/visualize_velocity_profile.py
create mode 100644 src/teb_local_planner/src/graph_search.cpp
create mode 100644 src/teb_local_planner/src/homotopy_class_planner.cpp
create mode 100644 src/teb_local_planner/src/obstacles.cpp
create mode 100644 src/teb_local_planner/src/optimal_planner.cpp
create mode 100644 src/teb_local_planner/src/recovery_behaviors.cpp
create mode 100644 src/teb_local_planner/src/teb_config.cpp
create mode 100644 src/teb_local_planner/src/teb_local_planner_ros.cpp
create mode 100644 src/teb_local_planner/src/test_optim_node.cpp
create mode 100644 src/teb_local_planner/src/timed_elastic_band.cpp
create mode 100644 src/teb_local_planner/src/visualization.cpp
create mode 100644 src/teb_local_planner/teb_local_planner_plugin.xml
create mode 100644 src/teb_local_planner/test/homotopy_class_planner_test.cpp
create mode 100644 src/teb_local_planner/test/teb_basics.cpp
create mode 100644 src/teb_msgs/CMakeLists.txt
create mode 100644 src/teb_msgs/msg/FeedbackMsg.msg
create mode 100644 src/teb_msgs/msg/TrajectoryMsg.msg
create mode 100644 src/teb_msgs/msg/TrajectoryPointMsg.msg
create mode 100644 src/teb_msgs/package.xml
diff --git a/src/androsot_approach_teb/CMakeLists.txt b/src/androsot_approach_teb/CMakeLists.txt
new file mode 100644
index 0000000..01d06ea
--- /dev/null
+++ b/src/androsot_approach_teb/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.8)
+project(androsot_approach_teb)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(teb_local_planner REQUIRED)
+find_package(command REQUIRED)
+
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
+find_package(SUITESPARSE REQUIRED)
+
+set(EXTERNAL_INCLUDE_DIRS ${SUITESPARSE_INCLUDE_DIRS} )
+
+include_directories(
+ include
+# ${Boost_INCLUDE_DIRS}
+ ${EXTERNAL_INCLUDE_DIRS}
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+add_executable(androsot_approach_teb src/androsot_approach_teb.cpp)
+ament_target_dependencies(androsot_approach_teb rclcpp teb_local_planner command)
+
+install(TARGETS
+androsot_approach_teb
+ DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
+
+
diff --git a/src/androsot_approach_teb/cmake_modules/FindG2O.cmake b/src/androsot_approach_teb/cmake_modules/FindG2O.cmake
new file mode 100644
index 0000000..b2670d3
--- /dev/null
+++ b/src/androsot_approach_teb/cmake_modules/FindG2O.cmake
@@ -0,0 +1,97 @@
+# Locate the g2o libraries
+# A general framework for graph optimization.
+#
+# This module defines
+# G2O_FOUND, if false, do not try to link against g2o
+# G2O_LIBRARIES, path to the libg2o
+# G2O_INCLUDE_DIR, where to find the g2o header files
+#
+# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
+# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
+
+IF(UNIX)
+
+ #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+ # in cache already
+ # SET(G2O_FIND_QUIETLY TRUE)
+ #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+
+ MESSAGE(STATUS "Searching for g2o ...")
+ FIND_PATH(G2O_INCLUDE_DIR
+ NAMES core math_groups types
+ PATHS /usr/local /usr
+ PATH_SUFFIXES include/g2o include)
+
+ IF (G2O_INCLUDE_DIR)
+ MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
+ ENDIF (G2O_INCLUDE_DIR)
+
+ FIND_LIBRARY(G2O_CORE_LIB
+ NAMES g2o_core g2o_core_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_STUFF_LIB
+ NAMES g2o_stuff g2o_stuff_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB
+ NAMES g2o_types_slam2d g2o_types_slam2d_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB
+ NAMES g2o_types_slam3d g2o_types_slam3d_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB
+ NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_PCG_LIB
+ NAMES g2o_solver_pcg g2o_solver_pcg_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB
+ NAMES g2o_solver_csparse g2o_solver_csparse_rd
+ PATHS /usr/local /usr
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_INCREMENTAL_LIB
+ NAMES g2o_incremental g2o_incremental_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
+ NAMES g2o_csparse_extension g2o_csparse_extension_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+
+ SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
+ ${G2O_CORE_LIB}
+ ${G2O_STUFF_LIB}
+ ${G2O_TYPES_SLAM2D_LIB}
+ ${G2O_TYPES_SLAM3D_LIB}
+ ${G2O_SOLVER_CHOLMOD_LIB}
+ ${G2O_SOLVER_PCG_LIB}
+ ${G2O_SOLVER_CSPARSE_LIB}
+ ${G2O_INCREMENTAL_LIB}
+ )
+
+ IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+ SET(G2O_FOUND "YES")
+ IF(NOT G2O_FIND_QUIETLY)
+ MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
+ ENDIF(NOT G2O_FIND_QUIETLY)
+ ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+ IF(NOT G2O_LIBRARIES)
+ IF(G2O_FIND_REQUIRED)
+ message(FATAL_ERROR "Could not find libg2o!")
+ ENDIF(G2O_FIND_REQUIRED)
+ ENDIF(NOT G2O_LIBRARIES)
+
+ IF(NOT G2O_INCLUDE_DIR)
+ IF(G2O_FIND_REQUIRED)
+ message(FATAL_ERROR "Could not find g2o include directory!")
+ ENDIF(G2O_FIND_REQUIRED)
+ ENDIF(NOT G2O_INCLUDE_DIR)
+ ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+
+ENDIF(UNIX)
+
diff --git a/src/androsot_approach_teb/cmake_modules/FindSUITESPARSE.cmake b/src/androsot_approach_teb/cmake_modules/FindSUITESPARSE.cmake
new file mode 100644
index 0000000..101b79b
--- /dev/null
+++ b/src/androsot_approach_teb/cmake_modules/FindSUITESPARSE.cmake
@@ -0,0 +1,133 @@
+# - Try to find SUITESPARSE
+# Once done this will define
+#
+# SUITESPARSE_FOUND - system has SUITESPARSE
+# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory
+# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE
+# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package)
+# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
+# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs
+# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs
+# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly
+
+IF (SUITESPARSE_INCLUDE_DIRS)
+ # Already in cache, be silent
+ SET(SUITESPARSE_FIND_QUIETLY TRUE)
+ENDIF (SUITESPARSE_INCLUDE_DIRS)
+
+if( WIN32 )
+ # Find cholmod part of the suitesparse library collection
+
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS "C:\\libs\\win32\\SuiteSparse\\Include" )
+
+ # Add cholmod include directory to collection include directories
+ IF ( CHOLMOD_INCLUDE_DIR )
+ list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+ ENDIF( CHOLMOD_INCLUDE_DIR )
+
+
+ # find path suitesparse library
+ FIND_PATH( SUITESPARSE_LIBRARY_DIRS
+ amd.lib
+ PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
+
+ # if we found the library, add it to the defined libraries
+ IF ( SUITESPARSE_LIBRARY_DIRS )
+ list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd )
+ ENDIF( SUITESPARSE_LIBRARY_DIRS )
+
+else( WIN32 )
+ IF(APPLE)
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS /opt/local/include/ufsparse
+ /usr/local/include )
+
+ FIND_PATH( SUITESPARSE_LIBRARY_DIR
+ NAMES libcholmod.a
+ PATHS /opt/local/lib
+ /usr/local/lib )
+ ELSE(APPLE)
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS /usr/local/include
+ /usr/include
+ /usr/include/suitesparse/
+ ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
+ PATH_SUFFIXES cholmod/ CHOLMOD/ )
+
+ FIND_PATH( SUITESPARSE_LIBRARY_DIR
+ NAMES libcholmod.so libcholmod.a
+ PATHS /usr/lib
+ /usr/lib64
+ /usr/lib/x86_64-linux-gnu
+ /usr/lib/i386-linux-gnu
+ /usr/local/lib
+ /usr/lib/arm-linux-gnueabihf/
+ /usr/lib/aarch64-linux-gnu/
+ /usr/lib/arm-linux-gnueabi/
+ /usr/lib/arm-linux-gnu)
+ ENDIF(APPLE)
+
+ # Add cholmod include directory to collection include directories
+ IF ( CHOLMOD_INCLUDE_DIR )
+ list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+ ENDIF( CHOLMOD_INCLUDE_DIR )
+
+ # if we found the library, add it to the defined libraries
+ IF ( SUITESPARSE_LIBRARY_DIR )
+
+ list ( APPEND SUITESPARSE_LIBRARIES amd)
+ list ( APPEND SUITESPARSE_LIBRARIES btf)
+ list ( APPEND SUITESPARSE_LIBRARIES camd)
+ list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
+ list ( APPEND SUITESPARSE_LIBRARIES cholmod)
+ list ( APPEND SUITESPARSE_LIBRARIES colamd)
+ # list ( APPEND SUITESPARSE_LIBRARIES csparse)
+ list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
+ list ( APPEND SUITESPARSE_LIBRARIES klu)
+ # list ( APPEND SUITESPARSE_LIBRARIES spqr)
+ list ( APPEND SUITESPARSE_LIBRARIES umfpack)
+
+ IF (APPLE)
+ list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
+ ENDIF (APPLE)
+
+ # Metis and spqr are optional
+ FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
+ NAMES metis
+ PATHS ${SUITESPARSE_LIBRARY_DIR} )
+ IF (SUITESPARSE_METIS_LIBRARY)
+ list ( APPEND SUITESPARSE_LIBRARIES metis)
+ ENDIF(SUITESPARSE_METIS_LIBRARY)
+
+ if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
+ SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
+ else()
+ SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
+ endif()
+
+ if(SUITESPARSE_SPQR_VALID)
+ FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
+ NAMES spqr
+ PATHS ${SUITESPARSE_LIBRARY_DIR} )
+ IF (SUITESPARSE_SPQR_LIBRARY)
+ list ( APPEND SUITESPARSE_LIBRARIES spqr)
+ ENDIF (SUITESPARSE_SPQR_LIBRARY)
+ endif()
+
+ ENDIF( SUITESPARSE_LIBRARY_DIR )
+
+endif( WIN32 )
+
+
+IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+ IF(WIN32)
+ list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
+ ENDIF(WIN32)
+ SET(SUITESPARSE_FOUND TRUE)
+ MESSAGE(STATUS "Found SuiteSparse")
+ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+ SET( SUITESPARSE_FOUND FALSE )
+ MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
+ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+
diff --git a/src/androsot_approach_teb/package.xml b/src/androsot_approach_teb/package.xml
new file mode 100644
index 0000000..6e47983
--- /dev/null
+++ b/src/androsot_approach_teb/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>androsot_approach_teb</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="egordv@gmail.com">egor</maintainer>
+ <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <depend>rclcpp</depend>
+
+ <build_depend>teb_local_planner</build_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/src/androsot_approach_teb/src/androsot_approach_teb.cpp b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
new file mode 100644
index 0000000..9c857d5
--- /dev/null
+++ b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
@@ -0,0 +1,398 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include <geometry_msgs/msg/pose2_d.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <teb_local_planner/homotopy_class_planner.h>
+#include <teb_local_planner/teb_local_planner_ros.h>
+
+#include "command/msg/command.hpp"
+
+using std::placeholders::_1;
+
+// Global vars
+// teb_local_planner::TebConfig config;
+double teb_robot_radius = 0.15;
+// teb_local_planner::TebVisualizationPtr visual;
+// teb_local_planner::RobotFootprintModelPtr robot_model;
+// teb_local_planner::ViaPointContainer via_points;
+// teb_local_planner::PlannerInterfacePtr planner;
+// std::vector<teb_local_planner::ObstaclePtr> obst_vector;
+// teb_local_planner::ObstContainer obstacles;
+
+nav_msgs::msg::Odometry current_odom_msg;
+geometry_msgs::msg::Pose2D current_plan_goal_msg;
+
+// void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
+// std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response)
+// {
+// response->sum = request->a + request->b;
+// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
+// request->a, request->b);
+// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
+// }
+
+void fillTebConfig(teb_local_planner::TebConfig &config)
+{
+
+ //config.map_frame = "cam_left_optical_frame";
+ config.map_frame = "world";
+
+ // == Trajectory ==
+ // Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)
+ config.trajectory.teb_autosize = true;
+ // Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)
+ //config.trajectory.dt_ref = 1/stepFrequency; //0.3;
+ config.trajectory.dt_ref = 0.5; //0.3;
+ // Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
+ config.trajectory.dt_hysteresis = 0.1*config.trajectory.dt_ref;
+ // Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically
+ config.trajectory.global_plan_overwrite_orientation = true;
+ // Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]
+ config.trajectory.global_plan_viapoint_sep = -0.1;
+ // If true, the planner adheres to the order of via-points in the storage container
+ config.trajectory.via_points_ordered = false;
+ // Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]
+ config.trajectory.max_global_plan_lookahead_dist = 0.0;
+ // Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
+ config.trajectory.force_reinit_new_goal_dist = 1.0;
+ // Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)
+ config.trajectory.force_reinit_new_goal_angular = 0.78;
+ // Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval
+ config.trajectory.feasibility_check_no_poses = 5;
+ // If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
+ config.trajectory.exact_arc_length = false;
+ // Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
+ config.trajectory.publish_feedback = false;
+
+ // == Robot ==
+ //# Robot/Carlike
+ // Maximum translational velocity of the robot
+ config.robot.max_vel_x = 0.1;
+ // Maximum translational velocity of the robot for driving backwards
+ config.robot.max_vel_x_backwards = 0.02;
+ // Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) [0.0, 100]
+ config.robot.max_vel_y = 0.1;
+ // Maximum angular velocity of the robot [0.01, 100]
+ config.robot.max_vel_theta = 0.1;
+ // Maximum translational acceleration of the robot
+ config.robot.acc_lim_x = 0.1*config.robot.max_vel_x;
+ // Maximum translational acceleration of the robot
+ config.robot.acc_lim_y = 0.1*config.robot.max_vel_y;
+ // Maximum angular acceleration of the robot
+ config.robot.acc_lim_theta = 0.1*config.robot.max_vel_theta;
+ // If true, updated the footprint before checking trajectory feasibility
+ config.robot.is_footprint_dynamic = false;
+ // Minimum turning radius of a carlike robot (diff-drive robot: zero)
+ config.robot.min_turning_radius = 0.0;
+ // The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
+ config.robot.wheelbase = 1.0;
+ config.robot.cmd_angle_instead_rotvel = false;
+
+ // == GoalTolerance ==
+ // Allowed final euclidean distance to the goal position
+ config.goal_tolerance.xy_goal_tolerance = 0.1;
+ // Allowed final orientation error to the goal orientation
+ //config.goal_tolerance.yaw_goal_tolerance = 0.1;
+ // Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)
+ config.goal_tolerance.free_goal_vel = false;
+
+ // == Obstacles ==
+ // Minimum desired separation from obstacles
+ config.obstacles.min_obstacle_dist = teb_robot_radius;
+ // Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
+ config.obstacles.inflation_dist = teb_robot_radius + 0.02;
+ // Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
+ config.obstacles.dynamic_obstacle_inflation_dist = 0.6;
+ // Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.
+ config.obstacles.include_dynamic_obstacles = false;
+ // Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)
+ config.obstacles.include_costmap_obstacles = true;
+ // If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).
+ config.obstacles.legacy_obstacle_association = false;
+ // The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
+ config.obstacles.obstacle_association_force_inclusion_factor = 1.5;
+ // See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
+ config.obstacles.obstacle_association_cutoff_factor = 5.0;
+ // Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
+ config.obstacles.costmap_obstacles_behind_robot_dist = 1.5;
+ // The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
+ config.obstacles.obstacle_poses_affected = 30;
+
+
+ // == Optimization ==
+ // Number of solver iterations called in each outerloop iteration
+ config.optim.no_inner_iterations = 5;
+ // Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
+ config.optim.no_outer_iterations = 4;
+ // Activate the optimization
+ config.optim.optimization_activate = true;
+ // Print verbose information
+ config.optim.optimization_verbose = false;
+ // Add a small safty margin to penalty functions for hard-constraint approximations
+ config.optim.penalty_epsilon = 0.1;
+ // Optimization weight for satisfying the maximum allowed translational velocity
+ config.optim.weight_max_vel_x = 2;
+ // Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
+ config.optim.weight_max_vel_y = 2;
+ // Optimization weight for satisfying the maximum allowed angular velocity
+ config.optim.weight_max_vel_theta = 1;
+ // Optimization weight for satisfying the maximum allowed translational acceleration
+ config.optim.weight_acc_lim_x = 1;
+ // Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
+ config.optim.weight_acc_lim_y = 1;
+ // Optimization weight for satisfying the maximum allowed angular acceleration
+ config.optim.weight_acc_lim_theta = 1;
+
+ // Optimization weight for satisfying the non-holonomic kinematics [0..10000]
+ // [Sol] make this weight highier to force robot not to use lateral steps
+ config.optim.weight_kinematics_nh = 20; //10; //1000
+
+ // Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot) [0..1000]
+ config.optim.weight_kinematics_forward_drive = 1;
+ // Optimization weight for enforcing a minimum turning radius (carlike robots) [0..1000]
+ config.optim.weight_kinematics_turning_radius = 1;
+ // Optimization weight for contracting the trajectory w.r.t. transition time [0..1000]
+ config.optim.weight_optimaltime = 1;
+ // Optimization weight for contracting the trajectory w.r.t. path length [0..100]
+ config.optim.weight_shortest_path = 0;
+ // Optimization weight for satisfying a minimum seperation from obstacles [0..1000]
+ config.optim.weight_obstacle = 50;
+ // Optimization weight for the inflation penalty (should be small) [0..10]
+ config.optim.weight_inflation = 0.1;
+ // Optimization weight for satisfying a minimum seperation from dynamic obstacles [0...1000]
+ config.optim.weight_dynamic_obstacle = 50;
+ // Optimization weight for the inflation penalty of dynamic obstacles (should be small) [0..10]
+ config.optim.weight_dynamic_obstacle_inflation = 0.1;
+ // Optimization weight for minimizing the distance to via-points [0..1000]
+ config.optim.weight_viapoint = 1;
+ // Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem. [0..100]
+ config.optim.weight_adapt_factor = 2.0;
+ // Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) [0.01..100]
+ config.optim.obstacle_cost_exponent = 1.0;
+
+ //[Sol] additional opt variables added to modified teb_local_planner
+ //config.optim.weight_look_at_target = 0.3; //1.0
+ //config.optim.look_at_target_shift_length = 0.1; //robot will finish approach oriented towards target shifted by 0.1mm at its direction (i.e. direction to the ball)
+ //TODO: walking backwards with large lateral/yaw causes robot to fall, make this an additional optimisation term
+
+ // == Homotopy Class Planner ==
+ config.hcp.enable_homotopy_class_planning = true;
+ // Activate multiple threading for planning multiple trajectories in parallel
+ config.hcp.enable_multithreading = true;
+ // Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
+ //config.hcp.max_number_classes = 3;
+ config.hcp.max_number_classes = 1;
+ // Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).
+ config.hcp.selection_cost_hysteresis = 0.9;
+ // Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.
+ config.hcp.selection_prefer_initial_plan = 0.95;
+ // Extra scaling of obstacle cost terms just for selecting the 'best' candidate.
+ config.hcp.selection_obst_cost_scale = 2.0;
+ // Extra scaling of via-point cost terms just for selecting the 'best' candidate.
+ config.hcp.selection_viapoint_cost_scale = 1.0;
+ // If true, time cost is replaced by the total transition time.
+ config.hcp.selection_alternative_time_cost = false;
+ // Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed.
+ config.hcp.switching_blocking_period = 2.0;
+ // Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration [0,1].
+ config.hcp.obstacle_heading_threshold = 0.45;
+ // Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off
+ config.hcp.roadmap_graph_no_samples = 15;
+ // Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.
+ config.hcp.roadmap_graph_area_width = 5;
+ // The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!
+ config.hcp.roadmap_graph_area_length_scale = 1.0;
+ // Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1).
+ config.hcp.h_signature_prescaler = 1.0;
+ // Two h-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.
+ config.hcp.h_signature_threshold = 0.1;
+ // If true, all trajectories of different topologies are attached to the current set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan.
+ config.hcp.viapoints_all_candidates = true;
+ // Visualize the graph that is created for exploring new homotopy classes.
+ config.hcp.visualize_hc_graph = false;
+
+ //# Recovery
+ // Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues
+ config.recovery.shrink_horizon_backup = true;
+ // Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards)
+ config.recovery.oscillation_recovery = false;//true;
+
+}
+
+class HomotopyClassPlannerTest : public teb_local_planner::HomotopyClassPlanner {
+ public:
+ void SetUp(rclcpp_lifecycle::LifecycleNode::SharedPtr node) {
+ teb_local_planner::RobotFootprintModelPtr robot_model;
+ teb_local_planner::HomotopyClassPlannerPtr homotopy_class_planner;
+
+ robot_model.reset(new teb_local_planner::CircularRobotFootprint(0.25));
+
+ // obstacles.push_back(
+ // teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 2))
+ // );
+
+ // obstacles.push_back(
+ // teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 3))
+ // );
+
+ // obstacles.push_back(
+ // teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 4))
+ // );
+
+ visualization.reset(
+ new teb_local_planner::TebVisualization(node, cfg)
+ );
+
+ cfg.hcp.visualize_hc_graph = true;
+ cfg.robot_model = robot_model;
+
+ initialize(node, cfg, &obstacles, /*robot_model,*/ visualization);
+ }
+ teb_local_planner::ObstContainer obstacles;
+ teb_local_planner::TebConfig cfg;
+ teb_local_planner::TebVisualizationPtr visualization;
+};
+
+void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+ current_odom_msg = *msg;
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Odom callback");
+}
+
+void planGoalCallback(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
+ current_plan_goal_msg = *msg;
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Odom callback");
+}
+
+int main(int argc, char **argv)
+{
+ const std::string node_name = "androsot_approach_teb_node";
+ const std::string robot_name = "ROKI-0060";
+
+ rclcpp::init(argc, argv);
+
+ HomotopyClassPlannerTest test;
+
+ //std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("stellazh_approach_teb_node");
+ //rclcpp::Node::SharedPtr node( new rclcpp::Node("stellazh_approach_teb_node") );
+
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node( new rclcpp_lifecycle::LifecycleNode(node_name) );
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Node created");
+
+ rclcpp::Subscription < nav_msgs::msg::Odometry >::SharedPtr odom_subscriber;
+ //odom_subscriber = node->create_subscription<nav_msgs::msg::Odometry>("motion_odometry", 1, std::bind(odomCallback, _1));
+ odom_subscriber = node->create_subscription < nav_msgs::msg::Odometry > ("true_odom", 1, std::bind(odomCallback, _1));
+
+ rclcpp::Subscription < geometry_msgs::msg::Pose2D >::SharedPtr plan_goal_subscriber;
+ plan_goal_subscriber = node->create_subscription < geometry_msgs::msg::Pose2D > ("plan_goal", 1, std::bind(planGoalCallback, _1));
+
+ rclcpp::Publisher < command::msg::Command >::SharedPtr target_vel_publisher;
+ target_vel_publisher = node->create_publisher < command::msg::Command > ("plan_cmd_vel", 1);
+
+ // Fill config with my parameters
+ fillTebConfig(test.cfg);
+
+ test.SetUp(node);
+
+ RCLCPP_INFO(rclcpp::get_logger(node_name), "test setupped");
+
+ test.visualization->on_configure();
+ test.visualization->on_activate();
+
+ rclcpp::executors::SingleThreadedExecutor executor;
+ executor.add_node(node->get_node_base_interface());
+
+ current_plan_goal_msg.x = 0.0;
+ current_plan_goal_msg.y = 0.0;
+ current_plan_goal_msg.theta = 0.0;
+
+ rclcpp::Rate rate(10);
+ while(rclcpp::ok())
+ {
+
+ using namespace teb_local_planner;
+
+ tf2::Quaternion quat_tf;
+ tf2::fromMsg(current_odom_msg.pose.pose.orientation, quat_tf);
+
+ tf2::Matrix3x3 m(quat_tf);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ PoseSE2 start(current_odom_msg.pose.pose.position.x, current_odom_msg.pose.pose.position.y, yaw);
+ PoseSE2 goal(current_plan_goal_msg.x, current_plan_goal_msg.y, current_plan_goal_msg.theta);
+
+ geometry_msgs::msg::Twist start_vel;
+ start_vel.linear.x = current_odom_msg.twist.twist.linear.x;
+ start_vel.linear.y = current_odom_msg.twist.twist.linear.y;
+ start_vel.angular.z = current_odom_msg.twist.twist.angular.z;
+
+ auto eps = 0.01;
+
+ if ( std::fabs(start.x() - goal.x()) > eps ||
+ std::fabs(start.y() - goal.y()) > eps ||
+ std::abs(start.theta() - goal.theta()) > eps )
+ {
+ // Plan start and stop points are different
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Planning by TEB");
+ std::vector < geometry_msgs::msg::PoseStamped > initial_plan;
+
+ geometry_msgs::msg::PoseStamped start_pose;
+ geometry_msgs::msg::PoseStamped goal_pose;
+
+ start.toPoseMsg(start_pose.pose);
+ goal.toPoseMsg(goal_pose.pose);
+
+ initial_plan.push_back(start_pose);
+ initial_plan.push_back(goal_pose);
+
+ test.plan(initial_plan, &start_vel, false);
+
+ //Gettin control speeds from TEB
+ auto vx = 0.0; //translational velocity [m/s]
+ auto vy = 0.0; //strafing velocity which can be nonzero for holonomic robots[m/s]
+ auto omega = 0.0; //rotational velocity [rad/s]
+
+ auto lookAheadPoses = 3;
+
+ test.getVelocityCommand(vx, vy, omega, lookAheadPoses);
+
+ // TODO: translation coefficients m/s -> parrots
+ command::msg::Command target_vel_msg;
+ target_vel_msg.robot_name = robot_name;
+
+ target_vel_msg.velocity.linear.x = vx;
+ target_vel_msg.velocity.linear.y = vy;
+ target_vel_msg.velocity.angular.z = omega;
+
+ target_vel_publisher->publish(target_vel_msg);
+
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Visualizing plan");
+ test.visualize();
+
+ }
+ else
+ {
+ // Plan start and stop points are equal, publishing zero velocity
+ //RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Planning by TEB skipped - start and goal are equal");
+ command::msg::Command target_vel_msg;
+
+ target_vel_msg.velocity.linear.x = 0.0;
+ target_vel_msg.velocity.linear.y = 0.0;
+ target_vel_msg.velocity.angular.z = 0.0;
+
+ target_vel_publisher->publish(target_vel_msg);
+ }
+
+ //executor.spin();
+ executor.spin_some();
+ rate.sleep();
+ }
+
+ //rclcpp::spin_some(node);
+ rclcpp::shutdown();
+}
diff --git a/src/command/CMakeLists.txt b/src/command/CMakeLists.txt
index 3013140..0bbd0f2 100644
--- a/src/command/CMakeLists.txt
+++ b/src/command/CMakeLists.txt
@@ -7,6 +7,7 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
@@ -27,6 +28,7 @@ endif()
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Command.msg"
+ DEPENDENCIES geometry_msgs
)
ament_package()
diff --git a/src/command/msg/Command.msg b/src/command/msg/Command.msg
index 2763d8d..16541a5 100644
--- a/src/command/msg/Command.msg
+++ b/src/command/msg/Command.msg
@@ -1,6 +1,3 @@
string robot_name
-uint8 command
-uint8 step_count
-uint8 step_gain
-int8 turn_gain
-int8 lateral_gain
\ No newline at end of file
+
+geometry_msgs/Twist velocity
\ No newline at end of file
diff --git a/src/command/package.xml b/src/command/package.xml
index 7fd2563..1410cd5 100644
--- a/src/command/package.xml
+++ b/src/command/package.xml
@@ -13,7 +13,8 @@
<test_depend>ament_lint_common</test_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
-
+ <build_depend>geometry_msgs</build_depend>
+
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
diff --git a/src/proxy/proxy/CommandSender.py b/src/proxy/proxy/CommandSender.py
index a129cd8..bff3845 100644
--- a/src/proxy/proxy/CommandSender.py
+++ b/src/proxy/proxy/CommandSender.py
@@ -10,7 +10,7 @@ class CommandSender(Node):
super().__init__('androsot_command_sender')
self.publisher_ = self.create_publisher(Command, 'androsot', 10)
- timer_period = 10 # seconds
+ timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
@@ -20,11 +20,14 @@ class CommandSender(Node):
# Hardcoded params.
# Actually this can be done by any other node with countings
msg.robot_name = 'ROKI-0060'
- msg.command = 1
- msg.step_count = 1
- msg.step_gain = 1
- msg.turn_gain = 1
- msg.lateral_gain = 1
+ msg.command = 0
+ msg.step_count = 10
+ msg.step_gain = 0 #int(input())
+ msg.turn_gain = 0 #int(input())
+ msg.lateral_gain = int(input())
+
+ if (msg.lateral_gain == 0):
+ msg.step_count = 0
self.publisher_.publish(msg)
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index cded5ee..a3e02c1 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -1,6 +1,7 @@
import asyncio
import socket
import threading
+import time
import rclpy
from rclpy.node import Node
@@ -14,24 +15,38 @@ class Proxy(Node):
super().__init__('androsot_proxy')
self.subscription = self.create_subscription(
- Command,
- 'androsot',
- self.listener_callback,
- 10
+ msg_type=Command,
+ topic='plan_cmd_vel',
+ callback=self.listener_callback,
+ qos_profile=10,
)
- self.host = host
- self.port = port
-
- self.n_conn_attempts = connection_atempts
- self.conn_delay = connection_delay
+ # TODO: wrap parameter declarations
+ self.declare_parameter('host', host)
+ self.declare_parameter('port', port)
+ self.declare_parameter('n_conn_attempts', connection_atempts)
+ self.declare_parameter('conn_delay', connection_delay)
self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.writer = None
- self.subscription
+ @property
+ def host(self):
+ return self.get_parameter('host').get_parameter_value().string_value
+
+ @property
+ def port(self):
+ return self.get_parameter('port').get_parameter_value().integer_value
+
+ @property
+ def n_conn_attempts(self):
+ return self.get_parameter('n_conn_attempts').get_parameter_value().integer_value
+
+ @property
+ def conn_delay(self):
+ return self.get_parameter('conn_delay').get_parameter_value().double_value
- async def connect(self):
+ async def connect(self) -> None:
for _ in range(self.n_conn_attempts):
try:
await asyncio.to_thread(self.connection.connect, (self.host, self.port))
@@ -45,54 +60,63 @@ class Proxy(Node):
if self.writer is None or self.writer.is_closing():
raise ConnectionError(f"Connection refused after {self.conn_delay * self.n_conn_attempts} [sec]")
- async def listener_callback(self, msg):
- self.writer.write(f"{msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}\n".encode())
+ async def listener_callback(self, msg: Command) -> None:
+ error_code = self.connection.getsockopt(socket.SOL_SOCKET, socket.SO_ERROR)
+
+ if error_code != 0:
+ self.get_logger().error("Lost connection to host")
+ raise ConnectionError("Lost connection to host")
+
+ self.writer.write(f"{msg.robot_name}|0|10|{msg.velocity.linear.x}|{msg.velocity.rotational.z}|{msg.velocity.linear.y}\n".encode())
await self.writer.drain()
+
self.get_logger().info(f'I heard: {msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}')
- async def disconnect(self):
+ async def disconnect(self) -> None:
if self.writer is not None:
self.writer.close()
await self.writer.wait_closed()
+
self.connection.close()
+async def launch_proxy() -> None:
+ try:
+ proxy = Proxy()
+ await proxy.connect()
+
+ cancel = proxy.create_guard_condition(lambda: None)
+ event_loop = asyncio.get_event_loop()
+ spin_task = event_loop.create_future()
-async def spin(node: Node):
- cancel = node.create_guard_condition(lambda: None)
+ rclpy.spin_until_future_complete(proxy, spin_task)
- def _spin(node: Node,
- future: asyncio.Future,
- event_loop: asyncio.AbstractEventLoop):
- while not future.cancelled():
- rclpy.spin_once(node)
- if not future.cancelled():
- event_loop.call_soon_threadsafe(future.set_result, None)
+ try:
+ await spin_task
+ except asyncio.CancelledError:
+ cancel.trigger()
- event_loop = asyncio.get_event_loop()
- spin_task = event_loop.create_future()
- spin_thread = threading.Thread(target=_spin, args=(node, spin_task, event_loop))
- spin_thread.start()
+ except Exception as exception:
+ print(exception)
+
+ finally:
+ proxy.destroy_guard_condition(cancel)
+ await proxy.disconnect()
+ proxy.destroy_node()
+
+def main(args=None) -> None:
try:
- await spin_task
- except asyncio.CancelledError:
- cancel.trigger()
- spin_thread.join()
- node.destroy_guard_condition(cancel)
-
-async def main(args=None):
- proxy = Proxy()
- await proxy.connect()
-
- await spin(proxy)
+ rclpy.init(args=args)
+ asyncio.run(launch_proxy())
- proxy.destroy_node()
+ except KeyboardInterrupt:
+ print(f"Exit from program.")
- return
+ except Exception as exception:
+ print(exception)
-print(__name__)
+ finally:
+ rclpy.shutdown()
-if __name__ == "proxy.Proxy":
- rclpy.init()
- asyncio.run(main())
- rclpy.shutdown()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/teb_local_planner/CHANGELOG.rst b/src/teb_local_planner/CHANGELOG.rst
new file mode 100644
index 0000000..11b4f73
--- /dev/null
+++ b/src/teb_local_planner/CHANGELOG.rst
@@ -0,0 +1,407 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package teb_local_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.9.1 (2020-05-29)
+------------------
+* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang)
+* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro)
+* CMake: Removed system include to avoid compiling issues on some platforms
+* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro
+
+0.9.0 (2020-05-26)
+------------------
+* Added pill resp. stadium-shaped obstacle
+* Changed minimum CMake version to 3.1
+* Improved efficiency of 3d h-signature computation
+* Changed default value for parameter penalty_epsilon to 0.05
+* Improved efficiency of findClosedTrajectoryPose()
+* Removed obsolete method isHorizonReductionAppropriate()
+* Contributors: Christoph Rösmann, XinyuKhan
+
+0.8.4 (2019-12-02)
+------------------
+* Fixed TEB autoResize if last TimeDiff is small
+* Add a rotational threshold for identifying a warm start goal
+* Contributors: Rainer Kümmerle
+
+0.8.3 (2019-10-25)
+------------------
+* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa)
+* test_optim_node fix circular obstacles (thanks to dtaranta)
+* Fix shadow variable warning (thanks to Victor Lopez)
+* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez)
+* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle)
+* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa)
+* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer.
+ Fixes `#158 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/158>`_.
+* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot)
+* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot)
+* added warning if parameter optimal_time is <= 0
+* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle.
+ See `#140 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/140>`_.
+* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server
+* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta
+
+0.8.2 (2019-07-02)
+------------------
+* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash)
+* Add nonlinear part to obstacle cost to improve narrow gap behavior.
+ Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term.
+ The default linear behavior is achieved by setting this parameter to 1 (default).
+ A value of 4 performed well in some tests and experiments (thanks to Howard Cochran).
+* Parameter `global_plan_prune_distance` added via ros parameter server.
+* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran)
+* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran)
+* On footprint visualization, avoid overshadowing by obstacles (thanks to corot)
+* Do not ignore robot model on the association stage.
+ Important mostly for polygon footprint model (thanks to corot).
+* Adjustable color for footprint visualization
+* Showing (detected) infeasible robot poses in a separate marker namespace and color
+* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`)
+* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew)
+* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU)
+* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa)
+* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa).
+* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa)
+* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa)
+* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights.
+ As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran).
+* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas)
+* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa)
+* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading.
+ Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb
+ and if a teb cannot be optimized (thanks to Marco Bassa).
+ Optionally allowing the usage of the initial plan orientation when initializing new tebs.
+* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot
+
+0.8.1 (2018-08-14)
+------------------
+* bugfix in calculateHSignature. Fixes `#90 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/90>`_.
+* fixed centroid computation in a special case of polygon-obstacles
+* Contributors: Christoph Rösmann
+
+0.8.0 (2018-08-06)
+------------------
+* First melodic release
+* Updated to new g2o API
+* Migration to tf2
+* Contributors: Christoph Rösmann
+
+0.7.3 (2018-07-05)
+------------------
+* Parameter `switching_blocking_period` added to homotopy class planner parameter group.
+ Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon
+ as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds
+ by default in order to not change the behavior of existing configurations.
+* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`.
+ The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`.
+ `global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name
+ in the code as well as `weight_viapoint` and the ros wiki description.
+ In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed.
+* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap:
+ In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start)
+ and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist).
+ Previously, the search algorithm stopped as soon as the distance to the robot increased once.
+ This caused troubles with more complex global plans, hence the new strategy checks the complete subset
+ of the global plan in the local costmap for the closest distance to the robot.
+* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode)
+* Edge creation: minor performance improvement for dynamic obstacle edges
+* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory
+* Contributors: Christoph Rösmann
+
+0.7.2 (2018-06-08)
+------------------
+* Adds the possibility to provide via-points via a topic.
+ Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan
+ (e.g., activate the latter by setting global_plan_viapoint_sep>0 as before).
+ A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node.
+* Contributors: Christoph Rösmann
+
+0.7.1 (2018-06-05)
+------------------
+* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default.
+ This cost function was only intended to be active only for recovering from an oscillating robot.
+ This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached.
+ Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor
+ (huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals).
+* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg
+* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...)
+* Update to use non deprecated pluginlib macro
+* Python scripts updated to new obstacle message definition.
+* Fixed issue when start and end are at the same location (PR #43)
+* Normalize marker quaternions in *test_optim_node*
+* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip
+
+0.7.0 (2017-09-23)
+------------------
+* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code).
+ Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated.
+ Note, this feature is still experimental and subject to testing.
+ Motion prediction is performed using a constant velocity model.
+ Dynamic obstacles might be incorporated as follows:
+ * via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg).
+ * via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental).
+ A tutorial is going to be provided soon.
+* FeedbackMsg includes a ObstacleMsg instead of a polygon
+* ObstacleMsg removed from package since it is now part of the costmap\_converter package.
+* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now
+ implemented as subclasses of more general interfaces.
+* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start.
+ Old methods are marked as deprecated. This change does not affect users settings.
+* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer
+* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format
+* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true.
+ We introduced the non-fast mode with the support of dynamic obstacles
+ (which leads to better results in terms of x-y-t homotopy planning).
+ However, we have not yet tested this mode intensively, so we keep
+ the previous mode as default until we finish our tests.
+* added parameter and code to update costmap footprint if it is dynamic (#49)
+* Contributors: Franz Albers, Christoph Rösmann, procopiostein
+
+0.6.6 (2016-12-23)
+------------------
+* Strategy for recovering from oscillating local plans added (see new parameters)
+* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected
+ (to avoid oscillations due to changing final orientations)
+* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true
+* Parameter max_samples added
+* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches)
+
+0.6.5 (2016-11-15)
+------------------
+* The trajectory is now initialized backwards for goals close to and behind the robot.
+ Parameter 'allow_init_with_backwards_motion' added.
+* Updated the TEB selection in the HomotopyClassPlanner.
+ * A new parameter is introduced to prefer the equivalence class of the initial plan
+ * Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan.
+* Weight adaptation added for obstacles edges.
+ Added parameter 'weight_adapt_factor'.
+ Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration.
+ Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions.
+* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint.
+ Validation is performed by only comparing the inscribed radii of the footprints.
+* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected.
+* Changed HSignature to a generic equivalence class
+* Minor changes
+
+0.6.4 (2016-10-23)
+------------------
+* New default obstacle association strategy:
+ During optimization graph creation, for each pose of the trajectory a
+ relevance detection is performed before considering the obstacle
+ during optimization. New parameters are introduced. The
+ old strategy is kept as 'legacy' strategy (see parameters).
+* Computation of velocities, acceleration and turning radii extended:
+ Added an option to compute the actual arc length
+ instead of using the Euclidean distance approximation (see parameter `exact_arc_length`.
+* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy.
+* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server
+* Removed TebConfig dependency in TebVisualization
+* PolygonObstacle can now be constructed using a vertices container
+* HomotopyClassPlanner public interface extended
+* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2
+* Removed inline flags in visualization.cpp
+* Removed inline flags in timed_elastic_band.cpp.
+ Fixes `#15 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/15>`_.
+* Increased bounds of many variables in dynamic_reconfigure.
+ Resolves `#14 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/14>`_.
+ The particular variables are maximum velocities, maximum accelerations,
+ minimum turning radius,...
+ Note: optimization weights and dt_ref as well as dt_hyst are not
+ tuned for velocities and accelerations beyond
+ the default values (e.g. >1 m/s). Just increasing the maximum velocity
+ bounds without adjusting the other parameters leads to an insufficient behavior.
+* Default parameter value update: 'costmap_obstacles_behind_robot_dist'
+* Additional minor fixes.
+
+0.6.3 (2016-08-17)
+------------------
+* Changed the f0 function for calculating the H-Signature.
+ The new one seems to be more robust for a much larger number of obstacles
+ after some testing.
+* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again
+* Fixed distance calculation polygon-to-polygon-obstacle
+* cmake config exports now *include directories* of external packages for dependent projects
+* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13.
+
+
+0.6.2 (2016-06-15)
+------------------
+* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error.
+* Inflation mode for obstacles added.
+* The homotopy class of the global plan is now always forced to be initialized as trajectory.
+* The initial velocity of the robot is now taken into account correctly for
+ all candidate trajectories.
+* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle.
+ This fix addresses issue `#7 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/7>`_
+
+0.6.1 (2016-05-23)
+------------------
+* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue)
+
+
+0.6.0 (2016-05-22)
+------------------
+* Extended support to holonomic robots
+* Wrong parameter namespace for *costmap_converter* plugins fixed
+* Added the option to scale the length of the hcp sampling area
+* Compiler warnings fixed.
+* Workaround for compilation issues that are caused by a bug in boost 1.58
+ concerning the graph library (missing move constructor/assignment operator
+ in boost source).
+* Using *tf_listener* from *move_base* now.
+* Via-point support improved.
+ Added the possibility to take the actual order of via-points into account.
+ Additionally, via-points beyond start and goal are now included.
+* Obsolete include of the angles package header removed
+* Update to package.xml version 2
+* Some other minor fixes.
+
+
+0.4.0 (2016-04-19)
+------------------
+* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now.
+ This allows the user to adapt the tradeoff between time-optimality and path-following.
+ Check out the new tutorial: "Following the Global Plan (Via-Points)".
+* All external configuration and launch files are removed, since they are part
+ of the new teb_local_planner_tutorials package.
+
+
+0.3.1 (2016-04-14)
+------------------
+* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models.
+* Trajectory selection strategy in case of multiple topologies updated:
+ * The obstacle costs for selection can now be scaling separately.
+ * The cost regarding time optimality can now be replaced by the actual transition time.
+ * Added a hysteresis to cost comparison between a new and the previously selected trajectory.
+ * In the default parameter setting the strategy is similar to release 0.3.0.
+* Warning message removed that occured if an odom message with only zeros was received.
+
+
+0.3.0 (2016-04-08)
+------------------
+* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!).
+* The new robot footprint is also visualized using the common marker topic.
+* The strategy of taking occupied costmap cells behind the robot into account has been improved.
+ These changes significantly improve navigation close to walls.
+* Parameter 'max_global_plan_lookahead_dist' added.
+ Previously, the complete subset of the global plan contained in the local costmap
+ was taken into account for choosing the current intermediate goal point. With this parameter, the maximum
+ length of the reference global plan can be limited. The actual global plan subset
+ is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'.
+* Bug fixes:
+ * Fixed a compilation issue on ARM architectures
+ * If custom obstacles are used, the container with old obstacles is now cleared properly.
+* Parameter cleanup:
+ * "weight_X_obstacle" parameters combined to single parameter "weight_obstacle".
+ * "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected".
+ * Deprecated parameter 'costmap_emergency_stop_dist' removed.
+* Code cleanup
+
+
+0.2.3 (2016-02-01)
+------------------
+* Marker lifetime changed
+* In case the local planner detects an infeasible trajectory it does now try to
+ reduce the horizon to 50 percent of the length. The trajectory is only reduced
+ if some predefined cases are detected.
+ This mechanism constitutes a backup behavior.
+* Improved carlike robot support.
+ Instead of commanding the robot using translational and rotational velocities,
+ the robot might also be commanded using the transl. velocity and steering angle.
+ Appropriate parameters are added to the config.
+* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision.
+* Some python scripts for data conversion added
+* Minor other changes
+
+0.2.2 (2016-01-11)
+------------------
+* Carlike robots (ackermann steering) are supported from now on (at least experimentally)
+ by specifying a minimum bound on the turning radius.
+ Currently, the output of the planner in carlike mode is still (v,omega).
+ Since I don't have any real carlike robot, I would be really happy if someone could provide me with
+ some feedback to further improve/extend the support.
+* Obstacle cost function modified to avoid undesired jerks in the trajectory.
+* Added a feedback message that contains current trajectory information (poses, velocities and temporal information).
+ This is useful for analyzing and debugging the velocity profile e.g. at runtime.
+ The message will be published only if it's activated (rosparam).
+ A small python script is added to plot the velocity profile (while *test_optim_node* runs).
+* Cost functions are now taking the direction/sign of the translational velocity into account:
+ Specifying a maximum backwards velocity other than forward velocity works now.
+ Additionally, the change in acceleration is now computed correctly if the robot switches directions.
+* The global plan is now pruned such that already passed posses are cut off
+ (relevant for global planners with *planning_rate=0*).
+* Fixed issue#1: If a global planner with *planning_rate=0* was used,
+ a TF timing/extrapolation issue appeared after some time.
+* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results.
+
+
+0.2.1 (2015-12-30)
+------------------
+* This is an important bugfix release.
+* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if
+ the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s).
+ From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not.
+* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid).
+
+0.2.0 (2015-12-23)
+------------------
+* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes.
+ The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.).
+ A tutorial will explain how to activate the converter using the ros-param server.
+
+0.1.11 (2015-12-12)
+-------------------
+* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal)
+
+0.1.10 (2015-08-13)
+-------------------
+* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation.
+* Some bugfixes and improvements
+
+0.1.9 (2015-06-24)
+------------------
+* Fixed a segmentation fault issue. This minor update is crucial for stability.
+
+0.1.8 (2015-06-08)
+------------------
+* Custom obstacles can be included via publishing dedicated messages
+* Goal-reached-condition also checks orientation error (desired yaw) now
+* Numerical improvements of the h-signature calculation
+* Minor bugfixes
+
+0.1.7 (2015-05-22)
+------------------
+* Finally fixed saucy compilation issue by retaining compatiblity to newer distros
+ (my "new" 13.10 VM helps me to stop spamming new releases for testing).
+
+0.1.6 (2015-05-22)
+------------------
+* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts.
+ I am not able to test releasing on saucy, forcing me to release again and again. Sorry.
+
+0.1.5 (2015-05-21)
+------------------
+* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure.
+* Fixed a wrong default-min-max tuple in the dynamic reconfigure config.
+* Useful config and launch files are now added to cmake install.
+* Added install target for the test_optim_node executable.
+
+0.1.4 (2015-05-20)
+------------------
+* Fixed compilation errors on ROS Jade
+
+0.1.3 (2015-05-20)
+------------------
+* Fixed compilation errors on ubuntu saucy
+
+0.1.2 (2015-05-19)
+------------------
+* Removed unused include that could break compilation.
+
+0.1.1 (2015-05-19)
+------------------
+* All files added to the indigo-devel branch
+* Initial commit
+* Contributors: Christoph Rösmann
diff --git a/src/teb_local_planner/CMakeLists.txt b/src/teb_local_planner/CMakeLists.txt
new file mode 100644
index 0000000..43859bc
--- /dev/null
+++ b/src/teb_local_planner/CMakeLists.txt
@@ -0,0 +1,162 @@
+cmake_minimum_required(VERSION 3.5)
+project(teb_local_planner)
+
+# Set to Release in order to speed up the program significantly
+set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+# add_compile_options(-Wall -Wextra -Wpedantic)
+#endif()
+
+## Find catkin macros and libraries
+find_package(ament_cmake REQUIRED)
+find_package(costmap_converter REQUIRED)
+find_package(dwb_critics REQUIRED)
+find_package(nav_2d_utils REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+#find_package(interactive_markers REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(teb_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+message(STATUS "System: ${CMAKE_SYSTEM}")
+## System dependencies are found with CMake's conventions
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
+message(STATUS "${CMAKE_MODULE_PATH}")
+find_package(SUITESPARSE REQUIRED)
+find_package(G2O REQUIRED)
+find_package(Boost REQUIRED)
+
+# Eigen3 FindScript Backward compatibility (ubuntu saucy)
+# Since FindEigen.cmake is deprecated starting from jade.
+if (EXISTS "FindEigen3.cmake")
+ find_package(Eigen3 REQUIRED)
+ set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
+elseif (EXISTS "FindEigen.cmake")
+ find_package(Eigen REQUIRED)
+elseif (EXISTS "FindEigen.cmake")
+ message(WARNING "No findEigen cmake script found. You must provde one of them,
+ e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.")
+endif (EXISTS "FindEigen3.cmake")
+
+set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR})
+set(EXTERNAL_LIBS ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES})
+
+###########
+## Build ##
+###########
+
+include_directories(
+ include
+ ${Boost_INCLUDE_DIRS}
+ ${EXTERNAL_INCLUDE_DIRS}
+)
+
+## Build the teb_local_planner library
+
+add_library(teb_local_planner SHARED
+ src/timed_elastic_band.cpp
+ src/optimal_planner.cpp
+ src/obstacles.cpp
+ src/visualization.cpp
+ src/recovery_behaviors.cpp
+ src/teb_config.cpp
+ src/homotopy_class_planner.cpp
+ src/teb_local_planner_ros.cpp
+ src/graph_search.cpp
+)
+
+set(ament_dependencies
+ costmap_converter
+ dwb_critics
+ nav_2d_utils
+ nav2_core
+ nav2_costmap_2d
+ nav2_util
+ geometry_msgs
+ nav_msgs
+ rclcpp
+ rclcpp_action
+ rclcpp_lifecycle
+ std_msgs
+ pluginlib
+ teb_msgs
+ tf2
+ tf2_eigen
+ tf2_geometry_msgs
+ tf2_ros
+ visualization_msgs
+ builtin_interfaces
+)
+ament_target_dependencies(teb_local_planner
+ ${ament_dependencies}
+)
+target_link_libraries(teb_local_planner
+ ${EXTERNAL_LIBS}
+)
+
+target_compile_definitions(teb_local_planner PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+
+#############
+## Install ##
+#############
+
+## Mark executables and/or libraries for installation
+install(TARGETS teb_local_planner
+ DESTINATION lib
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/
+ DESTINATION include/
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+ teb_local_planner_plugin.xml
+ DESTINATION share
+ )
+
+install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_libraries(teb_local_planner)
+ament_export_dependencies(${ament_dependencies})
+pluginlib_export_plugin_description_file(nav2_core teb_local_planner_plugin.xml)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+ find_package(ament_cmake_gtest REQUIRED)
+ #ament_add_gtest(homotopy_class_planner_test
+ # test/homotopy_class_planner_test.cpp
+ #)
+ #target_link_libraries(homotopy_class_planner_test
+ # teb_local_planner
+ #)
+endif()
+
+ament_package()
diff --git a/src/teb_local_planner/LICENSE b/src/teb_local_planner/LICENSE
new file mode 100644
index 0000000..d4658cc
--- /dev/null
+++ b/src/teb_local_planner/LICENSE
@@ -0,0 +1,28 @@
+Copyright (c) 2016, TU Dortmund - Lehrstuhl RST
+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 teb_local_planner nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
diff --git a/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg b/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
new file mode 100755
index 0000000..f403d7e
--- /dev/null
+++ b/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
@@ -0,0 +1,420 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+#from local_planner_limits import add_generic_localplanner_params
+
+gen = ParameterGenerator()
+
+# This unusual line allows to reuse existing parameter definitions
+# that concern all localplanners
+#add_generic_localplanner_params(gen)
+
+# For integers and doubles:
+# Name Type Reconfiguration level
+# Description
+# Default Min Max
+
+
+grp_trajectory = gen.add_group("Trajectory", type="tab")
+
+# Trajectory
+grp_trajectory.add("teb_autosize", bool_t, 0,
+ "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
+ True)
+
+grp_trajectory.add("dt_ref", double_t, 0,
+ "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
+ 0.3, 0.01, 1)
+
+grp_trajectory.add("dt_hysteresis", double_t, 0,
+ "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
+ 0.1, 0.002, 0.5)
+
+grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0,
+ "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
+ True)
+
+grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0,
+ "If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
+ False)
+
+grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0,
+ "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
+ 3.0, 0, 50.0)
+
+grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0,
+ "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
+ 1.0, 0.0, 10.0)
+
+grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0,
+ "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
+ 0.78, 0.0, 4.0)
+
+grp_trajectory.add("feasibility_check_no_poses", int_t, 0,
+ "Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
+ 5, -1, 50)
+
+grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0,
+ "Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
+ -1, -1, 20)
+
+grp_trajectory.add("exact_arc_length", bool_t, 0,
+ "If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
+ False)
+
+grp_trajectory.add("publish_feedback", bool_t, 0,
+ "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
+ False)
+
+grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0,
+ "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
+ 0, 0, 1)
+
+# ViaPoints
+grp_viapoints = gen.add_group("ViaPoints", type="tab")
+
+grp_viapoints.add("global_plan_viapoint_sep", double_t, 0,
+ "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
+ -0.1, -0.1, 5.0)
+
+grp_viapoints.add("via_points_ordered", bool_t, 0,
+ "If true, the planner adheres to the order of via-points in the storage container",
+ False)
+
+# Robot
+grp_robot = gen.add_group("Robot", type="tab")
+
+grp_robot.add("max_vel_x", double_t, 0,
+ "Maximum translational velocity of the robot",
+ 0.4, 0.01, 100)
+
+grp_robot.add("max_vel_x_backwards", double_t, 0,
+ "Maximum translational velocity of the robot for driving backwards",
+ 0.2, 0.01, 100)
+
+grp_robot.add("max_vel_theta", double_t, 0,
+ "Maximum angular velocity of the robot",
+ 0.3, 0.01, 100)
+
+grp_robot.add("acc_lim_x", double_t, 0,
+ "Maximum translational acceleration of the robot",
+ 0.5, 0.01, 100)
+
+grp_robot.add("acc_lim_theta", double_t, 0,
+ "Maximum angular acceleration of the robot",
+ 0.5, 0.01, 100)
+
+grp_robot.add("is_footprint_dynamic", bool_t, 0,
+ "If true, updated the footprint before checking trajectory feasibility",
+ False)
+
+grp_robot.add("use_proportional_saturation", bool_t, 0,
+ "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
+ False)
+grp_robot.add("transform_tolerance", double_t, 0,
+ "Tolerance when querying the TF Tree for a transformation (seconds)",
+ 0.5, 0.001, 20)
+
+# Robot/Carlike
+
+grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")
+
+grp_robot_carlike.add("min_turning_radius", double_t, 0,
+ "Minimum turning radius of a carlike robot (diff-drive robot: zero)",
+ 0.0, 0.0, 50.0)
+
+grp_robot_carlike.add("wheelbase", double_t, 0,
+ "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!",
+ 1.0, -10.0, 10.0)
+
+grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0,
+ "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')",
+ False)
+
+# Robot/Omni
+
+grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
+
+grp_robot_omni.add("max_vel_y", double_t, 0,
+ "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
+ 0.0, 0.0, 100)
+
+grp_robot_omni.add("acc_lim_y", double_t, 0,
+ "Maximum strafing acceleration of the robot",
+ 0.5, 0.01, 100)
+
+# GoalTolerance
+grp_goal = gen.add_group("GoalTolerance", type="tab")
+
+grp_goal.add("free_goal_vel", bool_t, 0,
+ "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)",
+ False)
+
+# Obstacles
+grp_obstacles = gen.add_group("Obstacles", type="tab")
+
+grp_obstacles.add("min_obstacle_dist", double_t, 0,
+ "Minimum desired separation from obstacles",
+ 0.5, 0, 10)
+
+grp_obstacles.add("inflation_dist", double_t, 0,
+ "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
+ 0.6, 0, 15)
+
+grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
+ "Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
+ 0.6, 0, 15)
+
+grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
+ "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
+ False)
+
+grp_obstacles.add("include_costmap_obstacles", bool_t, 0,
+ "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)",
+ True)
+
+grp_obstacles.add("legacy_obstacle_association", bool_t, 0,
+ "If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).",
+ False)
+
+grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0,
+ "The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.",
+ 1.5, 0.0, 100.0)
+
+grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0,
+ "See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.",
+ 5.0, 1.0, 100.0)
+
+grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0,
+ "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)",
+ 1.5, 0.0, 20.0)
+
+grp_obstacles.add("obstacle_poses_affected", int_t, 0,
+ "The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well",
+ 30, 0, 200)
+
+# Obstacle - Velocity ratio parameters
+grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
+ "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
+ 1, 0, 1)
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
+ "Distance to a static obstacle for which the velocity should be lower",
+ 0, 0, 10)
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
+ "Distance to a static obstacle for which the velocity should be higher",
+ 0.5, 0, 10)
+
+# Optimization
+grp_optimization = gen.add_group("Optimization", type="tab")
+
+grp_optimization.add("no_inner_iterations", int_t, 0,
+ "Number of solver iterations called in each outerloop iteration",
+ 5, 1, 100)
+
+grp_optimization.add("no_outer_iterations", int_t, 0,
+ "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations",
+ 4, 1, 100)
+
+grp_optimization.add("optimization_activate", bool_t, 0,
+ "Activate the optimization",
+ True)
+
+grp_optimization.add("optimization_verbose", bool_t, 0,
+ "Print verbose information",
+ False)
+
+grp_optimization.add("penalty_epsilon", double_t, 0,
+ "Add a small safty margin to penalty functions for hard-constraint approximations",
+ 0.05, 0, 1.0)
+
+grp_optimization.add("weight_max_vel_x", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed translational velocity",
+ 2, 0, 1000)
+
+grp_optimization.add("weight_max_vel_y", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)",
+ 2, 0, 1000)
+
+grp_optimization.add("weight_max_vel_theta", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed angular velocity",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_acc_lim_x", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed translational acceleration",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_acc_lim_y", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_acc_lim_theta", double_t, 0,
+ "Optimization weight for satisfying the maximum allowed angular acceleration",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_kinematics_nh", double_t, 0,
+ "Optimization weight for satisfying the non-holonomic kinematics",
+ 1000 , 0, 10000)
+
+grp_optimization.add("weight_kinematics_forward_drive", double_t, 0,
+ "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)",
+ 1, 0, 10000)
+
+grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
+ "Optimization weight for enforcing a minimum turning radius (carlike robots)",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_optimaltime", double_t, 0,
+ "Optimization weight for contracting the trajectory w.r.t. transition time",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_shortest_path", double_t, 0,
+ "Optimization weight for contracting the trajectory w.r.t. path length",
+ 0, 0, 100)
+
+grp_optimization.add("weight_obstacle", double_t, 0,
+ "Optimization weight for satisfying a minimum seperation from obstacles",
+ 50, 0, 1000)
+
+grp_optimization.add("weight_inflation", double_t, 0,
+ "Optimization weight for the inflation penalty (should be small)",
+ 0.1, 0, 10)
+
+grp_optimization.add("weight_dynamic_obstacle", double_t, 0,
+ "Optimization weight for satisfying a minimum seperation from dynamic obstacles",
+ 50, 0, 1000)
+
+grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
+ "Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
+ 0.1, 0, 10)
+
+grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
+ "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
+ 0, 0, 1000)
+
+grp_optimization.add("weight_viapoint", double_t, 0,
+ "Optimization weight for minimizing the distance to via-points",
+ 1, 0, 1000)
+
+grp_optimization.add("weight_adapt_factor", double_t, 0,
+ "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.",
+ 2, 1, 100)
+
+grp_optimization.add("obstacle_cost_exponent", double_t, 0,
+ "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
+ 1, 0.01, 100)
+
+
+
+# Homotopy Class Planner
+grp_hcp = gen.add_group("HCPlanning", type="tab")
+
+grp_hcp.add("enable_multithreading", bool_t, 0,
+ "Activate multiple threading for planning multiple trajectories in parallel",
+ True)
+
+grp_hcp.add("max_number_classes", int_t, 0,
+ "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)",
+ 5, 1, 100)
+
+grp_hcp.add("max_number_plans_in_current_class", int_t, 0,
+ "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
+ 1, 1, 10)
+
+grp_hcp.add("selection_cost_hysteresis", double_t, 0,
+ "Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)",
+ 1.0, 0, 2)
+
+
+grp_hcp.add("selection_prefer_initial_plan", double_t, 0,
+ "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)",
+ 0.95, 0, 1)
+
+grp_hcp.add("selection_obst_cost_scale", double_t, 0,
+ "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)",
+ 2.0, 0, 1000)
+
+grp_hcp.add("selection_viapoint_cost_scale", double_t, 0,
+ "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)",
+ 1.0, 0, 100)
+
+grp_hcp.add("selection_alternative_time_cost", bool_t, 0,
+ "If true, time cost is replaced by the total transition time.",
+ False)
+
+grp_hcp.add("selection_dropping_probability", double_t, 0,
+ "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.",
+ 0.0, 0.0, 1.0)
+
+grp_hcp.add("switching_blocking_period", double_t, 0,
+ "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
+ 0.0, 0.0, 60)
+
+grp_hcp.add("roadmap_graph_no_samples", int_t, 0,
+ "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off",
+ 15, 1, 100)
+
+grp_hcp.add("roadmap_graph_area_width", double_t, 0,
+ "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)",
+ 5, 0.1, 20)
+
+grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0,
+ "The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)",
+ 1.0, 0.5, 2)
+
+grp_hcp.add("h_signature_prescaler", double_t, 0,
+ "Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)",
+ 1, 0.2, 1)
+
+grp_hcp.add("h_signature_threshold", double_t, 0,
+ "Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold",
+ 0.1, 0, 1)
+
+grp_hcp.add("obstacle_heading_threshold", double_t, 0,
+ "Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)",
+ 0.45, 0, 1)
+
+grp_hcp.add("viapoints_all_candidates", bool_t, 0,
+ "If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).",
+ True)
+
+grp_hcp.add("visualize_hc_graph", bool_t, 0,
+ "Visualize the graph that is created for exploring new homotopy classes",
+ False)
+
+
+# Recovery
+grp_recovery = gen.add_group("Recovery", type="tab")
+
+grp_recovery.add("shrink_horizon_backup", bool_t, 0,
+ "Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
+ True)
+
+grp_recovery.add("oscillation_recovery", bool_t, 0,
+ "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
+ True)
+
+grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
+
+grp_recovery_divergence.add(
+ "divergence_detection_enable",
+ bool_t,
+ 0,
+ "True to enable divergence detection.",
+ False
+)
+
+grp_recovery_divergence.add(
+ "divergence_detection_max_chi_squared",
+ double_t,
+ 0,
+ "Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
+ 10,
+ 0,
+ 100
+)
+
+exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))
diff --git a/src/teb_local_planner/cfg/rviz_test_optim.rviz b/src/teb_local_planner/cfg/rviz_test_optim.rviz
new file mode 100644
index 0000000..02ee250
--- /dev/null
+++ b/src/teb_local_planner/cfg/rviz_test_optim.rviz
@@ -0,0 +1,183 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 81
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ Splitter Ratio: 0.5
+ Tree Height: 562
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 0.100000001
+ Cell Size: 0.100000001
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Fine Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 100
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.300000012
+ Head Length: 0.200000003
+ Length: 0.300000012
+ Line Style: Lines
+ Line Width: 0.0299999993
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Style: None
+ Radius: 0.0299999993
+ Shaft Diameter: 0.100000001
+ Shaft Length: 0.100000001
+ Topic: /test_optim_node/local_plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Arrow Length: 0.300000012
+ Axes Length: 0.300000012
+ Axes Radius: 0.00999999978
+ Class: rviz/PoseArray
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.0700000003
+ Head Radius: 0.0299999993
+ Name: PoseArray
+ Shaft Length: 0.230000004
+ Shaft Radius: 0.00999999978
+ Shape: Arrow (Flat)
+ Topic: /test_optim_node/teb_poses
+ Unreliable: false
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /test_optim_node/teb_markers
+ Name: Marker
+ Namespaces:
+ PointObstacles: true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/InteractiveMarkers
+ Enable Transparency: true
+ Enabled: true
+ Name: InteractiveMarkers
+ Show Axes: false
+ Show Descriptions: true
+ Show Visual Aids: false
+ Update Topic: /marker_obstacles/update
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: odom
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 7.77247
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 1.56979632
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 4.71043873
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1354
+ X: 137
+ Y: 50
diff --git a/src/teb_local_planner/cmake_modules/FindG2O.cmake b/src/teb_local_planner/cmake_modules/FindG2O.cmake
new file mode 100644
index 0000000..b2670d3
--- /dev/null
+++ b/src/teb_local_planner/cmake_modules/FindG2O.cmake
@@ -0,0 +1,97 @@
+# Locate the g2o libraries
+# A general framework for graph optimization.
+#
+# This module defines
+# G2O_FOUND, if false, do not try to link against g2o
+# G2O_LIBRARIES, path to the libg2o
+# G2O_INCLUDE_DIR, where to find the g2o header files
+#
+# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
+# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
+
+IF(UNIX)
+
+ #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+ # in cache already
+ # SET(G2O_FIND_QUIETLY TRUE)
+ #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+
+ MESSAGE(STATUS "Searching for g2o ...")
+ FIND_PATH(G2O_INCLUDE_DIR
+ NAMES core math_groups types
+ PATHS /usr/local /usr
+ PATH_SUFFIXES include/g2o include)
+
+ IF (G2O_INCLUDE_DIR)
+ MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
+ ENDIF (G2O_INCLUDE_DIR)
+
+ FIND_LIBRARY(G2O_CORE_LIB
+ NAMES g2o_core g2o_core_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_STUFF_LIB
+ NAMES g2o_stuff g2o_stuff_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB
+ NAMES g2o_types_slam2d g2o_types_slam2d_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB
+ NAMES g2o_types_slam3d g2o_types_slam3d_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB
+ NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_PCG_LIB
+ NAMES g2o_solver_pcg g2o_solver_pcg_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB
+ NAMES g2o_solver_csparse g2o_solver_csparse_rd
+ PATHS /usr/local /usr
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_INCREMENTAL_LIB
+ NAMES g2o_incremental g2o_incremental_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+ FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
+ NAMES g2o_csparse_extension g2o_csparse_extension_rd
+ PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+ PATH_SUFFIXES lib)
+
+ SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
+ ${G2O_CORE_LIB}
+ ${G2O_STUFF_LIB}
+ ${G2O_TYPES_SLAM2D_LIB}
+ ${G2O_TYPES_SLAM3D_LIB}
+ ${G2O_SOLVER_CHOLMOD_LIB}
+ ${G2O_SOLVER_PCG_LIB}
+ ${G2O_SOLVER_CSPARSE_LIB}
+ ${G2O_INCREMENTAL_LIB}
+ )
+
+ IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+ SET(G2O_FOUND "YES")
+ IF(NOT G2O_FIND_QUIETLY)
+ MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
+ ENDIF(NOT G2O_FIND_QUIETLY)
+ ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+ IF(NOT G2O_LIBRARIES)
+ IF(G2O_FIND_REQUIRED)
+ message(FATAL_ERROR "Could not find libg2o!")
+ ENDIF(G2O_FIND_REQUIRED)
+ ENDIF(NOT G2O_LIBRARIES)
+
+ IF(NOT G2O_INCLUDE_DIR)
+ IF(G2O_FIND_REQUIRED)
+ message(FATAL_ERROR "Could not find g2o include directory!")
+ ENDIF(G2O_FIND_REQUIRED)
+ ENDIF(NOT G2O_INCLUDE_DIR)
+ ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+
+ENDIF(UNIX)
+
diff --git a/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake b/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
new file mode 100644
index 0000000..101b79b
--- /dev/null
+++ b/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
@@ -0,0 +1,133 @@
+# - Try to find SUITESPARSE
+# Once done this will define
+#
+# SUITESPARSE_FOUND - system has SUITESPARSE
+# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory
+# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE
+# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package)
+# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
+# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs
+# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs
+# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly
+
+IF (SUITESPARSE_INCLUDE_DIRS)
+ # Already in cache, be silent
+ SET(SUITESPARSE_FIND_QUIETLY TRUE)
+ENDIF (SUITESPARSE_INCLUDE_DIRS)
+
+if( WIN32 )
+ # Find cholmod part of the suitesparse library collection
+
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS "C:\\libs\\win32\\SuiteSparse\\Include" )
+
+ # Add cholmod include directory to collection include directories
+ IF ( CHOLMOD_INCLUDE_DIR )
+ list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+ ENDIF( CHOLMOD_INCLUDE_DIR )
+
+
+ # find path suitesparse library
+ FIND_PATH( SUITESPARSE_LIBRARY_DIRS
+ amd.lib
+ PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
+
+ # if we found the library, add it to the defined libraries
+ IF ( SUITESPARSE_LIBRARY_DIRS )
+ list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd )
+ ENDIF( SUITESPARSE_LIBRARY_DIRS )
+
+else( WIN32 )
+ IF(APPLE)
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS /opt/local/include/ufsparse
+ /usr/local/include )
+
+ FIND_PATH( SUITESPARSE_LIBRARY_DIR
+ NAMES libcholmod.a
+ PATHS /opt/local/lib
+ /usr/local/lib )
+ ELSE(APPLE)
+ FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+ PATHS /usr/local/include
+ /usr/include
+ /usr/include/suitesparse/
+ ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
+ PATH_SUFFIXES cholmod/ CHOLMOD/ )
+
+ FIND_PATH( SUITESPARSE_LIBRARY_DIR
+ NAMES libcholmod.so libcholmod.a
+ PATHS /usr/lib
+ /usr/lib64
+ /usr/lib/x86_64-linux-gnu
+ /usr/lib/i386-linux-gnu
+ /usr/local/lib
+ /usr/lib/arm-linux-gnueabihf/
+ /usr/lib/aarch64-linux-gnu/
+ /usr/lib/arm-linux-gnueabi/
+ /usr/lib/arm-linux-gnu)
+ ENDIF(APPLE)
+
+ # Add cholmod include directory to collection include directories
+ IF ( CHOLMOD_INCLUDE_DIR )
+ list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+ ENDIF( CHOLMOD_INCLUDE_DIR )
+
+ # if we found the library, add it to the defined libraries
+ IF ( SUITESPARSE_LIBRARY_DIR )
+
+ list ( APPEND SUITESPARSE_LIBRARIES amd)
+ list ( APPEND SUITESPARSE_LIBRARIES btf)
+ list ( APPEND SUITESPARSE_LIBRARIES camd)
+ list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
+ list ( APPEND SUITESPARSE_LIBRARIES cholmod)
+ list ( APPEND SUITESPARSE_LIBRARIES colamd)
+ # list ( APPEND SUITESPARSE_LIBRARIES csparse)
+ list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
+ list ( APPEND SUITESPARSE_LIBRARIES klu)
+ # list ( APPEND SUITESPARSE_LIBRARIES spqr)
+ list ( APPEND SUITESPARSE_LIBRARIES umfpack)
+
+ IF (APPLE)
+ list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
+ ENDIF (APPLE)
+
+ # Metis and spqr are optional
+ FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
+ NAMES metis
+ PATHS ${SUITESPARSE_LIBRARY_DIR} )
+ IF (SUITESPARSE_METIS_LIBRARY)
+ list ( APPEND SUITESPARSE_LIBRARIES metis)
+ ENDIF(SUITESPARSE_METIS_LIBRARY)
+
+ if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
+ SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
+ else()
+ SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
+ endif()
+
+ if(SUITESPARSE_SPQR_VALID)
+ FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
+ NAMES spqr
+ PATHS ${SUITESPARSE_LIBRARY_DIR} )
+ IF (SUITESPARSE_SPQR_LIBRARY)
+ list ( APPEND SUITESPARSE_LIBRARIES spqr)
+ ENDIF (SUITESPARSE_SPQR_LIBRARY)
+ endif()
+
+ ENDIF( SUITESPARSE_LIBRARY_DIR )
+
+endif( WIN32 )
+
+
+IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+ IF(WIN32)
+ list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
+ ENDIF(WIN32)
+ SET(SUITESPARSE_FOUND TRUE)
+ MESSAGE(STATUS "Found SuiteSparse")
+ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+ SET( SUITESPARSE_FOUND FALSE )
+ MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
+ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+
diff --git a/src/teb_local_planner/include/teb_local_planner/distance_calculations.h b/src/teb_local_planner/include/teb_local_planner/distance_calculations.h
new file mode 100644
index 0000000..62f6b88
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/distance_calculations.h
@@ -0,0 +1,464 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef DISTANCE_CALCULATIONS_H
+#define DISTANCE_CALCULATIONS_H
+
+#include <Eigen/Core>
+#include "teb_local_planner/misc.h"
+
+
+namespace teb_local_planner
+{
+
+//! Abbrev. for a container storing 2d points
+typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > Point2dContainer;
+
+
+/**
+ * @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point
+ * @param point 2D point
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @return Closest point on the line segment
+ */
+inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
+{
+ Eigen::Vector2d diff = line_end - line_start;
+ double sq_norm = diff.squaredNorm();
+
+ if (sq_norm == 0)
+ return line_start;
+
+ double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm;
+
+ if (u <= 0) return line_start;
+ else if (u >= 1) return line_end;
+
+ return line_start + u*diff;
+}
+
+/**
+ * @brief Helper function to calculate the distance between a line segment and a point
+ * @param point 2D point
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @return minimum distance to a given line segment
+ */
+inline double distance_point_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
+{
+ return (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm();
+}
+
+/**
+ * @brief Helper function to check whether two line segments intersects
+ * @param line1_start 2D point representing the start of the first line segment
+ * @param line1_end 2D point representing the end of the first line segment
+ * @param line2_start 2D point representing the start of the second line segment
+ * @param line2_end 2D point representing the end of the second line segment
+ * @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true)
+ * @return \c true if both line segments intersect
+ */
+inline bool check_line_segments_intersection_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
+ const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end, Eigen::Vector2d* intersection = NULL)
+{
+ // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
+ double s_numer, t_numer, denom, t;
+ Eigen::Vector2d line1 = line1_end - line1_start;
+ Eigen::Vector2d line2 = line2_end - line2_start;
+
+ denom = line1.x() * line2.y() - line2.x() * line1.y();
+ if (denom == 0) return false; // Collinear
+ bool denomPositive = denom > 0;
+
+ Eigen::Vector2d aux = line1_start - line2_start;
+
+ s_numer = line1.x() * aux.y() - line1.y() * aux.x();
+ if ((s_numer < 0) == denomPositive) return false; // No collision
+
+ t_numer = line2.x() * aux.y() - line2.y() * aux.x();
+ if ((t_numer < 0) == denomPositive) return false; // No collision
+
+ if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision
+
+ // Otherwise collision detected
+ t = t_numer / denom;
+ if (intersection)
+ {
+ *intersection = line1_start + t * line1;
+ }
+
+ return true;
+}
+
+
+/**
+ * @brief Helper function to calculate the smallest distance between two line segments
+ * @param line1_start 2D point representing the start of the first line segment
+ * @param line1_end 2D point representing the end of the first line segment
+ * @param line2_start 2D point representing the start of the second line segment
+ * @param line2_end 2D point representing the end of the second line segment
+ * @return smallest distance between both segments
+*/
+inline double distance_segment_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
+ const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end)
+{
+ // TODO more efficient implementation
+
+ // check if segments intersect
+ if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end))
+ return 0;
+
+ // check all 4 combinations
+ std::array<double,4> distances;
+
+ distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end);
+ distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end);
+ distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end);
+ distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end);
+
+ return *std::min_element(distances.begin(), distances.end());
+}
+
+
+/**
+ * @brief Helper function to calculate the smallest distance between a point and a closed polygon
+ * @param point 2D point
+ * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/
+inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
+{
+ double dist = HUGE_VAL;
+
+ // the polygon is a point
+ if (vertices.size() == 1)
+ {
+ return (point - vertices.front()).norm();
+ }
+
+ // check each polygon edge
+ for (int i=0; i<(int)vertices.size()-1; ++i)
+ {
+ double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
+// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
+ if (new_dist < dist)
+ dist = new_dist;
+ }
+
+ if (vertices.size()>2) // if not a line close polygon
+ {
+ double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
+ if (new_dist < dist)
+ return new_dist;
+ }
+
+ return dist;
+}
+
+/**
+ * @brief Helper function to calculate the smallest distance between a line segment and a closed polygon
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/
+inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices)
+{
+ double dist = HUGE_VAL;
+
+ // the polygon is a point
+ if (vertices.size() == 1)
+ {
+ return distance_point_to_segment_2d(vertices.front(), line_start, line_end);
+ }
+
+ // check each polygon edge
+ for (int i=0; i<(int)vertices.size()-1; ++i)
+ {
+ double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1));
+// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
+ if (new_dist < dist)
+ dist = new_dist;
+ }
+
+ if (vertices.size()>2) // if not a line close polygon
+ {
+ double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge
+ if (new_dist < dist)
+ return new_dist;
+ }
+
+ return dist;
+}
+
+/**
+ * @brief Helper function to calculate the smallest distance between two closed polygons
+ * @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)
+ * @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/
+inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
+{
+ double dist = HUGE_VAL;
+
+ // the polygon1 is a point
+ if (vertices1.size() == 1)
+ {
+ return distance_point_to_polygon_2d(vertices1.front(), vertices2);
+ }
+
+ // check each edge of polygon1
+ for (int i=0; i<(int)vertices1.size()-1; ++i)
+ {
+ double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
+ if (new_dist < dist)
+ dist = new_dist;
+ }
+
+ if (vertices1.size()>2) // if not a line close polygon1
+ {
+ double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
+ if (new_dist < dist)
+ return new_dist;
+ }
+
+ return dist;
+}
+
+
+
+
+// Further distance calculations:
+
+
+// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html
+// Copyright 2001 softSurfer, 2012 Dan Sunday
+// This code may be freely used and modified for any purpose
+// providing that this copyright notice is included with it.
+// SoftSurfer makes no warranty for this code, and cannot be held
+// liable for any real or imagined damage resulting from its use.
+// Users of this code must verify correctness for their application.
+
+inline double calc_distance_line_to_line_3d(const Eigen::Ref<const Eigen::Vector3d>& x1, Eigen::Ref<const Eigen::Vector3d>& u,
+ const Eigen::Ref<const Eigen::Vector3d>& x2, Eigen::Ref<const Eigen::Vector3d>& v)
+{
+ Eigen::Vector3d w = x2 - x1;
+ double a = u.squaredNorm(); // dot(u,u) always >= 0
+ double b = u.dot(v);
+ double c = v.squaredNorm(); // dot(v,v) always >= 0
+ double d = u.dot(w);
+ double e = v.dot(w);
+ double D = a*c - b*b; // always >= 0
+ double sc, tc;
+
+ // compute the line parameters of the two closest points
+ if (D < SMALL_NUM) { // the lines are almost parallel
+ sc = 0.0;
+ tc = (b>c ? d/b : e/c); // use the largest denominator
+ }
+ else {
+ sc = (b*e - c*d) / D;
+ tc = (a*e - b*d) / D;
+ }
+
+ // get the difference of the two closest points
+ Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
+
+ return dP.norm(); // return the closest distance
+}
+
+
+
+
+
+inline double calc_distance_segment_to_segment3D(const Eigen::Ref<const Eigen::Vector3d>& line1_start, Eigen::Ref<const Eigen::Vector3d>& line1_end,
+ const Eigen::Ref<const Eigen::Vector3d>& line2_start, Eigen::Ref<const Eigen::Vector3d>& line2_end)
+{
+ Eigen::Vector3d u = line1_end - line1_start;
+ Eigen::Vector3d v = line2_end - line2_start;
+ Eigen::Vector3d w = line2_start - line1_start;
+ double a = u.squaredNorm(); // dot(u,u) always >= 0
+ double b = u.dot(v);
+ double c = v.squaredNorm(); // dot(v,v) always >= 0
+ double d = u.dot(w);
+ double e = v.dot(w);
+ double D = a*c - b*b; // always >= 0
+ double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
+ double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
+
+ // compute the line parameters of the two closest points
+ if (D < SMALL_NUM)
+ { // the lines are almost parallel
+ sN = 0.0; // force using point P0 on segment S1
+ sD = 1.0; // to prevent possible division by 0.0 later
+ tN = e;
+ tD = c;
+ }
+ else
+ { // get the closest points on the infinite lines
+ sN = (b*e - c*d);
+ tN = (a*e - b*d);
+ if (sN < 0.0)
+ { // sc < 0 => the s=0 edge is visible
+ sN = 0.0;
+ tN = e;
+ tD = c;
+ }
+ else if (sN > sD)
+ { // sc > 1 => the s=1 edge is visible
+ sN = sD;
+ tN = e + b;
+ tD = c;
+ }
+ }
+
+ if (tN < 0.0)
+ { // tc < 0 => the t=0 edge is visible
+ tN = 0.0;
+ // recompute sc for this edge
+ if (-d < 0.0)
+ sN = 0.0;
+ else if (-d > a)
+ sN = sD;
+ else
+ {
+ sN = -d;
+ sD = a;
+ }
+ }
+ else if (tN > tD)
+ { // tc > 1 => the t=1 edge is visible
+ tN = tD;
+ // recompute sc for this edge
+ if ((-d + b) < 0.0)
+ sN = 0;
+ else if ((-d + b) > a)
+ sN = sD;
+ else
+ {
+ sN = (-d + b);
+ sD = a;
+ }
+ }
+ // finally do the division to get sc and tc
+ sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
+ tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
+
+ // get the difference of the two closest points
+ Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
+
+ return dP.norm(); // return the closest distance
+}
+
+
+
+
+template <typename VectorType>
+double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2)
+{
+ VectorType dv = vel1 - vel2;
+
+ double dv2 = dv.squaredNorm(); // dot(v,v)
+ if (dv2 < SMALL_NUM) // the tracks are almost parallel
+ return 0.0; // any time is ok. Use time 0.
+
+ VectorType w0 = x1 - x2;
+ double cpatime = -w0.dot(dv) / dv2;
+
+ return cpatime; // time of CPA
+}
+
+template <typename VectorType>
+double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0)
+{
+ double ctime = calc_closest_point_to_approach_time<VectorType>(x1, vel1, x2, vel2);
+ if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time;
+ VectorType P1 = x1 + (ctime * vel1);
+ VectorType P2 = x2 + (ctime * vel2);
+
+ return (P2-P1).norm(); // distance at CPA
+}
+
+
+
+// dist_Point_to_Line(): get the distance of a point to a line
+// Input: a Point P and a Line L (in any dimension)
+// Return: the shortest distance from P to L
+template <typename VectorType>
+double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir)
+{
+ VectorType w = point - line_base;
+
+ double c1 = w.dot(line_dir);
+ double c2 = line_dir.dot(line_dir);
+ double b = c1 / c2;
+
+ VectorType Pb = line_base + b * line_dir;
+ return (point-Pb).norm();
+}
+//===================================================================
+
+
+// dist_Point_to_Segment(): get the distance of a point to a segment
+// Input: a Point P and a Segment S (in any dimension)
+// Return: the shortest distance from P to S
+template <typename VectorType>
+double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end)
+{
+ VectorType v = line_end - line_start;
+ VectorType w = point - line_start;
+
+ double c1 = w.dot(v);
+ if ( c1 <= 0 )
+ return w.norm();
+
+ double c2 = v.dot(v);
+ if ( c2 <= c1 )
+ return (point-line_end).norm();
+
+ double b = c1 / c2;
+ VectorType Pb = line_start + b * v;
+ return (point-Pb).norm();
+}
+
+
+
+} // namespace teb_local_planner
+
+#endif /* DISTANCE_CALCULATIONS_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h b/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
new file mode 100644
index 0000000..8551152
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
@@ -0,0 +1,103 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EQUIVALENCE_RELATIONS_H_
+#define EQUIVALENCE_RELATIONS_H_
+
+#include <memory>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EquivalenceClass
+ * @brief Abstract class that defines an interface for computing and comparing equivalence classes
+ *
+ * Equivalence relations are utilized in order to test if two trajectories are belonging to the same
+ * equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is
+ * the concept of homotopy classes. All trajectories belonging to the same homotopy class
+ * can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely
+ * share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation
+ * is defined by the concept of homology classes (e.g. refer to HSignature).
+ *
+ * Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object.
+ *
+ * @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass.
+ * Call the "compute"-methods directly on the subclass.
+ */
+class EquivalenceClass
+{
+public:
+
+ /**
+ * @brief Default constructor
+ */
+ EquivalenceClass() {}
+
+ /**
+ * @brief virtual destructor
+ */
+ virtual ~EquivalenceClass() {}
+
+ /**
+ * @brief Check if two candidate classes are equivalent
+ * @param other The other equivalence class to test with
+ */
+ virtual bool isEqual(const EquivalenceClass& other) const = 0;
+
+ /**
+ * @brief Check if the equivalence value is detected correctly
+ * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+ */
+ virtual bool isValid() const = 0;
+
+ /**
+ * @brief Check if the trajectory is non-looping around an obstacle
+ * @return Returns false, if the trajectory loops around an obstacle
+ */
+ virtual bool isReasonable() const = 0;
+
+};
+
+using EquivalenceClassPtr = std::shared_ptr<EquivalenceClass>;
+
+
+} // namespace teb_local_planner
+
+
+#endif /* EQUIVALENCE_RELATIONS_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
new file mode 100644
index 0000000..dd35147
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
@@ -0,0 +1,278 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef _BASE_TEB_EDGES_H_
+#define _BASE_TEB_EDGES_H_
+
+#include "teb_local_planner/teb_config.h"
+
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/base_multi_edge.h>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class BaseTebUnaryEdge
+ * @brief Base edge connecting a single vertex in the TEB optimization problem
+ *
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */
+template <int D, typename E, typename VertexXi>
+class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
+{
+public:
+
+ using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
+ using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
+
+ /**
+ * @brief Compute and return error / cost value.
+ *
+ * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+ * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+ */
+ ErrorVector& getError()
+ {
+ computeError();
+ return _error;
+ }
+
+ /**
+ * @brief Read values from input stream
+ */
+ virtual bool read(std::istream& is)
+ {
+ // TODO generic read
+ return true;
+ }
+
+ /**
+ * @brief Write values to an output stream
+ */
+ virtual bool write(std::ostream& os) const
+ {
+ // TODO generic write
+ return os.good();
+ }
+
+ /**
+ * @brief Assign the TebConfig class for parameters.
+ * @param cfg TebConfig class
+ */
+ void setTebConfig(const TebConfig& cfg)
+ {
+ cfg_ = &cfg;
+ }
+
+protected:
+
+ using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
+ using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
+
+ const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * @class BaseTebBinaryEdge
+ * @brief Base edge connecting two vertices in the TEB optimization problem
+ *
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */
+template <int D, typename E, typename VertexXi, typename VertexXj>
+class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
+{
+public:
+
+ using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
+ using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
+
+ /**
+ * @brief Compute and return error / cost value.
+ *
+ * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+ * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+ */
+ ErrorVector& getError()
+ {
+ computeError();
+ return _error;
+ }
+
+ /**
+ * @brief Read values from input stream
+ */
+ virtual bool read(std::istream& is)
+ {
+ // TODO generic read
+ return true;
+ }
+
+ /**
+ * @brief Write values to an output stream
+ */
+ virtual bool write(std::ostream& os) const
+ {
+ // TODO generic write
+ return os.good();
+ }
+
+ /**
+ * @brief Assign the TebConfig class for parameters.
+ * @param cfg TebConfig class
+ */
+ void setTebConfig(const TebConfig& cfg)
+ {
+ cfg_ = &cfg;
+ }
+
+protected:
+
+ using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
+ using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
+
+ const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+/**
+ * @class BaseTebMultiEdge
+ * @brief Base edge connecting multiple vertices in the TEB optimization problem
+ *
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */
+template <int D, typename E>
+class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
+{
+public:
+
+ using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
+ using g2o::BaseMultiEdge<D, E>::computeError;
+
+ // Overwrites resize() from the parent class
+ virtual void resize(size_t size)
+ {
+ g2o::BaseMultiEdge<D, E>::resize(size);
+
+ for(std::size_t i=0; i<_vertices.size(); ++i)
+ _vertices[i] = NULL;
+ }
+
+ /**
+ * @brief Compute and return error / cost value.
+ *
+ * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+ * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+ */
+ ErrorVector& getError()
+ {
+ computeError();
+ return _error;
+ }
+
+ /**
+ * @brief Read values from input stream
+ */
+ virtual bool read(std::istream& is)
+ {
+ // TODO generic read
+ return true;
+ }
+
+ /**
+ * @brief Write values to an output stream
+ */
+ virtual bool write(std::ostream& os) const
+ {
+ // TODO generic write
+ return os.good();
+ }
+
+ /**
+ * @brief Assign the TebConfig class for parameters.
+ * @param cfg TebConfig class
+ */
+ void setTebConfig(const TebConfig& cfg)
+ {
+ cfg_ = &cfg;
+ }
+
+protected:
+
+ using g2o::BaseMultiEdge<D, E>::_error;
+ using g2o::BaseMultiEdge<D, E>::_vertices;
+
+ const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
new file mode 100644
index 0000000..5d3ff98
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
@@ -0,0 +1,732 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_ACCELERATION_H_
+#define EDGE_ACCELERATION_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/misc.h"
+
+#include <geometry_msgs/msg/twist.hpp>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeAcceleration
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
+ *
+ * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationStart
+ * @see EdgeAccelerationGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values!
+ */
+class EdgeAcceleration : public BaseTebMultiEdge<2, double>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAcceleration()
+ {
+ this->resize(5);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+ const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
+ const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+ const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+
+ // VELOCITY & ACCELERATION
+ const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
+ const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
+
+ double dist1 = diff1.norm();
+ double dist2 = diff2.norm();
+ const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());
+ const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());
+
+ if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation
+ {
+ if (angle_diff1 != 0)
+ {
+ const double radius = dist1/(2*sin(angle_diff1/2));
+ dist1 = fabs( angle_diff1 * radius ); // actual arg length!
+ }
+ if (angle_diff2 != 0)
+ {
+ const double radius = dist2/(2*sin(angle_diff2/2));
+ dist2 = fabs( angle_diff2 * radius ); // actual arg length!
+ }
+ }
+
+ double vel1 = dist1 / dt1->dt();
+ double vel2 = dist2 / dt2->dt();
+
+
+ // consider directions
+// vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta()));
+// vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta()));
+ vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) );
+ vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) );
+
+ const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
+
+
+ _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ const double omega1 = angle_diff1 / dt1->dt();
+ const double omega2 = angle_diff2 / dt2->dt();
+ const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
+
+ _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
+ }
+
+
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0
+ /*
+ * @brief Jacobi matrix of the cost function specified in computeError().
+ */
+ void linearizeOplus()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+ const VertexPointXY* conf1 = static_cast<const VertexPointXY*>(_vertices[0]);
+ const VertexPointXY* conf2 = static_cast<const VertexPointXY*>(_vertices[1]);
+ const VertexPointXY* conf3 = static_cast<const VertexPointXY*>(_vertices[2]);
+ const VertexTimeDiff* deltaT1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+ const VertexTimeDiff* deltaT2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+ const VertexOrientation* angle1 = static_cast<const VertexOrientation*>(_vertices[5]);
+ const VertexOrientation* angle2 = static_cast<const VertexOrientation*>(_vertices[6]);
+ const VertexOrientation* angle3 = static_cast<const VertexOrientation*>(_vertices[7]);
+
+ Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate();
+ Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate();
+ double dist1 = deltaS1.norm();
+ double dist2 = deltaS2.norm();
+
+ double sum_time = deltaT1->estimate() + deltaT2->estimate();
+ double sum_time_inv = 1 / sum_time;
+ double dt1_inv = 1/deltaT1->estimate();
+ double dt2_inv = 1/deltaT2->estimate();
+ double aux0 = 2/sum_time_inv;
+ double aux1 = dist1 * deltaT1->estimate();
+ double aux2 = dist2 * deltaT2->estimate();
+
+ double vel1 = dist1 * dt1_inv;
+ double vel2 = dist2 * dt2_inv;
+ double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv;
+ double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv;
+ double acc = (vel2 - vel1) * aux0;
+ double omegadot = (omega2 - omega1) * aux0;
+ double aux3 = -acc/2;
+ double aux4 = -omegadot/2;
+
+ double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
+ double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
+
+ _jacobianOplus[0].resize(2,2); // conf1
+ _jacobianOplus[1].resize(2,2); // conf2
+ _jacobianOplus[2].resize(2,2); // conf3
+ _jacobianOplus[3].resize(2,1); // deltaT1
+ _jacobianOplus[4].resize(2,1); // deltaT2
+ _jacobianOplus[5].resize(2,1); // angle1
+ _jacobianOplus[6].resize(2,1); // angle2
+ _jacobianOplus[7].resize(2,1); // angle3
+
+ if (aux1==0) aux1=1e-20;
+ if (aux2==0) aux2=1e-20;
+
+ if (dev_border_acc!=0)
+ {
+ // TODO: double aux = aux0 * dev_border_acc;
+ // double aux123 = aux / aux1;
+ _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1
+ _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1
+ _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2
+ _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2
+ _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3
+ _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3
+ _jacobianOplus[2](0,0) = 0;
+ _jacobianOplus[2](0,1) = 0;
+ _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1
+ _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2
+ }
+ else
+ {
+ _jacobianOplus[0](0,0) = 0; // acc x1
+ _jacobianOplus[0](0,1) = 0; // acc y1
+ _jacobianOplus[1](0,0) = 0; // acc x2
+ _jacobianOplus[1](0,1) = 0; // acc y2
+ _jacobianOplus[2](0,0) = 0; // acc x3
+ _jacobianOplus[2](0,1) = 0; // acc y3
+ _jacobianOplus[3](0,0) = 0; // acc deltaT1
+ _jacobianOplus[4](0,0) = 0; // acc deltaT2
+ }
+
+ if (dev_border_omegadot!=0)
+ {
+ _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1
+ _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2
+ _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1
+ _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2
+ _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3
+ }
+ else
+ {
+ _jacobianOplus[3](1,0) = 0; // omegadot deltaT1
+ _jacobianOplus[4](1,0) = 0; // omegadot deltaT2
+ _jacobianOplus[5](1,0) = 0; // omegadot angle1
+ _jacobianOplus[6](1,0) = 0; // omegadot angle2
+ _jacobianOplus[7](1,0) = 0; // omegadot angle3
+ }
+
+ _jacobianOplus[0](1,0) = 0; // omegadot x1
+ _jacobianOplus[0](1,1) = 0; // omegadot y1
+ _jacobianOplus[1](1,0) = 0; // omegadot x2
+ _jacobianOplus[1](1,1) = 0; // omegadot y2
+ _jacobianOplus[2](1,0) = 0; // omegadot x3
+ _jacobianOplus[2](1,1) = 0; // omegadot y3
+ _jacobianOplus[5](0,0) = 0; // acc angle1
+ _jacobianOplus[6](0,0) = 0; // acc angle2
+ _jacobianOplus[7](0,0) = 0; // acc angle3
+ }
+#endif
+#endif
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+/**
+ * @class EdgeAccelerationStart
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAcceleration
+ * @see EdgeAccelerationGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory!
+ */
+class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAccelerationStart()
+ {
+ _measurement = NULL;
+ this->resize(3);
+ }
+
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
+ const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ // VELOCITY & ACCELERATION
+ const Eigen::Vector2d diff = pose2->position() - pose1->position();
+ double dist = diff.norm();
+ const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta());
+ if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+ {
+ const double radius = dist/(2*sin(angle_diff/2));
+ dist = fabs( angle_diff * radius ); // actual arg length!
+ }
+
+ const double vel1 = _measurement->linear.x;
+ double vel2 = dist / dt->dt();
+
+ // consider directions
+ //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta()));
+ vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) );
+
+ const double acc_lin = (vel2 - vel1) / dt->dt();
+
+ _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ const double omega1 = _measurement->angular.z;
+ const double omega2 = angle_diff / dt->dt();
+ const double acc_rot = (omega2 - omega1) / dt->dt();
+
+ _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
+ }
+
+ /**
+ * @brief Set the initial velocity that is taken into account for calculating the acceleration
+ * @param vel_start twist message containing the translational and rotational velocity
+ */
+ void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
+ {
+ _measurement = &vel_start;
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+/**
+ * @class EdgeAccelerationGoal
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAcceleration
+ * @see EdgeAccelerationStart
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory
+ */
+class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAccelerationGoal()
+ {
+ _measurement = NULL;
+ this->resize(3);
+ }
+
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
+ const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ // VELOCITY & ACCELERATION
+
+ const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
+ double dist = diff.norm();
+ const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta());
+ if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+ {
+ double radius = dist/(2*sin(angle_diff/2));
+ dist = fabs( angle_diff * radius ); // actual arg length!
+ }
+
+ double vel1 = dist / dt->dt();
+ const double vel2 = _measurement->linear.x;
+
+ // consider directions
+ //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta()));
+ vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) );
+
+ const double acc_lin = (vel2 - vel1) / dt->dt();
+
+ _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ const double omega1 = angle_diff / dt->dt();
+ const double omega2 = _measurement->angular.z;
+ const double acc_rot = (omega2 - omega1) / dt->dt();
+
+ _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
+ }
+
+ /**
+ * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
+ * @param vel_goal twist message containing the translational and rotational velocity
+ */
+ void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
+ {
+ _measurement = &vel_goal;
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+/**
+ * @class EdgeAccelerationHolonomic
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
+ *
+ * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n
+ * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir),
+ * the second one the strafing acceleration and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomicStart
+ * @see EdgeAccelerationHolonomicGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values!
+ */
+class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAccelerationHolonomic()
+ {
+ this->resize(5);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+ const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
+ const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+ const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+
+ // VELOCITY & ACCELERATION
+ Eigen::Vector2d diff1 = pose2->position() - pose1->position();
+ Eigen::Vector2d diff2 = pose3->position() - pose2->position();
+
+ double cos_theta1 = std::cos(pose1->theta());
+ double sin_theta1 = std::sin(pose1->theta());
+ double cos_theta2 = std::cos(pose2->theta());
+ double sin_theta2 = std::sin(pose2->theta());
+
+ // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+ double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y();
+ double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y();
+ // transform pose3 into robot frame pose2 (inverse 2d rotation matrix)
+ double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y();
+ double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y();
+
+ double vel1_x = p1_dx / dt1->dt();
+ double vel1_y = p1_dy / dt1->dt();
+ double vel2_x = p2_dx / dt2->dt();
+ double vel2_y = p2_dy / dt2->dt();
+
+ double dt12 = dt1->dt() + dt2->dt();
+
+ double acc_x = (vel2_x - vel1_x)*2 / dt12;
+ double acc_y = (vel2_y - vel1_y)*2 / dt12;
+
+ _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
+ double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
+ double acc_rot = (omega2 - omega1)*2 / dt12;
+
+ _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]);
+ TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]);
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+/**
+ * @class EdgeAccelerationHolonomicStart
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n
+ * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
+ * the second one the strafing acceleration and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomic
+ * @see EdgeAccelerationHolonomicGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory!
+ */
+class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAccelerationHolonomicStart()
+ {
+ this->resize(3);
+ _measurement = NULL;
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
+ const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ // VELOCITY & ACCELERATION
+ Eigen::Vector2d diff = pose2->position() - pose1->position();
+
+ double cos_theta1 = std::cos(pose1->theta());
+ double sin_theta1 = std::sin(pose1->theta());
+
+ // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+ double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
+ double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
+
+ double vel1_x = _measurement->linear.x;
+ double vel1_y = _measurement->linear.y;
+ double vel2_x = p1_dx / dt->dt();
+ double vel2_y = p1_dy / dt->dt();
+
+ double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
+ double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
+
+ _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ double omega1 = _measurement->angular.z;
+ double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
+ double acc_rot = (omega2 - omega1) / dt->dt();
+
+ _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
+ TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]);
+ }
+
+ /**
+ * @brief Set the initial velocity that is taken into account for calculating the acceleration
+ * @param vel_start twist message containing the translational and rotational velocity
+ */
+ void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
+ {
+ _measurement = &vel_start;
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+/**
+ * @class EdgeAccelerationHolonomicGoal
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n
+ * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
+ * the second one is the strafing velocity and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomic
+ * @see EdgeAccelerationHolonomicStart
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory
+ */
+class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeAccelerationHolonomicGoal()
+ {
+ _measurement = NULL;
+ this->resize(3);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
+ const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ // VELOCITY & ACCELERATION
+
+ Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
+
+ double cos_theta1 = std::cos(pose_pre_goal->theta());
+ double sin_theta1 = std::sin(pose_pre_goal->theta());
+
+ // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+ double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
+ double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
+
+ double vel1_x = p1_dx / dt->dt();
+ double vel1_y = p1_dy / dt->dt();
+ double vel2_x = _measurement->linear.x;
+ double vel2_y = _measurement->linear.y;
+
+ double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
+ double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
+
+ _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+
+ // ANGULAR ACCELERATION
+ double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
+ double omega2 = _measurement->angular.z;
+ double acc_rot = (omega2 - omega1) / dt->dt();
+
+ _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
+ TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);
+ TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]);
+ }
+
+
+ /**
+ * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
+ * @param vel_goal twist message containing the translational and rotational velocity
+ */
+ void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
+ {
+ _measurement = &vel_goal;
+ }
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+}; // end namespace
+
+#endif /* EDGE_ACCELERATION_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
new file mode 100755
index 0000000..8224042
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
@@ -0,0 +1,154 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef EDGE_DYNAMICOBSTACLE_H
+#define EDGE_DYNAMICOBSTACLE_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/misc.h"
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeDynamicObstacle
+ * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles.
+ *
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n
+ * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n
+ * @see TebOptimalPlanner::AddEdgesDynamicObstacles
+ * @remarks Do not forget to call setTebConfig(), setVertexIdx() and
+ * @warning Experimental
+ */
+class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeDynamicObstacle() : t_(0)
+ {
+ }
+
+ /**
+ * @brief Construct edge and specify the time for its associated pose (neccessary for computeError).
+ * @param t_ Estimated time until current pose is reached
+ */
+ EdgeDynamicObstacle(double t) : t_(t)
+ {
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
+ const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+ double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
+
+ _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]);
+ }
+
+
+ /**
+ * @brief Set Obstacle for the underlying cost function
+ * @param obstacle Const pointer to an Obstacle or derived Obstacle
+ */
+ void setObstacle(const Obstacle* obstacle)
+ {
+ _measurement = obstacle;
+ }
+
+ /**
+ * @brief Set pointer to the robot model
+ * @param robot_model Robot model required for distance calculation
+ */
+ void setRobotModel(const BaseRobotFootprintModel* robot_model)
+ {
+ robot_model_ = robot_model;
+ }
+
+ /**
+ * @brief Set all parameters at once
+ * @param cfg TebConfig class
+ * @param robot_model Robot model required for distance calculation
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+ {
+ cfg_ = &cfg;
+ robot_model_ = robot_model;
+ _measurement = obstacle;
+ }
+
+protected:
+
+ const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+ double t_; //!< Estimated time until current pose is reached
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
new file mode 100755
index 0000000..012a865
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
@@ -0,0 +1,231 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef _EDGE_KINEMATICS_H
+#define _EDGE_KINEMATICS_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <cmath>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeKinematicsDiffDrive
+ * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot.
+ *
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
+ * of the non-holonomic constraint:
+ * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
+ *
+ * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
+ * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n
+ * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n
+ * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost,
+ * the second one backward-drive cost.
+ * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeKinematicsDiffDrive()
+ {
+ this->setMeasurement(0.);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+
+ Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+ // non holonomic constraint
+ _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+
+ // positive-drive-direction constraint
+ Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );
+ _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
+ // epsilon=0, otherwise it pushes the first bandpoints away from start
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+ }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 1
+ /**
+ * @brief Jacobi matrix of the cost function specified in computeError().
+ */
+ void linearizeOplus()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+
+ Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+ double cos1 = cos(conf1->theta());
+ double cos2 = cos(conf2->theta());
+ double sin1 = sin(conf1->theta());
+ double sin2 = sin(conf2->theta());
+ double aux1 = sin1 + sin2;
+ double aux2 = cos1 + cos2;
+
+ double dd_error_1 = deltaS[0]*cos1;
+ double dd_error_2 = deltaS[1]*sin1;
+ double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
+
+ double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] -
+ ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+
+ // conf1
+ _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
+ _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1
+ _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1
+ _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1
+ _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle
+ _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1
+
+ // conf2
+ _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2
+ _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2
+ _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2
+ _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2
+ _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle
+ _jacobianOplusXj(1,2) = 0; // drive-dir angle1
+ }
+#endif
+#endif
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+/**
+ * @class EdgeKinematicsCarlike
+ * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot.
+ *
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
+ * of the non-holonomic constraint:
+ * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
+ *
+ * The definition is identically to the one of the differential drive robot.
+ * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots.
+ * The turning radius is defined by \f$ r=v/omega \f$.
+ *
+ * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
+ * The second equation enforces a minimum turning radius.
+ * The \e weight can be set using setInformation(): Matrix element 2,2. \n
+ * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost,
+ * the second one backward-drive cost and the third one the minimum turning radius
+ * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive
+ * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter,
+ * the user might add an extra margin to the min_turning_radius param.
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeKinematicsCarlike()
+ {
+ this->setMeasurement(0.);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+
+ Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+ // non holonomic constraint
+ _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+
+ // limit minimum turning radius
+ double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
+ if (angle_diff == 0)
+ _error[1] = 0; // straight line motion
+ else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius
+ _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0);
+ else
+ _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0);
+ // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter.
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
new file mode 100755
index 0000000..23c8bc4
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
@@ -0,0 +1,295 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_OBSTACLE_H_
+#define EDGE_OBSTACLE_H_
+
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeObstacle
+ * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
+ *
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */
+class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeObstacle()
+ {
+ _measurement = NULL;
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()");
+ const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+ double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+
+ // Original obstacle cost.
+ _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+
+ if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
+ {
+ // Optional non-linear cost. Note the max cost (before weighting) is
+ // the same as the straight line version and that all other costs are
+ // below the straight line (for positive exponent), so it may be
+ // necessary to increase weight_obstacle and/or the inflation_weight
+ // when using larger exponents.
+ _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
+ }
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]);
+ }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0
+
+ /**
+ * @brief Jacobi matrix of the cost function specified in computeError().
+ */
+ void linearizeOplus()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()");
+ const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+ Eigen::Vector2d deltaS = *_measurement - bandpt->position();
+ double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta();
+
+ double dist_squared = deltaS.squaredNorm();
+ double dist = sqrt(dist_squared);
+
+ double aux0 = sin(angdiff);
+ double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon);
+
+ if (dev_left_border==0)
+ {
+ _jacobianOplusXi( 0 , 0 ) = 0;
+ _jacobianOplusXi( 0 , 1 ) = 0;
+ _jacobianOplusXi( 0 , 2 ) = 0;
+ return;
+ }
+
+ double aux1 = -fabs(aux0) / dist;
+ double dev_norm_x = deltaS[0]*aux1;
+ double dev_norm_y = deltaS[1]*aux1;
+
+ double aux2 = cos(angdiff) * g2o::sign(aux0);
+ double aux3 = aux2 / dist_squared;
+ double dev_proj_x = aux3 * deltaS[1] * dist;
+ double dev_proj_y = -aux3 * deltaS[0] * dist;
+ double dev_proj_angle = -aux2;
+
+ _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x );
+ _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y );
+ _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle;
+ }
+#endif
+#endif
+
+ /**
+ * @brief Set pointer to associated obstacle for the underlying cost function
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setObstacle(const Obstacle* obstacle)
+ {
+ _measurement = obstacle;
+ }
+
+ /**
+ * @brief Set pointer to the robot model
+ * @param robot_model Robot model required for distance calculation
+ */
+ void setRobotModel(const BaseRobotFootprintModel* robot_model)
+ {
+ robot_model_ = robot_model;
+ }
+
+ /**
+ * @brief Set all parameters at once
+ * @param cfg TebConfig class
+ * @param robot_model Robot model required for distance calculation
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+ {
+ cfg_ = &cfg;
+ robot_model_ = robot_model;
+ _measurement = obstacle;
+ }
+
+protected:
+
+ const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+/**
+ * @class EdgeInflatedObstacle
+ * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles.
+ *
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n
+ * Additional, a second penalty is provided with \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$.
+ * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation.
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */
+class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeInflatedObstacle()
+ {
+ _measurement = NULL;
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()");
+ const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+ double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+
+ // Original "straight line" obstacle cost. The max possible value
+ // before weighting is min_obstacle_dist
+ _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+
+ if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
+ {
+ // Optional non-linear cost. Note the max cost (before weighting) is
+ // the same as the straight line version and that all other costs are
+ // below the straight line (for positive exponent), so it may be
+ // necessary to increase weight_obstacle and/or the inflation_weight
+ // when using larger exponents.
+ _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
+ }
+
+ // Additional linear inflation cost
+ _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0);
+
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]);
+ }
+
+ /**
+ * @brief Set pointer to associated obstacle for the underlying cost function
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setObstacle(const Obstacle* obstacle)
+ {
+ _measurement = obstacle;
+ }
+
+ /**
+ * @brief Set pointer to the robot model
+ * @param robot_model Robot model required for distance calculation
+ */
+ void setRobotModel(const BaseRobotFootprintModel* robot_model)
+ {
+ robot_model_ = robot_model;
+ }
+
+ /**
+ * @brief Set all parameters at once
+ * @param cfg TebConfig class
+ * @param robot_model Robot model required for distance calculation
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+ {
+ cfg_ = &cfg;
+ robot_model_ = robot_model;
+ _measurement = obstacle;
+ }
+
+protected:
+
+ const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
new file mode 100755
index 0000000..6e5ec9e
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
@@ -0,0 +1,115 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_PREFER_ROTDIR_H_
+#define EDGE_PREFER_ROTDIR_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "g2o/core/base_unary_edge.h"
+#include "teb_local_planner/misc.h"
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgePreferRotDir
+ * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns
+ *
+ * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction
+ * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$)
+ * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n
+ * \e weight can be set using setInformation(). \n
+ * @see TebOptimalPlanner::AddEdgePreferRotDir
+ */
+class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgePreferRotDir()
+ {
+ _measurement = 1;
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+
+ _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]);
+ }
+
+ /**
+ * @brief Specify the prefered direction of rotation
+ * @param dir +1 to prefer the left side, -1 to prefer the right side
+ */
+ void setRotDir(double dir)
+ {
+ _measurement = dir;
+ }
+
+ /** Prefer rotations to the right */
+ void preferRight() {_measurement = -1;}
+
+ /** Prefer rotations to the right */
+ void preferLeft() {_measurement = 1;}
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
new file mode 100644
index 0000000..8d5bb87
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
@@ -0,0 +1,88 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_SHORTEST_PATH_H_
+#define EDGE_SHORTEST_PATH_H_
+
+#include <float.h>
+
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/misc.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner {
+
+/**
+ * @class EdgeShortestPath
+ * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses.
+ *
+ * @see TebOptimalPlanner::AddEdgesShortestPath
+ */
+class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> {
+public:
+ /**
+ * @brief Construct edge.
+ */
+ EdgeShortestPath() { this->setMeasurement(0.); }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError() {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()");
+ const VertexPose *pose1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose *pose2 = static_cast<const VertexPose*>(_vertices[1]);
+ _error[0] = (pose2->position() - pose1->position()).norm();
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]);
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // end namespace
+
+#endif /* EDGE_SHORTEST_PATH_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
new file mode 100644
index 0000000..0f7671b
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
@@ -0,0 +1,117 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_TIMEOPTIMAL_H_
+#define EDGE_TIMEOPTIMAL_H_
+
+#include <float.h>
+
+//#include <base_local_planner/BaseLocalPlannerConfig.h>
+
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class EdgeTimeOptimal
+ * @brief Edge defining the cost function for minimizing transition time of the trajectory.
+ *
+ * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n
+ * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n
+ * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n
+ * @see TebOptimalPlanner::AddEdgesTimeOptimal
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeTimeOptimal()
+ {
+ this->setMeasurement(0.);
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
+ const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);
+
+ _error[0] = timediff->dt();
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]);
+ }
+
+#ifdef USE_ANALYTIC_JACOBI
+ /**
+ * @brief Jacobi matrix of the cost function specified in computeError().
+ */
+ void linearizeOplus()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
+ _jacobianOplusXi( 0 , 0 ) = 1;
+ }
+#endif
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}; // end namespace
+
+#endif /* EDGE_TIMEOPTIMAL_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
new file mode 100755
index 0000000..29956c0
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
@@ -0,0 +1,275 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_VELOCITY_H
+#define EDGE_VELOCITY_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <rclcpp/logging.hpp>
+
+#include <exception>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class EdgeVelocity
+ * @brief Edge defining the cost function for limiting the translational and rotational velocity.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n
+ * \e v is calculated using the difference quotient and the position parts of both poses. \n
+ * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational velocity and
+ * the second one the rotational velocity.
+ * @see TebOptimalPlanner::AddEdgesVelocity
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeVelocity : public BaseTebMultiEdge<2, double>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeVelocity()
+ {
+ this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
+
+ double dist = deltaS.norm();
+ const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
+ if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+ {
+ double radius = dist/(2*sin(angle_diff/2));
+ dist = fabs( angle_diff * radius ); // actual arg length!
+ }
+ double vel = dist / deltaT->estimate();
+
+// vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
+ vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+
+ const double omega = angle_diff / deltaT->estimate();
+
+ _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+ }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
+ // Change accordingly...
+
+ /**
+ * @brief Jacobi matrix of the cost function specified in computeError().
+ */
+ void linearizeOplus()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+ double dist = deltaS.norm();
+ double aux1 = dist*deltaT->estimate();
+ double aux2 = 1/deltaT->estimate();
+
+ double vel = dist * aux2;
+ double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
+
+ double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
+ double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+ _jacobianOplus[0].resize(2,3); // conf1
+ _jacobianOplus[1].resize(2,3); // conf2
+ _jacobianOplus[2].resize(2,1); // deltaT
+
+// if (aux1==0) aux1=1e-6;
+// if (aux2==0) aux2=1e-6;
+
+ if (dev_border_vel!=0)
+ {
+ double aux3 = dev_border_vel / aux1;
+ _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1
+ _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1
+ _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2
+ _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2
+ _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT
+ }
+ else
+ {
+ _jacobianOplus[0](0,0) = 0; // vel x1
+ _jacobianOplus[0](0,1) = 0; // vel y1
+ _jacobianOplus[1](0,0) = 0; // vel x2
+ _jacobianOplus[1](0,1) = 0; // vel y2
+ _jacobianOplus[2](0,0) = 0; // vel deltaT
+ }
+
+ if (dev_border_omega!=0)
+ {
+ double aux4 = aux2 * dev_border_omega;
+ _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT
+ _jacobianOplus[0](1,2) = -aux4; // omega angle1
+ _jacobianOplus[1](1,2) = aux4; // omega angle2
+ }
+ else
+ {
+ _jacobianOplus[2](1,0) = 0; // omega deltaT
+ _jacobianOplus[0](1,2) = 0; // omega angle1
+ _jacobianOplus[1](1,2) = 0; // omega angle2
+ }
+
+ _jacobianOplus[0](1,0) = 0; // omega x1
+ _jacobianOplus[0](1,1) = 0; // omega y1
+ _jacobianOplus[1](1,0) = 0; // omega x2
+ _jacobianOplus[1](1,1) = 0; // omega y2
+ _jacobianOplus[0](0,2) = 0; // vel angle1
+ _jacobianOplus[1](0,2) = 0; // vel angle2
+ }
+#endif
+#endif
+
+
+public:
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+
+
+
+/**
+ * @class EdgeVelocityHolonomic
+ * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n
+ * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n
+ * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n
+ * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis,
+ * the second one w.r.t. the y-axis and the third one the rotational velocity.
+ * @see TebOptimalPlanner::AddEdgesVelocity
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeVelocityHolonomic()
+ {
+ this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+ Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+ double cos_theta1 = std::cos(conf1->theta());
+ double sin_theta1 = std::sin(conf1->theta());
+
+ // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
+ double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
+ double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
+
+ double vx = r_dx / deltaT->estimate();
+ double vy = r_dy / deltaT->estimate();
+ double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
+
+ _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
+ _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
+ _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
+ "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);
+ }
+
+
+public:
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
new file mode 100644
index 0000000..1dff242
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
@@ -0,0 +1,127 @@
+#pragma once
+
+
+#include <teb_local_planner/optimal_planner.h>
+#include <teb_local_planner/g2o_types/base_teb_edges.h>
+#include <teb_local_planner/g2o_types/vertex_timediff.h>
+#include <teb_local_planner/g2o_types/vertex_pose.h>
+#include <teb_local_planner/robot_footprint_model.h>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class EdgeVelocityObstacleRatio
+ * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
+ *
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */
+class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeVelocityObstacleRatio() :
+ robot_model_(nullptr)
+ {
+ // The three vertices are two poses and one time difference
+ this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()");
+ const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+ const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+ const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+ const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
+
+ double dist = deltaS.norm();
+ const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
+ if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+ {
+ double radius = dist/(2*sin(angle_diff/2));
+ dist = fabs( angle_diff * radius ); // actual arg length!
+ }
+ double vel = dist / deltaT->estimate();
+
+ vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+
+ const double omega = angle_diff / deltaT->estimate();
+
+ double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement);
+
+ double ratio;
+ if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound)
+ ratio = 0;
+ else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound)
+ ratio = 1;
+ else
+ ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) /
+ (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound);
+ ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel;
+
+ const double max_vel_fwd = ratio * cfg_->robot.max_vel_x;
+ const double max_omega = ratio * cfg_->robot.max_vel_theta;
+ _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0);
+ _error[1] = penaltyBoundToInterval(omega, max_omega, 0);
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::computeError() _error[0]=%f , _error[1]=%f\n",_error[0],_error[1]);
+ }
+
+ /**
+ * @brief Set pointer to associated obstacle for the underlying cost function
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setObstacle(const Obstacle* obstacle)
+ {
+ _measurement = obstacle;
+ }
+
+ /**
+ * @brief Set pointer to the robot model
+ * @param robot_model Robot model required for distance calculation
+ */
+ void setRobotModel(const BaseRobotFootprintModel* robot_model)
+ {
+ robot_model_ = robot_model;
+ }
+
+ /**
+ * @brief Set all parameters at once
+ * @param cfg TebConfig class
+ * @param robot_model Robot model required for distance calculation
+ * @param obstacle 2D position vector containing the position of the obstacle
+ */
+ void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+ {
+ cfg_ = &cfg;
+ robot_model_ = robot_model;
+ _measurement = obstacle;
+ }
+
+protected:
+
+ const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+} // end namespace
+
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
new file mode 100755
index 0000000..5575d0f
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
@@ -0,0 +1,121 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_VIA_POINT_H_
+#define EDGE_VIA_POINT_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/misc.h"
+
+#include "g2o/core/base_unary_edge.h"
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeViaPoint
+ * @brief Edge defining the cost function for pushing a configuration towards a via point
+ *
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min dist2point \cdot weight \f$. \n
+ * \e dist2point denotes the distance to the via point. \n
+ * \e weight can be set using setInformation(). \n
+ * @see TebOptimalPlanner::AddEdgesViaPoints
+ * @remarks Do not forget to call setTebConfig() and setViaPoint()
+ */
+class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
+{
+public:
+
+ /**
+ * @brief Construct edge.
+ */
+ EdgeViaPoint()
+ {
+ _measurement = NULL;
+ }
+
+ /**
+ * @brief Actual cost function
+ */
+ void computeError()
+ {
+ TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
+ const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+ _error[0] = (bandpt->position() - *_measurement).norm();
+
+ TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
+ }
+
+ /**
+ * @brief Set pointer to associated via point for the underlying cost function
+ * @param via_point 2D position vector containing the position of the via point
+ */
+ void setViaPoint(const Eigen::Vector2d* via_point)
+ {
+ _measurement = via_point;
+ }
+
+ /**
+ * @brief Set all parameters at once
+ * @param cfg TebConfig class
+ * @param via_point 2D position vector containing the position of the via point
+ */
+ void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
+ {
+ cfg_ = &cfg;
+ _measurement = via_point;
+ }
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+} // end namespace
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
new file mode 100755
index 0000000..6870558
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
@@ -0,0 +1,193 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef PENALTIES_H
+#define PENALTIES_H
+
+#include <cmath>
+#include <Eigen/Core>
+#include <g2o/stuff/misc.h>
+
+namespace teb_local_planner
+{
+
+/**
+ * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
+ * @param var The scalar that should be bounded
+ * @param a lower and upper absolute bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToIntervalDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
+{
+ if (var < -a+epsilon)
+ {
+ return (-var - (a - epsilon));
+ }
+ if (var <= a-epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return (var - (a - epsilon));
+ }
+}
+
+/**
+ * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param b upper bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToIntervalDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
+{
+ if (var < a+epsilon)
+ {
+ return (-var + (a + epsilon));
+ }
+ if (var <= b-epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return (var - (b - epsilon));
+ }
+}
+
+
+/**
+ * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundFromBelowDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
+{
+ if (var >= a+epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return (-var + (a+epsilon));
+ }
+}
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
+ * @param var The scalar that should be bounded
+ * @param a lower and upper absolute bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToInterval
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
+{
+ if (var < -a+epsilon)
+ {
+ return -1;
+ }
+ if (var <= a-epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return 1;
+ }
+}
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param b upper bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToInterval
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon)
+{
+ if (var < a+epsilon)
+ {
+ return -1;
+ }
+ if (var <= b-epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return 1;
+ }
+}
+
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundFromBelow
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon)
+{
+ if (var >= a+epsilon)
+ {
+ return 0.;
+ }
+ else
+ {
+ return -1;
+ }
+}
+
+
+} // namespace teb_local_planner
+
+
+#endif // PENALTIES_H
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
new file mode 100755
index 0000000..781a272
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
@@ -0,0 +1,229 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef VERTEX_POSE_H_
+#define VERTEX_POSE_H_
+
+#include <g2o/config.h>
+#include <g2o/core/base_vertex.h>
+#include <g2o/core/hyper_graph_action.h>
+#include <g2o/stuff/misc.h>
+
+#include "teb_local_planner/pose_se2.h"
+
+namespace teb_local_planner
+{
+
+/**
+ * @class VertexPose
+ * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o
+ * @see PoseSE2
+ * @see VertexTimeDiff
+ */
+class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
+{
+public:
+
+ /**
+ * @brief Default constructor
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexPose(bool fixed = false)
+ {
+ setToOriginImpl();
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Construct pose using a given PoseSE2
+ * @param pose PoseSE2 defining the pose [x, y, angle_rad]
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexPose(const PoseSE2& pose, bool fixed = false)
+ {
+ _estimate = pose;
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Construct pose using a given 2D position vector and orientation
+ * @param position Eigen::Vector2d containing x and y coordinates
+ * @param theta yaw-angle
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed = false)
+ {
+ _estimate.position() = position;
+ _estimate.theta() = theta;
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Construct pose using single components x, y, and the yaw angle
+ * @param x x-coordinate
+ * @param y y-coordinate
+ * @param theta yaw angle in rad
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexPose(double x, double y, double theta, bool fixed = false)
+ {
+ _estimate.x() = x;
+ _estimate.y() = y;
+ _estimate.theta() = theta;
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Access the pose
+ * @see estimate
+ * @return reference to the PoseSE2 estimate
+ */
+ inline PoseSE2& pose() {return _estimate;}
+
+ /**
+ * @brief Access the pose (read-only)
+ * @see estimate
+ * @return const reference to the PoseSE2 estimate
+ */
+ inline const PoseSE2& pose() const {return _estimate;}
+
+
+ /**
+ * @brief Access the 2D position part
+ * @see estimate
+ * @return reference to the 2D position part
+ */
+ inline Eigen::Vector2d& position() {return _estimate.position();}
+
+ /**
+ * @brief Access the 2D position part (read-only)
+ * @see estimate
+ * @return const reference to the 2D position part
+ */
+ inline const Eigen::Vector2d& position() const {return _estimate.position();}
+
+ /**
+ * @brief Access the x-coordinate the pose
+ * @return reference to the x-coordinate
+ */
+ inline double& x() {return _estimate.x();}
+
+ /**
+ * @brief Access the x-coordinate the pose (read-only)
+ * @return const reference to the x-coordinate
+ */
+ inline const double& x() const {return _estimate.x();}
+
+ /**
+ * @brief Access the y-coordinate the pose
+ * @return reference to the y-coordinate
+ */
+ inline double& y() {return _estimate.y();}
+
+ /**
+ * @brief Access the y-coordinate the pose (read-only)
+ * @return const reference to the y-coordinate
+ */
+ inline const double& y() const {return _estimate.y();}
+
+ /**
+ * @brief Access the orientation part (yaw angle) of the pose
+ * @return reference to the yaw angle
+ */
+ inline double& theta() {return _estimate.theta();}
+
+ /**
+ * @brief Access the orientation part (yaw angle) of the pose (read-only)
+ * @return const reference to the yaw angle
+ */
+ inline const double& theta() const {return _estimate.theta();}
+
+ /**
+ * @brief Set the underlying estimate (2D vector) to zero.
+ */
+ virtual void setToOriginImpl() override
+ {
+ _estimate.setZero();
+ }
+
+ /**
+ * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$.
+ * A simple addition for the position.
+ * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$
+ * @param update increment that should be added to the previous esimate
+ */
+ virtual void oplusImpl(const double* update) override
+ {
+ _estimate.plus(update);
+ }
+
+ /**
+ * @brief Read an estimate from an input stream.
+ * First the x-coordinate followed by y and the yaw angle.
+ * @param is input stream
+ * @return always \c true
+ */
+ virtual bool read(std::istream& is) override
+ {
+ is >> _estimate.x() >> _estimate.y() >> _estimate.theta();
+ return true;
+ }
+
+ /**
+ * @brief Write the estimate to an output stream
+ * First the x-coordinate followed by y and the yaw angle.
+ * @param os output stream
+ * @return \c true if the export was successful, otherwise \c false
+ */
+ virtual bool write(std::ostream& os) const override
+ {
+ os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta();
+ return os.good();
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}
+
+#endif // VERTEX_POSE_H_
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
new file mode 100755
index 0000000..4eead6c
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
@@ -0,0 +1,145 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef VERTEX_TIMEDIFF_H
+#define VERTEX_TIMEDIFF_H
+
+
+#include "g2o/config.h"
+#include "g2o/core/base_vertex.h"
+#include "g2o/core/hyper_graph_action.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class VertexTimeDiff
+ * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o
+ */
+class VertexTimeDiff : public g2o::BaseVertex<1, double>
+{
+public:
+
+ /**
+ * @brief Default constructor
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexTimeDiff(bool fixed = false)
+ {
+ setToOriginImpl();
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Construct the TimeDiff vertex with a value
+ * @param dt time difference value of the vertex
+ * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+ */
+ VertexTimeDiff(double dt, bool fixed = false)
+ {
+ _estimate = dt;
+ setFixed(fixed);
+ }
+
+ /**
+ * @brief Access the timediff value of the vertex
+ * @see estimate
+ * @return reference to dt
+ */
+ inline double& dt() {return _estimate;}
+
+ /**
+ * @brief Access the timediff value of the vertex (read-only)
+ * @see estimate
+ * @return const reference to dt
+ */
+ inline const double& dt() const {return _estimate;}
+
+ /**
+ * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default.
+ */
+ virtual void setToOriginImpl() override
+ {
+ _estimate = 0.1;
+ }
+
+ /**
+ * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$.
+ * A simple addition implements what we want.
+ * @param update increment that should be added to the previous esimate
+ */
+ virtual void oplusImpl(const double* update) override
+ {
+ _estimate += *update;
+ }
+
+ /**
+ * @brief Read an estimate of \f$ \Delta T \f$ from an input stream
+ * @param is input stream
+ * @return always \c true
+ */
+ virtual bool read(std::istream& is) override
+ {
+ is >> _estimate;
+ return true;
+ }
+
+ /**
+ * @brief Write the estimate \f$ \Delta T \f$ to an output stream
+ * @param os output stream
+ * @return \c true if the export was successful, otherwise \c false
+ */
+ virtual bool write(std::ostream& os) const override
+ {
+ os << estimate();
+ return os.good();
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/graph_search.h b/src/teb_local_planner/include/teb_local_planner/graph_search.h
new file mode 100644
index 0000000..3c4bd7c
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/graph_search.h
@@ -0,0 +1,215 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef GRAPH_SEARCH_INTERFACE_H
+#define GRAPH_SEARCH_INTERFACE_H
+
+#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+ #include <boost/graph/adjacency_list.hpp>
+#else
+ // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
+ // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
+ // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
+ #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+ #include <boost/graph/adjacency_list.hpp>
+ #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+#endif
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/depth_first_search.hpp>
+#include <boost/utility.hpp>
+#include <boost/random.hpp>
+
+#include <Eigen/Core>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/pose_se2.h"
+#include "teb_local_planner/teb_config.h"
+
+namespace teb_local_planner
+{
+
+class HomotopyClassPlanner; // Forward declaration
+
+//! Vertex in the graph that is used to find homotopy classes (only stores 2D positions)
+struct HcGraphVertex
+{
+public:
+ Eigen::Vector2d pos; // position of vertices in the map
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//! Abbrev. for the homotopy class search-graph type @see HcGraphVertex
+typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
+//! Abbrev. for vertex type descriptors in the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
+//! Abbrev. for edge type descriptors in the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
+//! Abbrev. for the vertices iterator of the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
+//! Abbrev. for the edges iterator of the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
+//! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one
+typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;
+
+//!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors
+inline std::complex<long double> getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
+{
+ return std::complex<long double>(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y());
+}
+
+//!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors
+inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
+{
+ return graph[vert_descriptor].pos;
+}
+
+/**
+ * @brief Base class for graph based path planning / homotopy class sampling
+ */
+class GraphSearchInterface
+{
+public:
+
+ virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false) = 0;
+
+ /**
+ * @brief Clear any existing graph of the homotopy class search
+ */
+ void clearGraph() {graph_.clear();}
+
+ // HcGraph graph() const {return graph_;}
+ // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above.
+ HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes.
+
+protected:
+ /**
+ * @brief Protected constructor that should be called by subclasses
+ */
+ GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){}
+
+ /**
+ * @brief Depth First Search implementation to find all paths between the start and the specified goal vertex.
+ *
+ * Complete paths are stored to the internal path container.
+ * @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/
+ * @param g Graph on which the depth first should be performed
+ * @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
+ * @param goal Desired goal vertex
+ * @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB
+ * @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ */
+
+ void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+protected:
+ const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+ HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding.
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+class lrKeyPointGraph : public GraphSearchInterface
+{
+public:
+ lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
+
+ virtual ~lrKeyPointGraph(){}
+
+ /**
+ * @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.
+ *
+ * This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading. \n
+ * All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n
+ * This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph()
+ * method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths.
+ *
+ * @see createProbRoadmapGraph
+ * @param start Start pose from wich to start on (e.g. the current robot pose).
+ * @param goal Goal pose to find paths to (e.g. the robot's goal).
+ * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+ * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ */
+ virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+};
+
+
+
+
+class ProbRoadmapGraph : public GraphSearchInterface
+{
+public:
+ ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
+
+ virtual ~ProbRoadmapGraph(){}
+
+
+ /**
+ * @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
+ *
+ * This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n
+ * Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n
+ * Use the sampling method for complex, non-point or huge obstacles. \n
+ * You may call createGraph() instead.
+ *
+ * @see createGraph
+ * @param start Start pose from wich to start on (e.g. the current robot pose).
+ * @param goal Goal pose to find paths to (e.g. the robot's goal).
+ * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+ * @param no_samples number of random samples
+ * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ */
+ virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+private:
+ boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints.
+};
+} // end namespace
+
+#endif // GRAPH_SEARCH_INTERFACE_H
diff --git a/src/teb_local_planner/include/teb_local_planner/h_signature.h b/src/teb_local_planner/include/teb_local_planner/h_signature.h
new file mode 100644
index 0000000..8a11f0f
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/h_signature.h
@@ -0,0 +1,432 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef H_SIGNATURE_H_
+#define H_SIGNATURE_H_
+
+#include <boost/math/special_functions.hpp>
+
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/misc.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/timed_elastic_band.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <math.h>
+#include <algorithm>
+#include <functional>
+#include <vector>
+#include <iterator>
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @brief The H-signature defines an equivalence relation based on homology in terms of complex calculus.
+ *
+ * The H-Signature depends on the obstacle configuration and can be utilized
+ * to check whether two trajectores belong to the same homology class.
+ * Refer to: \n
+ * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
+ */
+class HSignature : public EquivalenceClass
+{
+
+public:
+
+ /**
+ * @brief Constructor accepting a TebConfig
+ * @param cfg TebConfig storing some user configuration options
+ */
+ HSignature(const TebConfig& cfg) : cfg_(&cfg) {}
+
+
+ /**
+ * @brief Calculate the H-Signature of a path
+ *
+ * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
+ * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
+ * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
+ * that returns a complex value for the position (Re(*)=x, Im(*)=y).
+ *
+ * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
+ *
+ * @param path_start Iterator to the first element in the path
+ * @param path_end Iterator to the last element in the path
+ * @param obstacles obstacle container
+ * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+ * @tparam BidirIter Bidirectional iterator type
+ * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+ */
+ template<typename BidirIter, typename Fun>
+ void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles)
+ {
+ if (obstacles->empty())
+ {
+ hsignature_ = std::complex<double>(0,0);
+ return;
+ }
+
+
+ TEB_ASSERT_MSG(cfg_->hcp.h_signature_prescaler>0.1 && cfg_->hcp.h_signature_prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
+
+ // guess values for f0
+ // paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
+ int m = std::max( (int)obstacles->size()-1, 5 ); // for only a few obstacles we need a min threshold in order to get significantly high H-Signatures
+
+ int a = (int) std::ceil(double(m)/2.0);
+ int b = m-a;
+
+ std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
+
+ typedef std::complex<long double> cplx;
+ // guess map size (only a really really coarse guess is required
+ // use distance from start to goal as distance to each direction
+ // TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
+ cplx start = fun_cplx_point(*path_start);
+ cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
+ cplx delta = end-start;
+ cplx normal(-delta.imag(), delta.real());
+ cplx map_bottom_left;
+ cplx map_top_right;
+ if (std::abs(delta) < 3.0)
+ { // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
+ map_bottom_left = start + cplx(0, -3);
+ map_top_right = start + cplx(3, 3);
+ }
+ else
+ {
+ map_bottom_left = start - normal;
+ map_top_right = start + delta + normal;
+ }
+
+ hsignature_ = 0; // reset local signature
+
+ std::vector<double> imag_proposals(5);
+
+ // iterate path
+ while(path_start != path_end)
+ {
+ cplx z1 = fun_cplx_point(*path_start);
+ cplx z2 = fun_cplx_point(*std::next(path_start));
+
+ for (std::size_t l=0; l<obstacles->size(); ++l) // iterate all obstacles
+ {
+ cplx obst_l = obstacles->at(l)->getCentroidCplx();
+ //cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
+ cplx f0 = (long double) cfg_->hcp.h_signature_prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
+
+ // denum contains product with all obstacles exepct j==l
+ cplx Al = f0;
+ for (std::size_t j=0; j<obstacles->size(); ++j)
+ {
+ if (j==l)
+ continue;
+ cplx obst_j = obstacles->at(j)->getCentroidCplx();
+ cplx diff = obst_l - obst_j;
+ //if (diff.real()!=0 || diff.imag()!=0)
+ if (std::abs(diff)<0.05) // skip really close obstacles
+ continue;
+ else
+ Al /= diff;
+ }
+ // compute log value
+ double diff2 = std::abs(z2-obst_l);
+ double diff1 = std::abs(z1-obst_l);
+ if (diff2 == 0 || diff1 == 0)
+ continue;
+ double log_real = std::log(diff2)-std::log(diff1);
+ // complex ln has more than one solution -> choose minimum abs angle -> paper
+ double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l);
+ imag_proposals.at(0) = arg_diff;
+ imag_proposals.at(1) = arg_diff+2*M_PI;
+ imag_proposals.at(2) = arg_diff-2*M_PI;
+ imag_proposals.at(3) = arg_diff+4*M_PI;
+ imag_proposals.at(4) = arg_diff-4*M_PI;
+ double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
+ cplx log_value(log_real,log_imag);
+ //cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
+ hsignature_ += Al*log_value;
+ }
+ ++path_start;
+ }
+ }
+
+
+ /**
+ * @brief Check if two candidate classes are equivalent
+ * @param other The other equivalence class to test with
+ */
+ virtual bool isEqual(const EquivalenceClass& other) const
+ {
+ const HSignature* hother = dynamic_cast<const HSignature*>(&other); // TODO: better architecture without dynamic_cast
+ if (hother)
+ {
+ double diff_real = std::abs(hother->hsignature_.real() - hsignature_.real());
+ double diff_imag = std::abs(hother->hsignature_.imag() - hsignature_.imag());
+ if (diff_real<=cfg_->hcp.h_signature_threshold && diff_imag<=cfg_->hcp.h_signature_threshold)
+ return true; // Found! Homotopy class already exists, therefore nothing added
+ }
+ else
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature equivalence classes with types other than HSignature.");
+
+ return false;
+ }
+
+ /**
+ * @brief Check if the equivalence value is detected correctly
+ * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+ */
+ virtual bool isValid() const
+ {
+ return std::isfinite(hsignature_.real()) && std::isfinite(hsignature_.imag());
+ }
+
+ /**
+ * @brief Check if the trajectory is non-looping around an obstacle.
+ * @return Returns always true, as this cannot be detected by this kind of H-Signature.
+ */
+ virtual bool isReasonable() const
+ {
+ return true;
+ }
+
+ /**
+ * @brief Get the current value of the h-signature (read-only)
+ * @return h-signature in complex-number format
+ */
+ const std::complex<long double>& value() const {return hsignature_;}
+
+
+private:
+
+ const TebConfig* cfg_;
+ std::complex<long double> hsignature_;
+};
+
+
+
+
+
+/**
+ * @brief The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism.
+ *
+ * The H-Signature depends on the obstacle configuration and can be utilized
+ * to check whether two trajectores belong to the same homology class.
+ * Refer to: \n
+ * - S. Bhattacharya et al.: Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D, 2011
+ */
+class HSignature3d : public EquivalenceClass
+{
+public:
+ /**
+ * @brief Constructor accepting a TebConfig
+ * @param cfg TebConfig storing some user configuration options
+ */
+ HSignature3d(const TebConfig& cfg) : cfg_(&cfg) {}
+
+
+ /**
+ * @brief Calculate the H-Signature of a path
+ *
+ * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
+ * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
+ * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
+ * that returns a complex value for the position (Re(*)=x, Im(*)=y).
+ *
+ * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
+ *
+ * @param path_start Iterator to the first element in the path
+ * @param path_end Iterator to the last element in the path
+ * @param obstacles obstacle container
+ * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+ * @tparam BidirIter Bidirectional iterator type
+ * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+ */
+ template<typename BidirIter, typename Fun>
+ void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
+ boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
+ {
+ hsignature3d_.resize(obstacles->size());
+
+ std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points)
+
+ constexpr int num_int_steps_per_segment = 10;
+
+ for (std::size_t l = 0; l < obstacles->size(); ++l) // iterate all obstacles
+ {
+ double H = 0;
+ double transition_time = 0;
+ double next_transition_time = 0;
+ BidirIter path_iter;
+ TimeDiffSequence::iterator timediff_iter;
+
+ Eigen::Vector3d s1 (obstacles->at(l)->getCentroid()(0), obstacles->at(l)->getCentroid()(1), 0);
+ double t = 120; // some large value for defining the end point of the obstacle/"conductor" model
+ Eigen::Vector3d s2;
+ obstacles->at(l)->predictCentroidConstantVelocity(t, s2.head(2));
+ s2[2] = t;
+ Eigen::Vector3d ds = s2 - s1;
+ double ds_sq_norm = ds.squaredNorm(); // by definition not zero as t > 0 (3rd component)
+
+ // iterate path
+ for (path_iter = path_start, timediff_iter = timediff_start.get(); path_iter != path_end; ++path_iter, ++timediff_iter)
+ {
+ std::complex<long double> z1 = fun_cplx_point(*path_iter);
+ std::complex<long double> z2 = fun_cplx_point(*std::next(path_iter));
+
+ transition_time = next_transition_time;
+ if (timediff_start == boost::none || timediff_end == boost::none) // if no time information is provided yet, approximate transition time
+ next_transition_time += std::abs(z2 - z1) / cfg_->robot.max_vel_x; // Approximate the time, if no time is known
+ else // otherwise use the time information from the teb trajectory
+ {
+ if (std::distance(path_iter, path_end) != std::distance(timediff_iter, timediff_end.get())) {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Size of poses and timediff vectors does not match. This is a bug.");
+ }
+ next_transition_time += (*timediff_iter)->dt();
+ }
+
+ Eigen::Vector3d direction_vec;
+ direction_vec[0] = z2.real() - z1.real();
+ direction_vec[1] = z2.imag() - z1.imag();
+ direction_vec[2] = next_transition_time - transition_time;
+
+ if(direction_vec.norm() < 1e-15) // Coincident poses
+ continue;
+
+ Eigen::Vector3d r(z1.real(), z1.imag(), transition_time);
+ Eigen::Vector3d dl = 1.0/static_cast<double>(num_int_steps_per_segment) * direction_vec; // Integrate with multiple steps between each pose
+ Eigen::Vector3d p1, p2, d, phi;
+ for (int i = 0; i < num_int_steps_per_segment; ++i, r += dl)
+ {
+ p1 = s1 - r;
+ p2 = s2 - r;
+ d = (ds.cross(p1.cross(p2))) / ds_sq_norm;
+ phi = 1.0 / d.squaredNorm() * ((d.cross(p2) / p2.norm()) - (d.cross(p1) / p1.norm()));
+ H += phi.dot(dl);
+ }
+ }
+
+ // normalize to 1
+ hsignature3d_.at(l) = H/(4.0*M_PI);
+ }
+ }
+
+ /**
+ * @brief Check if two candidate classes are equivalent
+ *
+ * If the absolute value of the H-Signature is equal or greater than 1, a loop (in x-y) around the obstacle is indicated.
+ * Positive H-Signature: Obstacle lies on the left hand side of the planned trajectory
+ * Negative H-Signature: Obstacle lies on the right hand side of the planned trajectory
+ * H-Signature equals zero: Obstacle lies in infinity, has no influence on trajectory
+ *
+ * @param other The other equivalence class to test with
+ */
+ virtual bool isEqual(const EquivalenceClass& other) const
+ {
+ const HSignature3d* hother = dynamic_cast<const HSignature3d*>(&other); // TODO: better architecture without dynamic_cast
+ if (hother)
+ {
+ if (hsignature3d_.size() == hother->hsignature3d_.size())
+ {
+ for(size_t i = 0; i < hsignature3d_.size(); ++i)
+ {
+ // If the H-Signature for one obstacle is below this threshold, that obstacle is far away from the planned trajectory,
+ // and therefore ignored in the homotopy class planning
+ if (std::abs(hother->hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold || std::abs(hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold)
+ continue;
+
+ if (boost::math::sign(hother->hsignature3d_.at(i)) != boost::math::sign(hsignature3d_.at(i)))
+ return false; // Signatures are not equal, new homotopy class
+ }
+ return true; // Found! Homotopy class already exists, therefore nothing added
+ }
+ }
+ else {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature3d equivalence classes with types other than HSignature3d.");
+ }
+
+ return false;
+ }
+
+ /**
+ * @brief Check if the equivalence value is detected correctly
+ * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+ */
+ virtual bool isValid() const
+ {
+ for(const double& value : hsignature3d_)
+ {
+ if (!std::isfinite(value))
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * @brief Check if the trajectory is non-looping around any obstacle. Values greater than 1 indicate a looping trajectory.
+ * @return Returns false, if the trajectory loops around an obstacle
+ */
+ virtual bool isReasonable() const
+ {
+ for(const double& value : hsignature3d_)
+ {
+ if (value > 1.0)
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * @brief Get the current h-signature (read-only)
+ * @return h-signature in complex-number format
+ */
+ const std::vector<double>& values() const {return hsignature3d_;}
+
+private:
+ const TebConfig* cfg_;
+ std::vector<double> hsignature3d_;
+};
+
+
+} // namespace teb_local_planner
+
+
+#endif /* H_SIGNATURE_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
new file mode 100644
index 0000000..841e630
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
@@ -0,0 +1,591 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef HOMOTOPY_CLASS_PLANNER_H_
+#define HOMOTOPY_CLASS_PLANNER_H_
+
+#include <math.h>
+
+#include <algorithm>
+#include <functional>
+#include <iterator>
+#include <memory>
+#include <vector>
+#include <iterator>
+#include <random>
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "teb_local_planner/planner_interface.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/optimal_planner.h"
+#include "teb_local_planner/visualization.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/graph_search.h"
+
+
+namespace teb_local_planner
+{
+
+//!< Inline function used for calculateHSignature() in combination with VertexPose pointers
+inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
+{
+ return std::complex<long double>(pose->x(), pose->y());
+};
+
+
+//!< Inline function used for calculateHSignature() in combination with geometry_msgs::msg::PoseStamped
+inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::msg::PoseStamped& pose)
+{
+ return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
+};
+
+/**
+ * @class HomotopyClassPlanner
+ * @brief Local planner that explores alternative homotopy classes, create a plan for each alternative
+ * and finally return the robot controls for the current best path (repeated in each sampling interval)
+ *
+ * Equivalence classes (e.g. homotopy) are explored using the help of a search-graph. \n
+ * A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate
+ * per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant: \n
+ * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
+ * - C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
+ *
+ * Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory
+ * optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand). \n
+ * Depending on the config parameters, the optimization is performed in parallel. \n
+ * After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the
+ * cost already contains important features like clearance from obstacles and transition time. \n
+ *
+ * Everyhting is performed by calling one of the overloaded plan() methods. \n
+ * Afterwards the velocity command to control the robot is obtained from the "best" candidate
+ * via getVelocityCommand(). \n
+ *
+ * All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories
+ * are preferred against new path initilizations in order to improve the hot-starting capability.
+ */
+class HomotopyClassPlanner : public PlannerInterface
+{
+public:
+
+ using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
+
+ /**
+ * @brief Default constructor
+ */
+ HomotopyClassPlanner();
+
+ /**
+ * @brief Construct and initialize the HomotopyClassPlanner
+ * @param node Shared pointer for rclcpp::Node
+ * @param cfg Const reference to the TebConfig class for internal parameters
+ * @param obstacles Container storing all relevant obstacles (see Obstacle)
+ * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
+ * @param visualization Shared pointer to the TebVisualization class (optional)
+ * @param via_points Container storing via-points (optional)
+ */
+ HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+ TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+ /**
+ * @brief Destruct the HomotopyClassPlanner.
+ */
+ virtual ~HomotopyClassPlanner();
+
+ /**
+ * @brief Initialize the HomotopyClassPlanner
+ * @param node Shared pointer for rclcpp::Node
+ * @param cfg Const reference to the TebConfig class for internal parameters
+ * @param obstacles Container storing all relevant obstacles (see Obstacle)
+ * @param visualization Shared pointer to the TebVisualization class (optional)
+ * @param via_points Container storing via-points (optional)
+ */
+ void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+ TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+ /** @name Plan a trajectory */
+ //@{
+
+ /**
+ * @brief Plan a trajectory based on an initial reference plan.
+ *
+ * Provide this method to create and optimize a trajectory that is initialized
+ * according to an initial reference plan (given as a container of poses).
+ * @warning The current implementation extracts only the start and goal pose and calls the overloaded plan()
+ * @param initial_plan vector of geometry_msgs::msg::PoseStamped (must be valid until clearPlanner() is called!)
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
+ *
+ * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+ * @param start tf::Pose containing the start pose of the trajectory
+ * @param goal tf::Pose containing the goal pose of the trajectory
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ // tf2 doesn't have tf::Pose
+// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose.
+ *
+ * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+ * @param start PoseSE2 containing the start pose of the trajectory
+ * @param goal PoseSE2 containing the goal pose of the trajectory
+ * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+ /**
+ * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
+ * @warning Call plan() first and check if the generated plan is feasible.
+ * @param[out] vx translational velocity [m/s]
+ * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
+ * @param[out] omega rotational velocity [rad/s]
+ * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
+ * @return \c true if command is valid, \c false otherwise
+ */
+ virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
+
+ /**
+ * @brief Access current best trajectory candidate (that relates to the "best" homotopy class).
+ *
+ * If no trajectory is available, the pointer will be empty.
+ * If only a single trajectory is available, return it.
+ * Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods).
+ * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+ */
+ TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
+
+ /**
+ * @brief Check whether the planned trajectory is feasible or not.
+ *
+ * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
+ * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
+ * @param costmap_model Pointer to the costmap model
+ * @param footprint_spec The specification of the footprint of the robot in world coordinates
+ * @param inscribed_radius The radius of the inscribed circle of the robot
+ * @param circumscribed_radius The radius of the circumscribed circle of the robot
+ * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
+ * @return \c true, if the robot footprint along the first part of the trajectory intersects with
+ * any obstacle in the costmap, \c false otherwise.
+ */
+ virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+ double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
+
+ /**
+ * @brief In case of empty best teb, scores again the available plans to find the best one.
+ * The best_teb_ variable is updated consequently.
+ * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+ * An empty pointer is returned if no plan is available.
+ */
+ TebOptimalPlannerPtr findBestTeb();
+
+ /**
+ * @brief Removes the specified teb and the corresponding homotopy class from the list of available ones.
+ * @param pointer to the teb Band to be removed
+ * @return Iterator to the next valid teb if available, else to the end of the tebs container.
+ */
+ TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);
+
+ //@}
+
+ /** @name Visualization */
+ //@{
+
+ /**
+ * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
+ * @param visualization shared pointer to a TebVisualization instance
+ * @see visualizeTeb
+ */
+ void setVisualization(const TebVisualizationPtr & visualization) override;
+
+ /**
+ * @brief Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
+ *
+ * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
+ * @see setVisualization
+ */
+ virtual void visualize();
+
+ //@}
+
+ /** @name Important steps that are called during planning */
+ //@{
+
+
+ /**
+ * @brief Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.
+ *
+ * This "all-in-one" method creates a graph with position keypoints from which
+ * feasible paths (with clearance from obstacles) are extracted. \n
+ * All obtained paths are filted to only keep a single path for each equivalence class. \n
+ * Each time a new equivalence class is explored (by means of \b no previous trajectory/TEB remain in that equivalence class),
+ * a new trajectory/TEB will be initialized. \n
+ *
+ * Everything is prepared now for the optimization step: see optimizeAllTEBs().
+ * @param start Current start pose (e.g. pose of the robot)
+ * @param goal Goal pose (e.g. robot's goal)
+ * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+ * @param @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ */
+ void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel = false);
+
+ /**
+ * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path
+ *
+ * Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters.
+ * @param path_start start iterator of a generic 2d path
+ * @param path_end end iterator of a generic 2d path
+ * @param fun_position unary function that returns the Eigen::Vector2d object
+ * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
+ * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ * @tparam BidirIter Bidirectional iterator type
+ * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
+ * @return Shared pointer to the newly created teb optimal planner
+ */
+ template<typename BidirIter, typename Fun>
+ TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+ /**
+ * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal
+ * @param start start pose
+ * @param goal goal pose
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ * @return Shared pointer to the newly created teb optimal planner
+ */
+ TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+ /**
+ * @brief Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container
+ * @param initial_plan container of poses (start and goal orientation should be valid!)
+ * @param start_velocity start velocity (optional)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+ * @return Shared pointer to the newly created teb optimal planner
+ */
+ TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+ /**
+ * @brief Update TEBs with new pose, goal and current velocity.
+ * @param start New start pose (optional)
+ * @param goal New goal pose (optional)
+ * @param start_velocity start velocity (optional)
+ */
+ void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity);
+
+
+ /**
+ * @brief Optimize all available trajectories by invoking the optimizer on each one.
+ *
+ * Depending on the configuration parameters, the optimization is performed either single or multi threaded.
+ * @param iter_innerloop Number of inner iterations (see TebOptimalPlanner::optimizeTEB())
+ * @param iter_outerloop Number of outer iterations (see TebOptimalPlanner::optimizeTEB())
+ */
+ void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
+
+ /**
+ * @brief Returns a shared pointer to the TEB related to the initial plan
+ * @return A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty.
+ */
+ TebOptimalPlannerPtr getInitialPlanTEB();
+
+ /**
+ * @brief In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
+ *
+ * The trajectory cost includes features such as transition time and clearance from obstacles. \n
+ * The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations.
+ * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+ */
+ TebOptimalPlannerPtr selectBestTeb();
+
+ //@}
+
+ /**
+ * @brief Reset the planner.
+ *
+ * Clear all previously found H-signatures, paths, tebs and the hcgraph.
+ */
+ virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
+
+
+ /**
+ * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
+ *
+ * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
+ * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
+ * Initial means that the penalty is applied only to the first few poses of the trajectory.
+ * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
+ */
+ virtual void setPreferredTurningDir(RotType dir);
+
+ /**
+ * @brief Calculate the equivalence class of a path
+ *
+ * Currently, only the H-signature (refer to HSignature) is implemented.
+ *
+ * @param path_start Iterator to the first element in the path
+ * @param path_end Iterator to the last element in the path
+ * @param obstacles obstacle container
+ * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+ * @tparam BidirIter Bidirectional iterator type
+ * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+ * @return pointer to the equivalence class base type
+ */
+ template<typename BidirIter, typename Fun>
+ EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
+ boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);
+
+ /**
+ * @brief Read-only access to the internal trajectory container.
+ * @return read-only reference to the teb container.
+ */
+ const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;}
+
+ bool hasDiverged() const override;
+
+ /**
+ * Compute and return the cost of the current optimization graph (supports multiple trajectories)
+ * @param[out] cost current cost value for each trajectory
+ * [for a planner with just a single trajectory: size=1, vector will not be cleared]
+ * @param obst_cost_scale Specify extra scaling for obstacle costs
+ * @param viapoint_cost_scale Specify extra scaling for via points.
+ * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
+ */
+ virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
+
+ /**
+ * @brief Check if two h-signatures are similar (w.r.t. a certain threshold)
+ * @param h1 first h-signature
+ * @param h2 second h-signature
+ * @return \c true if both h-signatures are similar, false otherwise.
+ */
+ inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
+ {
+ double diff_real = std::abs(h2.real() - h1.real());
+ double diff_imag = std::abs(h2.imag() - h1.imag());
+ if (diff_real<=threshold && diff_imag<=threshold)
+ return true; // Found! Homotopy class already exists, therefore nothing added
+ return false;
+ }
+ /**
+ * @brief Checks if the orientation of the computed trajectories differs from that of the best plan of more than the
+ * specified threshold and eventually deletes them.
+ * Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration).
+ * @param orient_threshold: Threshold paramter for allowed orientation changes in radians
+ * @param len_orientation_vector: length of the vector used to compute the start orientation
+ */
+ void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
+ /**
+ * @brief Given a plan, computes its start orientation using a vector of length >= len_orientation_vector
+ * starting from the initial pose.
+ * @param plan: Teb to be analyzed
+ * @param len_orientation_vector: min length of the vector used to compute the start orientation
+ * @param orientation: computed start orientation
+ * @return: Could the vector for the orientation check be computed? (False if the plan has no pose with a distance
+ * > len_orientation_vector from the start poseq)
+ */
+ bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);
+
+
+ /**
+ * @brief Access config (read-only)
+ * @return const pointer to the config instance
+ */
+ const TebConfig* config() const {return cfg_;}
+
+ /**
+ * @brief Access current obstacle container (read-only)
+ * @return const pointer to the obstacle container instance
+ */
+ const ObstContainer* obstacles() const {return obstacles_;}
+
+ /**
+ * @brief Returns true if the planner is initialized
+ */
+ bool isInitialized() const {return initialized_;}
+
+ /**
+ * @brief Clear any existing graph of the homotopy class search
+ */
+ void clearGraph() {if(graph_search_) graph_search_->clearGraph();}
+
+ /**
+ * @brief find the index of the currently best TEB in the container
+ * @remarks bestTeb() should be preferred whenever possible
+ * @return index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1.
+ */
+ int bestTebIdx() const;
+
+
+ /**
+ * @brief Internal helper function that adds a new equivalence class to the list of known classes only if it is unique.
+ * @param eq_class equivalence class that should be tested
+ * @param lock if \c true, exclude the H-signature from deletion.
+ * @return \c true if the h-signature was added and no duplicate was found, \c false otherwise
+ */
+ bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
+
+ /**
+ * @brief Return the current set of equivalence erelations (read-only)
+ * @return reference to the internal set of currently tracked equivalence relations
+ */
+ const EquivalenceClassContainer& getEquivalenceClassRef() const {return equivalence_classes_;}
+
+ bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const;
+
+ int numTebsInClass(const EquivalenceClassPtr& eq_class) const;
+
+ int numTebsInBestTebClass() const;
+
+ /**
+ * @brief Randomly drop non-optimal TEBs to so we can explore other alternatives
+ *
+ * The HCP has a tendency to become "fixated" once its tebs_ list becomes
+ * fully populated, repeatedly refining and evaluating paths from the same
+ * few homotopy classes until the robot moves far enough for a teb to become
+ * invalid. As a result, it can fail to discover a more optimal path. This
+ * function alleviates this problem by randomly dropping TEBs other than the
+ * current "best" one with a probability controlled by
+ * selection_dropping_probability parameter.
+ */
+ void randomlyDropTebs();
+
+protected:
+
+ /** @name Explore new paths and keep only a single one for each homotopy class */
+ //@{
+
+ /**
+ * @brief Check if a h-signature exists already.
+ * @param eq_class equivalence class that should be tested
+ * @return \c true if the h-signature is found, \c false otherwise
+ */
+ bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
+
+
+ /**
+ * @brief Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.
+ *
+ * Calling this method in each new planning interval is really important.
+ * First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed).
+ * Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures.
+ * Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths.
+ * @param delete_detours if this param is \c true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards().
+ */
+ void renewAndAnalyzeOldTebs(bool delete_detours);
+
+ /**
+ * @brief Associate trajectories with via-points
+ *
+ * If \c all_trajectories is true, all trajectory candidates are connected with the set of via_points,
+ * otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed)
+ * @remarks Requires that the plan method is called with an initial plan provided and that via-points are enabled (config)
+ * @param all_trajectories see method description
+ */
+ void updateReferenceTrajectoryViaPoints(bool all_trajectories);
+
+ //@}
+
+ // external objects (store weak pointers)
+ const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+ ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
+ const ViaPointContainer* via_points_; //!< Store the current list of via-points
+
+ // internal objects (memory management owned)
+ TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
+ TebOptimalPlannerPtr best_teb_; //!< Store the current best teb.
+ EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb
+ RobotFootprintModelPtr robot_model_; //!< Robot model shared instance
+
+ const std::vector<geometry_msgs::msg::PoseStamped>* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization
+ EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan
+ TebOptimalPlannerPtr initial_plan_teb_; //!< Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.)
+
+ TebOptPlannerContainer tebs_; //!< Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs
+
+ EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
+ // The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
+
+ std::shared_ptr<GraphSearchInterface> graph_search_;
+
+ rclcpp::Time last_eq_class_switching_time_; //!< Store the time at which the equivalence class changed recently
+
+ std::default_random_engine random_;
+ bool initialized_; //!< Keeps track about the correct initialization of this class
+
+ TebOptimalPlannerPtr last_best_teb_; //!< Points to the plan used in the previous control cycle
+
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+
+};
+
+//! Abbrev. for a shared pointer of a HomotopyClassPlanner instance.
+typedef std::shared_ptr<HomotopyClassPlanner> HomotopyClassPlannerPtr;
+
+
+} // namespace teb_local_planner
+
+// include template implementations / definitions
+#include "teb_local_planner/homotopy_class_planner.hpp"
+
+#endif /* HOMOTOPY_CLASS_PLANNER_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
new file mode 100644
index 0000000..db7cdef
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
@@ -0,0 +1,95 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/homotopy_class_planner.h"
+#include "teb_local_planner/h_signature.h"
+
+namespace teb_local_planner
+{
+
+
+template<typename BidirIter, typename Fun>
+EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
+ boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
+{
+ if(cfg_->obstacles.include_dynamic_obstacles)
+ {
+ HSignature3d* H = new HSignature3d(*cfg_);
+ H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end);
+ return EquivalenceClassPtr(H);
+ }
+ else
+ {
+ HSignature* H = new HSignature(*cfg_);
+ H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles);
+ return EquivalenceClassPtr(H);
+ }
+}
+
+
+template<typename BidirIter, typename Fun>
+TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_));
+
+ candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
+ cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
+ cfg_->trajectory.allow_init_with_backwards_motion);
+
+ if (start_velocity)
+ candidate->setVelocityStart(*start_velocity);
+
+ EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
+ candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
+
+
+ if (free_goal_vel)
+ candidate->setVelocityGoalFree();
+
+ if(addEquivalenceClassIfNew(H))
+ {
+ tebs_.push_back(candidate);
+ return tebs_.back();
+ }
+
+ // If the candidate constitutes no new equivalence class, return a null pointer
+ return TebOptimalPlannerPtr();
+}
+
+} // namespace teb_local_planner
+
diff --git a/src/teb_local_planner/include/teb_local_planner/misc.h b/src/teb_local_planner/include/teb_local_planner/misc.h
new file mode 100644
index 0000000..67f5d90
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/misc.h
@@ -0,0 +1,210 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef MISC_H
+#define MISC_H
+
+#include <builtin_interfaces/msg/duration.hpp>
+
+#include <Eigen/Core>
+
+#include <exception>
+#include <type_traits>
+
+#include <rclcpp/logging.hpp>
+
+namespace teb_local_planner
+{
+#define SMALL_NUM 0.00000001
+
+//! Symbols for left/none/right rotations
+enum class RotType { left, none, right };
+
+/**
+ * @brief Check whether two variables (double) are close to each other
+ * @param a the first value to compare
+ * @param b the second value to compare
+ * @param epsilon precision threshold
+ * @return \c true if |a-b| < epsilon, false otherwise
+ */
+inline bool is_close(double a, double b, double epsilon = 1e-4)
+{
+ return std::fabs(a - b) < epsilon;
+}
+
+/**
+ * @brief Return the average angle of an arbitrary number of given angles [rad]
+ * @param angles vector containing all angles
+ * @return average / mean angle, that is normalized to [-pi, pi]
+ */
+inline double average_angles(const std::vector<double>& angles)
+{
+ double x=0, y=0;
+ for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
+ {
+ x += cos(*it);
+ y += sin(*it);
+ }
+ if(x == 0 && y == 0)
+ return 0;
+ else
+ return std::atan2(y, x);
+}
+
+/** @brief Small helper function: check if |a|<|b| */
+inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
+
+
+/**
+ * @brief Calculate a fast approximation of a sigmoid function
+ * @details The following function is implemented: \f$ x / (1 + |x|) \f$
+ * @param x the argument of the function
+*/
+inline double fast_sigmoid(double x)
+{
+ return x / (1 + fabs(x));
+}
+
+/**
+ * @brief Calculate Euclidean distance between two 2D point datatypes
+ * @param point1 object containing fields x and y
+ * @param point2 object containing fields x and y
+ * @return Euclidean distance: ||point2-point1||
+*/
+template <typename P1, typename P2>
+inline double distance_points2d(const P1& point1, const P2& point2)
+{
+ return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
+}
+
+
+/**
+ * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
+ * @param v1 object containing public methods x() and y()
+ * @param v2 object containing fields x() and y()
+ * @return magnitude that would result in the 3D case (along the z-axis)
+*/
+template <typename V1, typename V2>
+inline double cross2d(const V1& v1, const V2& v2)
+{
+ return v1.x()*v2.y() - v2.x()*v1.y();
+}
+
+/**
+ * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
+ *
+ * Return a constant reference for boths input variants (pointer or reference).
+ * @remarks Makes only sense in combination with the overload getConstReference(const T& val).
+ * @param ptr pointer of type T
+ * @tparam T arbitrary type
+ * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
+ */
+template<typename T>
+inline const T& get_const_reference(const T* ptr) {return *ptr;}
+
+/**
+ * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
+ *
+ * Return a constant reference for boths input variants (pointer or reference).
+ * @remarks Makes only sense in combination with the overload getConstReference(const T* val).
+ * @param val
+ * @param dummy SFINAE helper variable
+ * @tparam T arbitrary type
+ * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
+ */
+template<typename T>
+inline const T& get_const_reference(const T& val, typename std::enable_if_t<!std::is_pointer<T>::value, T>* dummy = nullptr) {return val;}
+
+inline builtin_interfaces::msg::Duration durationFromSec(double t_sec)
+{
+ int32_t sec;
+ uint32_t nsec;
+ sec = static_cast<int32_t>(floor(t_sec));
+ nsec = static_cast<int32_t>(std::round((t_sec - sec) * 1e9));
+ // avoid rounding errors
+ sec += (nsec / 1000000000l);
+ nsec %= 1000000000l;
+
+ builtin_interfaces::msg::Duration duration;
+ duration.sec = sec;
+ duration.nanosec = nsec;
+ return duration;
+}
+
+struct TebAssertionFailureException : public std::runtime_error
+{
+ TebAssertionFailureException(const std::string &msg)
+ : std::runtime_error(msg)
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), msg.c_str());
+ }
+};
+
+#define TEB_ASSERT_MSG_IMPL(...) \
+ { \
+ char arg_string[1024]; \
+ std::sprintf(arg_string, __VA_ARGS__); \
+ const std::string msg(arg_string); \
+ throw TebAssertionFailureException(msg); \
+ }
+
+template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_arithmetic<T>::value>* = nullptr>
+void teb_assert_msg_impl(const T expression, ARGS ...args) {
+ if(expression == 0) {
+ char arg_string[1024];
+ std::sprintf(arg_string, args..., "");
+ const std::string msg(arg_string);
+ throw TebAssertionFailureException(msg);
+ }
+}
+
+template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_pointer<T>::value>* = nullptr>
+void teb_assert_msg_impl(const T expression, ARGS ...args) {
+ if(expression == nullptr) {
+ char arg_string[1024];
+ std::sprintf(arg_string, args..., "");
+ const std::string msg(arg_string);
+ throw TebAssertionFailureException(msg);
+ }
+}
+
+#define TEB_ASSERT_MSG(expression, ...) teb_assert_msg_impl(expression, __VA_ARGS__)
+
+} // namespace teb_local_planner
+
+#endif /* MISC_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/obstacles.h b/src/teb_local_planner/include/teb_local_planner/obstacles.h
new file mode 100644
index 0000000..6893589
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/obstacles.h
@@ -0,0 +1,1112 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+
+#ifndef OBSTACLES_H
+#define OBSTACLES_H
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <Eigen/Geometry>
+
+#include <complex>
+
+#include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <geometry_msgs/msg/quaternion_stamped.hpp>
+
+#include "teb_local_planner/distance_calculations.h"
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @class Obstacle
+ * @brief Abstract class that defines the interface for modelling obstacles
+ */
+class Obstacle
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ */
+ Obstacle() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero())
+ {
+ }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~Obstacle()
+ {
+ }
+
+
+ /** @name Centroid coordinates (abstract, obstacle type depending) */
+ //@{
+
+ /**
+ * @brief Get centroid coordinates of the obstacle
+ * @return Eigen::Vector2d containing the centroid
+ */
+ virtual const Eigen::Vector2d& getCentroid() const = 0;
+
+ /**
+ * @brief Get centroid coordinates of the obstacle as complex number
+ * @return std::complex containing the centroid coordinate
+ */
+ virtual std::complex<double> getCentroidCplx() const = 0;
+
+ //@}
+
+
+ /** @name Collision checking and distance calculations (abstract, obstacle type depending) */
+ //@{
+
+ /**
+ * @brief Check if a given point collides with the obstacle
+ * @param position 2D reference position that should be checked
+ * @param min_dist Minimum distance allowed to the obstacle to be collision free
+ * @return \c true if position is inside the region of the obstacle or if the minimum distance is lower than min_dist
+ */
+ virtual bool checkCollision(const Eigen::Vector2d& position, double min_dist) const = 0;
+
+ /**
+ * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
+ * @param line_start 2D point for the end of the reference line
+ * @param line_end 2D point for the end of the reference line
+ * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
+ * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
+ */
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const = 0;
+
+ /**
+ * @brief Get the minimum euclidean distance to the obstacle (point as reference)
+ * @param position 2d reference position
+ * @return The nearest possible distance to the obstacle
+ */
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const = 0;
+
+ /**
+ * @brief Get the minimum euclidean distance to the obstacle (line as reference)
+ * @param line_start 2d position of the begin of the reference line
+ * @param line_end 2d position of the end of the reference line
+ * @return The nearest possible distance to the obstacle
+ */
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const = 0;
+
+ /**
+ * @brief Get the minimum euclidean distance to the obstacle (polygon as reference)
+ * @param polygon Vertices (2D points) describing a closed polygon
+ * @return The nearest possible distance to the obstacle
+ */
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const = 0;
+
+ /**
+ * @brief Get the closest point on the boundary of the obstacle w.r.t. a specified reference position
+ * @param position reference 2d position
+ * @return closest point on the obstacle boundary
+ */
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const = 0;
+
+ //@}
+
+
+
+ /** @name Velocity related methods for non-static, moving obstacles */
+ //@{
+
+ /**
+ * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference)
+ * @param position 2d reference position
+ * @param t time, for which the minimum distance to the obstacle is estimated
+ * @return The nearest possible distance to the obstacle at time t
+ */
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const = 0;
+
+ /**
+ * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference)
+ * @param line_start 2d position of the begin of the reference line
+ * @param line_end 2d position of the end of the reference line
+ * @param t time, for which the minimum distance to the obstacle is estimated
+ * @return The nearest possible distance to the obstacle at time t
+ */
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const = 0;
+
+ /**
+ * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference)
+ * @param polygon Vertices (2D points) describing a closed polygon
+ * @param t time, for which the minimum distance to the obstacle is estimated
+ * @return The nearest possible distance to the obstacle at time t
+ */
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const = 0;
+
+ /**
+ * @brief Predict position of the centroid assuming a constant velocity model
+ * @param[in] t time in seconds for the prediction (t>=0)
+ * @param[out] position predicted 2d position of the centroid
+ */
+ virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
+ {
+ position = getCentroid() + t * getCentroidVelocity();
+ }
+
+ /**
+ * @brief Check if the obstacle is a moving with a (non-zero) velocity
+ * @return \c true if the obstacle is not marked as static, \c false otherwise
+ */
+ bool isDynamic() const {return dynamic_;}
+
+ /**
+ * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
+ * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
+ * @param vel 2D vector containing the velocities of the centroid in x and y directions
+ */
+ void setCentroidVelocity(const Eigen::Ref<const Eigen::Vector2d>& vel) {centroid_velocity_ = vel; dynamic_=true;}
+
+ /**
+ * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
+ * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
+ * @param velocity geometry_msgs::msg::TwistWithCovariance containing the velocity of the obstacle
+ * @param orientation geometry_msgs::msg::QuaternionStamped containing the orientation of the obstacle
+ */
+ void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity,
+ const geometry_msgs::msg::Quaternion& orientation)
+ {
+ // Set velocity, if obstacle is moving
+ Eigen::Vector2d vel;
+ vel.coeffRef(0) = velocity.twist.linear.x;
+ vel.coeffRef(1) = velocity.twist.linear.y;
+
+ // If norm of velocity is less than 0.001, consider obstacle as not dynamic
+ // TODO: Get rid of constant
+ if (vel.norm() < 0.001)
+ return;
+
+ // currently velocity published by stage is already given in the map frame
+// double yaw = tf::getYaw(orientation.quaternion);
+// ROS_INFO("Yaw: %f", yaw);
+// Eigen::Rotation2Dd rot(yaw);
+// vel = rot * vel;
+ setCentroidVelocity(vel);
+ }
+
+ void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity,
+ const geometry_msgs::msg::QuaternionStamped& orientation)
+ {
+ setCentroidVelocity(velocity, orientation.quaternion);
+ }
+
+ /**
+ * @brief Get the obstacle velocity (vx, vy) (w.r.t. to the centroid)
+ * @returns 2D vector containing the velocities of the centroid in x and y directions
+ */
+ const Eigen::Vector2d& getCentroidVelocity() const {return centroid_velocity_;}
+
+ //@}
+
+
+
+ /** @name Helper Functions */
+ //@{
+
+ /**
+ * @brief Convert the obstacle to a polygon message
+ *
+ * Convert the obstacle to a corresponding polygon msg.
+ * Point obstacles have one vertex, lines have two vertices
+ * and polygons might are implictly closed such that the start vertex must not be repeated.
+ * @param[out] polygon the polygon message
+ */
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) = 0;
+
+ virtual void toTwistWithCovarianceMsg(geometry_msgs::msg::TwistWithCovariance& twistWithCovariance)
+ {
+ if (dynamic_)
+ {
+ twistWithCovariance.twist.linear.x = centroid_velocity_(0);
+ twistWithCovariance.twist.linear.y = centroid_velocity_(1);
+ }
+ else
+ {
+ twistWithCovariance.twist.linear.x = 0;
+ twistWithCovariance.twist.linear.y = 0;
+ }
+
+ // TODO:Covariance
+ }
+
+ //@}
+
+protected:
+
+ bool dynamic_; //!< Store flag if obstacle is dynamic (resp. a moving obstacle)
+ Eigen::Vector2d centroid_velocity_; //!< Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is \c true)
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+//! Abbrev. for shared obstacle pointers
+typedef std::shared_ptr<Obstacle> ObstaclePtr;
+//! Abbrev. for shared obstacle const pointers
+typedef std::shared_ptr<const Obstacle> ObstacleConstPtr;
+//! Abbrev. for containers storing multiple obstacles
+typedef std::vector<ObstaclePtr> ObstContainer;
+
+
+
+/**
+ * @class PointObstacle
+ * @brief Implements a 2D point obstacle
+ */
+class PointObstacle : public Obstacle
+{
+public:
+
+ /**
+ * @brief Default constructor of the point obstacle class
+ */
+ PointObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
+ {}
+
+ /**
+ * @brief Construct PointObstacle using a 2d position vector
+ * @param position 2d position that defines the current obstacle position
+ */
+ PointObstacle(const Eigen::Ref< const Eigen::Vector2d>& position) : Obstacle(), pos_(position)
+ {}
+
+ /**
+ * @brief Construct PointObstacle using x- and y-coordinates
+ * @param x x-coordinate
+ * @param y y-coordinate
+ */
+ PointObstacle(double x, double y) : Obstacle(), pos_(Eigen::Vector2d(x,y))
+ {}
+
+
+ // implements checkCollision() of the base class
+ virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
+ {
+ return getMinimumDistance(point) < min_dist;
+ }
+
+
+ // implements checkLineIntersection() of the base class
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
+ {
+ // Distance Line - Circle
+ // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
+ Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
+ Eigen::Vector2d b = pos_-line_start; // b=m-x
+
+ // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
+ double t = a.dot(b)/a.dot(a);
+ if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
+ else if (t>1) t=1;
+ Eigen::Vector2d nearest_point = line_start + a*t;
+
+ // check collision
+ return checkCollision(nearest_point, min_dist);
+ }
+
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const
+ {
+ return (position-pos_).norm();
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
+ {
+ return distance_point_to_segment_2d(pos_, line_start, line_end);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const
+ {
+ return distance_point_to_polygon_2d(pos_, polygon);
+ }
+
+ // implements getMinimumDistanceVec() of the base class
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
+ {
+ return pos_;
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
+ {
+ return (pos_ + t*centroid_velocity_ - position).norm();
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
+ {
+ return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
+ {
+ return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon);
+ }
+
+ // implements predictCentroidConstantVelocity() of the base class
+ virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
+ {
+ position = pos_ + t*centroid_velocity_;
+ }
+
+ // implements getCentroid() of the base class
+ virtual const Eigen::Vector2d& getCentroid() const
+ {
+ return pos_;
+ }
+
+ // implements getCentroidCplx() of the base class
+ virtual std::complex<double> getCentroidCplx() const
+ {
+ return std::complex<double>(pos_[0],pos_[1]);
+ }
+
+ // Accessor methods
+ const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
+ Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
+ double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
+ const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
+ double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
+ const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)
+
+ // implements toPolygonMsg() of the base class
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
+ {
+ polygon.points.resize(1);
+ polygon.points.front().x = pos_.x();
+ polygon.points.front().y = pos_.y();
+ polygon.points.front().z = 0;
+ }
+
+protected:
+
+ Eigen::Vector2d pos_; //!< Store the position of the PointObstacle
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * @class CircularObstacle
+ * @brief Implements a 2D circular obstacle (point obstacle plus radius)
+ */
+class CircularObstacle : public Obstacle
+{
+public:
+
+ /**
+ * @brief Default constructor of the circular obstacle class
+ */
+ CircularObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
+ {}
+
+ /**
+ * @brief Construct CircularObstacle using a 2d center position vector and radius
+ * @param position 2d position that defines the current obstacle position
+ * @param radius radius of the obstacle
+ */
+ CircularObstacle(const Eigen::Ref< const Eigen::Vector2d>& position, double radius) : Obstacle(), pos_(position), radius_(radius)
+ {}
+
+ /**
+ * @brief Construct CircularObstacle using x- and y-center-coordinates and radius
+ * @param x x-coordinate
+ * @param y y-coordinate
+ * @param radius radius of the obstacle
+ */
+ CircularObstacle(double x, double y, double radius) : Obstacle(), pos_(Eigen::Vector2d(x,y)), radius_(radius)
+ {}
+
+
+ // implements checkCollision() of the base class
+ virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
+ {
+ return getMinimumDistance(point) < min_dist;
+ }
+
+
+ // implements checkLineIntersection() of the base class
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
+ {
+ // Distance Line - Circle
+ // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
+ Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
+ Eigen::Vector2d b = pos_-line_start; // b=m-x
+
+ // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
+ double t = a.dot(b)/a.dot(a);
+ if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
+ else if (t>1) t=1;
+ Eigen::Vector2d nearest_point = line_start + a*t;
+
+ // check collision
+ return checkCollision(nearest_point, min_dist);
+ }
+
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const
+ {
+ return (position-pos_).norm() - radius_;
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
+ {
+ return distance_point_to_segment_2d(pos_, line_start, line_end) - radius_;
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const
+ {
+ return distance_point_to_polygon_2d(pos_, polygon) - radius_;
+ }
+
+ // implements getMinimumDistanceVec() of the base class
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
+ {
+ return pos_ + radius_*(position-pos_).normalized();
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
+ {
+ return (pos_ + t*centroid_velocity_ - position).norm() - radius_;
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
+ {
+ return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end) - radius_;
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
+ {
+ return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon) - radius_;
+ }
+
+ // implements predictCentroidConstantVelocity() of the base class
+ virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
+ {
+ position = pos_ + t*centroid_velocity_;
+ }
+
+ // implements getCentroid() of the base class
+ virtual const Eigen::Vector2d& getCentroid() const
+ {
+ return pos_;
+ }
+
+ // implements getCentroidCplx() of the base class
+ virtual std::complex<double> getCentroidCplx() const
+ {
+ return std::complex<double>(pos_[0],pos_[1]);
+ }
+
+ // Accessor methods
+ const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
+ Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
+ double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
+ const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
+ double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
+ const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)
+ double& radius() {return radius_;} //!< Return the current radius of the obstacle
+ const double& radius() const {return radius_;} //!< Return the current radius of the obstacle
+
+ // implements toPolygonMsg() of the base class
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
+ {
+ // TODO(roesmann): the polygon message type cannot describe a "perfect" circle
+ // We could switch to ObstacleMsg if required somewhere...
+ polygon.points.resize(1);
+ polygon.points.front().x = pos_.x();
+ polygon.points.front().y = pos_.y();
+ polygon.points.front().z = 0;
+ }
+
+protected:
+
+ Eigen::Vector2d pos_; //!< Store the center position of the CircularObstacle
+ double radius_ = 0.0; //!< Radius of the obstacle
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+* @class LineObstacle
+* @brief Implements a 2D line obstacle
+*/
+
+class LineObstacle : public Obstacle
+{
+public:
+ //! Abbrev. for a container storing vertices (2d points defining the edge points of the polygon)
+ typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > VertexContainer;
+
+ /**
+ * @brief Default constructor of the point obstacle class
+ */
+ LineObstacle() : Obstacle()
+ {
+ start_.setZero();
+ end_.setZero();
+ centroid_.setZero();
+ }
+
+ /**
+ * @brief Construct LineObstacle using 2d position vectors as start and end of the line
+ * @param line_start 2d position that defines the start of the line obstacle
+ * @param line_end 2d position that defines the end of the line obstacle
+ */
+ LineObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end)
+ : Obstacle(), start_(line_start), end_(line_end)
+ {
+ calcCentroid();
+ }
+
+ /**
+ * @brief Construct LineObstacle using start and end coordinates
+ * @param x1 x-coordinate of the start of the line
+ * @param y1 y-coordinate of the start of the line
+ * @param x2 x-coordinate of the end of the line
+ * @param y2 y-coordinate of the end of the line
+ */
+ LineObstacle(double x1, double y1, double x2, double y2) : Obstacle()
+ {
+ start_.x() = x1;
+ start_.y() = y1;
+ end_.x() = x2;
+ end_.y() = y2;
+ calcCentroid();
+ }
+
+ // implements checkCollision() of the base class
+ virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
+ {
+ return getMinimumDistance(point) <= min_dist;
+ }
+
+ // implements checkLineIntersection() of the base class
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
+ {
+ return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const
+ {
+ return distance_point_to_segment_2d(position, start_, end_);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
+ {
+ return distance_segment_to_segment_2d(start_, end_, line_start, line_end);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const
+ {
+ return distance_segment_to_polygon_2d(start_, end_, polygon);
+ }
+
+ // implements getMinimumDistanceVec() of the base class
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
+ {
+ return closest_point_on_line_segment_2d(position, start_, end_);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_point_to_segment_2d(position, start_ + offset, end_ + offset);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon);
+ }
+
+ // implements getCentroid() of the base class
+ virtual const Eigen::Vector2d& getCentroid() const
+ {
+ return centroid_;
+ }
+
+ // implements getCentroidCplx() of the base class
+ virtual std::complex<double> getCentroidCplx() const
+ {
+ return std::complex<double>(centroid_.x(), centroid_.y());
+ }
+
+ // Access or modify line
+ const Eigen::Vector2d& start() const {return start_;}
+ void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
+ const Eigen::Vector2d& end() const {return end_;}
+ void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}
+
+ // implements toPolygonMsg() of the base class
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
+ {
+ polygon.points.resize(2);
+ polygon.points.front().x = start_.x();
+ polygon.points.front().y = start_.y();
+
+ polygon.points.back().x = end_.x();
+ polygon.points.back().y = end_.y();
+ polygon.points.back().z = polygon.points.front().z = 0;
+ }
+
+protected:
+ void calcCentroid() { centroid_ = 0.5*(start_ + end_); }
+
+private:
+ Eigen::Vector2d start_;
+ Eigen::Vector2d end_;
+
+ Eigen::Vector2d centroid_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+/**
+* @class PillObstacle
+* @brief Implements a 2D pill/stadium/capsular-shaped obstacle (line + distance/radius)
+*/
+
+class PillObstacle : public Obstacle
+{
+public:
+
+ /**
+ * @brief Default constructor of the point obstacle class
+ */
+ PillObstacle() : Obstacle()
+ {
+ start_.setZero();
+ end_.setZero();
+ centroid_.setZero();
+ }
+
+ /**
+ * @brief Construct LineObstacle using 2d position vectors as start and end of the line
+ * @param line_start 2d position that defines the start of the line obstacle
+ * @param line_end 2d position that defines the end of the line obstacle
+ */
+ PillObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end, double radius)
+ : Obstacle(), start_(line_start), end_(line_end), radius_(radius)
+ {
+ calcCentroid();
+ }
+
+ /**
+ * @brief Construct LineObstacle using start and end coordinates
+ * @param x1 x-coordinate of the start of the line
+ * @param y1 y-coordinate of the start of the line
+ * @param x2 x-coordinate of the end of the line
+ * @param y2 y-coordinate of the end of the line
+ */
+ PillObstacle(double x1, double y1, double x2, double y2, double radius) : Obstacle(), radius_(radius)
+ {
+ start_.x() = x1;
+ start_.y() = y1;
+ end_.x() = x2;
+ end_.y() = y2;
+ calcCentroid();
+ }
+
+ // implements checkCollision() of the base class
+ virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
+ {
+ return getMinimumDistance(point) <= min_dist;
+ }
+
+ // implements checkLineIntersection() of the base class
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
+ {
+ return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const
+ {
+ return distance_point_to_segment_2d(position, start_, end_) - radius_;
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
+ {
+ return distance_segment_to_segment_2d(start_, end_, line_start, line_end) - radius_;
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const
+ {
+ return distance_segment_to_polygon_2d(start_, end_, polygon) - radius_;
+ }
+
+ // implements getMinimumDistanceVec() of the base class
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
+ {
+ Eigen::Vector2d closed_point_line = closest_point_on_line_segment_2d(position, start_, end_);
+ return closed_point_line + radius_*(position-closed_point_line).normalized();
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_point_to_segment_2d(position, start_ + offset, end_ + offset) - radius_;
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end) - radius_;
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
+ {
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon) - radius_;
+ }
+
+ // implements getCentroid() of the base class
+ virtual const Eigen::Vector2d& getCentroid() const
+ {
+ return centroid_;
+ }
+
+ // implements getCentroidCplx() of the base class
+ virtual std::complex<double> getCentroidCplx() const
+ {
+ return std::complex<double>(centroid_.x(), centroid_.y());
+ }
+
+ // Access or modify line
+ const Eigen::Vector2d& start() const {return start_;}
+ void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
+ const Eigen::Vector2d& end() const {return end_;}
+ void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}
+
+ // implements toPolygonMsg() of the base class
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
+ {
+ // Currently, we only export the line
+ // TODO(roesmann): export whole pill
+ polygon.points.resize(2);
+ polygon.points.front().x = start_.x();
+ polygon.points.front().y = start_.y();
+
+ polygon.points.back().x = end_.x();
+ polygon.points.back().y = end_.y();
+ polygon.points.back().z = polygon.points.front().z = 0;
+ }
+
+protected:
+ void calcCentroid() { centroid_ = 0.5*(start_ + end_); }
+
+private:
+ Eigen::Vector2d start_;
+ Eigen::Vector2d end_;
+ double radius_ = 0.0;
+
+ Eigen::Vector2d centroid_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * @class PolygonObstacle
+ * @brief Implements a polygon obstacle with an arbitrary number of vertices
+ * @details If the polygon has only 2 vertices, than it is considered as a line,
+ * otherwise the polygon will always be closed (a connection between the first and the last vertex
+ * is included automatically).
+ */
+class PolygonObstacle : public Obstacle
+{
+public:
+
+ /**
+ * @brief Default constructor of the polygon obstacle class
+ */
+ PolygonObstacle() : Obstacle(), finalized_(false)
+ {
+ centroid_.setConstant(NAN);
+ }
+
+ /**
+ * @brief Construct polygon obstacle with a list of vertices
+ */
+ PolygonObstacle(const Point2dContainer& vertices) : Obstacle(), vertices_(vertices)
+ {
+ finalizePolygon();
+ }
+
+
+ /* FIXME Not working at the moment due to the aligned allocator version of std::vector
+ * And it is C++11 code that is disabled atm to ensure compliance with ROS indigo/jade
+ template <typename... Vector2dType>
+ PolygonObstacle(const Vector2dType&... vertices) : _vertices({vertices...})
+ {
+ calcCentroid();
+ _finalized = true;
+ }
+ */
+
+
+ // implements checkCollision() of the base class
+ virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
+ {
+ // line case
+ if (noVertices()==2)
+ return getMinimumDistance(point) <= min_dist;
+
+ // check if point is in the interior of the polygon
+ // point in polygon test - raycasting (http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html)
+ // using the following algorithm we may obtain false negatives on edge-cases, but that's ok for our purposes
+ int i, j;
+ bool c = false;
+ for (i = 0, j = noVertices()-1; i < noVertices(); j = i++)
+ {
+ if ( ((vertices_.at(i).y()>point.y()) != (vertices_.at(j).y()>point.y())) &&
+ (point.x() < (vertices_.at(j).x()-vertices_.at(i).x()) * (point.y()-vertices_.at(i).y()) / (vertices_.at(j).y()-vertices_.at(i).y()) + vertices_.at(i).x()) )
+ c = !c;
+ }
+ if (c>0) return true;
+
+ // If this statement is reached, the point lies outside the polygon or maybe on its edges
+ // Let us check the minium distance as well
+ return min_dist == 0 ? false : getMinimumDistance(point) < min_dist;
+ }
+
+
+ /**
+ * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
+ * @param line_start 2D point for the end of the reference line
+ * @param line_end 2D point for the end of the reference line
+ * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
+ * @remarks we ignore \c min_dist here
+ * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
+ */
+ virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const;
+
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& position) const
+ {
+ return distance_point_to_polygon_2d(position, vertices_);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
+ {
+ return distance_segment_to_polygon_2d(line_start, line_end, vertices_);
+ }
+
+ // implements getMinimumDistance() of the base class
+ virtual double getMinimumDistance(const Point2dContainer& polygon) const
+ {
+ return distance_polygon_to_polygon_2d(polygon, vertices_);
+ }
+
+ // implements getMinimumDistanceVec() of the base class
+ virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const;
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
+ {
+ Point2dContainer pred_vertices;
+ predictVertices(t, pred_vertices);
+ return distance_point_to_polygon_2d(position, pred_vertices);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
+ {
+ Point2dContainer pred_vertices;
+ predictVertices(t, pred_vertices);
+ return distance_segment_to_polygon_2d(line_start, line_end, pred_vertices);
+ }
+
+ // implements getMinimumSpatioTemporalDistance() of the base class
+ virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
+ {
+ Point2dContainer pred_vertices;
+ predictVertices(t, pred_vertices);
+ return distance_polygon_to_polygon_2d(polygon, pred_vertices);
+ }
+
+ virtual void predictVertices(double t, Point2dContainer& pred_vertices) const
+ {
+ // Predict obstacle (polygon) at time t
+ pred_vertices.resize(vertices_.size());
+ Eigen::Vector2d offset = t*centroid_velocity_;
+ for (std::size_t i = 0; i < vertices_.size(); i++)
+ {
+ pred_vertices[i] = vertices_[i] + offset;
+ }
+ }
+
+ // implements getCentroid() of the base class
+ virtual const Eigen::Vector2d& getCentroid() const
+ {
+ assert(finalized_ && "Finalize the polygon after all vertices are added.");
+ return centroid_;
+ }
+
+ // implements getCentroidCplx() of the base class
+ virtual std::complex<double> getCentroidCplx() const
+ {
+ assert(finalized_ && "Finalize the polygon after all vertices are added.");
+ return std::complex<double>(centroid_.coeffRef(0), centroid_.coeffRef(1));
+ }
+
+ // implements toPolygonMsg() of the base class
+ virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon);
+
+
+ /** @name Define the polygon */
+ ///@{
+
+ // Access or modify polygon
+ const Point2dContainer& vertices() const {return vertices_;} //!< Access vertices container (read-only)
+ Point2dContainer& vertices() {return vertices_;} //!< Access vertices container
+
+ /**
+ * @brief Add a vertex to the polygon (edge-point)
+ * @remarks You do not need to close the polygon (do not repeat the first vertex)
+ * @warning Do not forget to call finalizePolygon() after adding all vertices
+ * @param vertex 2D point defining a new polygon edge
+ */
+ void pushBackVertex(const Eigen::Ref<const Eigen::Vector2d>& vertex)
+ {
+ vertices_.push_back(vertex);
+ finalized_ = false;
+ }
+
+ /**
+ * @brief Add a vertex to the polygon (edge-point)
+ * @remarks You do not need to close the polygon (do not repeat the first vertex)
+ * @warning Do not forget to call finalizePolygon() after adding all vertices
+ * @param x x-coordinate of the new vertex
+ * @param y y-coordinate of the new vertex
+ */
+ void pushBackVertex(double x, double y)
+ {
+ vertices_.push_back(Eigen::Vector2d(x,y));
+ finalized_ = false;
+ }
+
+ /**
+ * @brief Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods
+ */
+ void finalizePolygon()
+ {
+ fixPolygonClosure();
+ calcCentroid();
+ finalized_ = true;
+ }
+
+ /**
+ * @brief Clear all vertices (Afterwards the polygon is not valid anymore)
+ */
+ void clearVertices() {vertices_.clear(); finalized_ = false;}
+
+ /**
+ * @brief Get the number of vertices defining the polygon (the first vertex is counted once)
+ */
+ int noVertices() const {return (int)vertices_.size();}
+
+
+ ///@}
+
+protected:
+
+ void fixPolygonClosure(); //!< Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one.
+
+ void calcCentroid(); //!< Compute the centroid of the polygon (called inside finalizePolygon())
+
+
+ Point2dContainer vertices_; //!< Store vertices defining the polygon (@see pushBackVertex)
+ Eigen::Vector2d centroid_; //!< Store the centroid coordinates of the polygon (@see calcCentroid)
+
+ bool finalized_; //!< Flat that keeps track if the polygon was finalized after adding all vertices
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+} // namespace teb_local_planner
+
+#endif /* OBSTACLES_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/optimal_planner.h b/src/teb_local_planner/include/teb_local_planner/optimal_planner.h
new file mode 100644
index 0000000..f76efe8
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/optimal_planner.h
@@ -0,0 +1,736 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef OPTIMAL_PLANNER_H_
+#define OPTIMAL_PLANNER_H_
+
+#include <math.h>
+
+
+// teb stuff
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+#include "teb_local_planner/timed_elastic_band.h"
+#include "teb_local_planner/planner_interface.h"
+#include "teb_local_planner/visualization.h"
+#include "teb_local_planner/robot_footprint_model.h"
+
+// g2o lib stuff
+#include "g2o/core/sparse_optimizer.h"
+#include "g2o/core/block_solver.h"
+#include "g2o/core/factory.h"
+#include "g2o/core/optimization_algorithm_gauss_newton.h"
+#include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/solvers/csparse/linear_solver_csparse.h"
+#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
+
+// g2o custom edges and vertices for the TEB planner
+#include "teb_local_planner/g2o_types/edge_velocity.h"
+#include "teb_local_planner/g2o_types/edge_acceleration.h"
+#include "teb_local_planner/g2o_types/edge_kinematics.h"
+#include "teb_local_planner/g2o_types/edge_time_optimal.h"
+#include "teb_local_planner/g2o_types/edge_shortest_path.h"
+#include "teb_local_planner/g2o_types/edge_obstacle.h"
+#include "teb_local_planner/g2o_types/edge_dynamic_obstacle.h"
+#include "teb_local_planner/g2o_types/edge_via_point.h"
+#include "teb_local_planner/g2o_types/edge_prefer_rotdir.h"
+
+// messages
+#include <nav_msgs/msg/path.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <teb_msgs/msg/trajectory_msg.hpp>
+#include <teb_msgs/msg/trajectory_point_msg.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <dwb_critics/obstacle_footprint.hpp>
+
+#include <tf2/transform_datatypes.h>
+
+#include <limits.h>
+
+namespace teb_local_planner
+{
+
+//! Typedef for the block solver utilized for optimization
+typedef g2o::BlockSolverX TEBBlockSolver;
+
+//! Typedef for the linear solver utilized for optimization
+typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
+//typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
+
+//! Typedef for a container storing via-points
+typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ViaPointContainer;
+
+
+/**
+ * @class TebOptimalPlanner
+ * @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
+ *
+ * For an introduction and further details about the TEB optimization problem refer to:
+ * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
+ * - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013.
+ * - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011.
+ *
+ * @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions.
+ * @todo: We introduced the non-fast mode with the support of dynamic obstacles
+ * (which leads to better results in terms of x-y-t homotopy planning).
+ * However, we have not tested this mode intensively yet, so we keep
+ * the legacy fast mode as default until we finish our tests.
+ */
+class TebOptimalPlanner : public PlannerInterface
+{
+public:
+
+ /**
+ * @brief Default constructor
+ */
+ TebOptimalPlanner();
+
+ /**
+ * @brief Construct and initialize the TEB optimal planner.
+ * @param cfg Const reference to the TebConfig class for internal parameters
+ * @param obstacles Container storing all relevant obstacles (see Obstacle)
+ * @param visual Shared pointer to the TebVisualization class (optional)
+ * @param via_points Container storing via-points (optional)
+ */
+ TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+ TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+ /**
+ * @brief Destruct the optimal planner.
+ */
+ virtual ~TebOptimalPlanner();
+
+ /**
+ * @brief Initializes the optimal planner
+ * @param node Shared pointer for rclcpp::Node
+ * @param cfg Const reference to the TebConfig class for internal parameters
+ * @param obstacles Container storing all relevant obstacles (see Obstacle)
+ * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
+ * @param visual Shared pointer to the TebVisualization class (optional)
+ * @param via_points Container storing via-points (optional)
+ */
+ void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+ TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+ /** @name Plan a trajectory */
+ //@{
+
+ /**
+ * @brief Plan a trajectory based on an initial reference plan.
+ *
+ * Call this method to create and optimize a trajectory that is initialized
+ * according to an initial reference plan (given as a container of poses). \n
+ * The method supports hot-starting from previous solutions, if avaiable: \n
+ * - If no trajectory exist yet, a new trajectory is initialized based on the initial plan,
+ * see TimedElasticBand::initTEBtoGoal
+ * - If a previous solution is avaiable, update the trajectory based on the initial plan,
+ * see bool TimedElasticBand::updateAndPruneTEB
+ * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
+ * @param initial_plan vector of geometry_msgs::msg::PoseStamped
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose (tf::Pose version)
+ *
+ * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
+ * The method supports hot-starting from previous solutions, if avaiable: \n
+ * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses,
+ * see TimedElasticBand::initTEBtoGoal
+ * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
+ * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
+ * @param start tf::Pose containing the start pose of the trajectory
+ * @param goal tf::Pose containing the goal pose of the trajectory
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ // tf2 doesn't have tf::Pose
+// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose
+ *
+ * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
+ * The method supports hot-starting from previous solutions, if avaiable: \n
+ * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses
+ * @see TimedElasticBand::initTEBtoGoal
+ * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
+ * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
+ * @param start PoseSE2 containing the start pose of the trajectory
+ * @param goal PoseSE2 containing the goal pose of the trajectory
+ * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+
+ /**
+ * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
+ * @warning Call plan() first and check if the generated plan is feasible.
+ * @param[out] vx translational velocity [m/s]
+ * @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s]
+ * @param[out] omega rotational velocity [rad/s]
+ * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
+ * @return \c true if command is valid, \c false otherwise
+ */
+ virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
+
+
+ /**
+ * @brief Optimize a previously initialized trajectory (actual TEB optimization loop).
+ *
+ * optimizeTEB implements the main optimization loop. \n
+ * It consist of two nested loops:
+ * - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize().
+ * Afterwards the internal method optimizeGraph() is called that constitutes the innerloop.
+ * - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified
+ * number of optimization calls (\c iterations_innerloop).
+ *
+ * The outer loop is repeated \c iterations_outerloop times. \n
+ * The ratio of inner and outer loop iterations significantly defines the contraction behavior
+ * and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n
+ * The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n
+ * Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost().
+ * @remarks This method is usually called from a plan() method
+ * @param iterations_innerloop Number of iterations for the actual solver loop
+ * @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop.
+ * @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(),
+ * the vector can be accessed afterwards using getCurrentCost().
+ * @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true)
+ * @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true)
+ * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
+ * (only used if \c compute_cost_afterwards is true).
+ * @return \c true if the optimization terminates successfully, \c false otherwise
+ */
+ bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false,
+ double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
+
+ //@}
+
+
+ /** @name Desired initial and final velocity */
+ //@{
+
+
+ /**
+ * @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload].
+ * @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method
+ * @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used,
+ * for holonomic robots also linear.y)
+ */
+ void setVelocityStart(const geometry_msgs::msg::Twist& vel_start);
+
+ /**
+ * @brief Set the desired final velocity at the trajectory's goal pose.
+ * @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan()
+ * @param vel_goal twist message containing the translational and angular final velocity
+ */
+ void setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal);
+
+ /**
+ * @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit
+ * @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan()
+ */
+ void setVelocityGoalFree() {vel_goal_.first = false;}
+
+ //@}
+
+
+ /** @name Take obstacles into account */
+ //@{
+
+
+ /**
+ * @brief Assign a new set of obstacles
+ * @param obst_vector pointer to an obstacle container (can also be a nullptr)
+ * @remarks This method overrids the obstacle container optinally assigned in the constructor.
+ */
+ void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;}
+
+ /**
+ * @brief Access the internal obstacle container.
+ * @return Const reference to the obstacle container
+ */
+ const ObstContainer& getObstVector() const {return *obstacles_;}
+
+ //@}
+
+ /** @name Take via-points into account */
+ //@{
+
+
+ /**
+ * @brief Assign a new set of via-points
+ * @param via_points pointer to a via_point container (can also be a nullptr)
+ * @details Any previously set container will be overwritten.
+ */
+ void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;}
+
+ /**
+ * @brief Access the internal via-point container.
+ * @return Const reference to the via-point container
+ */
+ const ViaPointContainer& getViaPoints() const {return *via_points_;}
+
+ //@}
+
+
+ /** @name Visualization */
+ //@{
+
+ /**
+ * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
+ * @param visualization shared pointer to a TebVisualization instance
+ * @see visualize
+ */
+ void setVisualization(const TebVisualizationPtr & visualization) override;
+
+ /**
+ * @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).
+ *
+ * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
+ * @see setVisualization
+ */
+ virtual void visualize();
+
+ //@}
+
+
+ /** @name Utility methods and more */
+ //@{
+
+ /**
+ * @brief Reset the planner by clearing the internal graph and trajectory.
+ */
+ virtual void clearPlanner()
+ {
+ clearGraph();
+ teb_.clearTimedElasticBand();
+ }
+
+ /**
+ * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
+ *
+ * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
+ * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
+ * Initial means that the penalty is applied only to the first few poses of the trajectory.
+ * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
+ */
+ virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;}
+
+ /**
+ * @brief Register the vertices and edges defined for the TEB to the g2o::Factory.
+ *
+ * This allows the user to export the internal graph to a text file for instance.
+ * Access the optimizer() for more details.
+ */
+ static void registerG2OTypes();
+
+ /**
+ * @brief Access the internal TimedElasticBand trajectory.
+ * @warning In general, the underlying teb must not be modified directly. Use with care...
+ * @return reference to the teb
+ */
+ TimedElasticBand& teb() {return teb_;};
+
+ /**
+ * @brief Access the internal TimedElasticBand trajectory (read-only).
+ * @return const reference to the teb
+ */
+ const TimedElasticBand& teb() const {return teb_;};
+
+ /**
+ * @brief Access the internal g2o optimizer.
+ * @warning In general, the underlying optimizer must not be modified directly. Use with care...
+ * @return const shared pointer to the g2o sparse optimizer
+ */
+ std::shared_ptr<g2o::SparseOptimizer> optimizer() {return optimizer_;};
+
+ /**
+ * @brief Access the internal g2o optimizer (read-only).
+ * @return const shared pointer to the g2o sparse optimizer
+ */
+ std::shared_ptr<const g2o::SparseOptimizer> optimizer() const {return optimizer_;};
+
+ /**
+ * @brief Check if last optimization was successful
+ * @return \c true if the last optimization returned without errors,
+ * otherwise \c false (also if no optimization has been called before).
+ */
+ bool isOptimized() const {return optimized_;};
+
+ /**
+ * @brief Returns true if the planner has diverged.
+ */
+ bool hasDiverged() const override;
+
+ /**
+ * @brief Compute the cost vector of a given optimization problen (hyper-graph must exist).
+ *
+ * Use this method to obtain information about the current edge errors / costs (local cost functions). \n
+ * The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n
+ * Refer to the method declaration for the detailed composition. \n
+ * The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single
+ * squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional
+ * or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n
+ * Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the
+ * implemented definition, if the value is scaled to match the magnitude of other cost values.
+ *
+ * @todo Remove the scaling term for the alternative time cost.
+ * @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself?
+ * @see getCurrentCost
+ * @see optimizeTEB
+ * @param obst_cost_scale Specify extra scaling for obstacle costs.
+ * @param viapoint_cost_scale Specify extra scaling for via points.
+ * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time.
+ * @return TebCostVec containing the cost values
+ */
+ void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
+
+ /**
+ * Compute and return the cost of the current optimization graph (supports multiple trajectories)
+ * @param[out] cost current cost value for each trajectory
+ * [for a planner with just a single trajectory: size=1, vector will not be cleared]
+ * @param obst_cost_scale Specify extra scaling for obstacle costs
+ * @param viapoint_cost_scale Specify extra scaling for via points.
+ * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
+ */
+ virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
+ {
+ computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
+ cost.push_back( getCurrentCost() );
+ }
+
+ /**
+ * @brief Access the cost vector.
+ *
+ * The accumulated cost value previously calculated using computeCurrentCost
+ * or by calling optimizeTEB with enabled cost flag.
+ * @return const reference to the TebCostVec.
+ */
+ double getCurrentCost() const {return cost_;}
+
+
+ /**
+ * @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots)
+ *
+ * The velocity is extracted using finite differences.
+ * The direction of the translational velocity is also determined.
+ * @param pose1 pose at time k
+ * @param pose2 consecutive pose at time k+1
+ * @param dt actual time difference between k and k+1 (must be >0 !!!)
+ * @param[out] vx translational velocity
+ * @param[out] vy strafing velocity which can be nonzero for holonomic robots
+ * @param[out] omega rotational velocity
+ */
+ inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const;
+
+ /**
+ * @brief Compute the velocity profile of the trajectory
+ *
+ * This method computes the translational and rotational velocity for the complete
+ * planned trajectory.
+ * The first velocity is the one that is provided as initial velocity (fixed).
+ * Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k.
+ * The last velocity is the final velocity (fixed).
+ * The number of Twist objects is therefore sizePoses()+1;
+ * In summary:
+ * v[0] = v_start,
+ * v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt,
+ * v(end) = v_goal
+ * It can be used for evaluation and debugging purposes or
+ * for open-loop control. For computing the velocity required for controlling the robot
+ * to the next step refer to getVelocityCommand().
+ * @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1
+ */
+ void getVelocityProfile(std::vector<geometry_msgs::msg::Twist>& velocity_profile) const;
+
+ /**
+ * @brief Return the complete trajectory including poses, velocity profiles and temporal information
+ *
+ * It is useful for evaluation and debugging purposes or for open-loop control.
+ * Since the velocity obtained using difference quotients is the mean velocity between consecutive poses,
+ * the velocity at each pose at time stamp k is obtained by taking the average between both velocities.
+ * The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off).
+ * See getVelocityProfile() for the list of velocities between consecutive points.
+ * @todo The acceleration profile is not added at the moment.
+ * @param[out] trajectory the resulting trajectory
+ */
+ void getFullTrajectory(std::vector<teb_msgs::msg::TrajectoryPointMsg>& trajectory) const;
+
+ /**
+ * @brief Check whether the planned trajectory is feasible or not.
+ *
+ * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
+ * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
+ * @param costmap_model Pointer to the costmap model
+ * @param footprint_spec The specification of the footprint of the robot in world coordinates
+ * @param inscribed_radius The radius of the inscribed circle of the robot
+ * @param circumscribed_radius The radius of the circumscribed circle of the robot
+ * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
+ * @return \c true, if the robot footprint along the first part of the trajectory intersects with
+ * any obstacle in the costmap, \c false otherwise.
+ */
+ virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec, double inscribed_radius = 0.0,
+ double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1);
+
+ /**
+ * @brief Check whether the footprint of the robot at the pose touches an obstacle or not.
+ *
+ * @param pose2d Pose to check
+ * @param costmap_model Pointer to the costmap model
+ * @param footprint_spec The specification of the footprint of the robot in world coordinates
+ * @return \c true, if the robot pose is valid, \c false otherwise.
+ */
+ virtual bool isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model,
+ const std::vector<geometry_msgs::msg::Point>& footprint_spec);
+
+ //@}
+
+protected:
+
+ /** @name Hyper-Graph creation and optimization */
+ //@{
+
+ /**
+ * @brief Build the hyper-graph representing the TEB optimization problem.
+ *
+ * This method creates the optimization problem according to the hyper-graph formulation. \n
+ * For more details refer to the literature cited in the TebOptimalPlanner class description.
+ * @see optimizeGraph
+ * @see clearGraph
+ * @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph
+ * This might be used for weight adapation strategies.
+ * Currently, only obstacle collision weights are considered.
+ * @return \c true, if the graph was created successfully, \c false otherwise.
+ */
+ bool buildGraph(double weight_multiplier=1.0);
+
+ /**
+ * @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB.
+ *
+ * This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n
+ * The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered
+ * by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description.
+ * @see buildGraph
+ * @see clearGraph
+ * @param no_iterations Number of solver iterations
+ * @param clear_after Clear the graph after optimization.
+ * @return \c true, if optimization terminates successfully, \c false otherwise.
+ */
+ bool optimizeGraph(int no_iterations, bool clear_after=true);
+
+ /**
+ * @brief Clear an existing internal hyper-graph.
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void clearGraph();
+
+ /**
+ * @brief Add all relevant vertices to the hyper-graph as optimizable variables.
+ *
+ * Vertices (if unfixed) represent the variables that will be optimized. \n
+ * In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n
+ * The order of insertion of vertices (to the graph) is important for efficiency,
+ * since it affect the sparsity pattern of the underlying hessian computed for optimization.
+ * @see VertexPose
+ * @see VertexTimeDiff
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddTEBVertices();
+
+ /**
+ * @brief Add all edges (local cost functions) for limiting the translational and angular velocity.
+ * @see EdgeVelocity
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesVelocity();
+
+ /**
+ * @brief Add all edges (local cost functions) for limiting the translational and angular acceleration.
+ * @see EdgeAcceleration
+ * @see EdgeAccelerationStart
+ * @see EdgeAccelerationGoal
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesAcceleration();
+
+ /**
+ * @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)
+ * @see EdgeTimeOptimal
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesTimeOptimal();
+
+ /**
+ * @brief Add all edges (local cost functions) for minimizing the path length
+ * @see EdgeShortestPath
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesShortestPath();
+
+ /**
+ * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles
+ * @warning do not combine with AddEdgesInflatedObstacles
+ * @see EdgeObstacle
+ * @see buildGraph
+ * @see optimizeGraph
+ * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
+ */
+ void AddEdgesObstacles(double weight_multiplier=1.0);
+
+ /**
+ * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)
+ * @see EdgeObstacle
+ * @see buildGraph
+ * @see optimizeGraph
+ * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
+ */
+ void AddEdgesObstaclesLegacy(double weight_multiplier=1.0);
+
+ /**
+ * @brief Add all edges (local cost functions) related to minimizing the distance to via-points
+ * @see EdgeViaPoint
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesViaPoints();
+
+ /**
+ * @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.
+ * @warning experimental
+ * @todo Should we also add neighbors to decrease jiggling/oscillations
+ * @see EdgeDynamicObstacle
+ * @see buildGraph
+ * @see optimizeGraph
+ * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
+
+ */
+ void AddEdgesDynamicObstacles(double weight_multiplier=1.0);
+
+ /**
+ * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot
+ * @warning do not combine with AddEdgesKinematicsCarlike()
+ * @see AddEdgesKinematicsCarlike
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesKinematicsDiffDrive();
+
+ /**
+ * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot
+ * @warning do not combine with AddEdgesKinematicsDiffDrive()
+ * @see AddEdgesKinematicsDiffDrive
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesKinematicsCarlike();
+
+ /**
+ * @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesPreferRotDir();
+
+ /**
+ * @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles
+ * @see buildGraph
+ * @see optimizeGraph
+ */
+ void AddEdgesVelocityObstacleRatio();
+
+ //@}
+
+
+ /**
+ * @brief Initialize and configure the g2o sparse optimizer.
+ * @return shared pointer to the g2o::SparseOptimizer instance
+ */
+ std::shared_ptr<g2o::SparseOptimizer> initOptimizer();
+
+
+ // external objects (store weak pointers)
+ const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+ ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
+ const ViaPointContainer* via_points_; //!< Store via points for planning
+ std::vector<ObstContainer> obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices
+
+ double cost_; //!< Store cost value of the current hyper-graph
+ RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)
+
+ // internal objects (memory management owned)
+ TebVisualizationPtr visualization_; //!< Instance of the visualization class
+ TimedElasticBand teb_; //!< Actual trajectory object
+ RobotFootprintModelPtr robot_model_; //!< Robot model
+ std::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
+ std::pair<bool, geometry_msgs::msg::Twist> vel_start_; //!< Store the initial velocity at the start pose
+ std::pair<bool, geometry_msgs::msg::Twist> vel_goal_; //!< Store the final velocity at the goal pose
+
+ bool initialized_; //!< Keeps track about the correct initialization of this class
+ bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//! Abbrev. for shared instances of the TebOptimalPlanner
+typedef std::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;
+//! Abbrev. for shared const TebOptimalPlanner pointers
+typedef std::shared_ptr<const TebOptimalPlanner> TebOptimalPlannerConstPtr;
+//! Abbrev. for containers storing multiple teb optimal planners
+typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;
+
+} // namespace teb_local_planner
+
+#endif /* OPTIMAL_PLANNER_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/planner_interface.h b/src/teb_local_planner/include/teb_local_planner/planner_interface.h
new file mode 100644
index 0000000..268ba84
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/planner_interface.h
@@ -0,0 +1,218 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef PLANNER_INTERFACE_H_
+#define PLANNER_INTERFACE_H_
+
+#include<memory>
+
+// ros
+#include <dwb_critics/obstacle_footprint.hpp>
+
+#include <rclcpp/node.hpp>
+// this package
+#include <teb_local_planner/pose_se2.h>
+#include <teb_local_planner/robot_footprint_model.h>
+
+#include <tf2/transform_datatypes.h>
+
+#include <nav2_costmap_2d/costmap_2d.hpp>
+
+#include <nav2_util/lifecycle_node.hpp>
+
+// messages
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+
+// this package
+#include "teb_local_planner/pose_se2.h"
+#include "teb_local_planner/visualization.h"
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class PlannerInterface
+ * @brief This abstract class defines an interface for local planners
+ */
+class PlannerInterface
+{
+public:
+
+ /**
+ * @brief Default constructor
+ */
+ PlannerInterface()
+ {
+ }
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~PlannerInterface()
+ {
+ }
+
+
+ /** @name Plan a trajectory */
+ //@{
+
+ /**
+ * @brief Plan a trajectory based on an initial reference plan.
+ *
+ * Provide this method to create and optimize a trajectory that is initialized
+ * according to an initial reference plan (given as a container of poses).
+ * @param initial_plan vector of geometry_msgs::msg::PoseStamped
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
+ *
+ * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+ * @param start tf::Pose containing the start pose of the trajectory
+ * @param goal tf::Pose containing the goal pose of the trajectory
+ * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ // tf2 doesn't have tf2::Pose
+ //virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
+
+ /**
+ * @brief Plan a trajectory between a given start and goal pose.
+ *
+ * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+ * @param start PoseSE2 containing the start pose of the trajectory
+ * @param goal PoseSE2 containing the goal pose of the trajectory
+ * @param start_vel Initial velocity at the start pose (twist msg containing the translational and angular velocity).
+ * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+ * otherwise the final velocity will be zero (default: false)
+ * @return \c true if planning was successful, \c false otherwise
+ */
+ virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
+
+ /**
+ * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
+ * @warning Call plan() first and check if the generated plan is feasible.
+ * @param[out] vx translational velocity [m/s]
+ * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
+ * @param[out] omega rotational velocity [rad/s]
+ * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
+ * @return \c true if command is valid, \c false otherwise
+ */
+ virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const = 0;
+
+ //@}
+
+
+ /**
+ * @brief Reset the planner.
+ */
+ virtual void clearPlanner() = 0;
+
+ /**
+ * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
+ *
+ * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
+ * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
+ * Initial means that the penalty is applied only to the first few poses of the trajectory.
+ * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
+ */
+ virtual void setPreferredTurningDir(RotType dir) {
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "setPreferredTurningDir() not implemented for this planner.");}
+
+ /**
+ * @brief Visualize planner specific stuff.
+ * Overwrite this method to provide an interface to perform all planner related visualizations at once.
+ */
+ virtual void visualize()
+ {
+ }
+
+ virtual void setVisualization(const TebVisualizationPtr & visualization) = 0;
+
+ /**
+ * @brief Check whether the planned trajectory is feasible or not.
+ *
+ * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
+ * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
+ * @param costmap_model Pointer to the costmap model
+ * @param footprint_spec The specification of the footprint of the robot in world coordinates
+ * @param inscribed_radius The radius of the inscribed circle of the robot
+ * @param circumscribed_radius The radius of the circumscribed circle of the robot
+ * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
+ * @return \c true, if the robot footprint along the first part of the trajectory intersects with
+ * any obstacle in the costmap, \c false otherwise.
+ */
+ virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+ double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0) = 0;
+
+ /**
+ * Compute and return the cost of the current optimization graph (supports multiple trajectories)
+ * @param[out] cost current cost value for each trajectory
+ * [for a planner with just a single trajectory: size=1, vector will not be cleared]
+ * @param obst_cost_scale Specify extra scaling for obstacle costs
+ * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
+ */
+ virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
+ {
+ }
+
+ /**
+ * @brief Returns true if the planner has diverged.
+ */
+ virtual bool hasDiverged() const = 0;
+
+ nav2_util::LifecycleNode::SharedPtr node_{nullptr};
+};
+
+//! Abbrev. for shared instances of PlannerInterface or it's subclasses
+typedef std::shared_ptr<PlannerInterface> PlannerInterfacePtr;
+
+
+} // namespace teb_local_planner
+
+#endif /* PLANNER_INTERFACE_H__ */
diff --git a/src/teb_local_planner/include/teb_local_planner/pose_se2.h b/src/teb_local_planner/include/teb_local_planner/pose_se2.h
new file mode 100755
index 0000000..55888c7
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/pose_se2.h
@@ -0,0 +1,433 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef POSE_SE2_H_
+#define POSE_SE2_H_
+
+#include <g2o/stuff/misc.h>
+
+#include <Eigen/Core>
+#include "teb_local_planner/misc.h"
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose2_d.hpp>
+
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class PoseSE2
+ * @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$
+ * The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].
+ */
+class PoseSE2
+{
+public:
+
+ /** @name Construct PoseSE2 instances */
+ ///@{
+
+ /**
+ * @brief Default constructor
+ */
+ PoseSE2()
+ {
+ setZero();
+ }
+
+ /**
+ * @brief Construct pose given a position vector and an angle theta
+ * @param position 2D position vector
+ * @param theta angle given in rad
+ */
+ PoseSE2(const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
+ {
+ _position = position;
+ _theta = theta;
+ }
+
+ /**
+ * @brief Construct pose using single components x, y, and the yaw angle
+ * @param x x-coordinate
+ * @param y y-coordinate
+ * @param theta yaw angle in rad
+ */
+ PoseSE2(double x, double y, double theta)
+ {
+ _position.coeffRef(0) = x;
+ _position.coeffRef(1) = y;
+ _theta = theta;
+ }
+
+ /**
+ * @brief Construct pose using a geometry_msgs::msg::Pose
+ * @param pose geometry_msgs::msg::Pose object
+ */
+ PoseSE2(const geometry_msgs::msg::PoseStamped& pose)
+ :PoseSE2(pose.pose)
+ {
+
+ }
+
+ /**
+ * @brief Construct pose using a geometry_msgs::msg::Pose
+ * @param pose geometry_msgs::msg::Pose object
+ */
+ PoseSE2(const geometry_msgs::msg::Pose& pose)
+ {
+ _position.coeffRef(0) = pose.position.x;
+ _position.coeffRef(1) = pose.position.y;
+ _theta = tf2::getYaw( pose.orientation );
+ }
+
+ /**
+ * @brief Construct pose using a geometry_msgs::msg::Pose2D
+ * @param pose geometry_msgs::msg::Pose2D object
+ */
+ PoseSE2(const geometry_msgs::msg::Pose2D& pose)
+ {
+ _position.coeffRef(0) = pose.x;
+ _position.coeffRef(1) = pose.y;
+ _theta = pose.theta;
+ }
+
+ /**
+ * @brief Copy constructor
+ * @param pose PoseSE2 instance
+ */
+ PoseSE2(const PoseSE2& pose)
+ {
+ _position = pose._position;
+ _theta = pose._theta;
+ }
+
+ ///@}
+
+
+ /**
+ * @brief Destructs the PoseSE2
+ */
+ ~PoseSE2() {}
+
+
+ /** @name Access and modify values */
+ ///@{
+
+ /**
+ * @brief Access the 2D position part
+ * @see estimate
+ * @return reference to the 2D position part
+ */
+ Eigen::Vector2d& position() {return _position;}
+
+ /**
+ * @brief Access the 2D position part (read-only)
+ * @see estimate
+ * @return const reference to the 2D position part
+ */
+ const Eigen::Vector2d& position() const {return _position;}
+
+ /**
+ * @brief Access the x-coordinate the pose
+ * @return reference to the x-coordinate
+ */
+ double& x() {return _position.coeffRef(0);}
+
+ /**
+ * @brief Access the x-coordinate the pose (read-only)
+ * @return const reference to the x-coordinate
+ */
+ const double& x() const {return _position.coeffRef(0);}
+
+ /**
+ * @brief Access the y-coordinate the pose
+ * @return reference to the y-coordinate
+ */
+ double& y() {return _position.coeffRef(1);}
+
+ /**
+ * @brief Access the y-coordinate the pose (read-only)
+ * @return const reference to the y-coordinate
+ */
+ const double& y() const {return _position.coeffRef(1);}
+
+ /**
+ * @brief Access the orientation part (yaw angle) of the pose
+ * @return reference to the yaw angle
+ */
+ double& theta() {return _theta;}
+
+ /**
+ * @brief Access the orientation part (yaw angle) of the pose (read-only)
+ * @return const reference to the yaw angle
+ */
+ const double& theta() const {return _theta;}
+
+ /**
+ * @brief Set pose to [0,0,0]
+ */
+ void setZero()
+ {
+ _position.setZero();
+ _theta = 0;
+ }
+
+ /**
+ * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose
+ * @param[out] pose Pose message
+ */
+ void toPoseMsg(geometry_msgs::msg::Pose& pose) const
+ {
+ pose.position.x = _position.x();
+ pose.position.y = _position.y();
+ pose.position.z = 0;
+ tf2::Quaternion q;
+ q.setRPY(0, 0, _theta);
+ pose.orientation = tf2::toMsg(q);
+ }
+
+ /**
+ * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose2D
+ * @param[out] pose Pose message
+ */
+ void toPoseMsg(geometry_msgs::msg::Pose2D& pose) const
+ {
+ pose.x = _position.x();
+ pose.y = _position.y();
+ pose.theta = _theta;
+ }
+
+ /**
+ * @brief Return the unit vector of the current orientation
+ * @returns [cos(theta), sin(theta))]^T
+ */
+ Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
+
+ ///@}
+
+
+ /** @name Arithmetic operations for which operators are not always reasonable */
+ ///@{
+
+ /**
+ * @brief Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]
+ * @param factor scale factor
+ */
+ void scale(double factor)
+ {
+ _position *= factor;
+ _theta = g2o::normalize_theta( _theta*factor );
+ }
+
+ /**
+ * @brief Increment the pose by adding a double[3] array
+ * The angle is normalized afterwards
+ * @param pose_as_array 3D double array [x, y, theta]
+ */
+ void plus(const double* pose_as_array)
+ {
+ _position.coeffRef(0) += pose_as_array[0];
+ _position.coeffRef(1) += pose_as_array[1];
+ _theta = g2o::normalize_theta( _theta + pose_as_array[2] );
+ }
+
+ /**
+ * @brief Get the mean / average of two poses and store it in the caller class
+ * For the position part: 0.5*(x1+x2)
+ * For the angle: take the angle of the mean direction vector
+ * @param pose1 first pose to consider
+ * @param pose2 second pose to consider
+ */
+ void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
+ {
+ _position = (pose1._position + pose2._position)/2;
+ _theta = g2o::average_angle(pose1._theta, pose2._theta);
+ }
+
+ /**
+ * @brief Get the mean / average of two poses and return the result (static)
+ * For the position part: 0.5*(x1+x2)
+ * For the angle: take the angle of the mean direction vector
+ * @param pose1 first pose to consider
+ * @param pose2 second pose to consider
+ * @return mean / average of \c pose1 and \c pose2
+ */
+ static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
+ {
+ return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
+ }
+
+ /**
+ * @brief Rotate pose globally
+ *
+ * Compute [pose_x, pose_y] = Rot(\c angle) * [pose_x, pose_y].
+ * if \c adjust_theta, pose_theta is also rotated by \c angle
+ * @param angle the angle defining the 2d rotation
+ * @param adjust_theta if \c true, the orientation theta is also rotated
+ */
+ void rotateGlobal(double angle, bool adjust_theta=true)
+ {
+ double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y();
+ double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y();
+ _position.x() = new_x;
+ _position.y() = new_y;
+ if (adjust_theta)
+ _theta = g2o::normalize_theta(_theta+angle);
+ }
+
+ ///@}
+
+
+ /** @name Operator overloads / Allow some arithmetic operations */
+ ///@{
+
+ /**
+ * @brief Asignment operator
+ * @param rhs PoseSE2 instance
+ * @todo exception safe version of the assignment operator
+ */
+ PoseSE2& operator=( const PoseSE2& rhs )
+ {
+ if (&rhs != this)
+ {
+ _position = rhs._position;
+ _theta = rhs._theta;
+ }
+ return *this;
+ }
+
+ /**
+ * @brief Compound assignment operator (addition)
+ * @param rhs addend
+ */
+ PoseSE2& operator+=(const PoseSE2& rhs)
+ {
+ _position += rhs._position;
+ _theta = g2o::normalize_theta(_theta + rhs._theta);
+ return *this;
+ }
+
+ /**
+ * @brief Arithmetic operator overload for additions
+ * @param lhs First addend
+ * @param rhs Second addend
+ */
+ friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs)
+ {
+ return lhs += rhs;
+ }
+
+ /**
+ * @brief Compound assignment operator (subtraction)
+ * @param rhs value to subtract
+ */
+ PoseSE2& operator-=(const PoseSE2& rhs)
+ {
+ _position -= rhs._position;
+ _theta = g2o::normalize_theta(_theta - rhs._theta);
+ return *this;
+ }
+
+ /**
+ * @brief Arithmetic operator overload for subtractions
+ * @param lhs First term
+ * @param rhs Second term
+ */
+ friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs)
+ {
+ return lhs -= rhs;
+ }
+
+ /**
+ * @brief Multiply pose with scalar and return copy without normalizing theta
+ * This operator is useful for calculating velocities ...
+ * @param pose pose to scale
+ * @param scalar factor to multiply with
+ * @warning theta is not normalized after multiplying
+ */
+ friend PoseSE2 operator*(PoseSE2 pose, double scalar)
+ {
+ pose._position *= scalar;
+ pose._theta *= scalar;
+ return pose;
+ }
+
+ /**
+ * @brief Multiply pose with scalar and return copy without normalizing theta
+ * This operator is useful for calculating velocities ...
+ * @param scalar factor to multiply with
+ * @param pose pose to scale
+ * @warning theta is not normalized after multiplying
+ */
+ friend PoseSE2 operator*(double scalar, PoseSE2 pose)
+ {
+ pose._position *= scalar;
+ pose._theta *= scalar;
+ return pose;
+ }
+
+ /**
+ * @brief Output stream operator
+ * @param stream output stream
+ * @param pose to be used
+ */
+ friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)
+ {
+ stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta;
+ return stream;
+ }
+
+ ///@}
+
+
+private:
+
+ Eigen::Vector2d _position;
+ double _theta;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+} // namespace teb_local_planner
+
+#endif // POSE_SE2_H_
diff --git a/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h b/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
new file mode 100644
index 0000000..2e7607d
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
@@ -0,0 +1,134 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef RECOVERY_BEHAVIORS_H__
+#define RECOVERY_BEHAVIORS_H__
+
+
+#include <boost/circular_buffer.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class FailureDetector
+ * @brief This class implements methods in order to detect if the robot got stucked or is oscillating
+ *
+ * The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot
+ * might got stucked or oscillates between left/right/forward/backwards motions.
+ */
+class FailureDetector
+{
+public:
+
+ /**
+ * @brief Default constructor
+ */
+ FailureDetector() {}
+
+ /**
+ * @brief destructor.
+ */
+ ~FailureDetector() {}
+
+ /**
+ * @brief Set buffer length (measurement history)
+ * @param length number of measurements to be kept
+ */
+ void setBufferLength(int length) {buffer_.set_capacity(length);}
+
+ /**
+ * @brief Add a new twist measurement to the internal buffer and compute a new decision
+ * @param twist geometry_msgs::msg::Twist velocity information
+ * @param v_max maximum forward translational velocity
+ * @param v_backwards_max maximum backward translational velocity
+ * @param omega_max maximum angular velocity
+ * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
+ * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
+ */
+ void update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps);
+
+ /**
+ * @brief Check if the robot got stucked
+ *
+ * This call does not compute the actual decision,
+ * since it is computed within each update() invocation.
+ * @return true if the robot got stucked, false otherwise.
+ */
+ bool isOscillating() const;
+
+ /**
+ * @brief Clear the current internal state
+ *
+ * This call also resets the internal buffer
+ */
+ void clear();
+
+protected:
+
+ /** Variables to be monitored */
+ struct VelMeasurement
+ {
+ double v = 0;
+ double omega = 0;
+ };
+
+ /**
+ * @brief Detect if the robot got stucked based on the current buffer content
+ *
+ * Afterwards the status might be checked using gotStucked();
+ * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
+ * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
+ * @return true if the robot got stucked, false otherwise
+ */
+ bool detect(double v_eps, double omega_eps);
+
+private:
+
+ boost::circular_buffer<VelMeasurement> buffer_; //!< Circular buffer to store the last measurements @see setBufferLength
+ bool oscillating_ = false; //!< Current state: true if robot is oscillating
+
+};
+
+
+} // namespace teb_local_planner
+
+#endif /* RECOVERY_BEHAVIORS_H__ */
diff --git a/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h b/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
new file mode 100644
index 0000000..65200f2
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
@@ -0,0 +1,684 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+
+#ifndef ROBOT_FOOTPRINT_MODEL_H
+#define ROBOT_FOOTPRINT_MODEL_H
+
+#include "teb_local_planner/pose_se2.h"
+#include "teb_local_planner/obstacles.h"
+#include <visualization_msgs/msg/marker.hpp>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class BaseRobotFootprintModel
+ * @brief Abstract class that defines the interface for robot footprint/contour models
+ *
+ * The robot model class is currently used in optimization only, since
+ * taking the navigation stack footprint into account might be
+ * inefficient. The footprint is only used for checking feasibility.
+ */
+class BaseRobotFootprintModel
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ */
+ BaseRobotFootprintModel()
+ {
+ }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~BaseRobotFootprintModel()
+ {
+ }
+
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const = 0;
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const = 0;
+
+ /**
+ * @brief Visualize the robot using a markers
+ *
+ * Fill a marker message with all necessary information (type, pose, scale and color).
+ * The header, namespace, id and marker lifetime will be overwritten.
+ * @param current_pose Current robot pose
+ * @param[out] markers container of marker messages describing the robot shape
+ * @param color Color of the footprint
+ */
+ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const {}
+
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius() = 0;
+
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+//! Abbrev. for shared obstacle pointers
+typedef std::shared_ptr<BaseRobotFootprintModel> RobotFootprintModelPtr;
+//! Abbrev. for shared obstacle const pointers
+typedef std::shared_ptr<const BaseRobotFootprintModel> RobotFootprintModelConstPtr;
+
+
+
+/**
+ * @class PointRobotShape
+ * @brief Class that defines a point-robot
+ *
+ * Instead of using a CircularRobotFootprint this class might
+ * be utitilzed and the robot radius can be added to the mininum distance
+ * parameter. This avoids a subtraction of zero each time a distance is calculated.
+ */
+class PointRobotFootprint : public BaseRobotFootprintModel
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ */
+ PointRobotFootprint() {}
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~PointRobotFootprint() {}
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
+ {
+ return obstacle->getMinimumDistance(current_pose.position());
+ }
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
+ {
+ return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t);
+ }
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius() {return 0.0;}
+
+};
+
+
+/**
+ * @class CircularRobotFootprint
+ * @brief Class that defines the a robot of circular shape
+ */
+class CircularRobotFootprint : public BaseRobotFootprintModel
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ * @param radius radius of the robot
+ */
+ CircularRobotFootprint(double radius) : radius_(radius) { }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~CircularRobotFootprint() { }
+
+ /**
+ * @brief Set radius of the circular robot
+ * @param radius radius of the robot
+ */
+ void setRadius(double radius) {radius_ = radius;}
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
+ {
+ return obstacle->getMinimumDistance(current_pose.position()) - radius_;
+ }
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
+ {
+ return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t) - radius_;
+ }
+
+ /**
+ * @brief Visualize the robot using a markers
+ *
+ * Fill a marker message with all necessary information (type, pose, scale and color).
+ * The header, namespace, id and marker lifetime will be overwritten.
+ * @param current_pose Current robot pose
+ * @param[out] markers container of marker messages describing the robot shape
+ * @param color Color of the footprint
+ */
+ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
+ {
+ markers.resize(1);
+ visualization_msgs::msg::Marker& marker = markers.back();
+ marker.type = visualization_msgs::msg::Marker::CYLINDER;
+ current_pose.toPoseMsg(marker.pose);
+ marker.scale.x = marker.scale.y = 2*radius_; // scale = diameter
+ marker.scale.z = 0.05;
+ marker.color = color;
+ }
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius() {return radius_;}
+
+private:
+
+ double radius_;
+};
+
+
+/**
+ * @class TwoCirclesRobotFootprint
+ * @brief Class that approximates the robot with two shifted circles
+ */
+class TwoCirclesRobotFootprint : public BaseRobotFootprintModel
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
+ * @param front_radius radius of the front circle
+ * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
+ * @param rear_radius radius of the front circle
+ */
+ TwoCirclesRobotFootprint(double front_offset, double front_radius, double rear_offset, double rear_radius)
+ : front_offset_(front_offset), front_radius_(front_radius), rear_offset_(rear_offset), rear_radius_(rear_radius) { }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~TwoCirclesRobotFootprint() { }
+
+ /**
+ * @brief Set parameters of the contour/footprint
+ * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
+ * @param front_radius radius of the front circle
+ * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
+ * @param rear_radius radius of the front circle
+ */
+ void setParameters(double front_offset, double front_radius, double rear_offset, double rear_radius)
+ {front_offset_=front_offset; front_radius_=front_radius; rear_offset_=rear_offset; rear_radius_=rear_radius;}
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
+ {
+ Eigen::Vector2d dir = current_pose.orientationUnitVec();
+ double dist_front = obstacle->getMinimumDistance(current_pose.position() + front_offset_*dir) - front_radius_;
+ double dist_rear = obstacle->getMinimumDistance(current_pose.position() - rear_offset_*dir) - rear_radius_;
+ return std::min(dist_front, dist_rear);
+ }
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
+ {
+ Eigen::Vector2d dir = current_pose.orientationUnitVec();
+ double dist_front = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() + front_offset_*dir, t) - front_radius_;
+ double dist_rear = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() - rear_offset_*dir, t) - rear_radius_;
+ return std::min(dist_front, dist_rear);
+ }
+
+ /**
+ * @brief Visualize the robot using a markers
+ *
+ * Fill a marker message with all necessary information (type, pose, scale and color).
+ * The header, namespace, id and marker lifetime will be overwritten.
+ * @param current_pose Current robot pose
+ * @param[out] markers container of marker messages describing the robot shape
+ * @param color Color of the footprint
+ */
+ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
+ {
+ Eigen::Vector2d dir = current_pose.orientationUnitVec();
+ if (front_radius_>0)
+ {
+ markers.push_back(visualization_msgs::msg::Marker());
+ visualization_msgs::msg::Marker& marker1 = markers.front();
+ marker1.type = visualization_msgs::msg::Marker::CYLINDER;
+ current_pose.toPoseMsg(marker1.pose);
+ marker1.pose.position.x += front_offset_*dir.x();
+ marker1.pose.position.y += front_offset_*dir.y();
+ marker1.scale.x = marker1.scale.y = 2*front_radius_; // scale = diameter
+// marker1.scale.z = 0.05;
+ marker1.color = color;
+
+ }
+ if (rear_radius_>0)
+ {
+ markers.push_back(visualization_msgs::msg::Marker());
+ visualization_msgs::msg::Marker& marker2 = markers.back();
+ marker2.type = visualization_msgs::msg::Marker::CYLINDER;
+ current_pose.toPoseMsg(marker2.pose);
+ marker2.pose.position.x -= rear_offset_*dir.x();
+ marker2.pose.position.y -= rear_offset_*dir.y();
+ marker2.scale.x = marker2.scale.y = 2*rear_radius_; // scale = diameter
+// marker2.scale.z = 0.05;
+ marker2.color = color;
+ }
+ }
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius()
+ {
+ double min_longitudinal = std::min(rear_offset_ + rear_radius_, front_offset_ + front_radius_);
+ double min_lateral = std::min(rear_radius_, front_radius_);
+ return std::min(min_longitudinal, min_lateral);
+ }
+
+private:
+
+ double front_offset_;
+ double front_radius_;
+ double rear_offset_;
+ double rear_radius_;
+
+};
+
+
+
+/**
+ * @class LineRobotFootprint
+ * @brief Class that approximates the robot with line segment (zero-width)
+ */
+class LineRobotFootprint : public BaseRobotFootprintModel
+{
+public:
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
+ * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
+ */
+ LineRobotFootprint(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end)
+ {
+ setLine(line_start, line_end);
+ }
+
+ /**
+ * @brief Default constructor of the abstract obstacle class (Eigen Version)
+ * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
+ * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
+ */
+ LineRobotFootprint(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
+ {
+ setLine(line_start, line_end);
+ }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~LineRobotFootprint() { }
+
+ /**
+ * @brief Set vertices of the contour/footprint
+ * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
+ */
+ void setLine(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end)
+ {
+ line_start_.x() = line_start.x;
+ line_start_.y() = line_start.y;
+ line_end_.x() = line_end.x;
+ line_end_.y() = line_end.y;
+ }
+
+ /**
+ * @brief Set vertices of the contour/footprint (Eigen version)
+ * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
+ */
+ void setLine(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
+ {
+ line_start_ = line_start;
+ line_end_ = line_end;
+ }
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
+ {
+ Eigen::Vector2d line_start_world;
+ Eigen::Vector2d line_end_world;
+ transformToWorld(current_pose, line_start_world, line_end_world);
+ return obstacle->getMinimumDistance(line_start_world, line_end_world);
+ }
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
+ {
+ Eigen::Vector2d line_start_world;
+ Eigen::Vector2d line_end_world;
+ transformToWorld(current_pose, line_start_world, line_end_world);
+ return obstacle->getMinimumSpatioTemporalDistance(line_start_world, line_end_world, t);
+ }
+
+ /**
+ * @brief Visualize the robot using a markers
+ *
+ * Fill a marker message with all necessary information (type, pose, scale and color).
+ * The header, namespace, id and marker lifetime will be overwritten.
+ * @param current_pose Current robot pose
+ * @param[out] markers container of marker messages describing the robot shape
+ * @param color Color of the footprint
+ */
+ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
+ {
+ markers.push_back(visualization_msgs::msg::Marker());
+ visualization_msgs::msg::Marker& marker = markers.front();
+ marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+ current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
+
+ // line
+ geometry_msgs::msg::Point line_start_world;
+ line_start_world.x = line_start_.x();
+ line_start_world.y = line_start_.y();
+ line_start_world.z = 0;
+ marker.points.push_back(line_start_world);
+
+ geometry_msgs::msg::Point line_end_world;
+ line_end_world.x = line_end_.x();
+ line_end_world.y = line_end_.y();
+ line_end_world.z = 0;
+ marker.points.push_back(line_end_world);
+
+ marker.scale.x = 0.05;
+ marker.color = color;
+ }
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius()
+ {
+ return 0.0; // lateral distance = 0.0
+ }
+
+private:
+
+ /**
+ * @brief Transforms a line to the world frame manually
+ * @param current_pose Current robot pose
+ * @param[out] line_start line_start_ in the world frame
+ * @param[out] line_end line_end_ in the world frame
+ */
+ void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const
+ {
+ double cos_th = std::cos(current_pose.theta());
+ double sin_th = std::sin(current_pose.theta());
+ line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y();
+ line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y();
+ line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y();
+ line_end_world.y() = current_pose.y() + sin_th * line_end_.x() + cos_th * line_end_.y();
+ }
+
+ Eigen::Vector2d line_start_;
+ Eigen::Vector2d line_end_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+/**
+ * @class PolygonRobotFootprint
+ * @brief Class that approximates the robot with a closed polygon
+ */
+class PolygonRobotFootprint : public BaseRobotFootprintModel
+{
+public:
+
+
+
+ /**
+ * @brief Default constructor of the abstract obstacle class
+ * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
+ */
+ PolygonRobotFootprint(const Point2dContainer& vertices) : vertices_(vertices) { }
+
+ /**
+ * @brief Virtual destructor.
+ */
+ virtual ~PolygonRobotFootprint() { }
+
+ /**
+ * @brief Set vertices of the contour/footprint
+ * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
+ */
+ void setVertices(const Point2dContainer& vertices) {vertices_ = vertices;}
+
+ /**
+ * @brief Calculate the distance between the robot and an obstacle
+ * @param current_pose Current robot pose
+ * @param obstacle Pointer to the obstacle
+ * @return Euclidean distance to the robot
+ */
+ virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
+ {
+ Point2dContainer polygon_world(vertices_.size());
+ transformToWorld(current_pose, polygon_world);
+ return obstacle->getMinimumDistance(polygon_world);
+ }
+
+ /**
+ * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
+ * @param current_pose robot pose, from which the distance to the obstacle is estimated
+ * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
+ * @param t time, for which the predicted distance to the obstacle is calculated
+ * @return Euclidean distance to the robot
+ */
+ virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
+ {
+ Point2dContainer polygon_world(vertices_.size());
+ transformToWorld(current_pose, polygon_world);
+ return obstacle->getMinimumSpatioTemporalDistance(polygon_world, t);
+ }
+
+ /**
+ * @brief Visualize the robot using a markers
+ *
+ * Fill a marker message with all necessary information (type, pose, scale and color).
+ * The header, namespace, id and marker lifetime will be overwritten.
+ * @param current_pose Current robot pose
+ * @param[out] markers container of marker messages describing the robot shape
+ * @param color Color of the footprint
+ */
+ virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
+ {
+ if (vertices_.empty())
+ return;
+
+ markers.push_back(visualization_msgs::msg::Marker());
+ visualization_msgs::msg::Marker& marker = markers.front();
+ marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+ current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
+
+ for (std::size_t i = 0; i < vertices_.size(); ++i)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = vertices_[i].x();
+ point.y = vertices_[i].y();
+ point.z = 0;
+ marker.points.push_back(point);
+ }
+ // add first point again in order to close the polygon
+ geometry_msgs::msg::Point point;
+ point.x = vertices_.front().x();
+ point.y = vertices_.front().y();
+ point.z = 0;
+ marker.points.push_back(point);
+
+ marker.scale.x = 0.025;
+ marker.color = color;
+
+ }
+
+ /**
+ * @brief Compute the inscribed radius of the footprint model
+ * @return inscribed radius
+ */
+ virtual double getInscribedRadius()
+ {
+ double min_dist = std::numeric_limits<double>::max();
+ Eigen::Vector2d center(0.0, 0.0);
+
+ if (vertices_.size() <= 2)
+ return 0.0;
+
+ for (int i = 0; i < (int)vertices_.size() - 1; ++i)
+ {
+ // compute distance from the robot center point to the first vertex
+ double vertex_dist = vertices_[i].norm();
+ double edge_dist = distance_point_to_segment_2d(center, vertices_[i], vertices_[i+1]);
+ min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
+ }
+
+ // we also need to check the last vertex and the first vertex
+ double vertex_dist = vertices_.back().norm();
+ double edge_dist = distance_point_to_segment_2d(center, vertices_.back(), vertices_.front());
+ return std::min(min_dist, std::min(vertex_dist, edge_dist));
+ }
+
+private:
+
+ /**
+ * @brief Transforms a polygon to the world frame manually
+ * @param current_pose Current robot pose
+ * @param[out] polygon_world polygon in the world frame
+ */
+ void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const
+ {
+ double cos_th = std::cos(current_pose.theta());
+ double sin_th = std::sin(current_pose.theta());
+ for (std::size_t i=0; i<vertices_.size(); ++i)
+ {
+ polygon_world[i].x() = current_pose.x() + cos_th * vertices_[i].x() - sin_th * vertices_[i].y();
+ polygon_world[i].y() = current_pose.y() + sin_th * vertices_[i].x() + cos_th * vertices_[i].y();
+ }
+ }
+
+ Point2dContainer vertices_;
+
+};
+
+
+
+
+
+} // namespace teb_local_planner
+
+#endif /* ROBOT_FOOTPRINT_MODEL_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/teb_config.h b/src/teb_local_planner/include/teb_local_planner/teb_config.h
new file mode 100644
index 0000000..9a70daa
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/teb_config.h
@@ -0,0 +1,432 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef TEB_CONFIG_H_
+#define TEB_CONFIG_H_
+
+#include <nav2_util/lifecycle_node.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <nav_2d_utils/parameters.hpp>
+#include "teb_local_planner/robot_footprint_model.h"
+#include <nav2_costmap_2d/footprint.hpp>
+
+// Definitions
+#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi
+
+namespace teb_local_planner
+{
+/**
+ * @class TebConfig
+ * @brief Config class for the teb_local_planner and its components.
+ */
+class TebConfig
+{
+public:
+ using UniquePtr = std::unique_ptr<TebConfig>;
+
+ std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator
+ std::string map_frame; //!< Global planning frame
+ std::string node_name; //!< node name used for parameter event callback
+
+ RobotFootprintModelPtr robot_model;
+ std::string model_name;
+ double radius;
+ std::vector<double> line_start, line_end;
+ double front_offset, front_radius, rear_offset, rear_radius;
+ std::string footprint_string;
+
+ //! Trajectory related parameters
+ struct Trajectory
+ {
+ double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended)
+ double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate)
+ double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
+ int min_samples; //!< Minimum number of samples (should be always greater than 2)
+ int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore.
+ bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner
+ bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)
+ double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled)
+ bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container
+ double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!]
+ double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning
+ bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
+ double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
+ double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting)
+ int feasibility_check_no_poses; //!< Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.
+ double feasibility_check_lookahead_distance; //!< Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.
+ bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
+ double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]
+ int control_look_ahead_poses; //! Index of the pose used to extract the velocity command
+ } trajectory; //!< Trajectory related parameters
+
+ //! Robot related parameters
+ struct Robot
+ {
+ double base_max_vel_x; //!< Maximum translational velocity of the robot before speed limit is applied
+ double base_max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards before speed limit is applied
+ double base_max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) before speed limit is applied
+ double base_max_vel_theta; //!< Maximum angular velocity of the robot before speed limit is applied
+ double max_vel_x; //!< Maximum translational velocity of the robot
+ double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
+ double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
+ double max_vel_theta; //!< Maximum angular velocity of the robot
+ double acc_lim_x; //!< Maximum translational acceleration of the robot
+ double acc_lim_y; //!< Maximum strafing acceleration of the robot
+ double acc_lim_theta; //!< Maximum angular acceleration of the robot
+ double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero);
+ double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
+ bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
+ bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
+ bool use_proportional_saturation; //<! If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually
+ double transform_tolerance = 0.5; //<! Tolerance when querying the TF Tree for a transformation (seconds)
+ } robot; //!< Robot related parameters
+
+ //! Goal tolerance related parameters
+ struct GoalTolerance
+ {
+ double xy_goal_tolerance; //!< Allowed final euclidean distance to the goal position
+ bool free_goal_vel; //!< Allow the robot's velocity to be nonzero (usally max_vel) for planning purposes
+ } goal_tolerance; //!< Goal tolerance related parameters
+
+ //! Obstacle related parameters
+ struct Obstacles
+ {
+ double min_obstacle_dist; //!< Minimum desired separation from obstacles
+ double inflation_dist; //!< buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
+ double dynamic_obstacle_inflation_dist; //!< Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
+ bool include_dynamic_obstacles; //!< Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also effects homotopy class planning); If false, all obstacles are considered to be static.
+ bool include_costmap_obstacles; //!< Specify whether the obstacles in the costmap should be taken into account directly
+ double costmap_obstacles_behind_robot_dist; //!< Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
+ int obstacle_poses_affected; //!< The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
+ bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only "relevant" obstacles).
+ double obstacle_association_force_inclusion_factor; //!< The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
+ double obstacle_association_cutoff_factor; //!< See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
+ std::string costmap_converter_plugin; //!< Define a plugin name of the costmap_converter package (costmap cells are converted to points/lines/polygons)
+ bool costmap_converter_spin_thread; //!< If \c true, the costmap converter invokes its callback queue in a different thread
+ int costmap_converter_rate; //!< The rate that defines how often the costmap_converter plugin processes the current costmap (the value should not be much higher than the costmap update rate)
+ double obstacle_proximity_ratio_max_vel; //!< Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to a static obstacles
+ double obstacle_proximity_lower_bound; //!< Distance to a static obstacle for which the velocity should be lower
+ double obstacle_proximity_upper_bound; //!< Distance to a static obstacle for which the velocity should be higher
+ } obstacles; //!< Obstacle related parameters
+
+
+ //! Optimization related parameters
+ struct Optimization
+ {
+ int no_inner_iterations; //!< Number of solver iterations called in each outerloop iteration
+ int no_outer_iterations; //!< Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
+
+ bool optimization_activate; //!< Activate the optimization
+ bool optimization_verbose; //!< Print verbose information
+
+ double penalty_epsilon; //!< Add a small safety margin to penalty functions for hard-constraint approximations
+
+ double weight_max_vel_x; //!< Optimization weight for satisfying the maximum allowed translational velocity
+ double weight_max_vel_y; //!< Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
+ double weight_max_vel_theta; //!< Optimization weight for satisfying the maximum allowed angular velocity
+ double weight_acc_lim_x; //!< Optimization weight for satisfying the maximum allowed translational acceleration
+ double weight_acc_lim_y; //!< Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
+ double weight_acc_lim_theta; //!< Optimization weight for satisfying the maximum allowed angular acceleration
+ double weight_kinematics_nh; //!< Optimization weight for satisfying the non-holonomic kinematics
+ double weight_kinematics_forward_drive; //!< Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
+ double weight_kinematics_turning_radius; //!< Optimization weight for enforcing a minimum turning radius (carlike robots)
+ double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t. transition time
+ double weight_shortest_path; //!< Optimization weight for contracting the trajectory w.r.t. path length
+ double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles
+ double weight_inflation; //!< Optimization weight for the inflation penalty (should be small)
+ double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles
+ double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles (should be small)
+ double weight_velocity_obstacle_ratio; //!< Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle
+ double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points
+ double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently only activated if an oscillation is detected, see 'oscillation_recovery'
+
+ double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
+ double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)
+ } optim; //!< Optimization related parameters
+
+
+ struct HomotopyClasses
+ {
+ bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).
+ bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel.
+ bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.
+ int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
+ int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima)
+ double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).
+ double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.
+ double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate.
+ double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' candidate.
+ bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time.
+ double selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.
+ double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed
+
+ int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off.
+ double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.
+ double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!
+ double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1).
+ double h_signature_threshold; //!< Two h-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.
+
+ double obstacle_keypoint_offset; //!< If simple_exploration is turned on, this parameter determines the distance on the left and right side of the obstacle at which a new keypoint will be cretead (in addition to min_obstacle_dist).
+ double obstacle_heading_threshold; //!< Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration [0,1]
+
+ bool viapoints_all_candidates; //!< If true, all trajectories of different topologies are attached to the current set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan.
+
+ bool visualize_hc_graph; //!< Visualize the graph that is created for exploring new homotopy classes.
+ double visualize_with_time_as_z_axis_scale; //!< If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.
+ bool delete_detours_backwards; //!< If enabled, the planner will discard the plans detouring backwards with respect to the best plan
+ double detours_orientation_tolerance; //!< A plan is considered a detour if its start orientation differs more than this from the best plan
+ double length_start_orientation_vector; //!< Length of the vector used to compute the start orientation of a plan
+ double max_ratio_detours_duration_best_duration; //!< Detours are discarted if their execution time / the execution time of the best teb is > this
+ } hcp;
+
+ //! Recovery/backup related parameters
+ struct Recovery
+ {
+ bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.
+ double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected.
+ bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards)
+ double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
+ double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
+ double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected.
+ double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations
+ bool divergence_detection_enable; //!< True to enable divergence detection.
+ int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.
+ } recovery; //!< Parameters related to recovery and backup strategies
+
+
+ /**
+ * @brief Construct the TebConfig using default values.
+ * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used,
+ * the default variables will be overwritten: \n
+ * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a
+ * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications.
+ * All parameters considered by the dynamic_reconfigure server (and their \b default values) are
+ * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n
+ * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file.
+ * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters.
+ * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n
+ * <b>TebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults</b>
+ */
+ TebConfig()
+ {
+
+ odom_topic = "odom";
+ map_frame = "odom";
+
+ // Trajectory
+
+ trajectory.teb_autosize = true;
+ trajectory.dt_ref = 0.3;
+ trajectory.dt_hysteresis = 0.1;
+ trajectory.min_samples = 3;
+ trajectory.max_samples = 500;
+ trajectory.global_plan_overwrite_orientation = true;
+ trajectory.allow_init_with_backwards_motion = false;
+ trajectory.global_plan_viapoint_sep = -1;
+ trajectory.via_points_ordered = false;
+ trajectory.max_global_plan_lookahead_dist = 1;
+ trajectory.global_plan_prune_distance = 1;
+ trajectory.exact_arc_length = false;
+ trajectory.force_reinit_new_goal_dist = 1;
+ trajectory.force_reinit_new_goal_angular = 0.5 * M_PI;
+ trajectory.feasibility_check_no_poses = 5;
+ trajectory.feasibility_check_lookahead_distance = -1;
+ trajectory.publish_feedback = false;
+ trajectory.min_resolution_collision_check_angular = M_PI;
+ trajectory.control_look_ahead_poses = 1;
+
+ // Robot
+
+ robot.max_vel_x = 0.4;
+ robot.max_vel_x_backwards = 0.2;
+ robot.max_vel_y = 0.0;
+ robot.max_vel_theta = 0.3;
+ robot.base_max_vel_x = robot.max_vel_x;
+ robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
+ robot.base_max_vel_y = robot.base_max_vel_y;
+ robot.base_max_vel_theta = robot.base_max_vel_theta;
+ robot.acc_lim_x = 0.5;
+ robot.acc_lim_y = 0.5;
+ robot.acc_lim_theta = 0.5;
+ robot.min_turning_radius = 0;
+ robot.wheelbase = 1.0;
+ robot.cmd_angle_instead_rotvel = false;
+ robot.is_footprint_dynamic = false;
+ robot.use_proportional_saturation = false;
+
+ // GoalTolerance
+
+ goal_tolerance.xy_goal_tolerance = 0.2;
+ goal_tolerance.free_goal_vel = false;
+
+ // Obstacles
+
+ obstacles.min_obstacle_dist = 0.5;
+ obstacles.inflation_dist = 0.6;
+ obstacles.dynamic_obstacle_inflation_dist = 0.6;
+ obstacles.include_dynamic_obstacles = true;
+ obstacles.include_costmap_obstacles = true;
+ obstacles.costmap_obstacles_behind_robot_dist = 1.5;
+ obstacles.obstacle_poses_affected = 25;
+ obstacles.legacy_obstacle_association = false;
+ obstacles.obstacle_association_force_inclusion_factor = 1.5;
+ obstacles.obstacle_association_cutoff_factor = 5;
+ obstacles.costmap_converter_plugin = "";
+ obstacles.costmap_converter_spin_thread = true;
+ obstacles.costmap_converter_rate = 5;
+ obstacles.obstacle_proximity_ratio_max_vel = 1;
+ obstacles.obstacle_proximity_lower_bound = 0;
+ obstacles.obstacle_proximity_upper_bound = 0.5;
+
+ // Optimization
+
+ optim.no_inner_iterations = 5;
+ optim.no_outer_iterations = 4;
+ optim.optimization_activate = true;
+ optim.optimization_verbose = false;
+ optim.penalty_epsilon = 0.05;
+ optim.weight_max_vel_x = 2; //1
+ optim.weight_max_vel_y = 2;
+ optim.weight_max_vel_theta = 1;
+ optim.weight_acc_lim_x = 1;
+ optim.weight_acc_lim_y = 1;
+ optim.weight_acc_lim_theta = 1;
+ optim.weight_kinematics_nh = 1000;
+ optim.weight_kinematics_forward_drive = 1;
+ optim.weight_kinematics_turning_radius = 1;
+ optim.weight_optimaltime = 1;
+ optim.weight_shortest_path = 0;
+ optim.weight_obstacle = 50;
+ optim.weight_inflation = 0.1;
+ optim.weight_dynamic_obstacle = 50;
+ optim.weight_dynamic_obstacle_inflation = 0.1;
+ optim.weight_velocity_obstacle_ratio = 0;
+ optim.weight_viapoint = 1;
+ optim.weight_prefer_rotdir = 50;
+
+ optim.weight_adapt_factor = 2.0;
+ optim.obstacle_cost_exponent = 1.0;
+
+ // Homotopy Class Planner
+
+ hcp.enable_homotopy_class_planning = true;
+ hcp.enable_multithreading = true;
+ hcp.simple_exploration = false;
+ hcp.max_number_classes = 5;
+ hcp.selection_cost_hysteresis = 1.0;
+ hcp.selection_prefer_initial_plan = 0.95;
+ hcp.selection_obst_cost_scale = 100.0;
+ hcp.selection_viapoint_cost_scale = 1.0;
+ hcp.selection_alternative_time_cost = false;
+ hcp.selection_dropping_probability = 0.0;
+
+ hcp.obstacle_keypoint_offset = 0.1;
+ hcp.obstacle_heading_threshold = 0.45;
+ hcp.roadmap_graph_no_samples = 15;
+ hcp.roadmap_graph_area_width = 6; // [m]
+ hcp.roadmap_graph_area_length_scale = 1.0;
+ hcp.h_signature_prescaler = 1;
+ hcp.h_signature_threshold = 0.1;
+ hcp.switching_blocking_period = 0.0;
+
+ hcp.viapoints_all_candidates = true;
+
+ hcp.visualize_hc_graph = false;
+ hcp.visualize_with_time_as_z_axis_scale = 0.0;
+ hcp.delete_detours_backwards = true;
+ hcp.detours_orientation_tolerance = M_PI / 2.0;
+ hcp.length_start_orientation_vector = 0.4;
+ hcp.max_ratio_detours_duration_best_duration = 3.0;
+
+ // Recovery
+
+ recovery.shrink_horizon_backup = true;
+ recovery.shrink_horizon_min_duration = 10;
+ recovery.oscillation_recovery = true;
+ recovery.oscillation_v_eps = 0.1;
+ recovery.oscillation_omega_eps = 0.1;
+ recovery.oscillation_recovery_min_duration = 10;
+ recovery.oscillation_filter_duration = 10;
+ recovery.divergence_detection_enable = false;
+ recovery.divergence_detection_max_chi_squared = 10;
+ }
+
+ void declareParameters(const nav2_util::LifecycleNode::SharedPtr, const std::string name);
+
+ /**
+ * @brief Load parmeters from the ros param server.
+ * @param nh const reference to the local rclcpp::Node::SharedPtr
+ */
+ void loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name);
+
+ /**
+ * @brief Callback executed when a paramter change is detected
+ * @param parameters list of changed parameters
+ */
+ rcl_interfaces::msg::SetParametersResult
+ dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+ /**
+ * @brief Check parameters and print warnings in case of discrepancies
+ *
+ * Call this method whenever parameters are changed using public interfaces to inform the user
+ * about some improper uses.
+ */
+ void checkParameters() const;
+
+ /**
+ * @brief Check if some deprecated parameters are found and print warnings
+ * @param nh const reference to the local rclcpp::Node::SharedPtr
+ */
+ void checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const;
+
+ /**
+ * @brief Return the internal config mutex
+ */
+ std::mutex& configMutex() {return config_mutex_;}
+
+private:
+ std::mutex config_mutex_; //!< Mutex for config accesses and changes
+ rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
+};
+} // namespace teb_local_planner
+
+#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h b/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
new file mode 100644
index 0000000..5e67792
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
@@ -0,0 +1,426 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef TEB_LOCAL_PLANNER_ROS_H_
+#define TEB_LOCAL_PLANNER_ROS_H_
+
+#include <pluginlib/class_loader.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+// Navigation2 local planner base class and utilities
+#include <nav2_core/controller.hpp>
+
+// timed-elastic-band related classes
+#include "teb_local_planner/optimal_planner.h"
+#include "teb_local_planner/homotopy_class_planner.h"
+#include "teb_local_planner/visualization.h"
+#include "teb_local_planner/recovery_behaviors.h"
+
+// message types
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <costmap_converter_msgs/msg/obstacle_msg.hpp>
+
+// transforms
+#include <tf2_ros/transform_listener.h>
+#include <tf2/transform_datatypes.h>
+
+// costmap
+#include <costmap_converter/costmap_converter_interface.h>
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+
+#include <nav2_util/lifecycle_node.hpp>
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <nav_2d_utils/parameters.hpp>
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+// dynamic reconfigure
+//#include "teb_local_planner/TebLocalPlannerReconfigureConfig.h>
+//#include <dynamic_reconfigure/server.h>
+
+
+namespace teb_local_planner
+{
+using TFBufferPtr = std::shared_ptr<tf2_ros::Buffer>;
+using CostmapROSPtr = std::shared_ptr<nav2_costmap_2d::Costmap2DROS>;
+
+/**
+ * @class TebLocalPlannerROS
+ * @brief Implements the actual abstract navigation stack routines of the teb_local_planner plugin
+ * @todo Escape behavior, more efficient obstacle handling
+ */
+class TebLocalPlannerROS : public nav2_core::Controller
+{
+
+public:
+ /**
+ * @brief Constructor of the teb plugin
+ */
+ TebLocalPlannerROS();
+
+ /**
+ * @brief Destructor of the plugin
+ */
+ ~TebLocalPlannerROS();
+
+ /**
+ * @brief Configure the teb plugin
+ *
+ * @param node The node of the instance
+ * @param tf Pointer to a transform listener
+ * @param costmap_ros Cost map representing occupied and free space
+ */
+ void configure(
+ const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+ std::string name,
+ std::shared_ptr<tf2_ros::Buffer> tf,
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+ void activate() override;
+ void deactivate() override;
+ void cleanup() override;
+
+ /**
+ * @brief Initializes the teb plugin
+ */
+ void initialize(nav2_util::LifecycleNode::SharedPtr node);
+
+ /**
+ * @brief Set the plan that the teb local planner is following
+ * @param orig_global_plan The plan to pass to the local planner
+ * @return
+ */
+ void setPlan(const nav_msgs::msg::Path & orig_global_plan) override;
+
+ /**
+ * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
+ * @param pose is the current position
+ * @param velocity is the current velocity
+ * @return velocity commands to send to the base
+ */
+ geometry_msgs::msg::TwistStamped computeVelocityCommands(
+ const geometry_msgs::msg::PoseStamped &pose,
+ const geometry_msgs::msg::Twist &velocity,
+ nav2_core::GoalChecker * goal_checker);
+
+
+ /** @name Public utility functions/methods */
+ //@{
+
+ /**
+ * @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.
+ *
+ * Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).
+ * @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)
+ * @return Translational and angular velocity combined into an Eigen::Vector2d
+ */
+// static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
+
+ /**
+ * @brief Get the current robot footprint/contour model
+ * @param nh const reference to the local rclcpp::Node::SharedPtr
+ * @return Robot footprint model used for optimization
+ */
+ RobotFootprintModelPtr getRobotFootprintFromParamServer(nav2_util::LifecycleNode::SharedPtr node);
+
+ /**
+ * @brief Set the footprint from the given XmlRpcValue.
+ * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
+ * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point
+ * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the
+ * sub-arrays should all have exactly 2 elements (x and y coordinates).
+ * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
+ * It is used only for reporting errors.
+ * @return container of vertices describing the polygon
+ */
+// Using ROS2 parameter server
+// static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
+
+ /**
+ * @brief Get a number from the given XmlRpcValue.
+ * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
+ * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point
+ * @param value double value type
+ * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
+ * It is used only for reporting errors.
+ * @returns double value
+ */
+// Using ROS2 parameter server
+// static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
+
+ //@}
+
+protected:
+
+ /**
+ * @brief Update internal obstacle vector based on occupied costmap cells
+ * @remarks All occupied cells will be added as point obstacles.
+ * @remarks All previous obstacles are cleared.
+ * @sa updateObstacleContainerWithCostmapConverter
+ * @todo Include temporal coherence among obstacle msgs (id vector)
+ * @todo Include properties for dynamic obstacles (e.g. using constant velocity model)
+ */
+ void updateObstacleContainerWithCostmap();
+
+ /**
+ * @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin
+ * @remarks Requires a loaded costmap_converter plugin.
+ * @remarks All previous obstacles are cleared.
+ * @sa updateObstacleContainerWithCostmap
+ */
+ void updateObstacleContainerWithCostmapConverter();
+
+ /**
+ * @brief Update internal obstacle vector based on custom messages received via subscriber
+ * @remarks All previous obstacles are NOT cleared. Call this method after other update methods.
+ * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter
+ */
+ void updateObstacleContainerWithCustomObstacles();
+
+
+ /**
+ * @brief Update internal via-point container based on the current reference plan
+ * @remarks All previous via-points will be cleared.
+ * @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame)
+ * @param min_separation minimum separation between two consecutive via-points
+ */
+ void updateViaPointsContainer(const std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, double min_separation);
+
+
+ /**
+ * @brief Callback for the dynamic_reconfigure node.
+ *
+ * This callback allows to modify parameters dynamically at runtime without restarting the node
+ * @param config Reference to the dynamic reconfigure config
+ * @param level Dynamic reconfigure level
+ */
+ // TODO : dynamic reconfigure is not supported on ROS2
+// void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
+
+
+ /**
+ * @brief Callback for custom obstacles that are not obtained from the costmap
+ * @param obst_msg pointer to the message containing a list of polygon shaped obstacles
+ */
+ void customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg);
+
+ /**
+ * @brief Callback for custom via-points
+ * @param via_points_msg pointer to the message containing a list of via-points
+ */
+ void customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg);
+
+ /**
+ * @brief Prune global plan such that already passed poses are cut off
+ *
+ * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform.
+ * If no valid transformation can be found, the method returns \c false.
+ * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot.
+ * If no pose within the specified treshold \c dist_behind_robot can be found,
+ * nothing will be pruned and the method returns \c false.
+ * @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.
+ * @param global_pose The global pose of the robot
+ * @param[in,out] global_plan The plan to be transformed
+ * @param dist_behind_robot Distance behind the robot that should be kept [meters]
+ * @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold
+ */
+ bool pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose,
+ std::vector<geometry_msgs::msg::PoseStamped>& global_plan, double dist_behind_robot=1);
+
+ /**
+ * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
+ *
+ * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
+ * such that the index of the current goal pose is returned as well as
+ * the transformation between the global plan and the planning frame.
+ * @param global_plan The plan to be transformed
+ * @param global_pose The global pose of the robot
+ * @param costmap A reference to the costmap being used so the window size for transforming can be computed
+ * @param global_frame The frame to transform the plan to
+ * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
+ * @param[out] transformed_plan Populated with the transformed plan
+ * @param[out] current_goal_idx Index of the current (local) goal pose in the global plan
+ * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
+ * @return \c true if the global plan is transformed, \c false otherwise
+ */
+ bool transformGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan,
+ const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap,
+ const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan,
+ int* current_goal_idx = NULL, geometry_msgs::msg::TransformStamped* tf_plan_to_global = NULL) const;
+
+ /**
+ * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.
+ *
+ * If the current (local) goal point is not the final one (global)
+ * substitute the goal orientation by the angle of the direction vector between
+ * the local goal and the subsequent pose of the global plan.
+ * This is often helpful, if the global planner does not consider orientations. \n
+ * A moving average filter is utilized to smooth the orientation.
+ * @param global_plan The global plan
+ * @param local_goal Current local goal
+ * @param current_goal_idx Index of the current (local) goal pose in the global plan
+ * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
+ * @param moving_average_length number of future poses of the global plan to be taken into account
+ * @return orientation (yaw-angle) estimate
+ */
+ double estimateLocalGoalOrientation(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan, const geometry_msgs::msg::PoseStamped& local_goal,
+ int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length=3) const;
+
+
+ /**
+ * @brief Saturate the translational and angular velocity to given limits.
+ *
+ * The limit of the translational velocity for backwards driving can be changed independently.
+ * Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for
+ * penalizing backwards driving instead.
+ * @param[in,out] vx The translational velocity that should be saturated.
+ * @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots
+ * @param[in,out] omega The angular velocity that should be saturated.
+ * @param max_vel_x Maximum translational velocity for forward driving
+ * @param max_vel_y Maximum strafing velocity (for holonomic robots)
+ * @param max_vel_theta Maximum (absolute) angular velocity
+ * @param max_vel_x_backwards Maximum translational velocity for backwards driving
+ */
+ void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
+ double max_vel_theta, double max_vel_x_backwards) const;
+
+
+ /**
+ * @brief Convert translational and rotational velocities to a steering angle of a carlike robot
+ *
+ * The conversion is based on the following equations:
+ * - The turning radius is defined by \f$ R = v/omega \f$
+ * - For a car like robot withe a distance L between both axles, the relation is: \f$ tan(\phi) = L/R \f$
+ * - phi denotes the steering angle.
+ * @remarks You might provide distances instead of velocities, since the temporal information is not required.
+ * @param v translational velocity [m/s]
+ * @param omega rotational velocity [rad/s]
+ * @param wheelbase distance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots
+ * @param min_turning_radius Specify a lower bound on the turning radius
+ * @return Resulting steering angle in [rad] inbetween [-pi/2, pi/2]
+ */
+ double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
+
+ /**
+ * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint
+ *
+ * This method prints warnings if validation fails.
+ * @remarks Currently, we only validate the inscribed radius of the footprints
+ * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization
+ * @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap
+ * @param min_obst_dist desired distance to obstacles
+ */
+ void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
+
+
+ void configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int& goal_idx);
+
+ /**
+ * @brief Limits the maximum linear speed of the robot.
+ * @param speed_limit expressed in absolute value (in m/s)
+ * or in percentage from maximum robot speed.
+ * @param percentage Setting speed limit in percentage if true
+ * or in absolute values in false case.
+ */
+ void setSpeedLimit(const double & speed_limit, const bool & percentage);
+
+private:
+ // Definition of member variables
+ rclcpp_lifecycle::LifecycleNode::WeakPtr nh_;
+ rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
+ rclcpp::Clock::SharedPtr clock_;
+ rclcpp::Node::SharedPtr intra_proc_node_;
+ // external objects (store weak pointers)
+ CostmapROSPtr costmap_ros_; //!< Pointer to the costmap ros wrapper, received from the navigation stack
+ nav2_costmap_2d::Costmap2D* costmap_; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper)
+ TFBufferPtr tf_; //!< pointer to Transform Listener
+ TebConfig::UniquePtr cfg_; //!< Config class that stores and manages all related parameters
+
+ // internal objects (memory management owned)
+ PlannerInterfacePtr planner_; //!< Instance of the underlying optimal planner class
+ ObstContainer obstacles_; //!< Obstacle vector that should be considered during local trajectory optimization
+ ViaPointContainer via_points_; //!< Container of via-points that should be considered during local trajectory optimization
+ TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
+ std::shared_ptr<dwb_critics::ObstacleFootprintCritic> costmap_model_;
+ FailureDetector failure_detector_; //!< Detect if the robot got stucked
+
+ std::vector<geometry_msgs::msg::PoseStamped> global_plan_; //!< Store the current global plan
+
+ pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> costmap_converter_loader_; //!< Load costmap converter plugins at runtime
+ std::shared_ptr<costmap_converter::BaseCostmapToPolygons> costmap_converter_; //!< Store the current costmap_converter
+
+ //std::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
+ rclcpp::Subscription<costmap_converter_msgs::msg::ObstacleArrayMsg>::SharedPtr custom_obst_sub_; //!< Subscriber for custom obstacles received via a ObstacleMsg.
+ std::mutex custom_obst_mutex_; //!< Mutex that locks the obstacle array (multi-threaded)
+ costmap_converter_msgs::msg::ObstacleArrayMsg custom_obstacle_msg_; //!< Copy of the most recent obstacle message
+
+ rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr via_points_sub_; //!< Subscriber for custom via-points received via a Path msg.
+ bool custom_via_points_active_; //!< Keep track whether valid via-points have been received from via_points_sub_
+ std::mutex via_point_mutex_; //!< Mutex that locks the via_points container (multi-threaded)
+
+ PoseSE2 robot_pose_; //!< Store current robot pose
+ PoseSE2 robot_goal_; //!< Store current robot goal
+ geometry_msgs::msg::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega)
+ rclcpp::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected
+ int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan.
+ rclcpp::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected
+ RotType last_preferred_rotdir_; //!< Store recent preferred turning direction
+ geometry_msgs::msg::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands()
+
+ std::vector<geometry_msgs::msg::Point> footprint_spec_; //!< Store the footprint of the robot
+ double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible)
+ double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot
+
+ // flags
+ bool initialized_; //!< Keeps track about the correct initialization of this class
+ std::string name_; //!< Name of plugin ID
+
+protected:
+ // Dynamic parameters handler
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}; // end namespace teb_local_planner
+
+#endif // TEB_LOCAL_PLANNER_ROS_H_
+
+
diff --git a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
new file mode 100644
index 0000000..f720906
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
@@ -0,0 +1,636 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef TIMED_ELASTIC_BAND_H_
+#define TIMED_ELASTIC_BAND_H_
+#include <boost/optional.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose2_d.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <tf2/buffer_core.h>
+
+#include <cassert>
+#include <complex>
+#include <iterator>
+
+#include "teb_local_planner/obstacles.h"
+
+// G2O Types
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+
+
+namespace teb_local_planner
+{
+
+//! Container of poses that represent the spatial part of the trajectory
+typedef std::vector<VertexPose*> PoseSequence;
+//! Container of time differences that define the temporal of the trajectory
+typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
+
+
+/**
+ * @class TimedElasticBand
+ * @brief Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.
+ *
+ * All trajectory related methods (initialization, modifying, ...) are implemented inside this class. \n
+ * Let \f$ Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \f$ be a sequence of poses, \n
+ * in which \f$ \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \f$ denotes a single pose of the robot. \n
+ * The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between
+ * two consecutive poses, resuting in a sequence of \c n-1 time intervals \f$ \Delta T_i \f$: \n
+ * \f$ \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \f$. \n
+ * Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration.
+ * The tuple of both sequences defines the underlying trajectory.
+ *
+ * Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. \n
+ * TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.
+ *
+ * @todo Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.
+ */
+class TimedElasticBand
+{
+public:
+
+ /**
+ * @brief Construct the class
+ */
+ TimedElasticBand();
+
+ /**
+ * @brief Destruct the class
+ */
+ virtual ~TimedElasticBand();
+
+
+
+ /** @name Access pose and timediff sequences */
+ //@{
+
+ /**
+ * @brief Access the complete pose sequence
+ * @return reference to the pose sequence
+ */
+ PoseSequence& poses() {return pose_vec_;};
+
+ /**
+ * @brief Access the complete pose sequence (read-only)
+ * @return const reference to the pose sequence
+ */
+ const PoseSequence& poses() const {return pose_vec_;};
+
+ /**
+ * @brief Access the complete timediff sequence
+ * @return reference to the dimediff sequence
+ */
+ TimeDiffSequence& timediffs() {return timediff_vec_;};
+
+ /**
+ * @brief Access the complete timediff sequence
+ * @return reference to the dimediff sequence
+ */
+ const TimeDiffSequence& timediffs() const {return timediff_vec_;};
+
+ /**
+ * @brief Access the time difference at pos \c index of the time sequence
+ * @param index element position inside the internal TimeDiffSequence
+ * @return reference to the time difference at pos \c index
+ */
+ double& TimeDiff(int index)
+ {
+ assert(index<sizeTimeDiffs());
+ return timediff_vec_.at(index)->dt();
+ }
+
+ /**
+ * @brief Access the time difference at pos \c index of the time sequence (read-only)
+ * @param index element position inside the internal TimeDiffSequence
+ * @return const reference to the time difference at pos \c index
+ */
+ const double& TimeDiff(int index) const
+ {
+ assert(index<sizeTimeDiffs());
+ return timediff_vec_.at(index)->dt();
+ }
+
+ /**
+ * @brief Access the pose at pos \c index of the pose sequence
+ * @param index element position inside the internal PoseSequence
+ * @return reference to the pose at pos \c index
+ */
+ PoseSE2& Pose(int index)
+ {
+ assert(index<sizePoses());
+ return pose_vec_.at(index)->pose();
+ }
+
+ /**
+ * @brief Access the pose at pos \c index of the pose sequence (read-only)
+ * @param index element position inside the internal PoseSequence
+ * @return const reference to the pose at pos \c index
+ */
+ const PoseSE2& Pose(int index) const
+ {
+ assert(index<sizePoses());
+ return pose_vec_.at(index)->pose();
+ }
+
+ /**
+ * @brief Access the last PoseSE2 in the pose sequence
+ */
+ PoseSE2& BackPose() {return pose_vec_.back()->pose(); }
+
+ /**
+ * @brief Access the last PoseSE2 in the pose sequence (read-only)
+ */
+ const PoseSE2& BackPose() const {return pose_vec_.back()->pose();}
+
+ /**
+ * @brief Access the last TimeDiff in the time diff sequence
+ */
+ double& BackTimeDiff() {return timediff_vec_.back()->dt(); }
+
+ /**
+ * @brief Access the last TimeDiff in the time diff sequence (read-only)
+ */
+ const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); }
+
+ /**
+ * @brief Access the vertex of a pose at pos \c index for optimization purposes
+ * @param index element position inside the internal PoseSequence
+ * @return Weak raw pointer to the pose vertex at pos \c index
+ */
+ VertexPose* PoseVertex(int index)
+ {
+ assert(index<sizePoses());
+ return pose_vec_.at(index);
+ }
+
+ /**
+ * @brief Access the vertex of a time difference at pos \c index for optimization purposes
+ * @param index element position inside the internal TimeDiffSequence
+ * @return Weak raw pointer to the timediff vertex at pos \c index
+ */
+ VertexTimeDiff* TimeDiffVertex(int index)
+ {
+ assert(index<sizeTimeDiffs());
+ return timediff_vec_.at(index);
+ }
+
+ //@}
+
+
+
+ /** @name Append new elements to the pose and timediff sequences */
+ //@{
+
+ /**
+ * @brief Append a new pose vertex to the back of the pose sequence
+ * @param pose PoseSE2 to push back on the internal PoseSequence
+ * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
+ */
+ void addPose(const PoseSE2& pose, bool fixed=false);
+
+ /**
+ * @brief Append a new pose vertex to the back of the pose sequence
+ * @param position 2D vector representing the position part
+ * @param theta yaw angle representing the orientation part
+ * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
+ */
+ void addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed=false);
+
+ /**
+ * @brief Append a new pose vertex to the back of the pose sequence
+ * @param x x-coordinate of the position part
+ * @param y y-coordinate of the position part
+ * @param theta yaw angle representing the orientation part
+ * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
+ */
+ void addPose(double x, double y, double theta, bool fixed=false);
+
+ /**
+ * @brief Append a new time difference vertex to the back of the time diff sequence
+ * @param dt time difference value to push back on the internal TimeDiffSequence
+ * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
+ */
+ void addTimeDiff(double dt, bool fixed=false);
+
+ /**
+ * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
+ * @param pose PoseSE2 to push back on the internal PoseSequence
+ * @param dt time difference value to push back on the internal TimeDiffSequence
+ * @warning Since the timediff is defined to connect two consecutive poses, this call is only
+ * allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...):
+ * therefore add a single pose using addPose() first!
+ */
+ void addPoseAndTimeDiff(const PoseSE2& pose, double dt);
+
+ /**
+ * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
+ * @param position 2D vector representing the position part
+ * @param theta yaw angle representing the orientation part
+ * @param dt time difference value to push back on the internal TimeDiffSequence
+ * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
+ */
+ void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt);
+
+ /**
+ * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
+ * @param x x-coordinate of the position part
+ * @param y y-coordinate of the position part
+ * @param theta yaw angle representing the orientation part
+ * @param dt time difference value to push back on the internal TimeDiffSequence
+ * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
+ */
+ void addPoseAndTimeDiff(double x, double y, double theta, double dt);
+
+ //@}
+
+
+ /** @name Insert new elements and remove elements of the pose and timediff sequences */
+ //@{
+
+ /**
+ * @brief Insert a new pose vertex at pos. \c index to the pose sequence
+ * @param index element position inside the internal PoseSequence
+ * @param pose PoseSE2 element to insert into the internal PoseSequence
+ */
+ void insertPose(int index, const PoseSE2& pose);
+
+ /**
+ * @brief Insert a new pose vertex at pos. \c index to the pose sequence
+ * @param index element position inside the internal PoseSequence
+ * @param position 2D vector representing the position part
+ * @param theta yaw-angle representing the orientation part
+ */
+ void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta);
+
+ /**
+ * @brief Insert a new pose vertex at pos. \c index to the pose sequence
+ * @param index element position inside the internal PoseSequence
+ * @param x x-coordinate of the position part
+ * @param y y-coordinate of the position part
+ * @param theta yaw-angle representing the orientation part
+ */
+ void insertPose(int index, double x, double y, double theta);
+
+ /**
+ * @brief Insert a new timediff vertex at pos. \c index to the timediff sequence
+ * @param index element position inside the internal TimeDiffSequence
+ * @param dt timediff value
+ */
+ void insertTimeDiff(int index, double dt);
+
+ /**
+ * @brief Delete pose at pos. \c index in the pose sequence
+ * @param index element position inside the internal PoseSequence
+ */
+ void deletePose(int index);
+
+ /**
+ * @brief Delete multiple (\c number) poses starting at pos. \c index in the pose sequence
+ * @param index first element position inside the internal PoseSequence
+ * @param number number of elements that should be deleted
+ */
+ void deletePoses(int index, int number);
+
+ /**
+ * @brief Delete pose at pos. \c index in the timediff sequence
+ * @param index element position inside the internal TimeDiffSequence
+ */
+ void deleteTimeDiff(int index);
+
+ /**
+ * @brief Delete multiple (\c number) time differences starting at pos. \c index in the timediff sequence
+ * @param index first element position inside the internal TimeDiffSequence
+ * @param number number of elements that should be deleted
+ */
+ void deleteTimeDiffs(int index, int number);
+
+ //@}
+
+
+ /** @name Init the trajectory */
+ //@{
+
+ /**
+ * @brief Initialize a trajectory between a given start and goal pose.
+ *
+ * The implemented algorithm subsamples the straight line between
+ * start and goal using a given discretiziation width. \n
+ * The discretization width can be defined in the euclidean space
+ * using the \c diststep parameter. Each time difference between two consecutive
+ * poses is initialized to \c timestep. \n
+ * If the \c diststep is chosen to be zero,
+ * the resulting trajectory contains the start and goal pose only.
+ * @param start PoseSE2 defining the start of the trajectory
+ * @param goal PoseSE2 defining the goal of the trajectory (final pose)
+ * @param diststep euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
+ * @param max_vel_x maximum translational velocity used for determining time differences
+ * @param min_samples Minimum number of samples that should be initialized at least
+ * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
+ * @return true if everything was fine, false otherwise
+ */
+ bool initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double max_vel_x=0.5, int min_samples = 3, bool guess_backwards_motion = false);
+
+
+ /**
+ * @brief Initialize a trajectory from a generic 2D reference path.
+ *
+ * The temporal information is determined using a given maximum velocity
+ * (2D vector containing the translational and angular velocity). \n
+ * A constant velocity profile is implemented. \n
+ * A possible maximum acceleration is considered if \c max_acceleration param is provided.
+ *
+ * Since the orientation is not included in the reference path, it can be provided seperately
+ * (e.g. from the robot pose and robot goal). \n
+ * Otherwise the goal heading will be used as start and goal orientation. \n
+ * The orientation along the trajectory will be determined using the connection vector
+ * between two consecutive positions of the reference path.
+ *
+ * The reference path is provided using a start and end iterator to a container class.
+ * You must provide a unary function that accepts the dereferenced iterator and returns
+ * a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.
+ *
+ * @param path_start start iterator of a generic 2d path
+ * @param path_end end iterator of a generic 2d path
+ * @param fun_position unary function that returns the Eigen::Vector2d object
+ * @param max_vel_x maximum translational velocity used for determining time differences
+ * @param max_vel_theta maximum angular velocity used for determining time differences
+ * @param max_acc_x specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
+ * @param max_acc_theta specify to satisfy a maxmimum angular acceleration and decceleration (optional)
+ * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
+ * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
+ * @param min_samples Minimum number of samples that should be initialized at least
+ * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
+ * @tparam BidirIter Bidirectional iterator type
+ * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
+ * @return true if everything was fine, false otherwise
+ * @remarks Use \c boost::none to skip optional arguments.
+ */
+ template<typename BidirIter, typename Fun>
+ bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
+ boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
+ boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false);
+
+ /**
+ * @brief Initialize a trajectory from a reference pose sequence (positions and orientations).
+ *
+ * This method initializes the timed elastic band using a pose container
+ * (e.g. as local plan from the ros navigation stack). \n
+ * The initial time difference between two consecutive poses can be uniformly set
+ * via the argument \c dt.
+ * @param plan vector of geometry_msgs::msg::PoseStamped
+ * @param max_vel_x maximum translational velocity used for determining time differences
+ * @param max_vel_theta maximum rotational velocity used for determining time differences
+ * @param estimate_orient if \c true, calculate orientation using the straight line distance vector between consecutive poses
+ * (only copy start and goal orientation; recommended if no orientation data is available).
+ * @param min_samples Minimum number of samples that should be initialized at least
+ * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if \c estimate_orient is enabled.
+ * @return true if everything was fine, false otherwise
+ */
+ bool initTrajectoryToGoal(const std::vector<geometry_msgs::msg::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false);
+
+ //@}
+
+ /** @name Update and modify the trajectory */
+ //@{
+
+
+ /**
+ * @brief Hot-Start from an existing trajectory with updated start and goal poses.
+ *
+ * This method updates a previously optimized trajectory with a new start and/or a new goal pose. \n
+ * The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start. \n
+ * Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed.
+ * The optimizer has to smooth the trajectory in TebOptimalPlanner. \n
+ *
+ * @todo Smooth the trajectory here and test the performance improvement of the optimization.
+ * @todo Implement a updateAndPruneTEB based on a new reference path / pose sequence.
+ *
+ * @param new_start New start pose (optional)
+ * @param new_goal New goal pose (optional)
+ * @param min_samples Specify the minimum number of samples that should at least remain in the trajectory
+ */
+ void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3);
+
+
+ /**
+ * @brief Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.
+ *
+ * Resizing the trajectory is helpful e.g. for the following scenarios:
+ *
+ * - Obstacles requires the teb to be extended in order to
+ * satisfy the given discritization width (plan resolution)
+ * and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$
+ * After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.
+ * - If the distance to the goal state is getting smaller,
+ * dt is decreasing as well. This leads to a heavily
+ * fine-grained discretization in combination with many
+ * discrete poses. Thus, the computation time will
+ * be/remain high and in addition numerical instabilities
+ * can appear (e.g. due to the division by a small \f$ \Delta T_i \f$).
+ *
+ * The implemented strategy checks all timediffs \f$ \Delta T_i \f$ and
+ *
+ * - inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$
+ * - removes a sample if \f$ \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \f$
+ *
+ * Each call only one new sample (pose-dt-pair) is inserted or removed.
+ * @param dt_ref reference temporal resolution
+ * @param dt_hysteresis hysteresis to avoid oscillations
+ * @param min_samples minimum number of samples that should be remain in the trajectory after resizing
+ * @param max_samples maximum number of samples that should not be exceeded during resizing
+ * @param fast_mode if true, the trajectory is iterated once to insert or erase points; if false the trajectory
+ * is repeatedly iterated until no poses are added or removed anymore
+ */
+ void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false);
+
+ /**
+ * @brief Set a pose vertex at pos \c index of the pose sequence to be fixed or unfixed during optimization.
+ * @param index index to the pose vertex
+ * @param status if \c true, the vertex will be fixed, otherwise unfixed
+ */
+ void setPoseVertexFixed(int index, bool status);
+
+ /**
+ * @brief Set a timediff vertex at pos \c index of the timediff sequence to be fixed or unfixed during optimization.
+ * @param index index to the timediff vertex
+ * @param status if \c true, the vertex will be fixed, otherwise unfixed
+ */
+ void setTimeDiffVertexFixed(int index, bool status);
+
+ /**
+ * @brief clear all poses and timediffs from the trajectory.
+ * The pose and timediff sequences will be empty and isInit() will return \c false
+ */
+ void clearTimedElasticBand();
+
+ //@}
+
+
+ /** @name Utility and status methods */
+ //@{
+
+ /**
+ * @brief Find the closest point on the trajectory w.r.t. to a provided reference point.
+ *
+ * This function can be useful to find the part of a trajectory that is close to an obstacle.
+ *
+ * @todo implement a more efficient version that first performs a coarse search.
+ * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
+ * Allows simple comparisons starting from the middle of the trajectory.
+ *
+ * @param ref_point reference point (2D position vector)
+ * @param[out] distance [optional] the resulting minimum distance
+ * @param begin_idx start search at this pose index
+ * @return Index to the closest pose in the pose sequence
+ */
+ int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance = NULL, int begin_idx=0) const;
+
+ /**
+ * @brief Find the closest point on the trajectory w.r.t. to a provided reference line.
+ *
+ * This function can be useful to find the part of a trajectory that is close to an (line) obstacle.
+ *
+ * @todo implement a more efficient version that first performs a coarse search.
+ * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
+ * Allows simple comparisons starting from the middle of the trajectory.
+ *
+ * @param ref_line_start start of the reference line (2D position vector)
+ * @param ref_line_end end of the reference line (2D position vector)
+ * @param[out] distance [optional] the resulting minimum distance
+ * @return Index to the closest pose in the pose sequence
+ */
+ int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance = NULL) const;
+
+ /**
+ * @brief Find the closest point on the trajectory w.r.t. to a provided reference polygon.
+ *
+ * This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.
+ *
+ * @todo implement a more efficient version that first performs a coarse search.
+ * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
+ * Allows simple comparisons starting from the middle of the trajectory.
+ *
+ * @param vertices vertex container containing Eigen::Vector2d points (the last and first point are connected)
+ * @param[out] distance [optional] the resulting minimum distance
+ * @return Index to the closest pose in the pose sequence
+ */
+ int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const;
+
+ /**
+ * @brief Find the closest point on the trajectory w.r.t to a provided obstacle type
+ *
+ * This function can be useful to find the part of a trajectory that is close to an obstacle.
+ * The method is calculates appropriate distance metrics for point, line and polygon obstacles.
+ * For all unknown obstacles the centroid is used.
+ *
+ * @param obstacle Subclass of the Obstacle base class
+ * @param[out] distance [optional] the resulting minimum distance
+ * @return Index to the closest pose in the pose sequence
+ */
+ int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const;
+
+
+ /**
+ * @brief Get the length of the internal pose sequence
+ */
+ int sizePoses() const {return (int)pose_vec_.size();};
+
+ /**
+ * @brief Get the length of the internal timediff sequence
+ */
+ int sizeTimeDiffs() const {return (int)timediff_vec_.size();};
+
+ /**
+ * @brief Check whether the trajectory is initialized (nonzero pose and timediff sequences)
+ */
+ bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}
+
+ /**
+ * @brief Calculate the total transition time (sum over all time intervals of the timediff sequence)
+ */
+ double getSumOfAllTimeDiffs() const;
+
+ /**
+ * @brief Calculate the estimated transition time up to the pose denoted by index
+ * @param index Index of the pose up to which the transition times are summed up
+ * @return Estimated transition time up to pose index
+ */
+ double getSumOfTimeDiffsUpToIdx(int index) const;
+
+ /**
+ * @brief Calculate the length (accumulated euclidean distance) of the trajectory
+ */
+ double getAccumulatedDistance() const;
+
+ /**
+ * @brief Check if all trajectory points are contained in a specific region
+ *
+ * The specific region is a circle around the current robot position (Pose(0)) with given radius \c radius.
+ * This method investigates a different radius for points behind the robot if \c max_dist_behind_robot >= 0.
+ * @param radius radius of the region with the robot position (Pose(0)) as center
+ * @param max_dist_behind_robot A separate radius for trajectory points behind the robot, activated if 0 or positive
+ * @param skip_poses If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested.
+ * @return \c true, if all tested trajectory points are inside the specified region, \c false otherwise.
+ */
+ bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0);
+
+
+
+ //@}
+
+protected:
+ PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices
+ TimeDiffSequence timediff_vec_; //!< Internal container storing the sequence of optimzable timediff vertices
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace teb_local_planner
+
+
+// include template implementations / definitions
+#include "teb_local_planner/timed_elastic_band.hpp"
+
+
+#endif /* TIMED_ELASTIC_BAND_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
new file mode 100644
index 0000000..9db4a25
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
@@ -0,0 +1,191 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/timed_elastic_band.h"
+
+#include <iterator>
+
+namespace teb_local_planner
+{
+template<typename BidirIter, typename Fun>
+bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
+ boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
+ boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples, bool guess_backwards_motion)
+{
+ Eigen::Vector2d start_position = fun_position( *path_start );
+ Eigen::Vector2d goal_position = fun_position( *std::prev(path_end) );
+
+ bool backwards = false;
+
+ double start_orient, goal_orient;
+ if (start_orientation)
+ {
+ start_orient = *start_orientation;
+
+ // check if the goal is behind the start pose (w.r.t. start orientation)
+ if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0)
+ backwards = true;
+ }
+ else
+ {
+ Eigen::Vector2d start2goal = goal_position - start_position;
+ start_orient = atan2(start2goal[1],start2goal[0]);
+ }
+
+ double timestep = 1; // TODO: time
+
+ if (goal_orientation)
+ {
+ goal_orient = *goal_orientation;
+ }
+ else
+ {
+ goal_orient = start_orient;
+ }
+
+ if (!isInit())
+ {
+ addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization
+
+ // we insert middle points now (increase start by 1 and decrease goal by 1)
+ std::advance(path_start,1);
+ std::advance(path_end,-1);
+ int idx=0;
+ for (; path_start != path_end; ++path_start) // insert middle-points
+ {
+ //Eigen::Vector2d point_to_goal = path.back()-*it;
+ //double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
+ // Alternative: Direction from last path
+ Eigen::Vector2d curr_point = fun_position(*path_start);
+ Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use std::prev(*path_start) for those cases,
+ // where fun_position() does not return a reference or is expensive.
+ double diff_norm = diff_last.norm();
+
+ double timestep_vel = diff_norm/max_vel_x; // constant velocity
+ double timestep_acc;
+
+ if (max_acc_x)
+ {
+ timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
+ if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
+ else timestep = timestep_vel;
+ }
+ else timestep = timestep_vel;
+
+ if (timestep<=0) timestep=0.2; // TODO: this is an assumption
+
+ double yaw = atan2(diff_last[1],diff_last[0]);
+ if (backwards)
+ yaw = g2o::normalize_theta(yaw + M_PI);
+ addPoseAndTimeDiff(curr_point, yaw ,timestep);
+
+ /*
+ // TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time.
+
+ Eigen::Vector2d diff_next = fun_position(*std::next(path_start))-curr_point; // TODO maybe store the std::next for the following iteration
+ double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
+ -atan2(diff_last[1],diff_last[0]) ) );
+
+ timestep_vel = ang_diff/max_vel_theta; // constant velocity
+ if (max_acc_theta)
+ {
+ timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
+ if (timestep_vel < timestep_acc) timestep = timestep_acc;
+ else timestep = timestep_vel;
+ }
+ else timestep = timestep_vel;
+
+ if (timestep<=0) timestep=0.2; // TODO: this is an assumption
+
+ yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished
+ if (backwards)
+ yaw = g2o::normalize_theta(yaw + M_PI);
+ addPoseAndTimeDiff(curr_point, yaw ,timestep);
+
+ */
+
+ ++idx;
+ }
+ Eigen::Vector2d diff = goal_position-Pose(idx).position();
+ double diff_norm = diff.norm();
+ double timestep_vel = diff_norm/max_vel_x; // constant velocity
+ if (max_acc_x)
+ {
+ double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
+ if (timestep_vel < timestep_acc) timestep = timestep_acc;
+ else timestep = timestep_vel;
+ }
+ else timestep = timestep_vel;
+
+
+ PoseSE2 goal(goal_position, goal_orient);
+
+ // if number of samples is not larger than min_samples, insert manually
+ if ( sizePoses() < min_samples-1 )
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
+ while (sizePoses() < min_samples-1) // subtract goal point that will be added later
+ {
+ // Each inserted point bisects the remaining distance. Thus the timestep is also bisected.
+ timestep /= 2;
+ // simple strategy: interpolate between the current pose and the goal
+ addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization
+ }
+ }
+
+ // now add goal
+ addPoseAndTimeDiff(goal, timestep); // add goal point
+ setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
+ }
+ else // size!=0
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
+ return false;
+ }
+ return true;
+}
+
+
+} // namespace teb_local_planner
+
+
+
diff --git a/src/teb_local_planner/include/teb_local_planner/visualization.h b/src/teb_local_planner/include/teb_local_planner/visualization.h
new file mode 100644
index 0000000..c46dbe8
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/visualization.h
@@ -0,0 +1,272 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef VISUALIZATION_H_
+#define VISUALIZATION_H_
+
+
+
+// teb stuff
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/timed_elastic_band.h"
+#include "teb_local_planner/robot_footprint_model.h"
+
+#include <teb_msgs/msg/feedback_msg.hpp>
+
+// boost
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/graph_traits.hpp>
+
+// std
+#include <iterator>
+
+#include <nav2_util/lifecycle_node.hpp>
+
+#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
+
+// messages
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <tf2/transform_datatypes.h>
+#include <visualization_msgs/msg/marker.hpp>
+
+namespace teb_local_planner
+{
+
+class TebOptimalPlanner; //!< Forward Declaration
+
+
+/**
+ * @class TebVisualization
+ * @brief Visualize stuff from the teb_local_planner
+ */
+class TebVisualization
+{
+public:
+ /**
+ * @brief Constructor that initializes the class and registers topics
+ * @param nh local rclcpp::Node::SharedPtr
+ * @param cfg const reference to the TebConfig class for parameters
+ */
+ TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg);
+
+ /** @name Publish to topics */
+ //@{
+
+ /**
+ * @brief Publish a given global plan to the ros topic \e ../../global_plan
+ * @param global_plan Pose array describing the global plan
+ */
+ void publishGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan) const;
+
+ /**
+ * @brief Publish a given local plan to the ros topic \e ../../local_plan
+ * @param local_plan Pose array describing the local plan
+ */
+ void publishLocalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& local_plan) const;
+
+ /**
+ * @brief Publish Timed_Elastic_Band related stuff (local plan, pose sequence).
+ *
+ * Given a Timed_Elastic_Band instance, publish the local plan to \e ../../local_plan
+ * and the pose sequence to \e ../../teb_poses.
+ * @param teb const reference to a Timed_Elastic_Band
+ */
+ void publishLocalPlanAndPoses(const TimedElasticBand& teb) const;
+
+ /**
+ * @brief Publish the visualization of the robot model
+ *
+ * @param current_pose Current pose of the robot
+ * @param robot_model Subclass of BaseRobotFootprintModel
+ * @param ns Namespace for the marker objects
+ * @param color Color of the footprint
+ */
+ void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel",
+ const std_msgs::msg::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0));
+
+ /**
+ * @brief Publish the robot footprints related to infeasible poses
+ *
+ * @param current_pose Current pose of the robot
+ * @param robot_model Subclass of BaseRobotFootprintModel
+ */
+ void publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model);
+
+ /**
+ * @brief Publish obstacle positions to the ros topic \e ../../teb_markers
+ * @todo Move filling of the marker message to polygon class in order to avoid checking types.
+ * @param obstacles Obstacle container
+ */
+ void publishObstacles(const ObstContainer& obstacles) const;
+
+ /**
+ * @brief Publish via-points to the ros topic \e ../../teb_markers
+ * @param via_points via-point container
+ */
+ void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns = "ViaPoints") const;
+
+ /**
+ * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers.
+ * @remarks Make sure that vertices of the graph contain a member \c pos as \c Eigen::Vector2d type
+ * to query metric position values.
+ * @param graph Const reference to the boost::adjacency_list (graph)
+ * @param ns_prefix Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended)
+ * @tparam GraphType boost::graph object in which vertices has the field/member \c pos.
+ */
+ template <typename GraphType>
+ void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph");
+
+ /**
+ * @brief Publish multiple 2D paths (each path given as a point sequence) from a container class.
+ *
+ * Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist
+ * and std::vector could be individually substituded by std::list / std::deque /...
+ *
+ * A common point-type for object T could be Eigen::Vector2d.
+ *
+ * T could be also a raw pointer std::vector< std::vector< T* > >.
+ *
+ * @code
+ * typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > PathType; // could be a list or deque as well ...
+ * std::vector<PathType> path_container(2); // init 2 empty paths; the container could be a list or deque as well ...
+ * // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here
+ * // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here
+ * publishPathContainer( path_container.begin(), path_container.end() );
+ * @endcode
+ *
+ * @remarks Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence.
+ * Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.\n
+ * @param first Bidirectional iterator pointing to the begin of the path
+ * @param last Bidirectional iterator pointing to the end of the path
+ * @param ns Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended)
+ * @tparam BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container
+ */
+ template <typename BidirIter>
+ void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer");
+
+ /**
+ * @brief Publish multiple Tebs from a container class (publish as marker message).
+ *
+ * @param teb_planner Container of std::shared_ptr< TebOptPlannerPtr >
+ * @param ns Namespace for the marker objects
+ */
+ void publishTebContainer(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planner, const std::string& ns = "TebContainer");
+
+ /**
+ * @brief Publish a feedback message (multiple trajectory version)
+ *
+ * The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on).
+ * Each trajectory is composed of the sequence of poses, the velocity profile and temporal information.
+ * The feedback message also contains a list of active obstacles.
+ * @param teb_planners container with multiple tebs (resp. their planner instances)
+ * @param selected_trajectory_idx Idx of the currently selected trajectory in \c teb_planners
+ * @param obstacles Container of obstacles
+ */
+ void publishFeedbackMessage(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles);
+
+ /**
+ * @brief Publish a feedback message (single trajectory overload)
+ *
+ * The feedback message contains the planned trajectory
+ * that is composed of the sequence of poses, the velocity profile and temporal information.
+ * The feedback message also contains a list of active obstacles.
+ * @param teb_planner the planning instance
+ * @param obstacles Container of obstacles
+ */
+ void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles);
+
+ nav2_util::CallbackReturn on_configure();
+ nav2_util::CallbackReturn on_activate();
+ nav2_util::CallbackReturn on_deactivate();
+ nav2_util::CallbackReturn on_cleanup();
+
+ //@}
+
+ /**
+ * @brief Helper function to generate a color message from single values
+ * @param a Alpha value
+ * @param r Red value
+ * @param g Green value
+ * @param b Blue value
+ * @return Color message
+ */
+ static std_msgs::msg::ColorRGBA toColorMsg(double a, double r, double g, double b);
+
+protected:
+
+ /**
+ * @brief Small helper function that checks if initialize() has been called and prints an error message if not.
+ * @return \c true if not initialized, \c false if everything is ok
+ */
+ bool printErrorWhenNotInitialized() const;
+
+ nav2_util::LifecycleNode::SharedPtr nh_;
+
+ rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr global_plan_pub_; //!< Publisher for the global plan
+ rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr local_plan_pub_; //!< Publisher for the local plan
+ rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr teb_poses_pub_; //!< Publisher for the trajectory pose sequence
+ rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr teb_marker_pub_; //!< Publisher for visualization markers
+ rclcpp_lifecycle::LifecyclePublisher<teb_msgs::msg::FeedbackMsg>::SharedPtr feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes
+
+ const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+
+ bool initialized_; //!< Keeps track about the correct initialization of this class
+
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//! Abbrev. for shared instances of the TebVisualization
+typedef std::shared_ptr<TebVisualization> TebVisualizationPtr;
+
+//! Abbrev. for shared instances of the TebVisualization (read-only)
+typedef std::shared_ptr<const TebVisualization> TebVisualizationConstPtr;
+
+
+} // namespace teb_local_planner
+
+
+// Include template method implementations / definitions
+#include "teb_local_planner/visualization.hpp"
+
+#endif /* VISUALIZATION_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/visualization.hpp b/src/teb_local_planner/include/teb_local_planner/visualization.hpp
new file mode 100644
index 0000000..9b45a54
--- /dev/null
+++ b/src/teb_local_planner/include/teb_local_planner/visualization.hpp
@@ -0,0 +1,227 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/visualization.h"
+
+#include <boost/utility.hpp>
+#include <iterator>
+
+namespace teb_local_planner
+{
+
+
+template <typename GraphType>
+void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix)
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+
+ typedef typename boost::graph_traits<GraphType>::vertex_iterator GraphVertexIterator;
+ typedef typename boost::graph_traits<GraphType>::edge_iterator GraphEdgeIterator;
+
+ // Visualize Edges
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = ns_prefix + "Edges";
+ marker.id = 0;
+ marker.pose.orientation.w = 1.0;
+// #define TRIANGLE
+#ifdef TRIANGLE
+ marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+#else
+ marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+#endif
+ marker.action = visualization_msgs::msg::Marker::ADD;
+
+ GraphEdgeIterator it_edge, end_edges;
+ for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge)
+ {
+#ifdef TRIANGLE
+ geometry_msgs::msg::Point point_start1;
+ point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05;
+ point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05;
+ point_start1.z = 0;
+ marker.points.push_back(point_start1);
+ geometry_msgs::msg::Point point_start2;
+ point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05;
+ point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05;
+ point_start2.z = 0;
+ marker.points.push_back(point_start2);
+
+#else
+ geometry_msgs::msg::Point point_start;
+ point_start.x = graph[boost::source(*it_edge,graph)].pos[0];
+ point_start.y = graph[boost::source(*it_edge,graph)].pos[1];
+ point_start.z = 0;
+ marker.points.push_back(point_start);
+#endif
+ geometry_msgs::msg::Point point_end;
+ point_end.x = graph[boost::target(*it_edge,graph)].pos[0];
+ point_end.y = graph[boost::target(*it_edge,graph)].pos[1];
+ point_end.z = 0;
+ marker.points.push_back(point_end);
+
+ // add color
+ std_msgs::msg::ColorRGBA color;
+ color.a = 1.0;
+ color.r = 0;
+ color.g = 0;
+ color.b = 1;
+ marker.colors.push_back(color);
+ marker.colors.push_back(color);
+#ifdef TRIANGLE
+ marker.colors.push_back(color);
+#endif
+ }
+
+#ifdef TRIANGLE
+ marker.scale.x = 1;
+ marker.scale.y = 1;
+ marker.scale.z = 1;
+#else
+ marker.scale.x = 0.01;
+#endif
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ // Now publish edge markers
+ teb_marker_pub_->publish( marker );
+
+ // Visualize vertices
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = ns_prefix + "Vertices";
+ marker.id = 0;
+ marker.type = visualization_msgs::msg::Marker::POINTS;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+
+ GraphVertexIterator it_vert, end_vert;
+ for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = graph[*it_vert].pos[0];
+ point.y = graph[*it_vert].pos[1];
+ point.z = 0;
+ marker.points.push_back(point);
+ // add color
+
+ std_msgs::msg::ColorRGBA color;
+ color.a = 1.0;
+ if (it_vert==end_vert-1)
+ {
+ color.r = 1;
+ color.g = 0;
+ color.b = 0;
+ }
+ else
+ {
+ color.r = 0;
+ color.g = 1;
+ color.b = 0;
+ }
+ marker.colors.push_back(color);
+ }
+ // set first color (start vertix) to blue
+ if (!marker.colors.empty())
+ {
+ marker.colors.front().b = 1;
+ marker.colors.front().g = 0;
+ }
+
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ // Now publish vertex markers
+ teb_marker_pub_->publish( marker );
+}
+
+template <typename BidirIter>
+void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns)
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = ns;
+ marker.id = 0;
+ marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+
+ typedef typename std::iterator_traits<BidirIter>::value_type PathType; // Get type of the path (point container)
+
+ // Iterate through path container
+ while(first != last)
+ {
+ // iterate single path points
+ typename PathType::const_iterator it_point, end_point;
+ for (it_point = first->begin(), end_point = std::prev(first->end()); it_point != end_point; ++it_point)
+ {
+ geometry_msgs::msg::Point point_start;
+ point_start.x = get_const_reference(*it_point).x();
+ point_start.y = get_const_reference(*it_point).y();
+ point_start.z = 0;
+ marker.points.push_back(point_start);
+
+ geometry_msgs::msg::Point point_end;
+ point_end.x = get_const_reference(*std::next(it_point)).x();
+ point_end.y = get_const_reference(*std::next(it_point)).y();
+ point_end.z = 0;
+ marker.points.push_back(point_end);
+ }
+ ++first;
+ }
+ marker.scale.x = 0.01;
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+}
+
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/package.xml b/src/teb_local_planner/package.xml
new file mode 100644
index 0000000..770a32a
--- /dev/null
+++ b/src/teb_local_planner/package.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>teb_local_planner</name>
+
+ <version>0.9.1</version>
+
+ <description>
+ The teb_local_planner package implements a plugin
+ to the base_local_planner of the 2D navigation stack.
+ The underlying method called Timed Elastic Band locally optimizes
+ the robot's trajectory with respect to trajectory execution time,
+ separation from obstacles and compliance with kinodynamic constraints at runtime.
+ </description>
+
+ <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
+
+ <license>BSD</license>
+
+ <url type="website">http://wiki.ros.org/teb_local_planner</url>
+
+ <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <depend>costmap_converter</depend>
+ <depend>costmap_converter_msgs</depend>
+
+ <depend>geometry_msgs</depend>
+ <depend>libg2o</depend>
+ <depend>dwb_critics</depend>
+ <depend>nav2_core</depend>
+ <depend>nav2_costmap_2d</depend>
+ <depend>nav2_msgs</depend>
+ <depend>nav2_util</depend>
+ <depend>pluginlib</depend>
+ <depend>rclcpp</depend>
+ <depend>rclcpp_action</depend>
+ <depend>rclcpp_lifecycle</depend>
+ <depend>std_msgs</depend>
+ <depend>teb_msgs</depend>
+ <depend>tf2</depend>
+ <depend>tf2_eigen</depend>
+ <depend>visualization_msgs</depend>
+ <depend>builtin_interfaces</depend>
+ <exec_depend>nav2_bringup</exec_depend>
+
+ <test_depend>ament_cmake_gtest</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ <nav2_core plugin="${prefix}/teb_local_planner_plugin.xml" />
+ </export>
+</package>
diff --git a/src/teb_local_planner/params/teb_params.yaml b/src/teb_local_planner/params/teb_params.yaml
new file mode 100644
index 0000000..4fc447b
--- /dev/null
+++ b/src/teb_local_planner/params/teb_params.yaml
@@ -0,0 +1,109 @@
+controller_server:
+ ros__parameters:
+ odom_topic: /odom
+ use_sim_time: True
+ controller_frequency: 5.0
+ controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"]
+ controller_plugins: ["FollowPath"]
+ FollowPath:
+ plugin: teb_local_planner::TebLocalPlannerROS
+
+ teb_autosize: 1.0
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: False
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: 0.3
+ global_plan_prune_distance: 1.0
+ exact_arc_length: False
+ feasibility_check_no_poses: 2
+ publish_feedback: False
+
+ # Robot
+
+ max_vel_x: 0.26
+ max_vel_theta: 1.0
+ acc_lim_x: 2.5
+ acc_lim_theta: 3.2
+
+ footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
+ type: "circular"
+ radius: 0.17 # for type "circular"
+
+ # GoalTolerance
+
+ free_goal_vel: False
+
+ # Obstacles
+
+ min_obstacle_dist: 0.27
+ inflation_dist: 0.6
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.0
+ obstacle_poses_affected: 15
+
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+
+ costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1
+ obstacle_cost_exponent: 4.0
+ weight_max_vel_x: 0.5
+ weight_max_vel_theta: 0.5
+ weight_acc_lim_x: 0.5
+ weight_acc_lim_theta: 10.5
+ weight_kinematics_nh: 1000.0
+ weight_kinematics_forward_drive: 3.0
+ weight_kinematics_turning_radius: 1.0
+ weight_optimaltime: 1.0 # must be > 0
+ weight_shortest_path: 0.0
+ weight_obstacle: 100.0
+ weight_inflation: 0.2
+ weight_dynamic_obstacle: 10.0 # not in use yet
+ weight_dynamic_obstacle_inflation: 0.2
+ weight_viapoint: 50.0
+ weight_adapt_factor: 2.0
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ max_number_classes: 4
+ selection_cost_hysteresis: 5.0
+ selection_prefer_initial_plan: 1.0
+ selection_obst_cost_scale: 1.0
+ selection_alternative_time_cost: True
+
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5.0
+ roadmap_graph_area_length_scale: 1.0
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_heading_threshold: 0.45
+ switching_blocking_period: 0.0
+ viapoints_all_candidates: True
+ delete_detours_backwards: True
+ max_ratio_detours_duration_best_duration: 3.0
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: 0.0
+
+ # Recovery
+
+ shrink_horizon_backup: True
+ shrink_horizon_min_duration: 10.0
+ oscillation_recovery: True
+ oscillation_v_eps: 0.1
+ oscillation_omega_eps: 0.1
+ oscillation_recovery_min_duration: 10.0
+ oscillation_filter_duration: 10.0
diff --git a/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py b/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
new file mode 100755
index 0000000..d8c5b5f
--- /dev/null
+++ b/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rclpy, math
+from geometry_msgs.msg import Twist
+from ackermann_msgs.msg import AckermannDriveStamped
+
+
+def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
+ if omega == 0 or v == 0:
+ return 0
+
+ radius = v / omega
+ return math.atan(wheelbase / radius)
+
+
+def cmd_callback(data):
+ global wheelbase
+ global ackermann_cmd_topic
+ global frame_id
+ global pub
+ global cmd_angle_instead_rotvel
+
+ v = data.linear.x
+ # if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node
+ # in this case this script only needs to do the msg conversion from twist to Ackermann drive
+ if cmd_angle_instead_rotvel:
+ steering = data.angular.z
+ else:
+ steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
+
+ msg = AckermannDriveStamped()
+ msg.header.stamp = node.get_clock().now().to_msg()
+ msg.header.frame_id = frame_id
+ msg.drive.steering_angle = float(steering)
+ msg.drive.speed = float(v)
+
+ pub.publish(msg)
+
+
+
+
+
+if __name__ == '__main__':
+ rclpy.init()
+ global node
+ node = rclpy.create_node('cmd_vel_to_ackermann_drive')
+
+ twist_cmd_topic = node.declare_parameter("twist_cmd_topic", "/cmd_vel").value
+ ackermann_cmd_topic = node.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd").value
+ wheelbase = node.declare_parameter("wheelbase", 1.0).value
+ frame_id = node.declare_parameter('frame_id', 'odom').value
+ cmd_angle_instead_rotvel = node.declare_parameter('cmd_angle_instead_rotvel', False).value
+
+ node.create_subscription(Twist, twist_cmd_topic, cmd_callback, 1)
+ pub = node.create_publisher(AckermannDriveStamped, ackermann_cmd_topic, 1)
+
+ rclpy.spin(node)
+
diff --git a/src/teb_local_planner/scripts/export_to_mat.py b/src/teb_local_planner/scripts/export_to_mat.py
new file mode 100755
index 0000000..1cc3351
--- /dev/null
+++ b/src/teb_local_planner/scripts/export_to_mat.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and exports data to a mat file.
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
+from tf.transformations import euler_from_quaternion
+import numpy as np
+import scipy.io as sio
+import time
+
+def feedback_callback(data):
+ global got_data
+
+ if not data.trajectories: # empty
+ trajectory = []
+ return
+
+ if got_data:
+ return
+
+ got_data = True
+
+ # copy trajectory
+ trajectories = []
+ for traj in data.trajectories:
+ trajectory = []
+# # store as struct and cell array
+# for point in traj.trajectory:
+# (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
+# pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
+# velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
+# time_from_start = point.time_from_start.to_sec()
+# trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
+
+ # store as all-in-one mat
+ arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
+ for index, point in enumerate(traj.trajectory):
+ arr[0,index] = point.pose.position.x
+ arr[1,index] = point.pose.position.y
+ (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
+ arr[2,index] = yaw
+ arr[3,index] = point.velocity.linear.x
+ arr[4,index] = point.velocity.angular.z
+ arr[5,index] = point.time_from_start.to_sec()
+
+# trajectories.append({'raw': trajectory, 'mat': arr})
+ trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
+
+ # copy obstacles
+ obstacles = []
+ for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
+ #polygon = []
+ #for point in obst.polygon.points:
+ # polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
+ obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
+ for index, point in enumerate(obst.polygon.points):
+ obst_arr[0, index] = point.x
+ obst_arr[1, index] = point.y
+ obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
+ obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
+
+ #obstacles.append(polygon)
+ obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
+
+
+ # create main struct:
+ mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
+
+ timestr = time.strftime("%Y%m%d_%H%M%S")
+ filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
+
+ rospy.loginfo("Saving mat-file '%s'.", filename)
+ sio.savemat(filename, mat)
+
+
+
+
+
+def feedback_exporter():
+ global got_data
+
+ rospy.init_node("export_to_mat", anonymous=True)
+
+
+ topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
+
+ rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
+
+ rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
+
+ r = rospy.Rate(2) # define rate here
+ while not rospy.is_shutdown():
+
+ if got_data:
+ rospy.loginfo("Data export completed.")
+ return
+
+ r.sleep()
+
+if __name__ == '__main__':
+ try:
+ global got_data
+ got_data = False
+ feedback_exporter()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/src/teb_local_planner/scripts/export_to_svg.py b/src/teb_local_planner/scripts/export_to_svg.py
new file mode 100755
index 0000000..8d5edf2
--- /dev/null
+++ b/src/teb_local_planner/scripts/export_to_svg.py
@@ -0,0 +1,244 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+========================================================================================
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and converts the current scene to a svg-image
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+It is recommendable to start this node after initialization of TEB is completed.
+
+Requirements:
+svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
+=======================================================================================
+"""
+import roslib;
+import rospy
+import svgwrite
+import math
+import sys
+import time
+import random
+from svgwrite import cm, mm
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
+
+
+# ================= PARAMETERS ==================
+# TODO: In case of a more general node, change parameter to ros-parameter
+# Drawing parameters:
+SCALE = 200 # Overall scaling: 100 pixel = 1 m
+MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
+SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
+GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction.
+GRID_X_MAX = 2
+GRID_Y_MIN = -2
+GRID_Y_MAX = 1
+
+# TEB parameters:
+OBSTACLE_DIST = 50 *SCALE/100 # cm
+
+
+# ================= FUNCTIONS ===================
+
+def sign(number):
+ """
+ Signum function: get sign of a number
+
+ @param number: get sign of this number
+ @type number: numeric type (eg. integer)
+ @return: sign of number
+ @rtype: integer {1, -1, 0}
+ """
+ return cmp(number,0)
+
+def arrowMarker(color='green', orientation='auto'):
+ """
+ Create an arrow marker with svgwrite
+
+ @return: arrow marker
+ @rtype: svg_write marker object
+ """
+ arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
+ arrow.viewbox(width=10, height=10)
+ arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
+ svg.defs.add(arrow)
+ return arrow
+
+def quaternion2YawDegree(orientation):
+ """
+ Get yaw angle [degree] from quaternion representation
+
+ @param orientation: orientation in quaternions to read from
+ @type orientation: geometry_msgs/Quaternion
+ @return: yaw angle [degree]
+ @rtype: float
+ """
+ yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
+ return yawRad*180/math.pi
+
+
+def feedback_callback(data):
+ """
+ Callback for receiving TEB and obstacle information
+
+ @param data: Received feedback message
+ @type data: visualization_msgs/Marker
+
+ @globalparam tebList: Received TEB List
+ @globaltype tebList: teb_local_planner/FeedbackMsg
+ """
+ # TODO: Remove global variables
+ global feedbackMsg
+
+ if not feedbackMsg:
+ feedbackMsg = data
+ rospy.loginfo("TEB feedback message received...")
+
+
+# ================ MAIN FUNCTION ================
+
+if __name__ == '__main__':
+ rospy.init_node('export_to_svg', anonymous=True)
+
+ topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
+
+ rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
+
+ rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
+
+ rate = rospy.Rate(10.0)
+ feedbackMsg = []
+
+ timestr = time.strftime("%Y%m%d_%H%M%S")
+ filename_string = "teb_svg_" + timestr + '.svg'
+
+ rospy.loginfo("SVG will be written to '%s'.", filename_string)
+
+ random.seed(0)
+
+ svg=svgwrite.Drawing(filename=filename_string, debug=True)
+
+ # Create viewbox -> this box defines the size of the visible drawing
+ svg.viewbox(GRID_X_MIN*SCALE-1*SCALE,GRID_Y_MIN*SCALE-1*SCALE,GRID_X_MAX*SCALE-GRID_X_MIN*SCALE+2*SCALE,GRID_Y_MAX*SCALE-GRID_Y_MIN*SCALE+2*SCALE)
+
+ # Draw grid:
+ hLines = svg.add(svg.g(id='hLines', stroke='black'))
+ hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
+ for y in range(GRID_Y_MAX):
+ hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
+ for y in range(-GRID_Y_MIN):
+ hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
+ vLines = svg.add(svg.g(id='vline', stroke='black'))
+ vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
+ for x in range(GRID_X_MAX):
+ vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
+ for x in range(-GRID_X_MIN):
+ vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
+
+
+ # Draw legend:
+ legend = svg.g(id='legend', font_size=25)
+ stringGeometry = "Geometry: 1 Unit = 1.0m"
+ legendGeometry = svg.text(stringGeometry)
+ legend.add(legendGeometry)
+ legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
+ svg.add(legend)
+
+
+ #arrow = arrowMarker() # Init arrow marker
+
+ rospy.loginfo("Initialization completed.\nWaiting for feedback message...")
+
+ # -------------------- WAIT FOR CALLBACKS --------------------------
+ while not rospy.is_shutdown():
+ if feedbackMsg:
+ break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
+ rate.sleep()
+ # ------------------------------------------------------------------
+
+ if not feedbackMsg.trajectories:
+ rospy.loginfo("Received message does not contain trajectories. Shutting down...")
+ sys.exit()
+
+ if len(feedbackMsg.trajectories[0].trajectory) < 2:
+ rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
+ sys.exit()
+
+ # iterate trajectories
+ for index, traj in enumerate(feedbackMsg.trajectories):
+
+ #color
+ traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
+
+ # Iterate through TEB positions -> Draw Paths
+ points = []
+ for point in traj.trajectory:
+ points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
+ # svgwrite rotates clockwise!
+
+
+ if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
+ line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
+ stroke_linejoin='round', opacity=1.0 ) )
+ else:
+ line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
+ stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
+ #marker_points = points[::7]
+ #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
+ #arrow = arrowMarker(traj_color)
+ #markerline.set_markers( (arrow, arrow, arrow) )
+ #line.set_markers( (arrow, arrow, arrow) )
+ #line['marker-start'] = arrow.get_funciri()
+
+
+ # Add Start and Goal Point
+ start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
+ goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
+ start_position = start_pose.position
+ goal_position = goal_pose.position
+ svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
+ svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
+ svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
+ svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
+
+ # draw start arrow
+ start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
+ start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
+ start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) )
+ start_arrow.scale(3)
+ svg.add(start_arrow)
+
+ # draw goal arrow
+ goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
+ goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
+ goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) )
+ goal_arrow.scale(3)
+ svg.add(goal_arrow)
+
+ # Draw obstacles
+ for obstacle in feedbackMsg.obstacles:
+ if len(obstacle.polygon.points) == 1: # point obstacle
+ point = obstacle.polygon.points[0]
+ svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
+ svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
+ svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
+ if len(obstacle.polygon.points) == 2: # line obstacle
+ line_start = obstacle.polygon.points[0]
+ line_end = obstacle.polygon.points[1]
+ svg.add(svg.line(start=(line_start.x*SCALE,-line_start.y*SCALE), end=(line_end.x*SCALE,-line_end.y*SCALE), stroke='black', fill='gray', stroke_width=1, opacity=1.0))
+ svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
+ if len(obstacle.polygon.points) > 2: # polygon obstacle
+ vertices = []
+ for point in obstacle.polygon.points:
+ vertices.append((point.x*SCALE, -point.y*SCALE))
+ svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
+ svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
+
+
+
+ # Save svg to file (svg_output.svg) and exit node
+ svg.save()
+
+ rospy.loginfo("Drawing completed.")
diff --git a/src/teb_local_planner/scripts/publish_dynamic_obstacle.py b/src/teb_local_planner/scripts/publish_dynamic_obstacle.py
new file mode 100755
index 0000000..0cc16d4
--- /dev/null
+++ b/src/teb_local_planner/scripts/publish_dynamic_obstacle.py
@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+# Author: franz.albers@tu-dortmund.de
+
+import rospy, math, tf
+from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
+from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance
+from tf.transformations import quaternion_from_euler
+
+
+def publish_obstacle_msg():
+ pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
+ #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
+ rospy.init_node("test_obstacle_msg")
+
+ y_0 = -3.0
+ vel_x = 0.0
+ vel_y = 0.3
+ range_y = 6.0
+
+ obstacle_msg = ObstacleArrayMsg()
+ obstacle_msg.header.stamp = rospy.Time.now()
+ obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
+
+ # Add point obstacle
+ obstacle_msg.obstacles.append(ObstacleMsg())
+ obstacle_msg.obstacles[0].id = 99
+ obstacle_msg.obstacles[0].polygon.points = [Point32()]
+ obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
+ obstacle_msg.obstacles[0].polygon.points[0].y = 0
+ obstacle_msg.obstacles[0].polygon.points[0].z = 0
+
+ yaw = math.atan2(vel_y, vel_x)
+ q = tf.transformations.quaternion_from_euler(0,0,yaw)
+ obstacle_msg.obstacles[0].orientation = Quaternion(*q)
+
+ obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
+ obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
+ obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
+ obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
+ obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
+ obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
+
+ r = rospy.Rate(10) # 10hz
+ t = 0.0
+ while not rospy.is_shutdown():
+
+ # Vary y component of the point obstacle
+ if (vel_y >= 0):
+ obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
+ else:
+ obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y
+
+ t = t + 0.1
+
+ pub.publish(obstacle_msg)
+
+ r.sleep()
+
+
+
+if __name__ == '__main__':
+ try:
+ publish_obstacle_msg()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/src/teb_local_planner/scripts/publish_test_obstacles.py b/src/teb_local_planner/scripts/publish_test_obstacles.py
new file mode 100755
index 0000000..800803d
--- /dev/null
+++ b/src/teb_local_planner/scripts/publish_test_obstacles.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
+from geometry_msgs.msg import PolygonStamped, Point32
+
+
+def publish_obstacle_msg():
+ pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
+ #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
+ rospy.init_node("test_obstacle_msg")
+
+
+ obstacle_msg = ObstacleArrayMsg()
+ obstacle_msg.header.stamp = rospy.Time.now()
+ obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
+
+ # Add point obstacle
+ obstacle_msg.obstacles.append(ObstacleMsg())
+ obstacle_msg.obstacles[0].id = 0
+ obstacle_msg.obstacles[0].polygon.points = [Point32()]
+ obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
+ obstacle_msg.obstacles[0].polygon.points[0].y = 0
+ obstacle_msg.obstacles[0].polygon.points[0].z = 0
+
+
+ # Add line obstacle
+ obstacle_msg.obstacles.append(ObstacleMsg())
+ obstacle_msg.obstacles[1].id = 1
+ line_start = Point32()
+ line_start.x = -2.5
+ line_start.y = 0.5
+ #line_start.y = -3
+ line_end = Point32()
+ line_end.x = -2.5
+ line_end.y = 2
+ #line_end.y = -4
+ obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
+
+ # Add polygon obstacle
+ obstacle_msg.obstacles.append(ObstacleMsg())
+ obstacle_msg.obstacles[1].id = 2
+ v1 = Point32()
+ v1.x = -1
+ v1.y = -1
+ v2 = Point32()
+ v2.x = -0.5
+ v2.y = -1.5
+ v3 = Point32()
+ v3.x = 0
+ v3.y = -1
+ obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
+
+
+ r = rospy.Rate(10) # 10hz
+ t = 0.0
+ while not rospy.is_shutdown():
+
+ # Vary y component of the point obstacle
+ obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
+ t = t + 0.1
+
+ pub.publish(obstacle_msg)
+
+ r.sleep()
+
+
+
+if __name__ == '__main__':
+ try:
+ publish_obstacle_msg()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/src/teb_local_planner/scripts/publish_viapoints.py b/src/teb_local_planner/scripts/publish_viapoints.py
new file mode 100755
index 0000000..7bd0c7e
--- /dev/null
+++ b/src/teb_local_planner/scripts/publish_viapoints.py
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+
+def publish_via_points_msg():
+ pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1)
+ rospy.init_node("test_via_points_msg")
+
+
+ via_points_msg = Path()
+ via_points_msg.header.stamp = rospy.Time.now()
+ via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
+
+ # Add via-points
+ point1 = PoseStamped()
+ point1.pose.position.x = 0.0;
+ point1.pose.position.y = 1.5;
+
+ point2 = PoseStamped()
+ point2.pose.position.x = 2.0;
+ point2.pose.position.y = -0.5;
+
+
+ via_points_msg.poses = [point1, point2]
+
+ r = rospy.Rate(5) # 10hz
+ t = 0.0
+ while not rospy.is_shutdown():
+
+ pub.publish(via_points_msg)
+
+ r.sleep()
+
+
+
+if __name__ == '__main__':
+ try:
+ publish_via_points_msg()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/src/teb_local_planner/scripts/visualize_velocity_profile.py b/src/teb_local_planner/scripts/visualize_velocity_profile.py
new file mode 100755
index 0000000..b9b6d78
--- /dev/null
+++ b/src/teb_local_planner/scripts/visualize_velocity_profile.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and plots the current velocity.
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32
+import numpy as np
+import matplotlib.pyplot as plotter
+
+def feedback_callback(data):
+ global trajectory
+
+ if not data.trajectories: # empty
+ trajectory = []
+ return
+ trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
+
+
+def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
+ ax_v.cla()
+ ax_v.grid()
+ ax_v.set_ylabel('Trans. velocity [m/s]')
+ ax_v.plot(t, v, '-bx')
+ ax_omega.cla()
+ ax_omega.grid()
+ ax_omega.set_ylabel('Rot. velocity [rad/s]')
+ ax_omega.set_xlabel('Time [s]')
+ ax_omega.plot(t, omega, '-bx')
+ fig.canvas.draw()
+
+
+
+def velocity_plotter():
+ global trajectory
+ rospy.init_node("visualize_velocity_profile", anonymous=True)
+
+ topic_name = "/test_optim_node/teb_feedback"
+ topic_name = rospy.get_param('~feedback_topic', topic_name)
+ rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
+
+ rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name)
+ rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
+
+ # two subplots sharing the same t axis
+ fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
+ plotter.ion()
+ plotter.show()
+
+
+ r = rospy.Rate(2) # define rate here
+ while not rospy.is_shutdown():
+
+ t = []
+ v = []
+ omega = []
+
+ for point in trajectory:
+ t.append(point.time_from_start.to_sec())
+ v.append(point.velocity.linear.x)
+ omega.append(point.velocity.angular.z)
+
+ plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
+
+ r.sleep()
+
+if __name__ == '__main__':
+ try:
+ trajectory = []
+ velocity_plotter()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/src/teb_local_planner/src/graph_search.cpp b/src/teb_local_planner/src/graph_search.cpp
new file mode 100644
index 0000000..f2d1218
--- /dev/null
+++ b/src/teb_local_planner/src/graph_search.cpp
@@ -0,0 +1,346 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#include "teb_local_planner/graph_search.h"
+#include "teb_local_planner/homotopy_class_planner.h"
+
+namespace teb_local_planner
+{
+
+void GraphSearchInterface::DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation,
+ double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ // see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths
+
+ if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
+ return; // We do not need to search for further possible alternative homotopy classes.
+
+ HcGraphVertexType back = visited.back();
+
+ /// Examine adjacent nodes
+ HcGraphAdjecencyIterator it, end;
+ for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
+ {
+ if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() )
+ continue; // already visited
+
+ if ( *it == goal ) // goal reached
+ {
+ visited.push_back(*it);
+
+ // Add new TEB, if this path belongs to a new homotopy class
+ hcp_->addAndInitNewTeb(visited.begin(), visited.end(), std::bind(getVector2dFromHcGraph, std::placeholders::_1, boost::cref(graph_)),
+ start_orientation, goal_orientation, start_velocity);
+
+ visited.pop_back();
+ break;
+ }
+ }
+
+ /// Recursion for all adjacent vertices
+ for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
+ {
+ if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() || *it == goal)
+ continue; // already visited || goal reached
+
+
+ visited.push_back(*it);
+
+ // recursion step
+ DepthFirst(g, visited, goal, start_orientation, goal_orientation, start_velocity, free_goal_vel);
+
+ visited.pop_back();
+ }
+}
+
+
+
+void lrKeyPointGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ // Clear existing graph and paths
+ clearGraph();
+ if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
+ return;
+ // Direction-vector between start and goal and normal-vector:
+ Eigen::Vector2d diff = goal.position()-start.position();
+
+ if (diff.norm()<cfg_->goal_tolerance.xy_goal_tolerance)
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
+ if (hcp_->getTrajectoryContainer().empty())
+ {
+ RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
+ hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
+ }
+ return;
+ }
+
+ Eigen::Vector2d normal(-diff[1],diff[0]); // normal-vector
+ normal.normalize();
+ normal = normal*dist_to_obst; // scale with obstacle_distance;
+
+ // Insert Vertices
+ HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
+ graph_[start_vtx].pos = start.position();
+ diff.normalize();
+
+ // store nearest obstacle keypoints -> only used if limit_obstacle_heading is enabled
+ std::pair<HcGraphVertexType,HcGraphVertexType> nearest_obstacle; // both vertices are stored
+ double min_dist = DBL_MAX;
+
+ if (hcp_->obstacles()!=NULL)
+ {
+ for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
+ {
+ // check if obstacle is placed in front of start point
+ Eigen::Vector2d start2obst = (*it_obst)->getCentroid() - start.position();
+ double dist = start2obst.norm();
+ if (start2obst.dot(diff)/dist<0.1)
+ continue;
+
+ // Add Keypoints
+ HcGraphVertexType u = boost::add_vertex(graph_);
+ graph_[u].pos = (*it_obst)->getCentroid() + normal;
+ HcGraphVertexType v = boost::add_vertex(graph_);
+ graph_[v].pos = (*it_obst)->getCentroid() - normal;
+
+ // store nearest obstacle
+ if (obstacle_heading_threshold && dist<min_dist)
+ {
+ min_dist = dist;
+ nearest_obstacle.first = u;
+ nearest_obstacle.second = v;
+ }
+ }
+ }
+
+ HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
+ graph_[goal_vtx].pos = goal.position();
+
+ // Insert Edges
+ HcGraphVertexIterator it_i, end_i, it_j, end_j;
+ for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=end_i-1; ++it_i) // ignore goal in this loop
+ {
+ for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
+ {
+ if (it_i==it_j)
+ continue;
+ // TODO: make use of knowing in which order obstacles are inserted and that for each obstacle 2 vertices are added,
+ // therefore we must only check one of them.
+ Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
+ distij.normalize();
+ // Check if the direction is backwards:
+ if (distij.dot(diff)<=obstacle_heading_threshold)
+ continue;
+
+
+ // Check start angle to nearest obstacle
+ if (obstacle_heading_threshold && *it_i==start_vtx && min_dist!=DBL_MAX)
+ {
+ if (*it_j == nearest_obstacle.first || *it_j == nearest_obstacle.second)
+ {
+ Eigen::Vector2d keypoint_dist = graph_[*it_j].pos-start.position();
+ keypoint_dist.normalize();
+ Eigen::Vector2d start_orient_vec( cos(start.theta()), sin(start.theta()) ); // already normalized
+ // check angle
+ if (start_orient_vec.dot(keypoint_dist) <= obstacle_heading_threshold)
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "createGraph() - deleted edge: limit_obstacle_heading");
+ continue;
+ }
+ }
+ }
+
+ // Collision Check
+
+ if (hcp_->obstacles()!=NULL)
+ {
+ bool collision = false;
+ for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
+ {
+ if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, 0.5*dist_to_obst) )
+ {
+ collision = true;
+ break;
+ }
+ }
+ if (collision)
+ continue;
+ }
+
+ // Create Edge
+ boost::add_edge(*it_i,*it_j,graph_);
+ }
+ }
+
+
+ // Find all paths between start and goal!
+ std::vector<HcGraphVertexType> visited;
+ visited.push_back(start_vtx);
+ DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
+}
+
+
+void ProbRoadmapGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ // Clear existing graph and paths
+ clearGraph();
+ if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
+ return;
+ // Direction-vector between start and goal and normal-vector:
+ Eigen::Vector2d diff = goal.position()-start.position();
+ double start_goal_dist = diff.norm();
+
+ if (start_goal_dist<cfg_->goal_tolerance.xy_goal_tolerance)
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
+ if (hcp_->getTrajectoryContainer().empty())
+ {
+ RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
+ hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
+ }
+ return;
+ }
+ Eigen::Vector2d normal(-diff.coeffRef(1),diff.coeffRef(0)); // normal-vector
+ normal.normalize();
+
+ // Now sample vertices between start, goal and a specified width between both sides
+ // Let's start with a square area between start and goal (maybe change it later to something like a circle or whatever)
+
+ double area_width = cfg_->hcp.roadmap_graph_area_width;
+
+ boost::random::uniform_real_distribution<double> distribution_x(0, start_goal_dist * cfg_->hcp.roadmap_graph_area_length_scale);
+ boost::random::uniform_real_distribution<double> distribution_y(0, area_width);
+
+ double phi = atan2(diff.coeffRef(1),diff.coeffRef(0)); // rotate area by this angle
+ Eigen::Rotation2D<double> rot_phi(phi);
+
+ Eigen::Vector2d area_origin;
+ if (cfg_->hcp.roadmap_graph_area_length_scale != 1.0)
+ area_origin = start.position() + 0.5*(1.0-cfg_->hcp.roadmap_graph_area_length_scale)*start_goal_dist*diff.normalized() - 0.5*area_width*normal; // bottom left corner of the origin
+ else
+ area_origin = start.position() - 0.5*area_width*normal; // bottom left corner of the origin
+
+ // Insert Vertices
+ HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
+ graph_[start_vtx].pos = start.position();
+ diff.normalize(); // normalize in place
+
+
+ // Start sampling
+ for (int i=0; i < cfg_->hcp.roadmap_graph_no_samples; ++i)
+ {
+ Eigen::Vector2d sample;
+// bool coll_free;
+// do // sample as long as a collision free sample is found
+// {
+ // Sample coordinates
+ sample = area_origin + rot_phi*Eigen::Vector2d(distribution_x(rnd_generator_), distribution_y(rnd_generator_));
+
+ // Test for collision
+ // we do not care for collision checking here to improve efficiency, since we perform resampling repeatedly.
+ // occupied vertices are ignored in the edge insertion state since they always violate the edge-obstacle collision check.
+// coll_free = true;
+// for (ObstContainer::const_iterator it_obst = obstacles_->begin(); it_obst != obstacles_->end(); ++it_obst)
+// {
+// if ( (*it_obst)->checkCollision(sample, dist_to_obst)) // TODO really keep dist_to_obst here?
+// {
+// coll_free = false;
+// break;
+// }
+// }
+//
+// } while (!coll_free && ros::ok());
+
+ // Add new vertex
+ HcGraphVertexType v = boost::add_vertex(graph_);
+ graph_[v].pos = sample;
+ }
+
+ // Now add goal vertex
+ HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
+ graph_[goal_vtx].pos = goal.position();
+
+
+ // Insert Edges
+ HcGraphVertexIterator it_i, end_i, it_j, end_j;
+ for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=boost::prior(end_i); ++it_i) // ignore goal in this loop
+ {
+ for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
+ {
+ if (it_i==it_j) // same vertex found
+ continue;
+
+ Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
+ distij.normalize(); // normalize in place
+
+ // Check if the direction is backwards:
+ if (distij.dot(diff)<=obstacle_heading_threshold)
+ continue; // diff is already normalized
+
+
+ // Collision Check
+ bool collision = false;
+ for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
+ {
+ if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, dist_to_obst) )
+ {
+ collision = true;
+ break;
+ }
+ }
+ if (collision)
+ continue;
+
+ // Create Edge
+ boost::add_edge(*it_i,*it_j,graph_);
+ }
+ }
+
+ /// Find all paths between start and goal!
+ std::vector<HcGraphVertexType> visited;
+ visited.push_back(start_vtx);
+ DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
+}
+
+} // end namespace
diff --git a/src/teb_local_planner/src/homotopy_class_planner.cpp b/src/teb_local_planner/src/homotopy_class_planner.cpp
new file mode 100644
index 0000000..6fbf995
--- /dev/null
+++ b/src/teb_local_planner/src/homotopy_class_planner.cpp
@@ -0,0 +1,854 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/homotopy_class_planner.h"
+
+#include <limits>
+
+namespace teb_local_planner
+{
+
+HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), robot_model_(new PointRobotFootprint()), initial_plan_(NULL), initialized_(false)
+{
+}
+
+HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
+ TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL)
+{
+ initialize(node, cfg, obstacles, visual, via_points);
+}
+
+HomotopyClassPlanner::~HomotopyClassPlanner()
+{
+}
+
+void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
+ TebVisualizationPtr visual, const ViaPointContainer* via_points)
+{
+ node_ = node;
+ cfg_ = &cfg;
+ obstacles_ = obstacles;
+ via_points_ = via_points;
+
+ if (cfg_->hcp.simple_exploration)
+ graph_search_ = std::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
+ else
+ graph_search_ = std::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));
+
+ std::random_device rd;
+ random_.seed(rd());
+
+ // This is needed to prevent different time sources error
+ last_eq_class_switching_time_ = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
+
+ initialized_ = true;
+
+ setVisualization(visual);
+}
+
+void HomotopyClassPlanner::setVisualization(const TebVisualizationPtr& visualization)
+{
+ visualization_ = visualization;
+}
+
+
+
+bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+{
+ TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
+
+ // store initial plan for further initializations (must be valid for the lifetime of this object or clearPlanner() is called!)
+ initial_plan_ = &initial_plan;
+
+ PoseSE2 start(initial_plan.front().pose);
+ PoseSE2 goal(initial_plan.back().pose);
+
+ return plan(start, goal, start_vel, free_goal_vel);
+}
+
+// tf2 doesn't have tf::Pose
+//bool HomotopyClassPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+//{
+// TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
+// PoseSE2 start_pose(start);
+// PoseSE2 goal_pose(goal);
+// return plan(start_pose, goal_pose, start_vel, free_goal_vel);
+//}
+
+bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+{
+ TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
+
+ // Update old TEBs with new start, goal and velocity
+ updateAllTEBs(&start, &goal, start_vel);
+
+ // Init new TEBs based on newly explored homotopy classes
+ exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel, free_goal_vel);
+ // update via-points if activated
+ updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
+ // Optimize all trajectories in alternative homotopy classes
+ optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
+ // Select which candidate (based on alternative homotopy classes) should be used
+ selectBestTeb();
+
+ initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
+ return true;
+}
+
+bool HomotopyClassPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
+{
+ TebOptimalPlannerConstPtr best_teb = bestTeb();
+ if (!best_teb)
+ {
+ vx = 0;
+ vy = 0;
+ omega = 0;
+ return false;
+ }
+
+ return best_teb->getVelocityCommand(vx, vy, omega, look_ahead_poses);
+}
+
+
+
+
+void HomotopyClassPlanner::visualize()
+{
+ if (visualization_)
+ {
+ // Visualize graph
+ if (cfg_->hcp.visualize_hc_graph && graph_search_)
+ visualization_->publishGraph(graph_search_->graph_);
+
+ // Visualize active tebs as marker
+ visualization_->publishTebContainer(tebs_);
+
+ // Visualize best teb and feedback message if desired
+ TebOptimalPlannerConstPtr best_teb = bestTeb();
+ if (best_teb)
+ {
+ visualization_->publishLocalPlanAndPoses(best_teb->teb());
+
+ if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.
+ visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *cfg_->robot_model);
+
+ // feedback message
+ if (cfg_->trajectory.publish_feedback)
+ {
+ int best_idx = bestTebIdx();
+ if (best_idx>=0)
+ visualization_->publishFeedbackMessage(tebs_, (unsigned int) best_idx, *obstacles_);
+ }
+ }
+ }
+ else RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
+}
+
+
+
+bool HomotopyClassPlanner::hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const
+{
+ // iterate existing h-signatures and check if there is an existing H-Signature similar the candidate
+ for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
+ {
+ if (eq_class->isEqual(*eqrel.first))
+ return true; // Found! Homotopy class already exists, therefore nothing added
+ }
+ return false;
+}
+
+bool HomotopyClassPlanner::addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock)
+{
+ if (!eq_class)
+ return false;
+
+ if (!eq_class->isValid())
+ {
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner: Ignoring invalid H-signature");
+ return false;
+ }
+
+ if (hasEquivalenceClass(eq_class))
+ {
+ // Allow up to configured number of Tebs that are in the same homotopy
+ // class as the current (best) Teb to avoid being stuck in a local minimum
+ if (!isInBestTebClass(eq_class) || numTebsInBestTebClass() >= cfg_->hcp.max_number_plans_in_current_class)
+ return false;
+ }
+
+ // Homotopy class not found -> Add to class-list, return that the h-signature is new
+ equivalence_classes_.push_back(std::make_pair(eq_class,lock));
+ return true;
+}
+
+
+void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours)
+{
+ // clear old h-signatures (since they could be changed due to new obstacle positions.
+ equivalence_classes_.clear();
+
+ // Adding the equivalence class of the latest best_teb_ first
+ TebOptPlannerContainer::iterator it_best_teb = best_teb_ ? std::find(tebs_.begin(), tebs_.end(), best_teb_) : tebs_.end();
+ bool has_best_teb = it_best_teb != tebs_.end();
+ if (has_best_teb)
+ {
+ std::iter_swap(tebs_.begin(), it_best_teb); // Putting the last best teb at the beginning of the container
+ best_teb_eq_class_ = calculateEquivalenceClass(best_teb_->teb().poses().begin(),
+ best_teb_->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
+ best_teb_->teb().timediffs().begin(), best_teb_->teb().timediffs().end());
+ addEquivalenceClassIfNew(best_teb_eq_class_);
+ }
+ // Collect h-signatures for all existing TEBs and store them together with the corresponding iterator / pointer:
+// typedef std::list< std::pair<TebOptPlannerContainer::iterator, std::complex<long double> > > TebCandidateType;
+// TebCandidateType teb_candidates;
+
+ // get new homotopy classes and delete multiple TEBs per homotopy class. Skips the best teb if available (added before).
+ TebOptPlannerContainer::iterator it_teb = has_best_teb ? std::next(tebs_.begin(), 1) : tebs_.begin();
+ while(it_teb != tebs_.end())
+ {
+ // calculate equivalence class for the current candidate
+ EquivalenceClassPtr equivalence_class = calculateEquivalenceClass(it_teb->get()->teb().poses().begin(), it_teb->get()->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
+ it_teb->get()->teb().timediffs().begin(), it_teb->get()->teb().timediffs().end());
+
+// teb_candidates.push_back(std::make_pair(it_teb,H));
+
+ // WORKAROUND until the commented code below works
+ // Here we do not compare cost values. Just first come first serve...
+ bool new_flag = addEquivalenceClassIfNew(equivalence_class);
+ if (!new_flag)
+ {
+ it_teb = tebs_.erase(it_teb);
+ continue;
+ }
+
+ ++it_teb;
+ }
+ if(delete_detours)
+ deletePlansDetouringBackwards(cfg_->hcp.detours_orientation_tolerance, cfg_->hcp.length_start_orientation_vector);
+
+ // Find multiple candidates and delete the one with higher cost
+ // TODO: this code needs to be adpated. Erasing tebs from the teb container_ could make iteratores stored in the candidate list invalid!
+// TebCandidateType::reverse_iterator cand_i = teb_candidates.rbegin();
+// int test_idx = 0;
+// while (cand_i != teb_candidates.rend())
+// {
+//
+// TebCandidateType::reverse_iterator cand_j = std::find_if(boost::next(cand_i),teb_candidates.rend(), boost::bind(compareH,_1,cand_i->second));
+// if (cand_j != teb_candidates.rend() && cand_j != cand_i)
+// {
+// TebOptimalPlannerPtr pt1 = *(cand_j->first);
+// TebOptimalPlannerPtr pt2 = *(cand_i->first);
+// assert(pt1);
+// assert(pt2);
+// if ( cand_j->first->get()->getCurrentCost().sum() > cand_i->first->get()->getCurrentCost().sum() )
+// {
+// // found one that has higher cost, therefore erase cand_j
+// tebs_.erase(cand_j->first);
+// teb_candidates.erase(cand_j);
+// }
+// else // otherwise erase cand_i
+// {
+// tebs_.erase(cand_i->first);
+// cand_i = teb_candidates.erase(cand_i);
+// }
+// }
+// else
+// {
+// ROS_WARN_STREAM("increase cand_i");
+// ++cand_i;
+// }
+// }
+
+ // now add the h-signatures to the internal lookup-table (but only if there is no existing duplicate)
+// for (TebCandidateType::iterator cand=teb_candidates.begin(); cand!=teb_candidates.end(); ++cand)
+// {
+// bool new_flag = addNewHSignatureIfNew(cand->second, cfg_->hcp.h_signature_threshold);
+// if (!new_flag)
+// {
+// // ROS_ERROR_STREAM("getAndFilterHomotopyClassesTEB() - This schould not be happen.");
+// tebs_.erase(cand->first);
+// }
+// }
+
+}
+
+void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories)
+{
+ if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0)
+ return;
+
+ if(equivalence_classes_.size() < tebs_.size())
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::updateReferenceTrajectoryWithViaPoints(): Number of h-signatures does not match number of trajectories.");
+ return;
+ }
+
+ if (all_trajectories)
+ {
+ // enable via-points for all tebs
+ for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
+ {
+ tebs_[i]->setViaPoints(via_points_);
+ }
+ }
+ else
+ {
+ // enable via-points for teb in the same hommotopy class as the initial_plan and deactivate it for all other ones
+ for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
+ {
+ if(initial_plan_eq_class_->isEqual(*equivalence_classes_[i].first))
+ tebs_[i]->setViaPoints(via_points_);
+ else
+ tebs_[i]->setViaPoints(NULL);
+ }
+ }
+}
+
+
+void HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+{
+ // first process old trajectories
+ renewAndAnalyzeOldTebs(cfg_->hcp.delete_detours_backwards);
+ randomlyDropTebs();
+
+ // inject initial plan if available and not yet captured
+ if (initial_plan_)
+ {
+ initial_plan_teb_ = addAndInitNewTeb(*initial_plan_, start_vel, free_goal_vel);
+ }
+ else
+ {
+ initial_plan_teb_.reset();
+ initial_plan_teb_ = getInitialPlanTEB(); // this method searches for initial_plan_eq_class_ in the teb container (-> if !initial_plan_teb_)
+ }
+
+ // now explore new homotopy classes and initialize tebs if new ones are found. The appropriate createGraph method is chosen via polymorphism.
+ graph_search_->createGraph(start,goal,dist_to_obst,cfg_->hcp.obstacle_heading_threshold, start_vel, free_goal_vel);
+}
+
+
+TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ if(tebs_.size() >= cfg_->hcp.max_number_classes)
+ return TebOptimalPlannerPtr();
+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
+
+ candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
+
+ if (start_velocity)
+ candidate->setVelocityStart(*start_velocity);
+
+ EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
+ candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
+
+ if (free_goal_vel)
+ candidate->setVelocityGoalFree();
+
+ if(addEquivalenceClassIfNew(H))
+ {
+ tebs_.push_back(candidate);
+ return tebs_.back();
+ }
+
+ // If the candidate constitutes no new equivalence class, return a null pointer
+ return TebOptimalPlannerPtr();
+}
+
+
+bool HomotopyClassPlanner::isInBestTebClass(const EquivalenceClassPtr& eq_class) const
+{
+ bool answer = false;
+ if (best_teb_eq_class_)
+ answer = best_teb_eq_class_->isEqual(*eq_class);
+ return answer;
+}
+
+int HomotopyClassPlanner::numTebsInClass(const EquivalenceClassPtr& eq_class) const
+{
+ int count = 0;
+ for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
+ {
+ if (eq_class->isEqual(*eqrel.first))
+ ++count;
+ }
+ return count;
+}
+
+int HomotopyClassPlanner::numTebsInBestTebClass() const
+{
+ int count = 0;
+ if (best_teb_eq_class_)
+ count = numTebsInClass(best_teb_eq_class_);
+ return count;
+}
+
+TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+ if(tebs_.size() >= cfg_->hcp.max_number_classes)
+ return TebOptimalPlannerPtr();
+ TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
+
+ candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
+ cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
+
+ if (start_velocity)
+ candidate->setVelocityStart(*start_velocity);
+
+ if (free_goal_vel)
+ candidate->setVelocityGoalFree();
+
+ // store the h signature of the initial plan to enable searching a matching teb later.
+ initial_plan_eq_class_ = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
+ candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
+
+ if(addEquivalenceClassIfNew(initial_plan_eq_class_, true)) // also prevent candidate from deletion
+ {
+ tebs_.push_back(candidate);
+ return tebs_.back();
+ }
+
+ // If the candidate constitutes no new equivalence class, return a null pointer
+ return TebOptimalPlannerPtr();
+}
+
+void HomotopyClassPlanner::updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity)
+{
+ // If new goal is too far away, clear all existing trajectories to let them reinitialize later.
+ // Since all Tebs are sharing the same fixed goal pose, just take the first candidate:
+ if (!tebs_.empty()
+ && ((goal->position() - tebs_.front()->teb().BackPose().position()).norm() >= cfg_->trajectory.force_reinit_new_goal_dist
+ || fabs(g2o::normalize_theta(goal->theta() - tebs_.front()->teb().BackPose().theta())) >= cfg_->trajectory.force_reinit_new_goal_angular))
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
+ tebs_.clear();
+ equivalence_classes_.clear();
+ }
+
+ // hot-start from previous solutions
+ for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ it_teb->get()->teb().updateAndPruneTEB(*start, *goal);
+ if (start_velocity)
+ it_teb->get()->setVelocityStart(*start_velocity);
+ }
+}
+
+
+void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
+{
+ // optimize TEBs in parallel since they are independend of each other
+ if (cfg_->hcp.enable_multithreading)
+ {
+ std::vector<std::thread> teb_threads;
+
+ for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ auto functor = [&, it_teb]() {
+ it_teb->get()->optimizeTEB(iter_innerloop, iter_outerloop,
+ true, cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
+ cfg_->hcp.selection_alternative_time_cost);
+ };
+
+ teb_threads.emplace_back(functor);
+ }
+
+ for(auto & thread : teb_threads) {
+ if(thread.joinable()) {
+ thread.join();
+ }
+ }
+ }
+ else
+ {
+ for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ it_teb->get()->optimizeTEB(iter_innerloop,iter_outerloop, true, cfg_->hcp.selection_obst_cost_scale,
+ cfg_->hcp.selection_viapoint_cost_scale, cfg_->hcp.selection_alternative_time_cost); // compute cost as well inside optimizeTEB (last argument = true)
+ }
+ }
+}
+
+TebOptimalPlannerPtr HomotopyClassPlanner::getInitialPlanTEB()
+{
+ // first check stored teb object
+ if (initial_plan_teb_)
+ {
+ // check if the teb is still part of the teb container
+ if ( std::find(tebs_.begin(), tebs_.end(), initial_plan_teb_ ) != tebs_.end() )
+ return initial_plan_teb_;
+ else
+ {
+ initial_plan_teb_.reset(); // reset pointer for next call
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "initial teb not found, trying to find a match according to the cached equivalence class");
+ }
+ }
+
+ // reset the locked state for equivalence classes // TODO: this might be adapted if not only the plan containing the initial plan is locked!
+ for (int i=0; i<equivalence_classes_.size(); ++i)
+ {
+ equivalence_classes_[i].second = false;
+ }
+
+ // otherwise check if the stored reference equivalence class exist in the list of known classes
+ if (initial_plan_eq_class_ && initial_plan_eq_class_->isValid())
+ {
+ if (equivalence_classes_.size() == tebs_.size())
+ {
+ for (int i=0; i<equivalence_classes_.size(); ++i)
+ {
+ if (equivalence_classes_[i].first->isEqual(*initial_plan_eq_class_))
+ {
+ equivalence_classes_[i].second = true;
+ return tebs_[i];
+ }
+ }
+ }
+ else
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::getInitialPlanTEB(): number of equivalence classes (%lu) and number of trajectories (%lu) does not match.", equivalence_classes_.size(), tebs_.size());
+ }
+ else
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::getInitialPlanTEB(): initial TEB not found in the set of available trajectories.");
+
+ return TebOptimalPlannerPtr();
+}
+
+void HomotopyClassPlanner::randomlyDropTebs()
+{
+ if (cfg_->hcp.selection_dropping_probability == 0.0)
+ {
+ return;
+ }
+ // interate both vectors in parallel
+ auto it_eqrel = equivalence_classes_.begin();
+ auto it_teb = tebs_.begin();
+ while (it_teb != tebs_.end() && it_eqrel != equivalence_classes_.end())
+ {
+ if (it_teb->get() != best_teb_.get() // Always preserve the "best" teb
+ && (random_() <= cfg_->hcp.selection_dropping_probability * random_.max()))
+ {
+ it_teb = tebs_.erase(it_teb);
+ it_eqrel = equivalence_classes_.erase(it_eqrel);
+ }
+ else
+ {
+ ++it_teb;
+ ++it_eqrel;
+ }
+ }
+}
+
+TebOptimalPlannerPtr HomotopyClassPlanner::selectBestTeb()
+{
+ double min_cost = std::numeric_limits<double>::max(); // maximum cost
+ double min_cost_last_best = std::numeric_limits<double>::max();
+ double min_cost_initial_plan_teb = std::numeric_limits<double>::max();
+ TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
+
+ // check if last best_teb is still a valid candidate
+ if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end())
+ {
+ // get cost of this candidate
+ min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis
+ last_best_teb_ = best_teb_;
+ }
+ else
+ {
+ last_best_teb_.reset();
+ }
+
+ if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB()
+ {
+ // get cost of this candidate
+ min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis
+ }
+
+
+ best_teb_.reset(); // reset pointer
+
+ for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ // check if the related TEB leaves the local costmap region
+// if (tebs_.size()>1 && !(*it_teb)->teb().isTrajectoryInsideRegion(20, -1, 1))
+// {
+// ROS_INFO("HomotopyClassPlanner::selectBestTeb(): skipping trajectories that are not inside the local costmap");
+// continue;
+// }
+
+ double teb_cost;
+
+ if (*it_teb == last_best_teb_)
+ teb_cost = min_cost_last_best; // skip already known cost value of the last best_teb
+ else if (*it_teb == initial_plan_teb)
+ teb_cost = min_cost_initial_plan_teb;
+ else
+ teb_cost = it_teb->get()->getCurrentCost();
+
+ if (teb_cost < min_cost)
+ {
+ // check if this candidate is currently not selected
+ best_teb_ = *it_teb;
+ min_cost = teb_cost;
+ }
+ }
+
+
+ // in case we haven't found any teb due to some previous checks, investigate list again
+// if (!best_teb_ && !tebs_.empty())
+// {
+// RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "all " << tebs_.size() << " tebs rejected previously");
+// if (tebs_.size()==1)
+// best_teb_ = tebs_.front();
+// else // if multiple TEBs are available:
+// {
+// // try to use the one that relates to the initial plan
+// TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
+// if (initial_plan_teb)
+// best_teb_ = initial_plan_teb;
+// else
+// {
+// // now compute the cost for the rest (we haven't computed it before)
+// for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+// {
+// double teb_cost = it_teb->get()->getCurrentCost();
+// if (teb_cost < min_cost)
+// {
+// // check if this candidate is currently not selected
+// best_teb_ = *it_teb;
+// min_cost = teb_cost;
+// }
+// }
+// }
+// }
+// }
+
+ // check if we are allowed to change
+ if (last_best_teb_ && best_teb_ != last_best_teb_)
+ {
+ rclcpp::Time now = node_->now();
+ if ((now - last_eq_class_switching_time_).seconds() > cfg_->hcp.switching_blocking_period)
+ {
+ last_eq_class_switching_time_ = now;
+ }
+ else
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "HomotopyClassPlanner::selectBestTeb(): Switching equivalence classes blocked (check parameter switching_blocking_period.");
+ // block switching, so revert best_teb_
+ best_teb_ = last_best_teb_;
+ }
+
+ }
+
+
+ return best_teb_;
+}
+
+int HomotopyClassPlanner::bestTebIdx() const
+{
+ if (tebs_.size() == 1)
+ return 0;
+
+ if (!best_teb_)
+ return -1;
+
+ int idx = 0;
+ for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb, ++idx)
+ {
+ if (*it_teb == best_teb_)
+ return idx;
+ }
+ return -1;
+}
+
+bool HomotopyClassPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+ double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
+{
+ bool feasible = false;
+ while(rclcpp::ok() && !feasible && tebs_.size() > 0)
+ {
+ TebOptimalPlannerPtr best = findBestTeb();
+ if (!best)
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Couldn't retrieve the best plan");
+ return false;
+ }
+ feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx, feasibility_check_lookahead_distance);
+ if(!feasible)
+ {
+ removeTeb(best);
+ if(last_best_teb_ && (last_best_teb_ == best)) // Same plan as before.
+ return feasible; // Not failing could result in oscillations between trajectories.
+ }
+ }
+ return feasible;
+}
+
+TebOptimalPlannerPtr HomotopyClassPlanner::findBestTeb()
+{
+ if(tebs_.empty())
+ return TebOptimalPlannerPtr();
+ if (!best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end())
+ best_teb_ = selectBestTeb();
+ return best_teb_;
+}
+
+TebOptPlannerContainer::iterator HomotopyClassPlanner::removeTeb(TebOptimalPlannerPtr& teb)
+{
+ TebOptPlannerContainer::iterator return_iterator = tebs_.end();
+ if(equivalence_classes_.size() != tebs_.size())
+ {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "removeTeb: size of eq classes != size of tebs");
+ return return_iterator;
+ }
+ auto it_eq_classes = equivalence_classes_.begin();
+ for(auto it = tebs_.begin(); it != tebs_.end(); ++it)
+ {
+ if(*it == teb)
+ {
+ return_iterator = tebs_.erase(it);
+ equivalence_classes_.erase(it_eq_classes);
+ break;
+ }
+ ++it_eq_classes;
+ }
+ return return_iterator;
+}
+
+void HomotopyClassPlanner::setPreferredTurningDir(RotType dir)
+{
+ // set preferred turning dir for all TEBs
+ for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ (*it_teb)->setPreferredTurningDir(dir);
+ }
+}
+
+bool HomotopyClassPlanner::hasDiverged() const
+{
+ // Early return if there is no best trajectory initialized
+ if (!best_teb_)
+ return false;
+
+ return best_teb_->hasDiverged();
+}
+
+void HomotopyClassPlanner::computeCurrentCost(std::vector<double>& cost, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
+{
+ for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
+ {
+ it_teb->get()->computeCurrentCost(cost, obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
+ }
+}
+
+void HomotopyClassPlanner::deletePlansDetouringBackwards(const double orient_threshold,
+ const double len_orientation_vector)
+{
+ if (tebs_.size() < 2 || !best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end() ||
+ best_teb_->teb().sizePoses() < 2)
+ {
+ return; // A moving direction wasn't chosen yet
+ }
+ double current_movement_orientation;
+ double best_plan_duration = std::max(best_teb_->teb().getSumOfAllTimeDiffs(), 1.0);
+ if(!computeStartOrientation(best_teb_, len_orientation_vector, current_movement_orientation))
+ return; // The plan is shorter than len_orientation_vector
+ for(auto it_teb = tebs_.begin(); it_teb != tebs_.end();)
+ {
+ if(*it_teb == best_teb_) // The current plan should not be considered a detour
+ {
+ ++it_teb;
+ continue;
+ }
+ if((*it_teb)->teb().sizePoses() < 2)
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Discarding a plan with less than 2 poses");
+ it_teb = removeTeb(*it_teb);
+ continue;
+ }
+ double plan_orientation;
+ if(!computeStartOrientation(*it_teb, len_orientation_vector, plan_orientation))
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Failed to compute the start orientation for one of the tebs, likely close to the target");
+ it_teb = removeTeb(*it_teb);
+ continue;
+ }
+ if(fabs(g2o::normalize_theta(plan_orientation - current_movement_orientation)) > orient_threshold)
+ {
+ it_teb = removeTeb(*it_teb); // Plan detouring backwards
+ continue;
+ }
+ if(!it_teb->get()->isOptimized())
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's not optimized");
+ it_teb = removeTeb(*it_teb); // Deletes tebs that cannot be optimized (last optim call failed)
+ continue;
+ }
+ if(it_teb->get()->teb().getSumOfAllTimeDiffs() / best_plan_duration > cfg_->hcp.max_ratio_detours_duration_best_duration)
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's duration is much longer than that of the best teb");
+ it_teb = removeTeb(*it_teb);
+ continue;
+ }
+ ++it_teb;
+ }
+}
+
+bool HomotopyClassPlanner::computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector,
+ double& orientation)
+{
+ VertexPose start_pose = plan->teb().Pose(0);
+ bool second_pose_found = false;
+ Eigen::Vector2d start_vector;
+ for(auto& pose : plan->teb().poses())
+ {
+ start_vector = start_pose.position() - pose->position();
+ if(start_vector.norm() > len_orientation_vector)
+ {
+ second_pose_found = true;
+ break;
+ }
+ }
+ if(!second_pose_found) // The current plan is too short to make assumptions on the start orientation
+ return false;
+ orientation = std::atan2(start_vector[1], start_vector[0]);
+ return true;
+}
+
+
+} // end namespace
diff --git a/src/teb_local_planner/src/obstacles.cpp b/src/teb_local_planner/src/obstacles.cpp
new file mode 100644
index 0000000..95253e3
--- /dev/null
+++ b/src/teb_local_planner/src/obstacles.cpp
@@ -0,0 +1,216 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/obstacles.h"
+// #include "teb_local_planner/misc.h"
+#include <rclcpp/logging.hpp>
+#include <rclcpp/logger.hpp>
+
+namespace teb_local_planner
+{
+
+
+void PolygonObstacle::fixPolygonClosure()
+{
+ if (vertices_.size()<2)
+ return;
+
+ if (vertices_.front().isApprox(vertices_.back()))
+ vertices_.pop_back();
+}
+
+void PolygonObstacle::calcCentroid()
+{
+ if (vertices_.empty())
+ {
+ centroid_.setConstant(NAN);
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs.");
+ return;
+ }
+
+ // if polygon is a point
+ if (noVertices()==1)
+ {
+ centroid_ = vertices_.front();
+ return;
+ }
+
+ // if polygon is a line:
+ if (noVertices()==2)
+ {
+ centroid_ = 0.5*(vertices_.front() + vertices_.back());
+ return;
+ }
+
+ // otherwise:
+
+ centroid_.setZero();
+
+ // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon)
+ double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i)
+ for (int i=0; i < noVertices()-1; ++i)
+ {
+ A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1);
+ }
+ A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1);
+ A *= 0.5;
+
+ if (A!=0)
+ {
+ for (int i=0; i < noVertices()-1; ++i)
+ {
+ double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1));
+ centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux;
+ }
+ double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1));
+ centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux;
+ centroid_ /= (6*A);
+ }
+ else // A == 0 -> all points are placed on a 'perfect' line
+ {
+ // seach for the two outer points of the line with the maximum distance inbetween
+ int i_cand = 0;
+ int j_cand = 0;
+ double max_dist = 0;
+ for (int i=0; i< noVertices(); ++i)
+ {
+ for (int j=i+1; j< noVertices(); ++j) // start with j=i+1
+ {
+ double dist = (vertices_[j] - vertices_[i]).norm();
+ if (dist > max_dist)
+ {
+ max_dist = dist;
+ i_cand = i;
+ j_cand = j;
+ }
+ }
+ }
+ // calc centroid of that line
+ centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]);
+ }
+}
+
+
+
+Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const
+{
+ // the polygon is a point
+ if (noVertices() == 1)
+ {
+ return vertices_.front();
+ }
+
+ if (noVertices() > 1)
+ {
+
+ Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1));
+
+ if (noVertices() > 2) // real polygon and not a line
+ {
+ double dist = (new_pt-position).norm();
+ Eigen::Vector2d closest_pt = new_pt;
+
+ // check each polygon edge
+ for (int i=1; i<noVertices()-1; ++i) // skip the first one, since we already checked it (new_pt)
+ {
+ new_pt = closest_point_on_line_segment_2d(position, vertices_.at(i), vertices_.at(i+1));
+ double new_dist = (new_pt-position).norm();
+ if (new_dist < dist)
+ {
+ dist = new_dist;
+ closest_pt = new_pt;
+ }
+ }
+ // afterwards we check the edge between goal and start (close polygon)
+ new_pt = closest_point_on_line_segment_2d(position, vertices_.back(), vertices_.front());
+ double new_dist = (new_pt-position).norm();
+ if (new_dist < dist)
+ return new_pt;
+ else
+ return closest_pt;
+ }
+ else
+ {
+ return new_pt; // closest point on line segment
+ }
+ }
+
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "PolygonObstacle::getClosestPoint() cannot find any closest point. Polygon ill-defined?");
+ return Eigen::Vector2d::Zero(); // todo: maybe boost::optional?
+}
+
+
+bool PolygonObstacle::checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist) const
+{
+ // Simple strategy, check all edge-line intersections until an intersection is found...
+ // check each polygon edge
+ for (int i=0; i<noVertices()-1; ++i)
+ {
+ if ( check_line_segments_intersection_2d(line_start, line_end, vertices_.at(i), vertices_.at(i+1)) )
+ return true;
+ }
+ if (noVertices()==2) // if polygon is a line
+ return false;
+
+ return check_line_segments_intersection_2d(line_start, line_end, vertices_.back(), vertices_.front()); //otherwise close polygon
+}
+
+
+
+// implements toPolygonMsg() of the base class
+void PolygonObstacle::toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
+{
+ polygon.points.resize(vertices_.size());
+ for (std::size_t i=0; i<vertices_.size(); ++i)
+ {
+ polygon.points[i].x = vertices_[i].x();
+ polygon.points[i].y = vertices_[i].y();
+ polygon.points[i].z = 0;
+ }
+}
+
+
+
+
+
+
+
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/optimal_planner.cpp b/src/teb_local_planner/src/optimal_planner.cpp
new file mode 100644
index 0000000..1ab883f
--- /dev/null
+++ b/src/teb_local_planner/src/optimal_planner.cpp
@@ -0,0 +1,1326 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include <tf2/time.h>
+#include <tf2_ros/buffer_interface.h>
+
+#include "teb_local_planner/optimal_planner.h"
+
+#include <map>
+// g2o custom edges and vertices for the TEB planner
+#include <teb_local_planner/g2o_types/edge_velocity.h>
+#include <teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h>
+#include <teb_local_planner/g2o_types/edge_acceleration.h>
+#include <teb_local_planner/g2o_types/edge_kinematics.h>
+#include <teb_local_planner/g2o_types/edge_time_optimal.h>
+#include <teb_local_planner/g2o_types/edge_shortest_path.h>
+#include <teb_local_planner/g2o_types/edge_obstacle.h>
+#include <teb_local_planner/g2o_types/edge_dynamic_obstacle.h>
+#include <teb_local_planner/g2o_types/edge_via_point.h>
+#include <teb_local_planner/g2o_types/edge_prefer_rotdir.h>
+
+#include <memory>
+#include <limits>
+
+namespace teb_local_planner
+{
+
+// ============== Implementation ===================
+
+TebOptimalPlanner::TebOptimalPlanner() : cfg_(nullptr), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none),
+ initialized_(false), optimized_(false)
+{
+}
+
+TebOptimalPlanner::TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points)
+{
+ initialize(node, cfg, obstacles, visual, via_points);
+}
+
+TebOptimalPlanner::~TebOptimalPlanner()
+{
+ clearGraph();
+ // free dynamically allocated memory
+ //if (optimizer_)
+ // g2o::Factory::destroy();
+ //g2o::OptimizationAlgorithmFactory::destroy();
+ //g2o::HyperGraphActionLibrary::destroy();
+}
+
+void TebOptimalPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points)
+{
+ node_ = node;
+ // init optimizer (set solver and block ordering settings)
+ optimizer_ = initOptimizer();
+
+ cfg_ = &cfg;
+ obstacles_ = obstacles;
+ via_points_ = via_points;
+ cost_ = HUGE_VAL;
+ prefer_rotdir_ = RotType::none;
+
+ vel_start_.first = true;
+ vel_start_.second.linear.x = 0;
+ vel_start_.second.linear.y = 0;
+ vel_start_.second.angular.z = 0;
+
+ vel_goal_.first = true;
+ vel_goal_.second.linear.x = 0;
+ vel_goal_.second.linear.y = 0;
+ vel_goal_.second.angular.z = 0;
+ initialized_ = true;
+
+ setVisualization(visual);
+}
+
+void TebOptimalPlanner::setVisualization(const TebVisualizationPtr& visualization)
+{
+ visualization_ = visualization;
+}
+
+void TebOptimalPlanner::visualize()
+{
+ if (!visualization_)
+ return;
+
+ visualization_->publishLocalPlanAndPoses(teb_);
+
+ if (teb_.sizePoses() > 0)
+ visualization_->publishRobotFootprintModel(teb_.Pose(0), *cfg_->robot_model);
+
+ if (cfg_->trajectory.publish_feedback)
+ visualization_->publishFeedbackMessage(*this, *obstacles_);
+
+}
+
+/*
+ * registers custom vertices and edges in g2o framework
+ */
+void TebOptimalPlanner::registerG2OTypes()
+{
+ g2o::Factory* factory = g2o::Factory::instance();
+ factory->registerType("VERTEX_POSE", std::make_shared<g2o::HyperGraphElementCreator<VertexPose>>());
+ factory->registerType("VERTEX_TIMEDIFF", std::make_shared<g2o::HyperGraphElementCreator<VertexTimeDiff>>());
+ factory->registerType("EDGE_TIME_OPTIMAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeTimeOptimal>>());
+ factory->registerType("EDGE_SHORTEST_PATH", std::make_shared<g2o::HyperGraphElementCreator<EdgeShortestPath>>());
+ factory->registerType("EDGE_VELOCITY", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocity>>());
+ factory->registerType("EDGE_VELOCITY_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>>());
+ factory->registerType("EDGE_ACCELERATION", std::make_shared<g2o::HyperGraphElementCreator<EdgeAcceleration>>());
+ factory->registerType("EDGE_ACCELERATION_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationStart>>());
+ factory->registerType("EDGE_ACCELERATION_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationGoal>>());
+ factory->registerType("EDGE_ACCELERATION_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>>());
+ factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>>());
+ factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>>());
+ factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>>());
+ factory->registerType("EDGE_KINEMATICS_CARLIKE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>>());
+ factory->registerType("EDGE_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeObstacle>>());
+ factory->registerType("EDGE_INFLATED_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeInflatedObstacle>>());
+ factory->registerType("EDGE_DYNAMIC_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeDynamicObstacle>>());
+ factory->registerType("EDGE_VIA_POINT", std::make_shared<g2o::HyperGraphElementCreator<EdgeViaPoint>>());
+ factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
+ return;
+}
+
+/*
+ * initialize g2o optimizer. Set solver settings here.
+ * Return: pointer to new SparseOptimizer Object.
+ */
+std::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
+{
+ // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
+ static std::once_flag flag;
+ std::call_once(flag, [this](){this->registerG2OTypes();});
+
+ // allocating the optimizer
+ std::shared_ptr<g2o::SparseOptimizer> optimizer = std::make_shared<g2o::SparseOptimizer>();
+ auto linearSolver = std::make_unique<TEBLinearSolver>(); // see typedef in optimization.h
+ linearSolver->setBlockOrdering(true);
+ auto blockSolver = std::make_unique<TEBBlockSolver>(std::move(linearSolver));
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
+
+ optimizer->setAlgorithm(solver);
+
+ optimizer->initMultiThreading(); // required for >Eigen 3.1
+
+ return optimizer;
+}
+
+
+bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
+ double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
+{
+ if (cfg_->optim.optimization_activate==false)
+ return false;
+
+ bool success = false;
+ optimized_ = false;
+
+ double weight_multiplier = 1.0;
+
+ // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
+ // (which leads to better results in terms of x-y-t homotopy planning).
+ // however, we have not tested this mode intensively yet, so we keep
+ // the legacy fast mode as default until we finish our tests.
+ bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
+
+ for(int i=0; i<iterations_outerloop; ++i)
+ {
+ if (cfg_->trajectory.teb_autosize)
+ {
+ //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
+ teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
+
+ }
+
+ success = buildGraph(weight_multiplier);
+ if (!success)
+ {
+ clearGraph();
+ return false;
+ }
+ success = optimizeGraph(iterations_innerloop, false);
+ if (!success)
+ {
+ clearGraph();
+ return false;
+ }
+ optimized_ = true;
+
+ if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
+ computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
+
+ clearGraph();
+
+ weight_multiplier *= cfg_->optim.weight_adapt_factor;
+ }
+
+ return true;
+}
+
+void TebOptimalPlanner::setVelocityStart(const geometry_msgs::msg::Twist& vel_start)
+{
+ vel_start_.first = true;
+ vel_start_.second.linear.x = vel_start.linear.x;
+ vel_start_.second.linear.y = vel_start.linear.y;
+ vel_start_.second.angular.z = vel_start.angular.z;
+}
+
+void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal)
+{
+ vel_goal_.first = true;
+ vel_goal_.second = vel_goal;
+}
+
+bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+{
+ TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
+ if (!teb_.isInit())
+ {
+ teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
+ cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
+ }
+ else // warm start
+ {
+ PoseSE2 start_(initial_plan.front().pose);
+ PoseSE2 goal_(initial_plan.back().pose);
+ if (teb_.sizePoses()>0
+ && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
+ && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
+ teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
+ else // goal too far away -> reinit
+ {
+ RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
+ teb_.clearTimedElasticBand();
+ teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
+ cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
+ }
+ }
+ if (start_vel)
+ setVelocityStart(*start_vel);
+ if (free_goal_vel)
+ setVelocityGoalFree();
+ else
+ vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
+
+ // now optimize
+ return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
+}
+
+
+//bool TebOptimalPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+//{
+// PoseSE2 start_(start);
+// PoseSE2 goal_(goal);
+// return plan(start_, goal_, start_vel);
+//}
+
+bool TebOptimalPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
+{
+ TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
+ if (!teb_.isInit())
+ {
+ // init trajectory
+ teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization
+ }
+ else // warm start
+ {
+ if (teb_.sizePoses() > 0
+ && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
+ && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
+ teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);
+ else // goal too far away -> reinit
+ {
+ RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
+ teb_.clearTimedElasticBand();
+ teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
+ }
+ }
+ if (start_vel)
+ setVelocityStart(*start_vel);
+ if (free_goal_vel)
+ setVelocityGoalFree();
+ else
+ vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
+
+ // now optimize
+ return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations, true);
+}
+
+
+bool TebOptimalPlanner::buildGraph(double weight_multiplier)
+{
+ if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
+ {
+ RCLCPP_WARN(node_->get_logger(), "Cannot build graph, because it is not empty. Call graphClear()!");
+ return false;
+ }
+
+ optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);
+
+ // add TEB vertices
+ AddTEBVertices();
+
+ // add Edges (local cost functions)
+ if (cfg_->obstacles.legacy_obstacle_association)
+ AddEdgesObstaclesLegacy(weight_multiplier);
+ else
+ AddEdgesObstacles(weight_multiplier);
+
+ if (cfg_->obstacles.include_dynamic_obstacles)
+ AddEdgesDynamicObstacles();
+
+ AddEdgesViaPoints();
+
+ AddEdgesVelocity();
+
+ AddEdgesAcceleration();
+
+ AddEdgesTimeOptimal();
+
+ AddEdgesShortestPath();
+
+ if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
+ AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
+ else
+ AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.
+
+ AddEdgesPreferRotDir();
+
+ if (cfg_->optim.weight_velocity_obstacle_ratio > 0)
+ AddEdgesVelocityObstacleRatio();
+
+ return true;
+}
+
+bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)
+{
+ if (cfg_->robot.max_vel_x<0.01)
+ {
+ RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted...");
+ if (clear_after) clearGraph();
+ return false;
+ }
+
+ if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
+ {
+ RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
+ if (clear_after) clearGraph();
+ return false;
+ }
+
+ optimizer_->setVerbose(cfg_->optim.optimization_verbose);
+ optimizer_->initializeOptimization();
+
+ int iter = optimizer_->optimize(no_iterations);
+
+ // Save Hessian for visualization
+ // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*> (optimizer_->solver());
+ // lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");
+
+ if(!iter)
+ {
+ RCLCPP_ERROR(node_->get_logger(), "optimizeGraph(): Optimization failed! iter=%i", iter);
+ return false;
+ }
+
+ if (clear_after) clearGraph();
+
+ return true;
+}
+
+void TebOptimalPlanner::clearGraph()
+{
+ // clear optimizer states
+ if (optimizer_)
+ {
+ // we will delete all edges but keep the vertices.
+ // before doing so, we will delete the link from the vertices to the edges.
+ auto& vertices = optimizer_->vertices();
+ for(auto& v : vertices)
+ v.second->edges().clear();
+
+ optimizer_->vertices().clear(); // necessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!)
+ optimizer_->clear();
+ }
+}
+
+
+
+void TebOptimalPlanner::AddTEBVertices()
+{
+ // add vertices to graph
+ RCLCPP_DEBUG_EXPRESSION(node_->get_logger(), cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
+ unsigned int id_counter = 0; // used for vertices ids
+ obstacles_per_vertex_.resize(teb_.sizePoses());
+ auto iter_obstacle = obstacles_per_vertex_.begin();
+ for (int i=0; i<teb_.sizePoses(); ++i)
+ {
+ teb_.PoseVertex(i)->setId(id_counter++);
+ optimizer_->addVertex(teb_.PoseVertex(i));
+ if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs())
+ {
+ teb_.TimeDiffVertex(i)->setId(id_counter++);
+ optimizer_->addVertex(teb_.TimeDiffVertex(i));
+ }
+ iter_obstacle->clear();
+ (iter_obstacle++)->reserve(obstacles_->size());
+ }
+}
+
+
+void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
+{
+ if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr )
+ return; // if weight equals zero skip adding edges!
+
+
+ bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
+
+ Eigen::Matrix<double,1,1> information;
+ information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
+
+ Eigen::Matrix<double,2,2> information_inflated;
+ information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
+ information_inflated(1,1) = cfg_->optim.weight_inflation;
+ information_inflated(0,1) = information_inflated(1,0) = 0;
+
+ auto iter_obstacle = obstacles_per_vertex_.begin();
+
+ auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) {
+ if (inflated)
+ {
+ EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
+ dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
+ dist_bandpt_obst->setInformation(information_inflated);
+ dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle);
+ optimizer_->addEdge(dist_bandpt_obst);
+ }
+ else
+ {
+ EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
+ dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
+ dist_bandpt_obst->setInformation(information);
+ dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle);
+ optimizer_->addEdge(dist_bandpt_obst);
+ };
+ };
+
+ // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too
+ const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0;
+ for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i)
+ {
+ double left_min_dist = std::numeric_limits<double>::max();
+ double right_min_dist = std::numeric_limits<double>::max();
+ ObstaclePtr left_obstacle;
+ ObstaclePtr right_obstacle;
+
+ const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();
+
+ // iterate obstacles
+ for (const ObstaclePtr& obst : *obstacles_)
+ {
+ // we handle dynamic obstacles differently below
+ if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
+ continue;
+
+ // calculate distance to robot model
+ double dist = cfg_->robot_model->calculateDistance(teb_.Pose(i), obst.get());
+
+ // force considering obstacle if really close to the current pose
+ if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor)
+ {
+ iter_obstacle->push_back(obst);
+ continue;
+ }
+ // cut-off distance
+ if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)
+ continue;
+
+ // determine side (left or right) and assign obstacle if closer than the previous one
+ if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
+ {
+ if (dist < left_min_dist)
+ {
+ left_min_dist = dist;
+ left_obstacle = obst;
+ }
+ }
+ else
+ {
+ if (dist < right_min_dist)
+ {
+ right_min_dist = dist;
+ right_obstacle = obst;
+ }
+ }
+ }
+
+ if (left_obstacle)
+ iter_obstacle->push_back(left_obstacle);
+ if (right_obstacle)
+ iter_obstacle->push_back(right_obstacle);
+
+ // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
+ if (i == 0)
+ {
+ ++iter_obstacle;
+ continue;
+ }
+
+ // create obstacle edges
+ for (const ObstaclePtr obst : *iter_obstacle)
+ create_edge(i, obst.get());
+ ++iter_obstacle;
+ }
+}
+
+
+void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier)
+{
+ if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr)
+ return; // if weight equals zero skip adding edges!
+
+ Eigen::Matrix<double,1,1> information;
+ information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
+
+ Eigen::Matrix<double,2,2> information_inflated;
+ information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
+ information_inflated(1,1) = cfg_->optim.weight_inflation;
+ information_inflated(0,1) = information_inflated(1,0) = 0;
+
+ bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
+
+ for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
+ {
+ if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below
+ continue;
+
+ int index;
+
+ if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses())
+ index = teb_.sizePoses() / 2;
+ else
+ index = teb_.findClosestTrajectoryPose(*(obst->get()));
+
+
+ // check if obstacle is outside index-range between start and goal
+ if ( (index <= 1) || (index > teb_.sizePoses()-2) ) // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range
+ continue;
+
+ if (inflated)
+ {
+ EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
+ dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
+ dist_bandpt_obst->setInformation(information_inflated);
+ dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst);
+ }
+ else
+ {
+ EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
+ dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
+ dist_bandpt_obst->setInformation(information);
+ dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst);
+ }
+
+ for (int neighbourIdx=0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected/2); neighbourIdx++)
+ {
+ if (index+neighbourIdx < teb_.sizePoses())
+ {
+ if (inflated)
+ {
+ EdgeInflatedObstacle* dist_bandpt_obst_n_r = new EdgeInflatedObstacle;
+ dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx));
+ dist_bandpt_obst_n_r->setInformation(information_inflated);
+ dist_bandpt_obst_n_r->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst_n_r);
+ }
+ else
+ {
+ EdgeObstacle* dist_bandpt_obst_n_r = new EdgeObstacle;
+ dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx));
+ dist_bandpt_obst_n_r->setInformation(information);
+ dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst_n_r);
+ }
+ }
+ if ( index - neighbourIdx >= 0) // needs to be casted to int to allow negative values
+ {
+ if (inflated)
+ {
+ EdgeInflatedObstacle* dist_bandpt_obst_n_l = new EdgeInflatedObstacle;
+ dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx));
+ dist_bandpt_obst_n_l->setInformation(information_inflated);
+ dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst_n_l);
+ }
+ else
+ {
+ EdgeObstacle* dist_bandpt_obst_n_l = new EdgeObstacle;
+ dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx));
+ dist_bandpt_obst_n_l->setInformation(information);
+ dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
+ optimizer_->addEdge(dist_bandpt_obst_n_l);
+ }
+ }
+ }
+
+ }
+}
+
+
+void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier)
+{
+ if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==NULL )
+ return; // if weight equals zero skip adding edges!
+
+ Eigen::Matrix<double,2,2> information;
+ information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
+ information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation;
+ information(0,1) = information(1,0) = 0;
+
+ for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
+ {
+ if (!(*obst)->isDynamic())
+ continue;
+
+ // Skip first and last pose, as they are fixed
+ double time = teb_.TimeDiff(0);
+ for (int i=1; i < teb_.sizePoses() - 1; ++i)
+ {
+ EdgeDynamicObstacle* dynobst_edge = new EdgeDynamicObstacle(time);
+ dynobst_edge->setVertex(0,teb_.PoseVertex(i));
+ dynobst_edge->setInformation(information);
+ dynobst_edge->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
+ optimizer_->addEdge(dynobst_edge);
+ time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1".
+ }
+ }
+}
+
+void TebOptimalPlanner::AddEdgesViaPoints()
+{
+ if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() )
+ return; // if weight equals zero skip adding edges!
+
+ int start_pose_idx = 0;
+
+ int n = teb_.sizePoses();
+ if (n<3) // we do not have any degrees of freedom for reaching via-points
+ return;
+
+ for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
+ {
+
+ int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
+ if (cfg_->trajectory.via_points_ordered)
+ start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points
+
+ // check if point conicides with goal or is located behind it
+ if ( index > n-2 )
+ index = n-2; // set to a pose before the goal, since we can move it away!
+ // check if point coincides with start or is located before it
+ if ( index < 1)
+ {
+ if (cfg_->trajectory.via_points_ordered)
+ {
+ index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later.
+ }
+ else
+ {
+ RCLCPP_DEBUG(node_->get_logger(), "TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
+ continue; // skip via points really close or behind the current robot pose
+ }
+ }
+ Eigen::Matrix<double,1,1> information;
+ information.fill(cfg_->optim.weight_viapoint);
+
+ EdgeViaPoint* edge_viapoint = new EdgeViaPoint;
+ edge_viapoint->setVertex(0,teb_.PoseVertex(index));
+ edge_viapoint->setInformation(information);
+ edge_viapoint->setParameters(*cfg_, &(*vp_it));
+ optimizer_->addEdge(edge_viapoint);
+ }
+}
+
+void TebOptimalPlanner::AddEdgesVelocity()
+{
+ if (cfg_->robot.max_vel_y == 0) // non-holonomic robot
+ {
+ if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0)
+ return; // if weight equals zero skip adding edges!
+
+ int n = teb_.sizePoses();
+ Eigen::Matrix<double,2,2> information;
+ information(0,0) = cfg_->optim.weight_max_vel_x;
+ information(1,1) = cfg_->optim.weight_max_vel_theta;
+ information(0,1) = 0.0;
+ information(1,0) = 0.0;
+
+ for (int i=0; i < n - 1; ++i)
+ {
+ EdgeVelocity* velocity_edge = new EdgeVelocity;
+ velocity_edge->setVertex(0,teb_.PoseVertex(i));
+ velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
+ velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
+ velocity_edge->setInformation(information);
+ velocity_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(velocity_edge);
+ }
+ }
+ else // holonomic-robot
+ {
+ if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_y==0 && cfg_->optim.weight_max_vel_theta==0)
+ return; // if weight equals zero skip adding edges!
+
+ int n = teb_.sizePoses();
+ Eigen::Matrix<double,3,3> information;
+ information.fill(0);
+ information(0,0) = cfg_->optim.weight_max_vel_x;
+ information(1,1) = cfg_->optim.weight_max_vel_y;
+ information(2,2) = cfg_->optim.weight_max_vel_theta;
+
+ for (int i=0; i < n - 1; ++i)
+ {
+ EdgeVelocityHolonomic* velocity_edge = new EdgeVelocityHolonomic;
+ velocity_edge->setVertex(0,teb_.PoseVertex(i));
+ velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
+ velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
+ velocity_edge->setInformation(information);
+ velocity_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(velocity_edge);
+ }
+
+ }
+}
+
+void TebOptimalPlanner::AddEdgesAcceleration()
+{
+ if (cfg_->optim.weight_acc_lim_x==0 && cfg_->optim.weight_acc_lim_theta==0)
+ return; // if weight equals zero skip adding edges!
+
+ int n = teb_.sizePoses();
+
+ if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot
+ {
+ Eigen::Matrix<double,2,2> information;
+ information.fill(0);
+ information(0,0) = cfg_->optim.weight_acc_lim_x;
+ information(1,1) = cfg_->optim.weight_acc_lim_theta;
+
+ // check if an initial velocity should be taken into accound
+ if (vel_start_.first)
+ {
+ EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(0));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(1));
+ acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
+ acceleration_edge->setInitialVelocity(vel_start_.second);
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+
+ // now add the usual acceleration edge for each tuple of three teb poses
+ for (int i=0; i < n - 2; ++i)
+ {
+ EdgeAcceleration* acceleration_edge = new EdgeAcceleration;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(i));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(i+1));
+ acceleration_edge->setVertex(2,teb_.PoseVertex(i+2));
+ acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i));
+ acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1));
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+
+ // check if a goal velocity should be taken into accound
+ if (vel_goal_.first)
+ {
+ EdgeAccelerationGoal* acceleration_edge = new EdgeAccelerationGoal;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(n-2));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(n-1));
+ acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 ));
+ acceleration_edge->setGoalVelocity(vel_goal_.second);
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+ }
+ else // holonomic robot
+ {
+ Eigen::Matrix<double,3,3> information;
+ information.fill(0);
+ information(0,0) = cfg_->optim.weight_acc_lim_x;
+ information(1,1) = cfg_->optim.weight_acc_lim_y;
+ information(2,2) = cfg_->optim.weight_acc_lim_theta;
+
+ // check if an initial velocity should be taken into accound
+ if (vel_start_.first)
+ {
+ EdgeAccelerationHolonomicStart* acceleration_edge = new EdgeAccelerationHolonomicStart;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(0));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(1));
+ acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
+ acceleration_edge->setInitialVelocity(vel_start_.second);
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+
+ // now add the usual acceleration edge for each tuple of three teb poses
+ for (int i=0; i < n - 2; ++i)
+ {
+ EdgeAccelerationHolonomic* acceleration_edge = new EdgeAccelerationHolonomic;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(i));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(i+1));
+ acceleration_edge->setVertex(2,teb_.PoseVertex(i+2));
+ acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i));
+ acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1));
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+
+ // check if a goal velocity should be taken into accound
+ if (vel_goal_.first)
+ {
+ EdgeAccelerationHolonomicGoal* acceleration_edge = new EdgeAccelerationHolonomicGoal;
+ acceleration_edge->setVertex(0,teb_.PoseVertex(n-2));
+ acceleration_edge->setVertex(1,teb_.PoseVertex(n-1));
+ acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 ));
+ acceleration_edge->setGoalVelocity(vel_goal_.second);
+ acceleration_edge->setInformation(information);
+ acceleration_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(acceleration_edge);
+ }
+ }
+}
+
+
+
+void TebOptimalPlanner::AddEdgesTimeOptimal()
+{
+ if (cfg_->optim.weight_optimaltime==0)
+ return; // if weight equals zero skip adding edges!
+
+ Eigen::Matrix<double,1,1> information;
+ information.fill(cfg_->optim.weight_optimaltime);
+
+ for (int i=0; i < teb_.sizeTimeDiffs(); ++i)
+ {
+ EdgeTimeOptimal* timeoptimal_edge = new EdgeTimeOptimal;
+ timeoptimal_edge->setVertex(0,teb_.TimeDiffVertex(i));
+ timeoptimal_edge->setInformation(information);
+ timeoptimal_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(timeoptimal_edge);
+ }
+}
+
+void TebOptimalPlanner::AddEdgesShortestPath()
+{
+ if (cfg_->optim.weight_shortest_path==0)
+ return; // if weight equals zero skip adding edges!
+
+ Eigen::Matrix<double,1,1> information;
+ information.fill(cfg_->optim.weight_shortest_path);
+
+ for (int i=0; i < teb_.sizePoses()-1; ++i)
+ {
+ EdgeShortestPath* shortest_path_edge = new EdgeShortestPath;
+ shortest_path_edge->setVertex(0,teb_.PoseVertex(i));
+ shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1));
+ shortest_path_edge->setInformation(information);
+ shortest_path_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(shortest_path_edge);
+ }
+}
+
+
+
+void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
+{
+ if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0)
+ return; // if weight equals zero skip adding edges!
+
+ // create edge for satisfiying kinematic constraints
+ Eigen::Matrix<double,2,2> information_kinematics;
+ information_kinematics.fill(0.0);
+ information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
+ information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;
+
+ for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only
+ {
+ EdgeKinematicsDiffDrive* kinematics_edge = new EdgeKinematicsDiffDrive;
+ kinematics_edge->setVertex(0,teb_.PoseVertex(i));
+ kinematics_edge->setVertex(1,teb_.PoseVertex(i+1));
+ kinematics_edge->setInformation(information_kinematics);
+ kinematics_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(kinematics_edge);
+ }
+}
+
+void TebOptimalPlanner::AddEdgesKinematicsCarlike()
+{
+ if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0)
+ return; // if weight equals zero skip adding edges!
+
+ // create edge for satisfiying kinematic constraints
+ Eigen::Matrix<double,2,2> information_kinematics;
+ information_kinematics.fill(0.0);
+ information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
+ information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius;
+
+ for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only
+ {
+ EdgeKinematicsCarlike* kinematics_edge = new EdgeKinematicsCarlike;
+ kinematics_edge->setVertex(0,teb_.PoseVertex(i));
+ kinematics_edge->setVertex(1,teb_.PoseVertex(i+1));
+ kinematics_edge->setInformation(information_kinematics);
+ kinematics_edge->setTebConfig(*cfg_);
+ optimizer_->addEdge(kinematics_edge);
+ }
+}
+
+
+void TebOptimalPlanner::AddEdgesPreferRotDir()
+{
+ //TODO(roesmann): Note, these edges can result in odd predictions, in particular
+ // we can observe a substantional mismatch between open- and closed-loop planning
+ // leading to a poor control performance.
+ // At the moment, we keep these functionality for oscillation recovery:
+ // Activating the edge for a short time period might not be crucial and
+ // could move the robot to a new oscillation-free state.
+ // This needs to be analyzed in more detail!
+ if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir==0)
+ return; // if weight equals zero skip adding edges!
+
+ if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
+ {
+ RCLCPP_WARN(node_->get_logger(), "TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
+ return;
+ }
+
+ // create edge for satisfiying kinematic constraints
+ Eigen::Matrix<double,1,1> information_rotdir;
+ information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
+
+ for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) // currently: apply to first 3 rotations
+ {
+ EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
+ rotdir_edge->setVertex(0,teb_.PoseVertex(i));
+ rotdir_edge->setVertex(1,teb_.PoseVertex(i+1));
+ rotdir_edge->setInformation(information_rotdir);
+
+ if (prefer_rotdir_ == RotType::left)
+ rotdir_edge->preferLeft();
+ else if (prefer_rotdir_ == RotType::right)
+ rotdir_edge->preferRight();
+
+ optimizer_->addEdge(rotdir_edge);
+ }
+}
+
+void TebOptimalPlanner::AddEdgesVelocityObstacleRatio()
+{
+ Eigen::Matrix<double,2,2> information;
+ information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio;
+ information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio;
+ information(0,1) = information(1,0) = 0;
+
+ auto iter_obstacle = obstacles_per_vertex_.begin();
+
+ for (int index = 0; index < teb_.sizePoses() - 1; ++index)
+ {
+ for (const ObstaclePtr obstacle : (*iter_obstacle++))
+ {
+ EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio;
+ edge->setVertex(0,teb_.PoseVertex(index));
+ edge->setVertex(1,teb_.PoseVertex(index + 1));
+ edge->setVertex(2,teb_.TimeDiffVertex(index));
+ edge->setInformation(information);
+ edge->setParameters(*cfg_, cfg_->robot_model.get(), obstacle.get());
+ optimizer_->addEdge(edge);
+ }
+ }
+}
+
+bool TebOptimalPlanner::hasDiverged() const
+{
+ // Early returns if divergence detection is not active
+ if (!cfg_->recovery.divergence_detection_enable)
+ return false;
+
+ auto stats_vector = optimizer_->batchStatistics();
+
+ // No statistics yet
+ if (stats_vector.empty())
+ return false;
+
+ // Grab the statistics of the final iteration
+ const auto last_iter_stats = stats_vector.back();
+
+ return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared;
+}
+
+void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
+{
+ // check if graph is empty/exist -> important if function is called between buildGraph and optimizeGraph/clearGraph
+ bool graph_exist_flag(false);
+ if (optimizer_->edges().empty() && optimizer_->vertices().empty())
+ {
+ // here the graph is build again, for time efficiency make sure to call this function
+ // between buildGraph and Optimize (deleted), but it depends on the application
+ buildGraph();
+ optimizer_->initializeOptimization();
+ }
+ else
+ {
+ graph_exist_flag = true;
+ }
+
+ optimizer_->computeInitialGuess();
+
+ cost_ = 0;
+
+ if (alternative_time_cost)
+ {
+ cost_ += teb_.getSumOfAllTimeDiffs();
+ // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
+ // since we are using an AutoResize Function with hysteresis.
+ }
+
+ // now we need pointers to all edges -> calculate error for each edge-type
+ // since we aren't storing edge pointers, we need to check every edge
+ for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++)
+ {
+ double cur_cost = (*it)->chi2();
+
+ if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
+ || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
+ || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
+ {
+ cur_cost *= obst_cost_scale;
+ }
+ else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
+ {
+ cur_cost *= viapoint_cost_scale;
+ }
+ else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
+ {
+ continue; // skip these edges if alternative_time_cost is active
+ }
+ cost_ += cur_cost;
+ }
+
+ // delete temporary created graph
+ if (!graph_exist_flag)
+ clearGraph();
+}
+
+
+void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const
+{
+ if (dt == 0)
+ {
+ vx = 0;
+ vy = 0;
+ omega = 0;
+ return;
+ }
+
+ Eigen::Vector2d deltaS = pose2.position() - pose1.position();
+
+ if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
+ {
+ Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
+ // translational velocity
+ double dir = deltaS.dot(conf1dir);
+ vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
+ vy = 0;
+ }
+ else // holonomic robot
+ {
+ // transform pose 2 into the current robot frame (pose1)
+ // for velocities only the rotation of the direction vector is necessary.
+ // (map->pose1-frame: inverse 2d rotation matrix)
+ double cos_theta1 = std::cos(pose1.theta());
+ double sin_theta1 = std::sin(pose1.theta());
+ double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
+ double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
+ vx = p1_dx / dt;
+ vy = p1_dy / dt;
+ }
+
+ // rotational velocity
+ double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
+ omega = orientdiff/dt;
+}
+
+bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
+{
+ if (teb_.sizePoses()<2)
+ {
+ RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist.");
+ vx = 0;
+ vy = 0;
+ omega = 0;
+ return false;
+ }
+ look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
+ double dt = 0.0;
+ for(int counter = 0; counter < look_ahead_poses; ++counter)
+ {
+ dt += teb_.TimeDiff(counter);
+ if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?
+ {
+ look_ahead_poses = counter + 1;
+ break;
+ }
+ }
+ if (dt<=0)
+ {
+ RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
+ vx = 0;
+ vy = 0;
+ omega = 0;
+ return false;
+ }
+
+ // Get velocity from the first two configurations
+ extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
+ return true;
+}
+
+void TebOptimalPlanner::getVelocityProfile(std::vector<geometry_msgs::msg::Twist>& velocity_profile) const
+{
+ int n = teb_.sizePoses();
+ velocity_profile.resize( n+1 );
+
+ // start velocity
+ velocity_profile.front().linear.z = 0;
+ velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0;
+ velocity_profile.front().linear.x = vel_start_.second.linear.x;
+ velocity_profile.front().linear.y = vel_start_.second.linear.y;
+ velocity_profile.front().angular.z = vel_start_.second.angular.z;
+
+ for (int i=1; i<n; ++i)
+ {
+ velocity_profile[i].linear.z = 0;
+ velocity_profile[i].angular.x = velocity_profile[i].angular.y = 0;
+ extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), velocity_profile[i].linear.x, velocity_profile[i].linear.y, velocity_profile[i].angular.z);
+ }
+
+ // goal velocity
+ velocity_profile.back().linear.z = 0;
+ velocity_profile.back().angular.x = velocity_profile.back().angular.y = 0;
+ velocity_profile.back().linear.x = vel_goal_.second.linear.x;
+ velocity_profile.back().linear.y = vel_goal_.second.linear.y;
+ velocity_profile.back().angular.z = vel_goal_.second.angular.z;
+}
+
+void TebOptimalPlanner::getFullTrajectory(std::vector<teb_msgs::msg::TrajectoryPointMsg>& trajectory) const
+{
+ int n = teb_.sizePoses();
+
+ trajectory.resize(n);
+
+ if (n == 0)
+ return;
+
+ double curr_time = 0;
+
+ // start
+ teb_msgs::msg::TrajectoryPointMsg& start = trajectory.front();
+ teb_.Pose(0).toPoseMsg(start.pose);
+ start.velocity.linear.z = 0;
+ start.velocity.angular.x = start.velocity.angular.y = 0;
+ start.velocity.linear.x = vel_start_.second.linear.x;
+ start.velocity.linear.y = vel_start_.second.linear.y;
+ start.velocity.angular.z = vel_start_.second.angular.z;
+ start.time_from_start = durationFromSec(curr_time);
+
+ curr_time += teb_.TimeDiff(0);
+
+ // intermediate points
+ for (int i=1; i < n-1; ++i)
+ {
+ teb_msgs::msg::TrajectoryPointMsg& point = trajectory[i];
+ teb_.Pose(i).toPoseMsg(point.pose);
+ point.velocity.linear.z = 0;
+ point.velocity.angular.x = point.velocity.angular.y = 0;
+ double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2;
+ extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), vel1_x, vel1_y, omega1);
+ extractVelocity(teb_.Pose(i), teb_.Pose(i+1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2);
+ point.velocity.linear.x = 0.5*(vel1_x+vel2_x);
+ point.velocity.linear.y = 0.5*(vel1_y+vel2_y);
+ point.velocity.angular.z = 0.5*(omega1+omega2);
+ point.time_from_start = durationFromSec(curr_time);
+
+ curr_time += teb_.TimeDiff(i);
+ }
+
+ // goal
+ teb_msgs::msg::TrajectoryPointMsg& goal = trajectory.back();
+ teb_.BackPose().toPoseMsg(goal.pose);
+ goal.velocity.linear.z = 0;
+ goal.velocity.angular.x = goal.velocity.angular.y = 0;
+ goal.velocity.linear.x = vel_goal_.second.linear.x;
+ goal.velocity.linear.y = vel_goal_.second.linear.y;
+ goal.velocity.angular.z = vel_goal_.second.angular.z;
+ goal.time_from_start = durationFromSec(curr_time);
+}
+
+
+bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+ double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
+{
+ if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses())
+ look_ahead_idx = teb().sizePoses() - 1;
+
+ if (feasibility_check_lookahead_distance > 0){
+ for (int i=1; i < teb().sizePoses(); ++i){
+ double pose_distance=std::hypot(teb().Pose(i).x()-teb().Pose(0).x(), teb().Pose(i).y()-teb().Pose(0).y());
+ if(pose_distance > feasibility_check_lookahead_distance){
+ look_ahead_idx = i - 1;
+ break;
+ }
+ }
+ }
+
+ geometry_msgs::msg::Pose2D pose2d;
+ for (int i=0; i <= look_ahead_idx; ++i)
+ {
+ teb().Pose(i).toPoseMsg(pose2d);
+ if (!isPoseValid(pose2d, costmap_model, footprint_spec)){
+ if (visualization_)
+ {
+ visualization_->publishInfeasibleRobotPose(teb().Pose(i), *cfg_->robot_model);
+ }
+ return false;
+ }
+ // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold
+ // and interpolates in that case.
+ // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)!
+ if (i<look_ahead_idx)
+ {
+ double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i+1).theta()) -
+ g2o::normalize_theta(teb().Pose(i).theta()));
+ Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
+ if(fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
+ {
+ int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular),
+ std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
+ PoseSE2 intermediate_pose = teb().Pose(i);
+ for(int step = 0; step < n_additional_samples; ++step)
+ {
+ intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
+ intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() +
+ delta_rot / (n_additional_samples + 1.0));
+ intermediate_pose.toPoseMsg(pose2d);
+
+ if (!isPoseValid(pose2d, costmap_model, footprint_spec)){
+ if (visualization_)
+ {
+ visualization_->publishInfeasibleRobotPose(intermediate_pose, *cfg_->robot_model);
+ }
+ return false;
+ }
+ }
+ }
+ }
+ }
+ return true;
+}
+
+bool TebOptimalPlanner::isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model,
+ const std::vector<geometry_msgs::msg::Point>& footprint_spec)
+{
+ try {
+ if ( costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)) < 0 ) {
+ return false;
+ }
+ } catch (...) {
+ return false;
+ }
+ return true;
+}
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/recovery_behaviors.cpp b/src/teb_local_planner/src/recovery_behaviors.cpp
new file mode 100644
index 0000000..eaeca56
--- /dev/null
+++ b/src/teb_local_planner/src/recovery_behaviors.cpp
@@ -0,0 +1,118 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/recovery_behaviors.h"
+#include <rclcpp/rclcpp.hpp>
+#include <limits>
+#include <functional>
+#include <numeric>
+#include <g2o/stuff/misc.h>
+
+namespace teb_local_planner
+{
+
+// ============== FailureDetector Implementation ===================
+
+void FailureDetector::update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
+{
+ if (buffer_.capacity() == 0)
+ return;
+
+ VelMeasurement measurement;
+ measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now
+ measurement.omega = twist.angular.z;
+
+ if (measurement.v > 0 && v_max>0)
+ measurement.v /= v_max;
+ else if (measurement.v < 0 && v_backwards_max > 0)
+ measurement.v /= v_backwards_max;
+
+ if (omega_max > 0)
+ measurement.omega /= omega_max;
+
+ buffer_.push_back(measurement);
+
+ // immediately compute new state
+ detect(v_eps, omega_eps);
+}
+
+void FailureDetector::clear()
+{
+ buffer_.clear();
+ oscillating_ = false;
+}
+
+bool FailureDetector::isOscillating() const
+{
+ return oscillating_;
+}
+
+bool FailureDetector::detect(double v_eps, double omega_eps)
+{
+ oscillating_ = false;
+
+ if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half
+ return false;
+
+ double n = (double)buffer_.size();
+
+ // compute mean for v and omega
+ double v_mean=0;
+ double omega_mean=0;
+ int omega_zero_crossings = 0;
+ for (int i=0; i < n; ++i)
+ {
+ v_mean += buffer_[i].v;
+ omega_mean += buffer_[i].omega;
+ if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
+ ++omega_zero_crossings;
+ }
+ v_mean /= n;
+ omega_mean /= n;
+
+ if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 )
+ {
+ oscillating_ = true;
+ }
+// ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings);
+ return oscillating_;
+}
+
+
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/teb_config.cpp b/src/teb_local_planner/src/teb_config.cpp
new file mode 100644
index 0000000..8123efd
--- /dev/null
+++ b/src/teb_local_planner/src/teb_config.cpp
@@ -0,0 +1,886 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/teb_config.h"
+
+using nav2_util::declare_parameter_if_not_declared;
+
+namespace teb_local_planner
+{
+
+void TebConfig::declareParameters(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) {
+ node_name = name;
+
+ declare_parameter_if_not_declared(nh, name + "." + "odom_topic", rclcpp::ParameterValue(odom_topic));
+
+ // Trajectory
+ declare_parameter_if_not_declared(nh, name + "." + "teb_autosize", rclcpp::ParameterValue(trajectory.teb_autosize));
+ declare_parameter_if_not_declared(nh, name + "." + "dt_ref", rclcpp::ParameterValue(trajectory.dt_ref));
+ declare_parameter_if_not_declared(nh, name + "." + "dt_hysteresis", rclcpp::ParameterValue(trajectory.dt_hysteresis));
+ declare_parameter_if_not_declared(nh, name + "." + "min_samples", rclcpp::ParameterValue(trajectory.min_samples));
+ declare_parameter_if_not_declared(nh, name + "." + "max_samples", rclcpp::ParameterValue(trajectory.max_samples));
+ declare_parameter_if_not_declared(nh, name + "." + "global_plan_overwrite_orientation", rclcpp::ParameterValue(trajectory.global_plan_overwrite_orientation));
+ declare_parameter_if_not_declared(nh, name + "." + "allow_init_with_backwards_motion", rclcpp::ParameterValue(trajectory.allow_init_with_backwards_motion));
+ declare_parameter_if_not_declared(nh, name + "." + "global_plan_viapoint_sep", rclcpp::ParameterValue(trajectory.global_plan_viapoint_sep));
+ declare_parameter_if_not_declared(nh, name + "." + "via_points_ordered", rclcpp::ParameterValue(trajectory.via_points_ordered));
+ declare_parameter_if_not_declared(nh, name + "." + "max_global_plan_lookahead_dist", rclcpp::ParameterValue(trajectory.max_global_plan_lookahead_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "global_plan_prune_distance", rclcpp::ParameterValue(trajectory.global_plan_prune_distance));
+ declare_parameter_if_not_declared(nh, name + "." + "exact_arc_length", rclcpp::ParameterValue(trajectory.exact_arc_length));
+ declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_dist", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_angular", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_angular));
+ declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_no_poses", rclcpp::ParameterValue(trajectory.feasibility_check_no_poses));
+ declare_parameter_if_not_declared(nh, name + "." + "publish_feedback", rclcpp::ParameterValue(trajectory.publish_feedback));
+ declare_parameter_if_not_declared(nh, name + "." + "min_resolution_collision_check_angular", rclcpp::ParameterValue(trajectory.min_resolution_collision_check_angular));
+ declare_parameter_if_not_declared(nh, name + "." + "control_look_ahead_poses", rclcpp::ParameterValue(trajectory.control_look_ahead_poses));
+ declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_lookahead_distance", rclcpp::ParameterValue(trajectory.feasibility_check_lookahead_distance));
+
+ // Robot
+ declare_parameter_if_not_declared(nh, name + "." + "max_vel_x", rclcpp::ParameterValue(robot.max_vel_x));
+ declare_parameter_if_not_declared(nh, name + "." + "max_vel_x_backwards", rclcpp::ParameterValue(robot.max_vel_x_backwards));
+ declare_parameter_if_not_declared(nh, name + "." + "max_vel_y", rclcpp::ParameterValue(robot.max_vel_y));
+ declare_parameter_if_not_declared(nh, name + "." + "max_vel_theta", rclcpp::ParameterValue(robot.max_vel_theta));
+ declare_parameter_if_not_declared(nh, name + "." + "acc_lim_x", rclcpp::ParameterValue(robot.acc_lim_x));
+ declare_parameter_if_not_declared(nh, name + "." + "acc_lim_y", rclcpp::ParameterValue(robot.acc_lim_y));
+ declare_parameter_if_not_declared(nh, name + "." + "acc_lim_theta", rclcpp::ParameterValue(robot.acc_lim_theta));
+ declare_parameter_if_not_declared(nh, name + "." + "min_turning_radius", rclcpp::ParameterValue(robot.min_turning_radius));
+ declare_parameter_if_not_declared(nh, name + "." + "wheelbase", rclcpp::ParameterValue(robot.wheelbase));
+ declare_parameter_if_not_declared(nh, name + "." + "cmd_angle_instead_rotvel", rclcpp::ParameterValue(robot.cmd_angle_instead_rotvel));
+ declare_parameter_if_not_declared(nh, name + "." + "is_footprint_dynamic", rclcpp::ParameterValue(robot.is_footprint_dynamic));
+
+ // GoalTolerance
+ declare_parameter_if_not_declared(nh, name + "." + "free_goal_vel", rclcpp::ParameterValue(goal_tolerance.free_goal_vel));
+
+ // Obstacles
+ declare_parameter_if_not_declared(nh, name + "." + "min_obstacle_dist", rclcpp::ParameterValue(obstacles.min_obstacle_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "inflation_dist", rclcpp::ParameterValue(obstacles.inflation_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "dynamic_obstacle_inflation_dist", rclcpp::ParameterValue(obstacles.dynamic_obstacle_inflation_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "include_dynamic_obstacles", rclcpp::ParameterValue(obstacles.include_dynamic_obstacles));
+ declare_parameter_if_not_declared(nh, name + "." + "include_costmap_obstacles", rclcpp::ParameterValue(obstacles.include_costmap_obstacles));
+ declare_parameter_if_not_declared(nh, name + "." + "costmap_obstacles_behind_robot_dist", rclcpp::ParameterValue(obstacles.costmap_obstacles_behind_robot_dist));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_poses_affected", rclcpp::ParameterValue(obstacles.obstacle_poses_affected));
+ declare_parameter_if_not_declared(nh, name + "." + "legacy_obstacle_association", rclcpp::ParameterValue(obstacles.legacy_obstacle_association));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_force_inclusion_factor", rclcpp::ParameterValue(obstacles.obstacle_association_force_inclusion_factor));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_cutoff_factor", rclcpp::ParameterValue(obstacles.obstacle_association_cutoff_factor));
+ declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_plugin", rclcpp::ParameterValue(obstacles.costmap_converter_plugin));
+ declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_spin_thread", rclcpp::ParameterValue(obstacles.costmap_converter_spin_thread));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_ratio_max_vel", rclcpp::ParameterValue(obstacles.obstacle_proximity_ratio_max_vel));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_lower_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_lower_bound));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_upper_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_upper_bound));
+
+ // Optimization
+ declare_parameter_if_not_declared(nh, name + "." + "no_inner_iterations", rclcpp::ParameterValue(optim.no_inner_iterations));
+ declare_parameter_if_not_declared(nh, name + "." + "no_outer_iterations", rclcpp::ParameterValue(optim.no_outer_iterations));
+ declare_parameter_if_not_declared(nh, name + "." + "optimization_activate", rclcpp::ParameterValue(optim.optimization_activate));
+ declare_parameter_if_not_declared(nh, name + "." + "optimization_verbose", rclcpp::ParameterValue(optim.optimization_verbose));
+ declare_parameter_if_not_declared(nh, name + "." + "penalty_epsilon", rclcpp::ParameterValue(optim.penalty_epsilon));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_x", rclcpp::ParameterValue(optim.weight_max_vel_x));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_y", rclcpp::ParameterValue(optim.weight_max_vel_y));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_theta", rclcpp::ParameterValue(optim.weight_max_vel_theta));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_x", rclcpp::ParameterValue(optim.weight_acc_lim_x));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_y", rclcpp::ParameterValue(optim.weight_acc_lim_y));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_theta", rclcpp::ParameterValue(optim.weight_acc_lim_theta));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_nh", rclcpp::ParameterValue(optim.weight_kinematics_nh));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_forward_drive", rclcpp::ParameterValue(optim.weight_kinematics_forward_drive));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_turning_radius", rclcpp::ParameterValue(optim.weight_kinematics_turning_radius));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_optimaltime", rclcpp::ParameterValue(optim.weight_optimaltime));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_shortest_path", rclcpp::ParameterValue(optim.weight_shortest_path));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_obstacle", rclcpp::ParameterValue(optim.weight_obstacle));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_inflation", rclcpp::ParameterValue(optim.weight_inflation));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle", rclcpp::ParameterValue(optim.weight_dynamic_obstacle));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle_inflation", rclcpp::ParameterValue(optim.weight_dynamic_obstacle_inflation));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_viapoint", rclcpp::ParameterValue(optim.weight_viapoint));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_prefer_rotdir", rclcpp::ParameterValue(optim.weight_prefer_rotdir));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_adapt_factor", rclcpp::ParameterValue(optim.weight_adapt_factor));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_cost_exponent", rclcpp::ParameterValue(optim.obstacle_cost_exponent));
+ declare_parameter_if_not_declared(nh, name + "." + "weight_velocity_obstacle_ratio", rclcpp::ParameterValue(optim.weight_velocity_obstacle_ratio));
+
+ // Homotopy Class Planner
+ declare_parameter_if_not_declared(nh, name + "." + "enable_homotopy_class_planning", rclcpp::ParameterValue(hcp.enable_homotopy_class_planning));
+ declare_parameter_if_not_declared(nh, name + "." + "enable_multithreading", rclcpp::ParameterValue(hcp.enable_multithreading));
+ declare_parameter_if_not_declared(nh, name + "." + "simple_exploration", rclcpp::ParameterValue(hcp.simple_exploration));
+ declare_parameter_if_not_declared(nh, name + "." + "max_number_classes", rclcpp::ParameterValue(hcp.max_number_classes));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_obst_cost_scale", rclcpp::ParameterValue(hcp.selection_obst_cost_scale));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_prefer_initial_plan", rclcpp::ParameterValue(hcp.selection_prefer_initial_plan));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_viapoint_cost_scale", rclcpp::ParameterValue(hcp.selection_viapoint_cost_scale));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_cost_hysteresis", rclcpp::ParameterValue(hcp.selection_cost_hysteresis));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_alternative_time_cost", rclcpp::ParameterValue(hcp.selection_alternative_time_cost));
+ declare_parameter_if_not_declared(nh, name + "." + "switching_blocking_period", rclcpp::ParameterValue(hcp.switching_blocking_period));
+ declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_samples", rclcpp::ParameterValue(hcp.roadmap_graph_no_samples));
+ declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_width", rclcpp::ParameterValue(hcp.roadmap_graph_area_width));
+ declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_length_scale", rclcpp::ParameterValue(hcp.roadmap_graph_area_length_scale));
+ declare_parameter_if_not_declared(nh, name + "." + "h_signature_prescaler", rclcpp::ParameterValue(hcp.h_signature_prescaler));
+ declare_parameter_if_not_declared(nh, name + "." + "h_signature_threshold", rclcpp::ParameterValue(hcp.h_signature_threshold));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_keypoint_offset", rclcpp::ParameterValue(hcp.obstacle_keypoint_offset));
+ declare_parameter_if_not_declared(nh, name + "." + "obstacle_heading_threshold", rclcpp::ParameterValue(hcp.obstacle_heading_threshold));
+ declare_parameter_if_not_declared(nh, name + "." + "viapoints_all_candidates", rclcpp::ParameterValue(hcp.viapoints_all_candidates));
+ declare_parameter_if_not_declared(nh, name + "." + "visualize_hc_graph", rclcpp::ParameterValue(hcp.visualize_hc_graph));
+ declare_parameter_if_not_declared(nh, name + "." + "visualize_with_time_as_z_axis_scale", rclcpp::ParameterValue(hcp.visualize_with_time_as_z_axis_scale));
+ declare_parameter_if_not_declared(nh, name + "." + "delete_detours_backwards", rclcpp::ParameterValue(hcp.delete_detours_backwards));
+ declare_parameter_if_not_declared(nh, name + "." + "detours_orientation_tolerance", rclcpp::ParameterValue(hcp.detours_orientation_tolerance));
+ declare_parameter_if_not_declared(nh, name + "." + "length_start_orientation_vector", rclcpp::ParameterValue(hcp.length_start_orientation_vector));
+ declare_parameter_if_not_declared(nh, name + "." + "max_ratio_detours_duration_best_duration", rclcpp::ParameterValue(hcp.max_ratio_detours_duration_best_duration));
+ declare_parameter_if_not_declared(nh, name + "." + "selection_dropping_probability", rclcpp::ParameterValue(hcp.selection_dropping_probability));
+
+ // Recovery
+ declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_backup", rclcpp::ParameterValue(recovery.shrink_horizon_backup));
+ declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_min_duration", rclcpp::ParameterValue(recovery.shrink_horizon_min_duration));
+ declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery", rclcpp::ParameterValue(recovery.oscillation_recovery));
+ declare_parameter_if_not_declared(nh, name + "." + "oscillation_v_eps", rclcpp::ParameterValue(recovery.oscillation_v_eps));
+ declare_parameter_if_not_declared(nh, name + "." + "oscillation_omega_eps", rclcpp::ParameterValue(recovery.oscillation_omega_eps));
+ declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery_min_duration", rclcpp::ParameterValue(recovery.oscillation_recovery_min_duration));
+ declare_parameter_if_not_declared(nh, name + "." + "oscillation_filter_duration", rclcpp::ParameterValue(recovery.oscillation_filter_duration));
+ declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_enable", rclcpp::ParameterValue(recovery.divergence_detection_enable));
+ declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_max_chi_squared", rclcpp::ParameterValue(recovery.divergence_detection_max_chi_squared));
+
+ // footprint model
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.type", rclcpp::ParameterType::PARAMETER_STRING);
+}
+
+void TebConfig::loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name)
+{
+ nh->get_parameter_or(name + "." + "odom_topic", odom_topic, odom_topic);
+
+ // Trajectory
+ nh->get_parameter_or(name + "." + "teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize);
+ nh->get_parameter_or(name + "." + "dt_ref", trajectory.dt_ref, trajectory.dt_ref);
+ nh->get_parameter_or(name + "." + "dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis);
+ nh->get_parameter_or(name + "." + "min_samples", trajectory.min_samples, trajectory.min_samples);
+ nh->get_parameter_or(name + "." + "max_samples", trajectory.max_samples, trajectory.max_samples);
+ nh->get_parameter_or(name + "." + "global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation);
+ nh->get_parameter_or(name + "." + "allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion);
+ nh->get_parameter_or(name + "." + "global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep);
+ nh->get_parameter_or(name + "." + "via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered);
+ nh->get_parameter_or(name + "." + "max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist);
+ nh->get_parameter_or(name + "." + "global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance);
+ nh->get_parameter_or(name + "." + "exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length);
+ nh->get_parameter_or(name + "." + "force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist);
+ nh->get_parameter_or(name + "." + "force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular);
+ nh->get_parameter_or(name + "." + "feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses);
+ nh->get_parameter_or(name + "." + "publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
+ nh->get_parameter_or(name + "." + "min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular);
+ nh->get_parameter_or(name + "." + "control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses);
+ nh->get_parameter_or(name + "." + "feasibility_check_lookahead_distance", trajectory.feasibility_check_lookahead_distance, trajectory.feasibility_check_lookahead_distance);
+
+ // Robot
+ nh->get_parameter_or(name + "." + "max_vel_x", robot.max_vel_x, robot.max_vel_x);
+ nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
+ nh->get_parameter_or(name + "." + "max_vel_y", robot.max_vel_y, robot.max_vel_y);
+ nh->get_parameter_or(name + "." + "max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
+ nh->get_parameter_or(name + "." + "acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
+ nh->get_parameter_or(name + "." + "acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
+ nh->get_parameter_or(name + "." + "acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
+ nh->get_parameter_or(name + "." + "min_turning_radius", robot.min_turning_radius, robot.min_turning_radius);
+ nh->get_parameter_or(name + "." + "wheelbase", robot.wheelbase, robot.wheelbase);
+ nh->get_parameter_or(name + "." + "cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel);
+ nh->get_parameter_or(name + "." + "is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic);
+
+ // GoalTolerance
+ nh->get_parameter_or(name + "." + "free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel);
+
+ // Obstacles
+ nh->get_parameter_or(name + "." + "min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist);
+ nh->get_parameter_or(name + "." + "inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist);
+ nh->get_parameter_or(name + "." + "dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist);
+ nh->get_parameter_or(name + "." + "include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles);
+ nh->get_parameter_or(name + "." + "include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles);
+ nh->get_parameter_or(name + "." + "costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist);
+ nh->get_parameter_or(name + "." + "obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected);
+ nh->get_parameter_or(name + "." + "legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association);
+ nh->get_parameter_or(name + "." + "obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, obstacles.obstacle_association_force_inclusion_factor);
+ nh->get_parameter_or(name + "." + "obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor);
+ nh->get_parameter_or(name + "." + "costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin);
+ nh->get_parameter_or(name + "." + "costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread);
+ nh->get_parameter_or(name + "." + "obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel);
+ nh->get_parameter_or(name + "." + "obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound);
+ nh->get_parameter_or(name + "." + "obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound);
+
+ // Optimization
+ nh->get_parameter_or(name + "." + "no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations);
+ nh->get_parameter_or(name + "." + "no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations);
+ nh->get_parameter_or(name + "." + "optimization_activate", optim.optimization_activate, optim.optimization_activate);
+ nh->get_parameter_or(name + "." + "optimization_verbose", optim.optimization_verbose, optim.optimization_verbose);
+ nh->get_parameter_or(name + "." + "penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon);
+ nh->get_parameter_or(name + "." + "weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x);
+ nh->get_parameter_or(name + "." + "weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y);
+ nh->get_parameter_or(name + "." + "weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta);
+ nh->get_parameter_or(name + "." + "weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x);
+ nh->get_parameter_or(name + "." + "weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y);
+ nh->get_parameter_or(name + "." + "weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta);
+ nh->get_parameter_or(name + "." + "weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh);
+ nh->get_parameter_or(name + "." + "weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive);
+ nh->get_parameter_or(name + "." + "weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius);
+ nh->get_parameter_or(name + "." + "weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime);
+ nh->get_parameter_or(name + "." + "weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path);
+ nh->get_parameter_or(name + "." + "weight_obstacle", optim.weight_obstacle, optim.weight_obstacle);
+ nh->get_parameter_or(name + "." + "weight_inflation", optim.weight_inflation, optim.weight_inflation);
+ nh->get_parameter_or(name + "." + "weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);
+ nh->get_parameter_or(name + "." + "weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation);
+ nh->get_parameter_or(name + "." + "weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
+ nh->get_parameter_or(name + "." + "weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
+ nh->get_parameter_or(name + "." + "weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
+ nh->get_parameter_or(name + "." + "obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent);
+ nh->get_parameter_or(name + "." + "weight_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio);
+
+ // Homotopy Class Planner
+ nh->get_parameter_or(name + "." + "enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning);
+ nh->get_parameter_or(name + "." + "enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading);
+ nh->get_parameter_or(name + "." + "simple_exploration", hcp.simple_exploration, hcp.simple_exploration);
+ nh->get_parameter_or(name + "." + "max_number_classes", hcp.max_number_classes, hcp.max_number_classes);
+ nh->get_parameter_or(name + "." + "selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale);
+ nh->get_parameter_or(name + "." + "selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan);
+ nh->get_parameter_or(name + "." + "selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale);
+ nh->get_parameter_or(name + "." + "selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis);
+ nh->get_parameter_or(name + "." + "selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost);
+ nh->get_parameter_or(name + "." + "switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period);
+ nh->get_parameter_or(name + "." + "roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples);
+ nh->get_parameter_or(name + "." + "roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width);
+ nh->get_parameter_or(name + "." + "roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale);
+ nh->get_parameter_or(name + "." + "h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler);
+ nh->get_parameter_or(name + "." + "h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold);
+ nh->get_parameter_or(name + "." + "obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset);
+ nh->get_parameter_or(name + "." + "obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold);
+ nh->get_parameter_or(name + "." + "viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates);
+ nh->get_parameter_or(name + "." + "visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph);
+ nh->get_parameter_or(name + "." + "visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale);
+ nh->get_parameter_or(name + "." + "delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards);
+ nh->get_parameter_or(name + "." + "detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance);
+ nh->get_parameter_or(name + "." + "length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector);
+ nh->get_parameter_or(name + "." + "max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration);
+ nh->get_parameter_or(name + "." + "selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability);
+
+ // Recovery
+ nh->get_parameter_or(name + "." + "shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup);
+ nh->get_parameter_or(name + "." + "shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration);
+ nh->get_parameter_or(name + "." + "oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery);
+ nh->get_parameter_or(name + "." + "oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps);
+ nh->get_parameter_or(name + "." + "oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps);
+ nh->get_parameter_or(name + "." + "oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration);
+ nh->get_parameter_or(name + "." + "oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration);
+ nh->get_parameter_or(name + "." + "divergence_detection_enable", recovery.divergence_detection_enable, recovery.divergence_detection_enable);
+ nh->get_parameter_or(name + "." + "divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared);
+
+ // footprint model
+ if (!nh->get_parameter(name + "." + "footprint_model.type", model_name))
+ {
+ RCLCPP_INFO(logger_, "No robot footprint model specified for trajectory optimization. Using point-shaped model.");
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+ // point
+ else if (model_name.compare("point") == 0)
+ {
+ RCLCPP_INFO(logger_, "Footprint model 'point' loaded for trajectory optimization.");
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+ // circular
+ else if (model_name.compare("circular") == 0)
+ {
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
+ // get radius
+ double radius;
+ if (!nh->get_parameter(name + "." + "footprint_model.radius", radius))
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'circular' cannot be loaded for trajectory optimization, since param '%s.footprint_model.radius' does not exist. Using point-model instead.",
+ nh->get_namespace());
+
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+ RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) loaded for trajectory optimization.", radius);
+ robot_model = std::make_shared<CircularRobotFootprint>(radius);
+ }
+
+
+ // line
+ else if (model_name.compare("line") == 0)
+ {
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_start", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY);
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_end", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY);
+ std::vector<double> line_start, line_end;
+ // check parameters
+ if (!nh->get_parameter(name + "." + "footprint_model.line_start", line_start) || !nh->get_parameter(name + "." + "footprint_model.line_end", line_end))
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not exist. Using point-model instead.",
+ nh->get_namespace());
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+ if (line_start.size() != 2 || line_end.size() != 2)
+ {
+ RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead.",
+ nh->get_namespace());
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+ RCLCPP_INFO(logger_,
+ "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) loaded for trajectory optimization.",
+ line_start[0], line_start[1], line_end[0], line_end[1]);
+
+ robot_model = std::make_shared<LineRobotFootprint>(Eigen::Map<const Eigen::Vector2d>(line_start.data()), Eigen::Map<const Eigen::Vector2d>(line_end.data()));
+ }
+
+ // two circles
+ else if (model_name.compare("two_circles") == 0)
+ {
+ rclcpp::Parameter dummy;
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_offset", rclcpp::ParameterType::PARAMETER_DOUBLE);
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_offset", rclcpp::ParameterType::PARAMETER_DOUBLE);
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
+ // check parameters
+ if (!nh->get_parameter(name + "." + "footprint_model.front_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.front_radius", dummy)
+ || !nh->get_parameter(name + "." + "footprint_model.rear_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.rear_radius", dummy))
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '%s.footprint_model.front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using point-model instead.",
+ nh->get_namespace());
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+ double front_offset, front_radius, rear_offset, rear_radius;
+ nh->get_parameter(name + "." + "footprint_model.front_offset", front_offset);
+ nh->get_parameter(name + "." + "footprint_model.front_radius", front_radius);
+ nh->get_parameter(name + "." + "footprint_model.rear_offset", rear_offset);
+ nh->get_parameter(name + "." + "footprint_model.rear_radius", rear_radius);
+ RCLCPP_INFO(logger_,
+ "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.",
+ front_offset, front_radius, rear_offset, rear_radius);
+
+ robot_model = std::make_shared<TwoCirclesRobotFootprint>(front_offset, front_radius, rear_offset, rear_radius);
+ }
+
+ // polygon
+ else if (model_name.compare("polygon") == 0)
+ {
+ declare_parameter_if_not_declared(nh, name + "." + "footprint_model.vertices", rclcpp::ParameterType::PARAMETER_STRING);
+ // check parameters
+ std::string footprint_string;
+ if (!nh->get_parameter(name + "." + "footprint_model.vertices", footprint_string) )
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not exist. Using point-model instead.",
+ nh->get_namespace());
+
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+ std::vector<geometry_msgs::msg::Point> footprint;
+ // get vertices
+ if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint))
+ {
+ Point2dContainer polygon;
+ for(const auto &pt : footprint) {
+ polygon.push_back(Eigen::Vector2d(pt.x, pt.y));
+ }
+ RCLCPP_INFO(logger_, "Footprint model 'polygon' loaded for trajectory optimization.");
+ robot_model = std::make_shared<PolygonRobotFootprint>(polygon);
+ }
+ else
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not define an array of coordinates. Using point-model instead.",
+ nh->get_namespace());
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+ }
+ // otherwise
+ else
+ {
+ RCLCPP_WARN(logger_, "Unknown robot footprint model specified with parameter '%s.footprint_model.type'. Using point model instead.",
+ nh->get_namespace());
+ robot_model = std::make_shared<PointRobotFootprint>();
+ }
+
+
+ checkParameters();
+ checkDeprecated(nh, name);
+}
+
+rcl_interfaces::msg::SetParametersResult
+ TebConfig::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+ auto result = rcl_interfaces::msg::SetParametersResult();
+ std::lock_guard<std::mutex> l(config_mutex_);
+
+ bool reload_footprint = false;
+
+ for (auto parameter : parameters) {
+ const auto & type = parameter.get_type();
+ const auto & name = parameter.get_name();
+
+ if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+ // Trajectory
+ if (name == node_name + ".teb_autosize") {
+ trajectory.teb_autosize = parameter.as_double();
+ } else if (name == node_name + ".dt_ref") {
+ trajectory.dt_ref = parameter.as_double();
+ } else if (name == node_name + ".dt_hysteresis") {
+ trajectory.dt_hysteresis = parameter.as_double();
+ } else if (name == node_name + ".global_plan_viapoint_sep") {
+ trajectory.global_plan_viapoint_sep = parameter.as_double();
+ } else if (name == node_name + ".max_global_plan_lookahead_dist") {
+ trajectory.max_global_plan_lookahead_dist = parameter.as_double();
+ } else if (name == node_name + ".global_plan_prune_distance") {
+ trajectory.global_plan_prune_distance = parameter.as_double();
+ } else if (name == node_name + ".force_reinit_new_goal_dist") {
+ trajectory.force_reinit_new_goal_dist = parameter.as_double();
+ } else if (name == node_name + ".force_reinit_new_goal_angular") {
+ trajectory.force_reinit_new_goal_angular = parameter.as_double();
+ } else if (name == node_name + ".min_resolution_collision_check_angular") {
+ trajectory.min_resolution_collision_check_angular = parameter.as_double();
+ } else if (name == node_name + ".feasibility_check_lookahead_distance") {
+ trajectory.feasibility_check_lookahead_distance = parameter.as_double();
+ }
+ // Robot
+ else if (name == node_name + ".max_vel_x") {
+ robot.max_vel_x = parameter.as_double();
+ robot.base_max_vel_x = parameter.as_double();
+ } else if (name == node_name + ".max_vel_x_backwards") {
+ robot.max_vel_x_backwards = parameter.as_double();
+ robot.base_max_vel_x_backwards = parameter.as_double();
+ } else if (name == node_name + ".max_vel_y") {
+ robot.max_vel_y = parameter.as_double();
+ robot.base_max_vel_y = parameter.as_double();
+ } else if (name == node_name + ".max_vel_theta") {
+ robot.max_vel_theta = parameter.as_double();
+ robot.base_max_vel_theta = parameter.as_double();
+ } else if (name == node_name + ".acc_lim_x") {
+ robot.acc_lim_x = parameter.as_double();
+ } else if (name == node_name + ".acc_lim_y") {
+ robot.acc_lim_y = parameter.as_double();
+ } else if (name == node_name + ".acc_lim_theta") {
+ robot.acc_lim_theta = parameter.as_double();
+ } else if (name == node_name + ".min_turning_radius") {
+ robot.min_turning_radius = parameter.as_double();
+ } else if (name == node_name + ".wheelbase") {
+ robot.wheelbase = parameter.as_double();
+ }
+ // GoalTolerance
+ // Obstacles
+ else if (name == node_name + ".min_obstacle_dist") {
+ obstacles.min_obstacle_dist = parameter.as_double();
+ } else if (name == node_name + ".inflation_dist") {
+ obstacles.inflation_dist = parameter.as_double();
+ } else if (name == node_name + ".dynamic_obstacle_inflation_dist") {
+ obstacles.dynamic_obstacle_inflation_dist = parameter.as_double();
+ } else if (name == node_name + ".costmap_obstacles_behind_robot_dist") {
+ obstacles.costmap_obstacles_behind_robot_dist = parameter.as_double();
+ } else if (name == node_name + ".obstacle_association_force_inclusion_factor") {
+ obstacles.obstacle_association_force_inclusion_factor = parameter.as_double();
+ } else if (name == node_name + ".obstacle_association_cutoff_factor") {
+ obstacles.obstacle_association_cutoff_factor = parameter.as_double();
+ } else if (name == node_name + ".obstacle_proximity_ratio_max_vel") {
+ obstacles.obstacle_proximity_ratio_max_vel = parameter.as_double();
+ } else if (name == node_name + ".obstacle_proximity_lower_bound") {
+ obstacles.obstacle_proximity_lower_bound = parameter.as_double();
+ } else if (name == node_name + ".obstacle_proximity_upper_bound") {
+ obstacles.obstacle_proximity_upper_bound = parameter.as_double();
+ }
+ // Optimization
+ else if (name == node_name + ".penalty_epsilon") {
+ optim.penalty_epsilon = parameter.as_double();
+ } else if (name == node_name + ".weight_max_vel_x") {
+ optim.weight_max_vel_x = parameter.as_double();
+ } else if (name == node_name + ".weight_max_vel_y") {
+ optim.weight_max_vel_y = parameter.as_double();
+ } else if (name == node_name + ".weight_max_vel_theta") {
+ optim.weight_max_vel_theta = parameter.as_double();
+ } else if (name == node_name + ".weight_acc_lim_x") {
+ optim.weight_acc_lim_x = parameter.as_double();
+ } else if (name == node_name + ".weight_acc_lim_y") {
+ optim.weight_acc_lim_y = parameter.as_double();
+ } else if (name == node_name + ".weight_acc_lim_theta") {
+ optim.weight_acc_lim_theta = parameter.as_double();
+ } else if (name == node_name + ".weight_kinematics_nh") {
+ optim.weight_kinematics_nh = parameter.as_double();
+ } else if (name == node_name + ".weight_kinematics_forward_drive") {
+ optim.weight_kinematics_forward_drive = parameter.as_double();
+ } else if (name == node_name + ".weight_kinematics_turning_radius") {
+ optim.weight_kinematics_turning_radius = parameter.as_double();
+ } else if (name == node_name + ".weight_optimaltime") {
+ optim.weight_optimaltime = parameter.as_double();
+ } else if (name == node_name + ".weight_shortest_path") {
+ optim.weight_shortest_path = parameter.as_double();
+ } else if (name == node_name + ".weight_obstacle") {
+ optim.weight_obstacle = parameter.as_double();
+ } else if (name == node_name + ".weight_inflation") {
+ optim.weight_inflation = parameter.as_double();
+ } else if (name == node_name + ".weight_dynamic_obstacle") {
+ optim.weight_dynamic_obstacle = parameter.as_double();
+ } else if (name == node_name + ".weight_dynamic_obstacle_inflation") {
+ optim.weight_dynamic_obstacle_inflation = parameter.as_double();
+ } else if (name == node_name + ".weight_viapoint") {
+ optim.weight_viapoint = parameter.as_double();
+ } else if (name == node_name + ".weight_prefer_rotdir") {
+ optim.weight_prefer_rotdir = parameter.as_double();
+ } else if (name == node_name + ".weight_adapt_factor") {
+ optim.weight_adapt_factor = parameter.as_double();
+ } else if (name == node_name + ".obstacle_cost_exponent") {
+ optim.obstacle_cost_exponent = parameter.as_double();
+ }
+ // Homotopy Class Planner
+ else if (name == node_name + ".selection_cost_hysteresis") {
+ hcp.selection_cost_hysteresis = parameter.as_double();
+ } else if (name == node_name + ".selection_prefer_initial_plan") {
+ hcp.selection_prefer_initial_plan = parameter.as_double();
+ } else if (name == node_name + ".selection_obst_cost_scale") {
+ hcp.selection_obst_cost_scale = parameter.as_double();
+ } else if (name == node_name + ".selection_viapoint_cost_scale") {
+ hcp.selection_viapoint_cost_scale = parameter.as_double();
+ } else if (name == node_name + ".switching_blocking_period") {
+ hcp.switching_blocking_period = parameter.as_double();
+ } else if (name == node_name + ".roadmap_graph_area_width") {
+ hcp.roadmap_graph_area_width = parameter.as_double();
+ } else if (name == node_name + ".roadmap_graph_area_length_scale") {
+ hcp.roadmap_graph_area_length_scale = parameter.as_double();
+ } else if (name == node_name + ".h_signature_prescaler") {
+ hcp.h_signature_prescaler = parameter.as_double();
+ } else if (name == node_name + ".h_signature_threshold") {
+ hcp.h_signature_threshold = parameter.as_double();
+ } else if (name == node_name + ".obstacle_keypoint_offset") {
+ hcp.obstacle_keypoint_offset = parameter.as_double();
+ } else if (name == node_name + ".obstacle_heading_threshold") {
+ hcp.obstacle_heading_threshold = parameter.as_double();
+ } else if (name == node_name + ".visualize_with_time_as_z_axis_scale") {
+ hcp.visualize_with_time_as_z_axis_scale = parameter.as_double();
+ } else if (name == node_name + ".detours_orientation_tolerance") {
+ hcp.detours_orientation_tolerance = parameter.as_double();
+ } else if (name == node_name + ".length_start_orientation_vector") {
+ hcp.length_start_orientation_vector = parameter.as_double();
+ } else if (name == node_name + ".max_ratio_detours_duration_best_duration") {
+ hcp.max_ratio_detours_duration_best_duration = parameter.as_double();
+ } else if (name == node_name + ".selection_dropping_probability") {
+ hcp.selection_dropping_probability = parameter.as_double();
+ }
+ // Recovery
+ else if (name == node_name + ".shrink_horizon_min_duration") {
+ recovery.shrink_horizon_min_duration = parameter.as_double();
+ } else if (name == node_name + ".oscillation_v_eps") {
+ recovery.oscillation_v_eps = parameter.as_double();
+ } else if (name == node_name + ".oscillation_omega_eps") {
+ recovery.oscillation_omega_eps = parameter.as_double();
+ } else if (name == node_name + ".oscillation_recovery_min_duration") {
+ recovery.oscillation_recovery_min_duration = parameter.as_double();
+ } else if (name == node_name + ".oscillation_filter_duration") {
+ recovery.oscillation_filter_duration = parameter.as_double();
+ } else if (name == node_name + ".divergence_detection_max_chi_squared") {
+ recovery.divergence_detection_max_chi_squared = parameter.as_double();
+ }
+ // Footprint model
+ else if (name == node_name + ".footprint_model.radius") {
+ reload_footprint = true;
+ radius = parameter.as_double();
+ } else if (name == node_name + ".footprint_model.front_offset") {
+ reload_footprint = true;
+ front_offset = parameter.as_double();
+ } else if (name == node_name + ".footprint_model.front_radius") {
+ reload_footprint = true;
+ front_radius = parameter.as_double();
+ } else if (name == node_name + ".footprint_model.rear_offset") {
+ reload_footprint = true;
+ rear_offset = parameter.as_double();
+ } else if (name == node_name + ".footprint_model.rear_radius") {
+ reload_footprint = true;
+ rear_radius = parameter.as_double();
+ }
+ }
+
+ else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) {
+ if (name == node_name + ".footprint_model.line_start") {
+ reload_footprint = true;
+ line_start = parameter.as_double_array();
+ } else if (name == node_name + ".footprint_model.line_end") {
+ reload_footprint = true;
+ line_end = parameter.as_double_array();
+ }
+ }
+
+ else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
+ // Trajectory
+ if (name == node_name + ".min_samples") {
+ trajectory.min_samples = parameter.as_int();
+ } else if (name == node_name + ".max_samples") {
+ trajectory.max_samples = parameter.as_int();
+ } else if (name == node_name + ".feasibility_check_no_poses") {
+ trajectory.feasibility_check_no_poses = parameter.as_int();
+ } else if (name == node_name + ".control_look_ahead_poses") {
+ trajectory.control_look_ahead_poses = parameter.as_int();
+ }
+ // Robot
+ // GoalTolerance
+ // Obstacles
+ else if (name == node_name + ".obstacle_poses_affected") {
+ obstacles.obstacle_poses_affected = parameter.as_int();
+ } else if (name == node_name + ".costmap_converter_rate") {
+ obstacles.costmap_converter_rate = parameter.as_int();
+ }
+ // Optimization
+ else if (name == node_name + ".no_inner_iterations") {
+ optim.no_inner_iterations = parameter.as_int();
+ } else if (name == node_name + ".no_outer_iterations") {
+ optim.no_outer_iterations = parameter.as_int();
+ }
+ // Homotopy Class Planner
+ else if (name == node_name + ".max_number_classes") {
+ hcp.max_number_classes = parameter.as_int();
+ } else if (name == node_name + ".roadmap_graph_no_samples") {
+ hcp.roadmap_graph_no_samples = parameter.as_int();
+ }
+ // Recovery
+ }
+
+ else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+ // Trajectory
+ if (name == node_name + ".global_plan_overwrite_orientation") {
+ trajectory.global_plan_overwrite_orientation = parameter.as_bool();
+ } else if (name == node_name + ".allow_init_with_backwards_motion") {
+ trajectory.allow_init_with_backwards_motion = parameter.as_bool();
+ } else if (name == node_name + ".via_points_ordered") {
+ trajectory.via_points_ordered = parameter.as_bool();
+ } else if (name == node_name + ".exact_arc_length") {
+ trajectory.exact_arc_length = parameter.as_bool();
+ } else if (name == node_name + ".publish_feedback") {
+ trajectory.publish_feedback = parameter.as_bool();
+ }
+ // Robot
+ else if (name == node_name + ".cmd_angle_instead_rotvel") {
+ robot.cmd_angle_instead_rotvel = parameter.as_bool();
+ } else if (name == node_name + ".is_footprint_dynamic") {
+ robot.is_footprint_dynamic = parameter.as_bool();
+ }
+ // GoalTolerance
+ else if (name == node_name + ".free_goal_vel") {
+ goal_tolerance.free_goal_vel = parameter.as_bool();
+ }
+ // Obstacles
+ else if (name == node_name + ".include_dynamic_obstacles") {
+ obstacles.include_dynamic_obstacles = parameter.as_bool();
+ } else if (name == node_name + ".include_costmap_obstacles") {
+ obstacles.include_costmap_obstacles = parameter.as_bool();
+ } else if (name == node_name + ".legacy_obstacle_association") {
+ obstacles.legacy_obstacle_association = parameter.as_bool();
+ } else if (name == node_name + ".costmap_converter_spin_thread") {
+ obstacles.costmap_converter_spin_thread = parameter.as_bool();
+ }
+ // Optimization
+ else if (name == node_name + ".optimization_activate") {
+ optim.optimization_activate = parameter.as_bool();
+ } else if (name == node_name + ".optimization_verbose") {
+ optim.optimization_verbose = parameter.as_bool();
+ }
+ // Homotopy Class Planner
+ else if (name == node_name + ".enable_homotopy_class_planning") {
+ hcp.enable_homotopy_class_planning = parameter.as_bool();
+ } else if (name == node_name + ".enable_multithreading") {
+ hcp.enable_multithreading = parameter.as_bool();
+ } else if (name == node_name + ".simple_exploration") {
+ hcp.simple_exploration = parameter.as_bool();
+ } else if (name == node_name + ".selection_alternative_time_cost") {
+ hcp.selection_alternative_time_cost = parameter.as_bool();
+ } else if (name == node_name + ".viapoints_all_candidates") {
+ hcp.viapoints_all_candidates = parameter.as_bool();
+ } else if (name == node_name + ".visualize_hc_graph") {
+ hcp.visualize_hc_graph = parameter.as_bool();
+ } else if (name == node_name + ".delete_detours_backwards") {
+ hcp.delete_detours_backwards = parameter.as_bool();
+ }
+ // Recovery
+ else if (name == node_name + ".shrink_horizon_backup") {
+ recovery.shrink_horizon_backup = parameter.as_bool();
+ } else if (name == node_name + ".oscillation_recovery") {
+ recovery.oscillation_recovery = parameter.as_bool();
+ } else if (name == node_name + ".divergence_detection_enable") {
+ recovery.divergence_detection_enable = parameter.as_bool();
+ }
+ }
+
+ else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
+ // Trajectory
+ // Robot
+ // GoalTolerance
+ // Obstacles
+ if (name == node_name + ".costmap_converter_plugin") {
+ obstacles.costmap_converter_plugin = parameter.as_string();
+ }
+ // Optimization
+ // Homotopy Class Planner
+ // Recovery
+ // Footprint model
+ else if (name == node_name + ".footprint_model.type") {
+ RCLCPP_WARN(logger_, "Changing footprint model type is not allowed at runtime");
+ } else if (name == node_name + ".footprint_model.vertices") {
+ reload_footprint = true;
+ footprint_string = parameter.as_string();
+ }
+ }
+ }
+ checkParameters();
+
+ if (reload_footprint) {
+ if (model_name.compare("circular") == 0)
+ {
+ RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) reloaded for trajectory optimization.", radius);
+ robot_model = std::make_shared<CircularRobotFootprint>(radius);
+ }
+ else if (model_name.compare("line") == 0) {
+ if (line_start.size() != 2 || line_end.size() != 2)
+ {
+ RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be reloaded for trajectory optimization");
+ } else {
+ RCLCPP_INFO(logger_,
+ "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) reloaded for trajectory optimization.",
+ line_start[0], line_start[1], line_end[0], line_end[1]);
+
+ robot_model = std::make_shared<LineRobotFootprint>(Eigen::Map<const Eigen::Vector2d>(line_start.data()), Eigen::Map<const Eigen::Vector2d>(line_end.data()));
+ }
+ }
+ else if (model_name.compare("two_circles") == 0) {
+ RCLCPP_INFO(logger_,
+ "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.",
+ front_offset, front_radius, rear_offset, rear_radius);
+
+ robot_model = std::make_shared<TwoCirclesRobotFootprint>(front_offset, front_radius, rear_offset, rear_radius);
+ }
+ else if (model_name.compare("polygon") == 0)
+ {
+ std::vector<geometry_msgs::msg::Point> footprint;
+ // get vertices
+ if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint))
+ {
+ Point2dContainer polygon;
+ for(const auto &pt : footprint) {
+ polygon.push_back(Eigen::Vector2d(pt.x, pt.y));
+ }
+ RCLCPP_INFO(logger_, "Footprint model 'polygon' reloaded for trajectory optimization.");
+ robot_model = std::make_shared<PolygonRobotFootprint>(polygon);
+ }
+ else
+ {
+ RCLCPP_ERROR(logger_,
+ "Footprint model 'polygon' cannot be reloaded for trajectory optimization, since param 'footprint_model.vertices' does not define an array of coordinates.");
+ }
+ }
+ }
+ result.successful = true;
+ return result;
+}
+
+
+void TebConfig::checkParameters() const
+{
+ //rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
+ // positive backward velocity?
+ if (robot.max_vel_x_backwards <= 0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
+
+ // bounds smaller than penalty epsilon
+ if (robot.max_vel_x <= optim.penalty_epsilon)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
+
+ if (robot.max_vel_x_backwards <= optim.penalty_epsilon)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
+
+ if (robot.max_vel_theta <= optim.penalty_epsilon)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
+
+ if (robot.acc_lim_x <= optim.penalty_epsilon)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
+
+ if (robot.acc_lim_theta <= optim.penalty_epsilon)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
+
+ // dt_ref and dt_hyst
+ if (trajectory.dt_ref <= trajectory.dt_hysteresis)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!");
+
+ // min number of samples
+ if (trajectory.min_samples <3)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ...");
+
+ // costmap obstacle behind robot
+ if (obstacles.costmap_obstacles_behind_robot_dist < 0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero.");
+
+ // hcp: obstacle heading threshold
+ if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle.");
+
+ // carlike
+ if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior.");
+
+ if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot");
+
+ // positive weight_adapt_factor
+ if (optim.weight_adapt_factor < 1.0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0");
+
+ if (recovery.oscillation_filter_duration < 0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0");
+
+ if (optim.weight_optimaltime <= 0)
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)");
+}
+
+void TebConfig::checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const
+{
+ rclcpp::Parameter dummy;
+
+ if (nh->get_parameter(name + "." + "line_obstacle_poses_affected", dummy) || nh->get_parameter(name + "." + "polygon_obstacle_poses_affected", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'.");
+
+ if (nh->get_parameter(name + "." + "weight_point_obstacle", dummy) || nh->get_parameter(name + "." + "weight_line_obstacle", dummy) || nh->get_parameter(name + "." + "weight_poly_obstacle", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'.");
+
+ if (nh->get_parameter(name + "." + "costmap_obstacles_front_only", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account.");
+
+ if (nh->get_parameter(name + "." + "costmap_emergency_stop_dist", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config.");
+
+ if (nh->get_parameter(name + "." + "alternative_time_cost", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'.");
+
+ if (nh->get_parameter(name + "." + "global_plan_via_point_sep", dummy))
+ RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons.");
+}
+
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/teb_local_planner_ros.cpp b/src/teb_local_planner/src/teb_local_planner_ros.cpp
new file mode 100644
index 0000000..5d90794
--- /dev/null
+++ b/src/teb_local_planner/src/teb_local_planner_ros.cpp
@@ -0,0 +1,1122 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/teb_local_planner_ros.h"
+
+//#include <tf_conversions/tf_eigen.h>
+#include <boost/algorithm/string.hpp>
+
+#include <string>
+
+// pluginlib macros
+#include <pluginlib/class_list_macros.hpp>
+
+#include "g2o/core/sparse_optimizer.h"
+#include "g2o/core/block_solver.h"
+#include "g2o/core/factory.h"
+#include "g2o/core/optimization_algorithm_gauss_newton.h"
+#include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/solvers/csparse/linear_solver_csparse.h"
+#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
+
+#include <nav2_core/planner_exceptions.hpp>
+#include <nav2_costmap_2d/footprint.hpp>
+#include <nav_2d_utils/tf_help.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+using nav2_util::declare_parameter_if_not_declared;
+
+namespace teb_local_planner
+{
+
+
+TebLocalPlannerROS::TebLocalPlannerROS()
+ : costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
+ costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
+ custom_via_points_active_(false), no_infeasible_plans_(0),
+ last_preferred_rotdir_(RotType::none), initialized_(false)
+{
+}
+
+
+TebLocalPlannerROS::~TebLocalPlannerROS()
+{
+}
+
+void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node)
+{
+ // check if the plugin is already initialized
+ if(!initialized_)
+ {
+ // declare parameters (ros2-dashing)
+ intra_proc_node_.reset(
+ new rclcpp::Node("costmap_converter", node->get_namespace(),
+ rclcpp::NodeOptions()));
+ cfg_->declareParameters(node, name_);
+
+ // get parameters of TebConfig via the nodehandle and override the default config
+ cfg_->loadRosParamFromNodeHandle(node, name_);
+
+ // reserve some memory for obstacles
+ obstacles_.reserve(500);
+
+ // create the planner instance
+ if (cfg_->hcp.enable_homotopy_class_planning)
+ {
+ planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_));
+ RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies enabled.");
+ }
+ else
+ {
+ planner_ = PlannerInterfacePtr(new TebOptimalPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_));
+ RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies disabled.");
+ }
+
+ // init other variables
+ costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase.
+
+ costmap_model_ = std::make_shared<dwb_critics::ObstacleFootprintCritic>();
+ std::string costmap_model_name("costmap_model");
+ costmap_model_->initialize(node, costmap_model_name, name_, costmap_ros_);
+
+ cfg_->map_frame = costmap_ros_->getGlobalFrameID(); // TODO
+
+ //Initialize a costmap to polygon converter
+ if (!cfg_->obstacles.costmap_converter_plugin.empty())
+ {
+ try
+ {
+ costmap_converter_ = costmap_converter_loader_.createSharedInstance(cfg_->obstacles.costmap_converter_plugin);
+ std::string converter_name = costmap_converter_loader_.getName(cfg_->obstacles.costmap_converter_plugin);
+ RCLCPP_INFO(logger_, "library path : %s", costmap_converter_loader_.getClassLibraryPath(cfg_->obstacles.costmap_converter_plugin).c_str());
+ // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
+ boost::replace_all(converter_name, "::", "/");
+
+ costmap_converter_->setOdomTopic(cfg_->odom_topic);
+ costmap_converter_->initialize(intra_proc_node_);
+ costmap_converter_->setCostmap2D(costmap_);
+ const auto rate = std::make_shared<rclcpp::Rate>((double)cfg_->obstacles.costmap_converter_rate);
+ costmap_converter_->startWorker(rate, costmap_, cfg_->obstacles.costmap_converter_spin_thread);
+ RCLCPP_INFO(logger_, "Costmap conversion plugin %s loaded.", cfg_->obstacles.costmap_converter_plugin.c_str());
+ }
+ catch(pluginlib::PluginlibException& ex)
+ {
+ RCLCPP_INFO(logger_,
+ "The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what());
+ costmap_converter_.reset();
+ }
+ }
+ else {
+ RCLCPP_INFO(logger_, "No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
+ }
+
+
+ // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
+ footprint_spec_ = costmap_ros_->getRobotFootprint();
+ nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
+
+ // Add callback for dynamic parameters
+ dyn_params_handler = node->add_on_set_parameters_callback(
+ std::bind(&TebConfig::dynamicParametersCallback, std::ref(cfg_), std::placeholders::_1));
+
+ // validate optimization footprint and costmap footprint
+ validateFootprints(cfg_->robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_->obstacles.min_obstacle_dist);
+
+ // setup callback for custom obstacles
+ custom_obst_sub_ = node->create_subscription<costmap_converter_msgs::msg::ObstacleArrayMsg>(
+ "obstacles",
+ rclcpp::SystemDefaultsQoS(),
+ std::bind(&TebLocalPlannerROS::customObstacleCB, this, std::placeholders::_1));
+
+ // setup callback for custom via-points
+ via_points_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+ "via_points",
+ rclcpp::SystemDefaultsQoS(),
+ std::bind(&TebLocalPlannerROS::customViaPointsCB, this, std::placeholders::_1));
+
+ // initialize failure detector
+ //rclcpp::Node::SharedPtr nh_move_base("~");
+ double controller_frequency = 5;
+ node->get_parameter("controller_frequency", controller_frequency);
+ failure_detector_.setBufferLength(std::round(cfg_->recovery.oscillation_filter_duration*controller_frequency));
+
+ // set initialized flag
+ initialized_ = true;
+
+ // This should be called since to prevent different time sources exception
+ time_last_infeasible_plan_ = clock_->now();
+ time_last_oscillation_ = clock_->now();
+ RCLCPP_DEBUG(logger_, "teb_local_planner plugin initialized.");
+ }
+ else
+ {
+ RCLCPP_INFO(logger_, "teb_local_planner has already been initialized, doing nothing.");
+ }
+}
+
+void TebLocalPlannerROS::configure(
+ const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+ std::string name,
+ std::shared_ptr<tf2_ros::Buffer> tf,
+ std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
+ nh_ = parent;
+
+ auto node = nh_.lock();
+ logger_ = node->get_logger();
+ clock_ = node->get_clock();
+
+ costmap_ros_ = costmap_ros;
+ tf_ = tf;
+ name_ = name;
+
+ initialize(node);
+ visualization_ = std::make_shared<TebVisualization>(node, *cfg_);
+ visualization_->on_configure();
+ planner_->setVisualization(visualization_);
+
+ return;
+}
+
+void TebLocalPlannerROS::setPlan(const nav_msgs::msg::Path & orig_global_plan)
+{
+ // check if plugin is initialized
+ if(!initialized_)
+ {
+ RCLCPP_ERROR(logger_, "teb_local_planner has not been initialized, please call initialize() before using this planner");
+ return;
+ }
+
+ // store the global plan
+ global_plan_.clear();
+ global_plan_.reserve(orig_global_plan.poses.size());
+ for(const auto &in_pose :orig_global_plan.poses) {
+ geometry_msgs::msg::PoseStamped out_pose;
+ out_pose.pose = in_pose.pose;
+ out_pose.header = orig_global_plan.header;
+ global_plan_.push_back(out_pose);
+ }
+
+ // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan.
+ // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step.
+
+ return;
+}
+
+
+geometry_msgs::msg::TwistStamped TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
+ const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker)
+{
+ // check if plugin initialized
+ if(!initialized_)
+ {
+ throw nav2_core::PlannerException(
+ std::string("teb_local_planner has not been initialized, please call initialize() before using this planner")
+ );
+ }
+ geometry_msgs::msg::TwistStamped cmd_vel;
+
+ cmd_vel.header.stamp = clock_->now();
+ cmd_vel.header.frame_id = costmap_ros_->getBaseFrameID();
+ cmd_vel.twist.linear.x = 0;
+ cmd_vel.twist.linear.y = 0;
+ cmd_vel.twist.angular.z = 0;
+
+ // Update for the current goal checker's state
+ geometry_msgs::msg::Pose pose_tolerance;
+ geometry_msgs::msg::Twist vel_tolerance;
+ if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) {
+ RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
+ } else {
+ cfg_->goal_tolerance.xy_goal_tolerance = pose_tolerance.position.x;
+ }
+
+ // Get robot pose
+ robot_pose_ = PoseSE2(pose.pose);
+ geometry_msgs::msg::PoseStamped robot_pose;
+ robot_pose.header = pose.header;
+ robot_pose_.toPoseMsg(robot_pose.pose);
+
+ // Get robot velocity
+ robot_vel_ = velocity;
+
+ // prune global plan to cut off parts of the past (spatially before the robot)
+ pruneGlobalPlan(robot_pose, global_plan_, cfg_->trajectory.global_plan_prune_distance);
+
+ // Transform global plan to the frame of interest (w.r.t. the local costmap)
+ std::vector<geometry_msgs::msg::PoseStamped> transformed_plan;
+ int goal_idx;
+ geometry_msgs::msg::TransformStamped tf_plan_to_global;
+ if (!transformGlobalPlan(global_plan_, robot_pose, *costmap_, cfg_->map_frame, cfg_->trajectory.max_global_plan_lookahead_dist,
+ transformed_plan, &goal_idx, &tf_plan_to_global))
+ {
+ throw nav2_core::PlannerException(
+ std::string("Could not transform the global plan to the frame of the controller")
+ );
+ }
+
+ // update via-points container
+ if (!custom_via_points_active_)
+ updateViaPointsContainer(transformed_plan, cfg_->trajectory.global_plan_viapoint_sep);
+
+ // check if we should enter any backup mode and apply settings
+ configureBackupModes(transformed_plan, goal_idx);
+
+ // Return false if the transformed global plan is empty
+ if (transformed_plan.empty())
+ {
+ throw nav2_core::PlannerException(
+ std::string("Transformed plan is empty. Cannot determine a local plan.")
+ );
+ }
+
+ // Get current goal point (last point of the transformed plan)
+ const geometry_msgs::msg::PoseStamped &goal_point = transformed_plan.back();
+ robot_goal_.x() = goal_point.pose.position.x;
+ robot_goal_.y() = goal_point.pose.position.y;
+ if (cfg_->trajectory.global_plan_overwrite_orientation)
+ {
+ robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);
+ // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
+ //transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
+ tf2::Quaternion q;
+ q.setRPY(0, 0, robot_goal_.theta());
+ transformed_plan.back().pose.orientation = tf2::toMsg(q);
+ }
+ else
+ {
+ robot_goal_.theta() = tf2::getYaw(goal_point.pose.orientation);
+ }
+
+ // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)
+ if (transformed_plan.size()==1) // plan only contains the goal
+ {
+ transformed_plan.insert(transformed_plan.begin(), geometry_msgs::msg::PoseStamped()); // insert start (not yet initialized)
+ }
+ //tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start;
+ transformed_plan.front().pose = robot_pose.pose;
+
+ // clear currently existing obstacles
+ obstacles_.clear();
+
+ // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin
+ if (costmap_converter_)
+ updateObstacleContainerWithCostmapConverter();
+ else
+ updateObstacleContainerWithCostmap();
+
+ // also consider custom obstacles (must be called after other updates, since the container is not cleared)
+ updateObstacleContainerWithCustomObstacles();
+
+
+ // Do not allow config changes during the following optimization step
+ std::lock_guard<std::mutex> cfg_lock(cfg_->configMutex());
+
+ // Now perform the actual planning
+// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_->goal_tolerance.free_goal_vel); // straight line init
+ bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_->goal_tolerance.free_goal_vel);
+ if (!success)
+ {
+ planner_->clearPlanner(); // force reinitialization for next time
+
+ ++no_infeasible_plans_; // increase number of infeasible solutions in a row
+ time_last_infeasible_plan_ = clock_->now();
+ last_cmd_ = cmd_vel.twist;
+
+ throw nav2_core::PlannerException(
+ std::string("teb_local_planner was not able to obtain a local plan for the current setting.")
+ );
+ }
+
+ // Check for divergence
+ if (planner_->hasDiverged())
+ {
+ cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
+
+ // Reset everything to start again with the initialization of new trajectories.
+ planner_->clearPlanner();
+ RCLCPP_WARN_THROTTLE(logger_, *(clock_), 1, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");
+
+ ++no_infeasible_plans_; // increase number of infeasible solutions in a row
+ time_last_infeasible_plan_ = clock_->now();
+ last_cmd_ = cmd_vel.twist;
+ throw nav2_core::PlannerException(
+ std::string("TebLocalPlannerROS: velocity command invalid (hasDiverged). Resetting planner...")
+ );
+ }
+
+ // Check feasibility (but within the first few states only)
+ if(cfg_->robot.is_footprint_dynamic)
+ {
+ // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
+ std::vector<geometry_msgs::msg::Point> updated_footprint_spec_ = costmap_ros_->getRobotFootprint();
+ if (updated_footprint_spec_ != footprint_spec_) {
+ updated_footprint_spec_ = footprint_spec_;
+ nav2_costmap_2d::calculateMinAndMaxDistances(updated_footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
+ }
+ }
+
+ bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_->trajectory.feasibility_check_no_poses, cfg_->trajectory.feasibility_check_lookahead_distance);
+ if (!feasible)
+ {
+ cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
+
+ // now we reset everything to start again with the initialization of new trajectories.
+ planner_->clearPlanner();
+
+ ++no_infeasible_plans_; // increase number of infeasible solutions in a row
+ time_last_infeasible_plan_ = clock_->now();
+ last_cmd_ = cmd_vel.twist;
+
+ throw nav2_core::PlannerException(
+ std::string("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...")
+ );
+ }
+
+ // Get the velocity command for this sampling interval
+ if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->trajectory.control_look_ahead_poses))
+ {
+ planner_->clearPlanner();
+ ++no_infeasible_plans_; // increase number of infeasible solutions in a row
+ time_last_infeasible_plan_ = clock_->now();
+ last_cmd_ = cmd_vel.twist;
+
+ throw nav2_core::PlannerException(
+ std::string("TebLocalPlannerROS: velocity command invalid. Resetting planner...")
+ );
+ }
+
+ // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
+ saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->robot.max_vel_x, cfg_->robot.max_vel_y,
+ cfg_->robot.max_vel_theta, cfg_->robot.max_vel_x_backwards);
+
+ // convert rot-vel to steering angle if desired (carlike robot).
+ // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
+ // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
+ if (cfg_->robot.cmd_angle_instead_rotvel)
+ {
+ cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z, cfg_->robot.wheelbase, 0.95*cfg_->robot.min_turning_radius);
+ if (!std::isfinite(cmd_vel.twist.angular.z))
+ {
+ cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
+ last_cmd_ = cmd_vel.twist;
+ planner_->clearPlanner();
+
+ ++no_infeasible_plans_; // increase number of infeasible solutions in a row
+ time_last_infeasible_plan_ = clock_->now();
+
+ throw nav2_core::PlannerException(
+ std::string("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...")
+ );
+ }
+ }
+
+ // a feasible solution should be found, reset counter
+ no_infeasible_plans_ = 0;
+
+ // store last command (for recovery analysis etc.)
+ last_cmd_ = cmd_vel.twist;
+
+ // Now visualize everything
+ planner_->visualize();
+ visualization_->publishObstacles(obstacles_);
+ visualization_->publishViaPoints(via_points_);
+ visualization_->publishGlobalPlan(global_plan_);
+
+ return cmd_vel;
+}
+
+void TebLocalPlannerROS::updateObstacleContainerWithCostmap()
+{
+ // Add costmap obstacles if desired
+ if (cfg_->obstacles.include_costmap_obstacles)
+ {
+ std::lock_guard<std::recursive_mutex> lock(*costmap_->getMutex());
+
+ Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();
+
+ for (unsigned int i=0; i<costmap_->getSizeInCellsX()-1; ++i)
+ {
+ for (unsigned int j=0; j<costmap_->getSizeInCellsY()-1; ++j)
+ {
+ if (costmap_->getCost(i,j) == nav2_costmap_2d::LETHAL_OBSTACLE)
+ {
+ Eigen::Vector2d obs;
+ costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1));
+
+ // check if obstacle is interesting (e.g. not far behind the robot)
+ Eigen::Vector2d obs_dir = obs-robot_pose_.position();
+ if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_->obstacles.costmap_obstacles_behind_robot_dist )
+ continue;
+
+ obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
+ }
+ }
+ }
+ }
+}
+
+void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
+{
+ if (!costmap_converter_)
+ return;
+
+ //Get obstacles from costmap converter
+ costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
+ if (!obstacles)
+ return;
+
+ for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
+ {
+ const costmap_converter_msgs::msg::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
+ const geometry_msgs::msg::Polygon* polygon = &obstacle->polygon;
+
+ if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
+ {
+ obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
+ }
+ else if (polygon->points.size()==1) // Point
+ {
+ obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
+ }
+ else if (polygon->points.size()==2) // Line
+ {
+ obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
+ polygon->points[1].x, polygon->points[1].y )));
+ }
+ else if (polygon->points.size()>2) // Real polygon
+ {
+ PolygonObstacle* polyobst = new PolygonObstacle;
+ for (std::size_t j=0; j<polygon->points.size(); ++j)
+ {
+ polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
+ }
+ polyobst->finalizePolygon();
+ obstacles_.push_back(ObstaclePtr(polyobst));
+ }
+
+ // Set velocity, if obstacle is moving
+ if(!obstacles_.empty())
+ obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
+ }
+}
+
+
+void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
+{
+ // Add custom obstacles obtained via message
+ std::lock_guard<std::mutex> l(custom_obst_mutex_);
+
+ if (!custom_obstacle_msg_.obstacles.empty())
+ {
+ // We only use the global header to specify the obstacle coordinate system instead of individual ones
+ Eigen::Affine3d obstacle_to_map_eig;
+ try
+ {
+ geometry_msgs::msg::TransformStamped obstacle_to_map = tf_->lookupTransform(
+ cfg_->map_frame, tf2::timeFromSec(0),
+ custom_obstacle_msg_.header.frame_id, tf2::timeFromSec(0),
+ custom_obstacle_msg_.header.frame_id, tf2::durationFromSec(0.5));
+ obstacle_to_map_eig = tf2::transformToEigen(obstacle_to_map);
+ //tf2::fromMsg(obstacle_to_map.transform, obstacle_to_map_eig);
+ }
+ catch (tf2::TransformException ex)
+ {
+ RCLCPP_ERROR(logger_, "%s",ex.what());
+ obstacle_to_map_eig.setIdentity();
+ }
+
+ for (size_t i=0; i<custom_obstacle_msg_.obstacles.size(); ++i)
+ {
+ if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 && custom_obstacle_msg_.obstacles.at(i).radius > 0 ) // circle
+ {
+ Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
+ obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius)));
+ }
+ else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point
+ {
+ Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
+ obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) )));
+ }
+ else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line
+ {
+ Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
+ Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z );
+ obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2),
+ (obstacle_to_map_eig * line_end).head(2) )));
+ }
+ else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty())
+ {
+ RCLCPP_INFO(logger_, "Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
+ continue;
+ }
+ else // polygon
+ {
+ PolygonObstacle* polyobst = new PolygonObstacle;
+ for (size_t j=0; j<custom_obstacle_msg_.obstacles.at(i).polygon.points.size(); ++j)
+ {
+ Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y,
+ custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z );
+ polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) );
+ }
+ polyobst->finalizePolygon();
+ obstacles_.push_back(ObstaclePtr(polyobst));
+ }
+
+ // Set velocity, if obstacle is moving
+ if(!obstacles_.empty())
+ obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation);
+ }
+ }
+}
+
+void TebLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, double min_separation)
+{
+ via_points_.clear();
+
+ if (min_separation<=0)
+ return;
+
+ std::size_t prev_idx = 0;
+ for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m]
+ {
+ // check separation to the previous via-point inserted
+ if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation)
+ continue;
+
+ // add via-point
+ via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) );
+ prev_idx = i;
+ }
+
+}
+
+//Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel)
+//{
+// Eigen::Vector2d vel;
+// vel.coeffRef(0) = std::sqrt( tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY() );
+// vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation());
+// return vel;
+//}
+
+
+bool TebLocalPlannerROS::pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose, std::vector<geometry_msgs::msg::PoseStamped>& global_plan, double dist_behind_robot)
+{
+ if (global_plan.empty())
+ return true;
+
+ try
+ {
+ // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times)
+ //geometry_msgs::msg::TransformStamped global_to_plan_transform = tf_->lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, tf2::timeFromSec(0));
+ geometry_msgs::msg::PoseStamped robot = tf_->transform(
+ global_pose,
+ global_plan.front().header.frame_id);
+
+ //robot.setData( global_to_plan_transform * global_pose );
+
+ double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
+
+ // iterate plan until a pose close the robot is found
+ std::vector<geometry_msgs::msg::PoseStamped>::iterator it = global_plan.begin();
+ std::vector<geometry_msgs::msg::PoseStamped>::iterator erase_end = it;
+ while (it != global_plan.end())
+ {
+ double dx = robot.pose.position.x - it->pose.position.x;
+ double dy = robot.pose.position.y - it->pose.position.y;
+ double dist_sq = dx * dx + dy * dy;
+ if (dist_sq < dist_thresh_sq)
+ {
+ erase_end = it;
+ break;
+ }
+ ++it;
+ }
+ if (erase_end == global_plan.end())
+ return false;
+
+ if (erase_end != global_plan.begin())
+ global_plan.erase(global_plan.begin(), erase_end);
+ }
+ catch (const tf2::TransformException& ex)
+ {
+ RCLCPP_DEBUG(logger_, "Cannot prune path since no transform is available: %s\n", ex.what());
+ return false;
+ }
+ return true;
+}
+
+
+bool TebLocalPlannerROS::transformGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan,
+ const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length,
+ std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int* current_goal_idx, geometry_msgs::msg::TransformStamped* tf_plan_to_global) const
+{
+ // this method is a slightly modified version of base_local_planner/goal_functions.h
+
+ const geometry_msgs::msg::PoseStamped& plan_pose = global_plan[0];
+
+ transformed_plan.clear();
+
+ try
+ {
+ if (global_plan.empty())
+ {
+ RCLCPP_ERROR(logger_, "Received plan with zero length");
+ *current_goal_idx = 0;
+ return false;
+ }
+
+ // get plan_to_global_transform from plan frame to global_frame
+ geometry_msgs::msg::TransformStamped plan_to_global_transform = tf_->lookupTransform(
+ global_frame, tf2_ros::fromMsg(plan_pose.header.stamp),
+ plan_pose.header.frame_id, tf2::timeFromSec(0),
+ plan_pose.header.frame_id, tf2::durationFromSec(0.5));
+
+// tf_->waitForTransform(global_frame, ros::Time::now(),
+// plan_pose.header.frame_id, plan_pose.header.stamp,
+// plan_pose.header.frame_id, ros::Duration(0.5));
+// tf_->lookupTransform(global_frame, ros::Time(),
+// plan_pose.header.frame_id, plan_pose.header.stamp,
+// plan_pose.header.frame_id, plan_to_global_transform);
+
+ //let's get the pose of the robot in the frame of the plan
+ geometry_msgs::msg::PoseStamped robot_pose = tf_->transform(global_pose, plan_pose.header.frame_id);
+
+ //we'll discard points on the plan that are outside the local costmap
+ double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
+ costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
+ dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
+ // located on the border of the local costmap
+
+
+ int i = 0;
+ double sq_dist_threshold = dist_threshold * dist_threshold;
+ double sq_dist = 1e10;
+
+ //we need to loop to a point on the plan that is within a certain distance of the robot
+ bool robot_reached = false;
+ for(int j=0; j < (int)global_plan.size(); ++j)
+ {
+ double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
+ double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y;
+ double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
+ if (new_sq_dist > sq_dist_threshold)
+ break; // force stop if we have reached the costmap border
+
+ if (robot_reached && new_sq_dist > sq_dist)
+ break;
+
+ if (new_sq_dist < sq_dist) // find closest distance
+ {
+ sq_dist = new_sq_dist;
+ i = j;
+ if (sq_dist < 0.05) // 2.5 cm to the robot; take the immediate local minima; if it's not the global
+ robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this
+ }
+ }
+
+ geometry_msgs::msg::PoseStamped newer_pose;
+
+ double plan_length = 0; // check cumulative Euclidean distance along the plan
+
+ //now we'll transform until points are outside of our distance threshold
+ while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length))
+ {
+ //const geometry_msgs::msg::PoseStamped& pose = global_plan[i];
+ //tf::poseStampedMsgToTF(pose, tf_pose);
+ //tf_pose.setData(plan_to_global_transform * tf_pose);
+ tf2::doTransform(global_plan[i], newer_pose, plan_to_global_transform);
+
+// tf_pose.stamp_ = plan_to_global_transform.stamp_;
+// tf_pose.frame_id_ = global_frame;
+// tf::poseStampedTFToMsg(tf_pose, newer_pose);
+
+ transformed_plan.push_back(newer_pose);
+
+ double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
+ double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
+ sq_dist = x_diff * x_diff + y_diff * y_diff;
+
+ // caclulate distance to previous pose
+ if (i>0 && max_plan_length>0)
+ plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);
+
+ ++i;
+ }
+
+ // if we are really close to the goal (<sq_dist_threshold) and the goal is not yet reached (e.g. orientation error >>0)
+ // the resulting transformed plan can be empty. In that case we explicitly inject the global goal.
+ if (transformed_plan.empty())
+ {
+// tf::poseStampedMsgToTF(global_plan.back(), tf_pose);
+// tf_pose.setData(plan_to_global_transform * tf_pose);
+// tf_pose.stamp_ = plan_to_global_transform.stamp_;
+// tf_pose.frame_id_ = global_frame;
+// tf::poseStampedTFToMsg(tf_pose, newer_pose);
+ tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform);
+
+ transformed_plan.push_back(newer_pose);
+
+ // Return the index of the current goal point (inside the distance threshold)
+ if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
+ }
+ else
+ {
+ // Return the index of the current goal point (inside the distance threshold)
+ if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop
+ }
+
+ // Return the transformation from the global plan to the global planning frame if desired
+ if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform;
+ }
+ catch(tf2::LookupException& ex)
+ {
+ RCLCPP_ERROR(logger_, "No Transform available Error: %s\n", ex.what());
+ return false;
+ }
+ catch(tf2::ConnectivityException& ex)
+ {
+ RCLCPP_ERROR(logger_, "Connectivity Error: %s\n", ex.what());
+ return false;
+ }
+ catch(tf2::ExtrapolationException& ex)
+ {
+ RCLCPP_ERROR(logger_, "Extrapolation Error: %s\n", ex.what());
+ if (global_plan.size() > 0)
+ RCLCPP_ERROR(logger_, "Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
+
+ return false;
+ }
+
+ return true;
+}
+
+
+
+
+double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan, const geometry_msgs::msg::PoseStamped& local_goal,
+ int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length) const
+{
+ int n = (int)global_plan.size();
+
+ // check if we are near the global goal already
+ if (current_goal_idx > n-moving_average_length-2)
+ {
+ if (current_goal_idx >= n-1) // we've exactly reached the goal
+ {
+ return tf2::getYaw(local_goal.pose.orientation);
+ }
+ else
+ {
+// tf::Quaternion global_orientation;
+// tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation);
+// return tf2::getYaw(tf_plan_to_global.getRotation() * global_orientation );
+
+ tf2::Quaternion global_orientation, tf_plan_to_global_orientation;
+ tf2::fromMsg(global_plan.back().pose.orientation, global_orientation);
+ tf2::fromMsg(tf_plan_to_global.transform.rotation, tf_plan_to_global_orientation);
+
+ return tf2::getYaw(tf_plan_to_global_orientation * global_orientation);
+ }
+ }
+
+ // reduce number of poses taken into account if the desired number of poses is not available
+ moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); // maybe redundant, since we have checked the vicinity of the goal before
+
+ std::vector<double> candidates;
+ geometry_msgs::msg::PoseStamped tf_pose_k = local_goal;
+ geometry_msgs::msg::PoseStamped tf_pose_kp1;
+
+ int range_end = current_goal_idx + moving_average_length;
+ for (int i = current_goal_idx; i < range_end; ++i)
+ {
+ // Transform pose of the global plan to the planning frame
+ const geometry_msgs::msg::PoseStamped& pose = global_plan.at(i+1);
+ tf2::doTransform(global_plan.at(i+1), tf_pose_kp1, tf_plan_to_global);
+
+ // calculate yaw angle
+ candidates.push_back( std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y,
+ tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x ) );
+
+ if (i<range_end-1)
+ tf_pose_k = tf_pose_kp1;
+ }
+ return average_angles(candidates);
+}
+
+
+void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
+{
+ double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
+ // Limit translational velocity for forward driving
+ if (vx > max_vel_x)
+ ratio_x = max_vel_x / vx;
+
+ // limit strafing velocity
+ if (vy > max_vel_y || vy < -max_vel_y)
+ ratio_y = std::abs(max_vel_y / vy);
+
+ // Limit angular velocity
+ if (omega > max_vel_theta || omega < -max_vel_theta)
+ ratio_omega = std::abs(max_vel_theta / omega);
+
+ // Limit backwards velocity
+ if (max_vel_x_backwards<=0)
+ {
+ RCLCPP_WARN_ONCE(
+ logger_,
+ "TebLocalPlannerROS(): Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
+ }
+ else if (vx < -max_vel_x_backwards)
+ ratio_x = - max_vel_x_backwards / vx;
+
+ if (cfg_->robot.use_proportional_saturation)
+ {
+ double ratio = std::min(std::min(ratio_x, ratio_y), ratio_omega);
+ vx *= ratio;
+ vy *= ratio;
+ omega *= ratio;
+ }
+ else
+ {
+ vx *= ratio_x;
+ vy *= ratio_y;
+ omega *= ratio_omega;
+ }
+}
+
+
+double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const
+{
+ if (omega==0 || v==0)
+ return 0;
+
+ double radius = v/omega;
+
+ if (fabs(radius) < min_turning_radius)
+ radius = double(g2o::sign(radius)) * min_turning_radius;
+
+ return std::atan(wheelbase / radius);
+}
+
+
+void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)
+{
+ RCLCPP_WARN_EXPRESSION(
+ logger_, opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius,
+ "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller "
+ "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). "
+ "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius);
+}
+
+
+
+void TebLocalPlannerROS::configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int& goal_idx)
+{
+ rclcpp::Time current_time = clock_->now();
+
+ // reduced horizon backup mode
+ if (cfg_->recovery.shrink_horizon_backup &&
+ goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations)
+ (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).seconds() < cfg_->recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds
+ {
+ RCLCPP_INFO_EXPRESSION(
+ logger_,
+ no_infeasible_plans_==1,
+ "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_->recovery.shrink_horizon_min_duration);
+
+
+ // Shorten horizon if requested
+ // reduce to 50 percent:
+ int horizon_reduction = goal_idx/2;
+
+ if (no_infeasible_plans_ > 9)
+ {
+ RCLCPP_INFO_EXPRESSION(
+ logger_,
+ no_infeasible_plans_==10,
+ "Infeasible trajectory detected 10 times in a row: further reducing horizon...");
+ horizon_reduction /= 2;
+ }
+
+ // we have a small overhead here, since we already transformed 50% more of the trajectory.
+ // But that's ok for now, since we do not need to make transformGlobalPlan more complex
+ // and a reduced horizon should occur just rarely.
+ int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;
+ goal_idx -= horizon_reduction;
+ if (new_goal_idx_transformed_plan>0 && goal_idx >= 0)
+ transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end());
+ else
+ goal_idx += horizon_reduction; // this should not happen, but safety first ;-)
+ }
+
+
+ // detect and resolve oscillations
+ if (cfg_->recovery.oscillation_recovery)
+ {
+ double max_vel_theta;
+ double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_->robot.max_vel_x : cfg_->robot.max_vel_x_backwards;
+ if (cfg_->robot.min_turning_radius!=0 && max_vel_current>0)
+ max_vel_theta = std::max( max_vel_current/std::abs(cfg_->robot.min_turning_radius), cfg_->robot.max_vel_theta );
+ else
+ max_vel_theta = cfg_->robot.max_vel_theta;
+
+ failure_detector_.update(last_cmd_, cfg_->robot.max_vel_x, cfg_->robot.max_vel_x_backwards, max_vel_theta,
+ cfg_->recovery.oscillation_v_eps, cfg_->recovery.oscillation_omega_eps);
+
+ bool oscillating = failure_detector_.isOscillating();
+ bool recently_oscillated = (clock_->now()-time_last_oscillation_).seconds() < cfg_->recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recently
+
+ if (oscillating)
+ {
+ if (!recently_oscillated)
+ {
+ // save current turning direction
+ if (robot_vel_.angular.z > 0)
+ last_preferred_rotdir_ = RotType::left;
+ else
+ last_preferred_rotdir_ = RotType::right;
+ RCLCPP_INFO(logger_, "TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).");
+ }
+ time_last_oscillation_ = clock_->now();
+ planner_->setPreferredTurningDir(last_preferred_rotdir_);
+ }
+ else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior
+ {
+ last_preferred_rotdir_ = RotType::none;
+ planner_->setPreferredTurningDir(last_preferred_rotdir_);
+ RCLCPP_INFO(logger_, "TebLocalPlannerROS: oscillation recovery disabled/expired.");
+ }
+ }
+
+}
+
+
+void TebLocalPlannerROS::setSpeedLimit(
+ const double & speed_limit, const bool & percentage)
+{
+ if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
+ // Restore default value
+ cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x;
+ cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
+ cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y;
+ cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta;
+ } else {
+ if (percentage) {
+ // Speed limit is expressed in % from maximum speed of robot
+ cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0;
+ cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
+ cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
+ cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
+ } else {
+ // Speed limit is expressed in absolute value
+ double max_speed_xy = std::max(
+ std::max(cfg_->robot.base_max_vel_x,cfg_->robot.base_max_vel_x_backwards),cfg_->robot.base_max_vel_y);
+ if (speed_limit < max_speed_xy) {
+ // Handling components and angular velocity changes:
+ // Max velocities are being changed in the same proportion
+ // as absolute linear speed changed in order to preserve
+ // robot moving trajectories to be the same after speed change.
+ // G. Doisy: not sure if that's applicable to base_max_vel_x_backwards.
+ const double ratio = speed_limit / max_speed_xy;
+ cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio;
+ cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
+ cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio;
+ cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
+ }
+ }
+ }
+}
+
+void TebLocalPlannerROS::customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg)
+{
+ std::lock_guard<std::mutex> l(custom_obst_mutex_);
+ custom_obstacle_msg_ = *obst_msg;
+}
+
+void TebLocalPlannerROS::customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg)
+{
+ RCLCPP_INFO_ONCE(logger_, "Via-points received. This message is printed once.");
+ if (cfg_->trajectory.global_plan_viapoint_sep > 0)
+ {
+ RCLCPP_INFO(logger_, "Via-points are already obtained from the global plan (global_plan_viapoint_sep>0)."
+ "Ignoring custom via-points.");
+ custom_via_points_active_ = false;
+ return;
+ }
+
+ std::lock_guard<std::mutex> l(via_point_mutex_);
+ via_points_.clear();
+ for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses)
+ {
+ via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y);
+ }
+ custom_via_points_active_ = !via_points_.empty();
+}
+
+void TebLocalPlannerROS::activate() {
+ visualization_->on_activate();
+
+ return;
+}
+void TebLocalPlannerROS::deactivate() {
+ visualization_->on_deactivate();
+
+ return;
+}
+void TebLocalPlannerROS::cleanup() {
+ visualization_->on_cleanup();
+ costmap_converter_->stopWorker();
+
+ return;
+}
+
+} // end namespace teb_local_planner
+
+// register this planner as a nav2_core::Controller plugin
+PLUGINLIB_EXPORT_CLASS(teb_local_planner::TebLocalPlannerROS, nav2_core::Controller)
diff --git a/src/teb_local_planner/src/test_optim_node.cpp b/src/teb_local_planner/src/test_optim_node.cpp
new file mode 100644
index 0000000..94d9c02
--- /dev/null
+++ b/src/teb_local_planner/src/test_optim_node.cpp
@@ -0,0 +1,322 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017.
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/teb_local_planner_ros.h"
+
+//#include <interactive_markers/interactive_marker_server.h>
+#include <visualization_msgs/msg/marker.hpp>
+
+
+using namespace teb_local_planner; // it is ok here to import everything for testing purposes
+
+// ============= Global Variables ================
+// Ok global variables are bad, but here we only have a simple testing node.
+PlannerInterfacePtr planner;
+TebVisualizationPtr visual;
+std::vector<ObstaclePtr> obst_vector;
+ViaPointContainer via_points;
+TebConfig config;
+//std::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg;
+ros::Subscriber custom_obst_sub;
+ros::Subscriber via_points_sub;
+ros::Subscriber clicked_points_sub;
+std::vector<ros::Subscriber> obst_vel_subs;
+unsigned int no_fixed_obstacles;
+
+// =========== Function declarations =============
+void CB_mainCycle(const ros::TimerEvent& e);
+void CB_publishCycle(const ros::TimerEvent& e);
+void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
+void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
+void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb);
+void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
+void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
+void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg);
+void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id);
+
+
+// =============== Main function =================
+int main( int argc, char** argv )
+{
+ ros::init(argc, argv, "test_optim_node");
+ rclcpp::Node::SharedPtr n("~");
+
+
+ // load ros parameters from node handle
+ config.loadRosParamFromNodeHandle(n);
+
+ ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
+ ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
+
+ // setup dynamic reconfigure
+ dynamic_recfg = std::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
+ dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
+ dynamic_recfg->setCallback(cb);
+
+ // setup callback for custom obstacles
+ custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle);
+
+ // setup callback for clicked points (in rviz) that are considered as via-points
+ clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points);
+
+ // setup callback for via-points (callback overwrites previously set via-points)
+ via_points_sub = n.subscribe("via_points", 1, CB_via_points);
+
+ // interactive marker server for simulated dynamic obstacles
+ interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
+
+ obst_vector.push_back( std::make_shared<PointObstacle>(-3,1) );
+ obst_vector.push_back( std::make_shared<PointObstacle>(6,2) );
+ obst_vector.push_back( std::make_shared<PointObstacle>(0,0.1) );
+// obst_vector.push_back( std::make_shared<LineObstacle>(1,1.5,1,-1.5) ); //90 deg
+// obst_vector.push_back( std::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg
+// obst_vector.push_back( std::make_shared<PointObstacle>(-1.5,-0.5) );
+
+ // Dynamic obstacles
+ Eigen::Vector2d vel (0.1, -0.3);
+ obst_vector.at(0)->setCentroidVelocity(vel);
+ vel = Eigen::Vector2d(-0.3, -0.2);
+ obst_vector.at(1)->setCentroidVelocity(vel);
+
+ /*
+ PolygonObstacle* polyobst = new PolygonObstacle;
+ polyobst->pushBackVertex(1, -1);
+ polyobst->pushBackVertex(0, 1);
+ polyobst->pushBackVertex(1, 1);
+ polyobst->pushBackVertex(2, 1);
+
+ polyobst->finalizePolygon();
+ obst_vector.emplace_back(polyobst);
+ */
+
+ for (unsigned int i=0; i<obst_vector.size(); ++i)
+ {
+ // setup callbacks for setting obstacle velocities
+ std::string topic = "/test_optim_node/obstacle_" + std::to_string(i) + "/cmd_vel";
+ obst_vel_subs.push_back(n.subscribe<geometry_msgs::msg::Twist>(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i)));
+
+ //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker);
+ // Add interactive markers for all point obstacles
+ std::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst_vector.at(i));
+ if (pobst)
+ {
+ CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker);
+ }
+ }
+ marker_server.applyChanges();
+
+ // Setup visualization
+ visual = TebVisualizationPtr(new TebVisualization(n, config));
+
+ // Setup robot shape model
+ RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n);
+
+ // Setup planner (homotopy class planning or just the local teb planner)
+ if (config.hcp.enable_homotopy_class_planning)
+ planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
+ else
+ planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
+
+
+ no_fixed_obstacles = obst_vector.size();
+ ros::spin();
+
+ return 0;
+}
+
+// Planning loop
+void CB_mainCycle(const ros::TimerEvent& e)
+{
+ planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes
+}
+
+// Visualization loop
+void CB_publishCycle(const ros::TimerEvent& e)
+{
+ planner->visualize();
+ visual->publishObstacles(obst_vector);
+ visual->publishViaPoints(via_points);
+}
+
+void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
+{
+ config.reconfigure(reconfig);
+}
+
+void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb)
+{
+ // create an interactive marker for our server
+ visualization_msgs::InteractiveMarker i_marker;
+ i_marker.header.frame_id = frame;
+ i_marker.header.stamp = ros::Time::now();
+ std::ostringstream oss;
+ //oss << "obstacle" << id;
+ oss << id;
+ i_marker.name = oss.str();
+ i_marker.description = "Obstacle";
+ i_marker.pose.position.x = init_x;
+ i_marker.pose.position.y = init_y;
+ i_marker.pose.orientation.w = 1.0f; // make quaternion normalized
+
+ // create a grey box marker
+ visualization_msgs::msg::Marker box_marker;
+ box_marker.type = visualization_msgs::msg::Marker::CUBE;
+ box_marker.id = id;
+ box_marker.scale.x = 0.2;
+ box_marker.scale.y = 0.2;
+ box_marker.scale.z = 0.2;
+ box_marker.color.r = 0.5;
+ box_marker.color.g = 0.5;
+ box_marker.color.b = 0.5;
+ box_marker.color.a = 1.0;
+ box_marker.pose.orientation.w = 1.0f; // make quaternion normalized
+
+ // create a non-interactive control which contains the box
+ visualization_msgs::InteractiveMarkerControl box_control;
+ box_control.always_visible = true;
+ box_control.markers.push_back( box_marker );
+
+ // add the control to the interactive marker
+ i_marker.controls.push_back( box_control );
+
+ // create a control which will move the box, rviz will insert 2 arrows
+ visualization_msgs::InteractiveMarkerControl move_control;
+ move_control.name = "move_x";
+ move_control.orientation.w = 0.707107f;
+ move_control.orientation.x = 0;
+ move_control.orientation.y = 0.707107f;
+ move_control.orientation.z = 0;
+ move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
+
+
+ // add the control to the interactive marker
+ i_marker.controls.push_back(move_control);
+
+ // add the interactive marker to our collection
+ marker_server->insert(i_marker);
+ marker_server->setCallback(i_marker.name,feedback_cb);
+}
+
+void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
+{
+ std::stringstream ss(feedback->marker_name);
+ unsigned int index;
+ ss >> index;
+
+ if (index>=no_fixed_obstacles)
+ return;
+ PointObstacle* pobst = static_cast<PointObstacle*>(obst_vector.at(index).get());
+ pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y);
+}
+
+void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
+{
+ // resize such that the vector contains only the fixed obstacles specified inside the main function
+ obst_vector.resize(no_fixed_obstacles);
+
+ // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)
+ for (size_t i = 0; i < obst_msg->obstacles.size(); ++i)
+ {
+ if (obst_msg->obstacles.at(i).polygon.points.size() == 1 )
+ {
+ if (obst_msg->obstacles.at(i).radius == 0)
+ {
+ obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
+ obst_msg->obstacles.at(i).polygon.points.front().y )));
+ }
+ else
+ {
+ obst_vector.push_back(ObstaclePtr(new CircularObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
+ obst_msg->obstacles.at(i).polygon.points.front().y,
+ obst_msg->obstacles.at(i).radius )));
+ }
+ }
+ else if (obst_msg->obstacles.at(i).polygon.points.empty())
+ {
+ ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
+ continue;
+ }
+ else
+ {
+ PolygonObstacle* polyobst = new PolygonObstacle;
+ for (size_t j=0; j<obst_msg->obstacles.at(i).polygon.points.size(); ++j)
+ {
+ polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x,
+ obst_msg->obstacles.at(i).polygon.points[j].y );
+ }
+ polyobst->finalizePolygon();
+ obst_vector.push_back(ObstaclePtr(polyobst));
+ }
+
+ if(!obst_vector.empty())
+ obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation);
+ }
+}
+
+
+void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
+{
+ // we assume for simplicity that the fixed frame is already the map/planning frame
+ // consider clicked points as via-points
+ via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
+ ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
+ if (config.optim.weight_viapoint<=0)
+ ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
+}
+
+void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg)
+{
+ ROS_INFO_ONCE("Via-points received. This message is printed once.");
+ via_points.clear();
+ for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses)
+ {
+ via_points.emplace_back(pose.pose.position.x, pose.pose.position.y);
+ }
+}
+
+void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id)
+{
+ if (id >= obst_vector.size())
+ {
+ ROS_WARN("Cannot set velocity: unknown obstacle id.");
+ return;
+ }
+
+ Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y);
+ obst_vector.at(id)->setCentroidVelocity(vel);
+}
diff --git a/src/teb_local_planner/src/timed_elastic_band.cpp b/src/teb_local_planner/src/timed_elastic_band.cpp
new file mode 100644
index 0000000..4d7d37c
--- /dev/null
+++ b/src/teb_local_planner/src/timed_elastic_band.cpp
@@ -0,0 +1,630 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/timed_elastic_band.h"
+
+namespace teb_local_planner
+{
+
+namespace
+{
+ /**
+ * estimate the time to move from start to end.
+ * Assumes constant velocity for the motion.
+ */
+ double estimateDeltaT(const PoseSE2& start, const PoseSE2& end,
+ double max_vel_x, double max_vel_theta)
+ {
+ double dt_constant_motion = 0.1;
+ if (max_vel_x > 0) {
+ double trans_dist = (end.position() - start.position()).norm();
+ dt_constant_motion = trans_dist / max_vel_x;
+ }
+ if (max_vel_theta > 0) {
+ double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta()));
+ dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta);
+ }
+ return dt_constant_motion;
+ }
+} // namespace
+
+
+TimedElasticBand::TimedElasticBand()
+{
+}
+
+TimedElasticBand::~TimedElasticBand()
+{
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Destructor Timed_Elastic_Band...");
+ clearTimedElasticBand();
+}
+
+
+void TimedElasticBand::addPose(const PoseSE2& pose, bool fixed)
+{
+ VertexPose* pose_vertex = new VertexPose(pose, fixed);
+ pose_vec_.push_back( pose_vertex );
+ return;
+}
+
+void TimedElasticBand::addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed)
+{
+ VertexPose* pose_vertex = new VertexPose(position, theta, fixed);
+ pose_vec_.push_back( pose_vertex );
+ return;
+}
+
+ void TimedElasticBand::addPose(double x, double y, double theta, bool fixed)
+{
+ VertexPose* pose_vertex = new VertexPose(x, y, theta, fixed);
+ pose_vec_.push_back( pose_vertex );
+ return;
+}
+
+void TimedElasticBand::addTimeDiff(double dt, bool fixed)
+{
+ assert(dt > 0.0 && "Adding a timediff requires a positive dt");
+ VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt, fixed);
+ timediff_vec_.push_back( timediff_vertex );
+ return;
+}
+
+
+void TimedElasticBand::addPoseAndTimeDiff(double x, double y, double angle, double dt)
+{
+ if (sizePoses() != sizeTimeDiffs())
+ {
+ addPose(x,y,angle,false);
+ addTimeDiff(dt,false);
+ }
+ else {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"),
+ "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
+ }
+ return;
+}
+
+
+
+void TimedElasticBand::addPoseAndTimeDiff(const PoseSE2& pose, double dt)
+{
+ if (sizePoses() != sizeTimeDiffs())
+ {
+ addPose(pose,false);
+ addTimeDiff(dt,false);
+ } else {
+ RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
+ }
+ return;
+}
+
+void TimedElasticBand::addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt)
+{
+ if (sizePoses() != sizeTimeDiffs())
+ {
+ addPose(position, theta,false);
+ addTimeDiff(dt,false);
+ } else {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
+ }
+ return;
+}
+
+
+void TimedElasticBand::deletePose(int index)
+{
+ assert(index<pose_vec_.size());
+ delete pose_vec_.at(index);
+ pose_vec_.erase(pose_vec_.begin()+index);
+}
+
+void TimedElasticBand::deletePoses(int index, int number)
+{
+ assert(index+number<=(int)pose_vec_.size());
+ for (int i = index; i<index+number; ++i)
+ delete pose_vec_.at(i);
+ pose_vec_.erase(pose_vec_.begin()+index, pose_vec_.begin()+index+number);
+}
+
+void TimedElasticBand::deleteTimeDiff(int index)
+{
+ assert(index<(int)timediff_vec_.size());
+ delete timediff_vec_.at(index);
+ timediff_vec_.erase(timediff_vec_.begin()+index);
+}
+
+void TimedElasticBand::deleteTimeDiffs(int index, int number)
+{
+ assert(index+number<=timediff_vec_.size());
+ for (int i = index; i<index+number; ++i)
+ delete timediff_vec_.at(i);
+ timediff_vec_.erase(timediff_vec_.begin()+index, timediff_vec_.begin()+index+number);
+}
+
+void TimedElasticBand::insertPose(int index, const PoseSE2& pose)
+{
+ VertexPose* pose_vertex = new VertexPose(pose);
+ pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
+}
+
+void TimedElasticBand::insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
+{
+ VertexPose* pose_vertex = new VertexPose(position, theta);
+ pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
+}
+
+void TimedElasticBand::insertPose(int index, double x, double y, double theta)
+{
+ VertexPose* pose_vertex = new VertexPose(x, y, theta);
+ pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
+}
+
+void TimedElasticBand::insertTimeDiff(int index, double dt)
+{
+ VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt);
+ timediff_vec_.insert(timediff_vec_.begin()+index, timediff_vertex);
+}
+
+
+void TimedElasticBand::clearTimedElasticBand()
+{
+ for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it)
+ delete *pose_it;
+ pose_vec_.clear();
+
+ for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
+ delete *dt_it;
+ timediff_vec_.clear();
+}
+
+
+void TimedElasticBand::setPoseVertexFixed(int index, bool status)
+{
+ assert(index<sizePoses());
+ pose_vec_.at(index)->setFixed(status);
+}
+
+void TimedElasticBand::setTimeDiffVertexFixed(int index, bool status)
+{
+ assert(index<sizeTimeDiffs());
+ timediff_vec_.at(index)->setFixed(status);
+}
+
+
+void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples, bool fast_mode)
+{
+ assert(sizeTimeDiffs() == 0 || sizeTimeDiffs() + 1 == sizePoses());
+ /// iterate through all TEB states and add/remove states!
+ bool modified = true;
+
+ for (int rep = 0; rep < 100 && modified; ++rep) // actually it should be while(), but we want to make sure to not get stuck in some oscillation, hence max 100 repitions.
+ {
+ modified = false;
+
+ for(int i=0; i < sizeTimeDiffs(); ++i) // TimeDiff connects Point(i) with Point(i+1)
+ {
+ if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
+ {
+ //RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "teb_local_planner: autoResize() inserting new bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs());
+
+ double newtime = 0.5*TimeDiff(i);
+
+ TimeDiff(i) = newtime;
+ insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
+ insertTimeDiff(i+1,newtime);
+
+ modified = true;
+ }
+ else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples) // only remove samples if size is larger than min_samples.
+ {
+ //RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs());
+
+ if(i < ((int)sizeTimeDiffs()-1))
+ {
+ TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
+ deleteTimeDiff(i);
+ deletePose(i+1);
+ }
+ else
+ { // last motion should be adjusted, shift time to the interval before
+ TimeDiff(i-1) += TimeDiff(i);
+ deleteTimeDiff(i);
+ deletePose(i);
+ }
+
+ modified = true;
+ }
+ }
+ if (fast_mode) break;
+ }
+}
+
+
+double TimedElasticBand::getSumOfAllTimeDiffs() const
+{
+ double time = 0;
+
+ for(TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
+ {
+ time += (*dt_it)->dt();
+ }
+ return time;
+}
+
+double TimedElasticBand::getSumOfTimeDiffsUpToIdx(int index) const
+{
+ assert(index<=timediff_vec_.size());
+
+ double time = 0;
+
+ for(int i = 0; i < index; ++i)
+ {
+ time += timediff_vec_.at(i)->dt();
+ }
+
+ return time;
+}
+
+double TimedElasticBand::getAccumulatedDistance() const
+{
+ double dist = 0;
+
+ for(int i=1; i<sizePoses(); ++i)
+ {
+ dist += (Pose(i).position() - Pose(i-1).position()).norm();
+ }
+ return dist;
+}
+
+bool TimedElasticBand::initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep, double max_vel_x, int min_samples, bool guess_backwards_motion)
+{
+ if (!isInit())
+ {
+ addPose(start); // add starting point
+ setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
+
+ double timestep = 0.1;
+
+ if (diststep!=0)
+ {
+ Eigen::Vector2d point_to_goal = goal.position()-start.position();
+ double dir_to_goal = std::atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
+ double dx = diststep*std::cos(dir_to_goal);
+ double dy = diststep*std::sin(dir_to_goal);
+ double orient_init = dir_to_goal;
+ // check if the goal is behind the start pose (w.r.t. start orientation)
+ if (guess_backwards_motion && point_to_goal.dot(start.orientationUnitVec()) < 0)
+ orient_init = g2o::normalize_theta(orient_init+M_PI);
+ // TODO: timestep ~ max_vel_x_backwards for backwards motions
+
+ double dist_to_goal = point_to_goal.norm();
+ double no_steps_d = dist_to_goal/std::abs(diststep); // ignore negative values
+ unsigned int no_steps = (unsigned int) std::floor(no_steps_d);
+
+ if (max_vel_x > 0) timestep = diststep / max_vel_x;
+
+ for (unsigned int i=1; i<=no_steps; i++) // start with 1! starting point had index 0
+ {
+ if (i==no_steps && no_steps_d==(float) no_steps)
+ break; // if last conf (depending on stepsize) is equal to goal conf -> leave loop
+ addPoseAndTimeDiff(start.x()+i*dx,start.y()+i*dy,orient_init,timestep);
+ }
+
+ }
+
+ // if number of samples is not larger than min_samples, insert manually
+ if ( sizePoses() < min_samples-1 )
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
+ while (sizePoses() < min_samples-1) // subtract goal point that will be added later
+ {
+ // simple strategy: interpolate between the current pose and the goal
+ PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
+ if (max_vel_x > 0) timestep = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x;
+ addPoseAndTimeDiff( intermediate_pose, timestep ); // let the optimier correct the timestep (TODO: better initialization
+ }
+ }
+
+ // add goal
+ if (max_vel_x > 0) timestep = (goal.position()-BackPose().position()).norm()/max_vel_x;
+ addPoseAndTimeDiff(goal,timestep); // add goal point
+ setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
+ }
+ else // size!=0
+ {
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
+ return false;
+ }
+ return true;
+}
+
+
+bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::msg::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)
+{
+
+ if (!isInit())
+ {
+ PoseSE2 start(plan.front().pose);
+ PoseSE2 goal(plan.back().pose);
+
+ addPose(start); // add starting point with given orientation
+ setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
+
+ bool backwards = false;
+ if (guess_backwards_motion && (goal.position()-start.position()).dot(start.orientationUnitVec()) < 0) // check if the goal is behind the start pose (w.r.t. start orientation)
+ backwards = true;
+ // TODO: dt ~ max_vel_x_backwards for backwards motions
+
+ for (int i=1; i<(int)plan.size()-1; ++i)
+ {
+ double yaw;
+ if (estimate_orient)
+ {
+ // get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
+ double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
+ double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
+ yaw = std::atan2(dy,dx);
+ if (backwards)
+ yaw = g2o::normalize_theta(yaw+M_PI);
+ }
+ else
+ {
+ yaw = tf2::getYaw(plan[i].pose.orientation);
+ }
+ PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);
+ double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
+ addPoseAndTimeDiff(intermediate_pose, dt);
+ }
+
+ // if number of samples is not larger than min_samples, insert manually
+ if ( sizePoses() < min_samples-1 )
+ {
+ RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
+ "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
+ while (sizePoses() < min_samples-1) // subtract goal point that will be added later
+ {
+ // simple strategy: interpolate between the current pose and the goal
+ PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
+ double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
+ addPoseAndTimeDiff( intermediate_pose, dt ); // let the optimier correct the timestep (TODO: better initialization
+ }
+ }
+
+ // Now add final state with given orientation
+ double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
+ addPoseAndTimeDiff(goal, dt);
+ setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
+ }
+ else // size!=0
+ {
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
+ RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
+ "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
+ return false;
+ }
+
+ return true;
+}
+
+
+int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance, int begin_idx) const
+{
+ int n = sizePoses();
+ if (begin_idx < 0 || begin_idx >= n)
+ return -1;
+
+ double min_dist_sq = std::numeric_limits<double>::max();
+ int min_idx = -1;
+
+ for (int i = begin_idx; i < n; i++)
+ {
+ double dist_sq = (ref_point - Pose(i).position()).squaredNorm();
+ if (dist_sq < min_dist_sq)
+ {
+ min_dist_sq = dist_sq;
+ min_idx = i;
+ }
+ }
+
+ if (distance)
+ *distance = std::sqrt(min_dist_sq);
+
+ return min_idx;
+}
+
+
+int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance) const
+{
+ double min_dist = std::numeric_limits<double>::max();
+ int min_idx = -1;
+
+ for (int i = 0; i < sizePoses(); i++)
+ {
+ Eigen::Vector2d point = Pose(i).position();
+ double dist = distance_point_to_segment_2d(point, ref_line_start, ref_line_end);
+ if (dist < min_dist)
+ {
+ min_dist = dist;
+ min_idx = i;
+ }
+ }
+
+ if (distance)
+ *distance = min_dist;
+ return min_idx;
+}
+
+int TimedElasticBand::findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance) const
+{
+ if (vertices.empty())
+ return 0;
+ else if (vertices.size() == 1)
+ return findClosestTrajectoryPose(vertices.front());
+ else if (vertices.size() == 2)
+ return findClosestTrajectoryPose(vertices.front(), vertices.back());
+
+ double min_dist = std::numeric_limits<double>::max();
+ int min_idx = -1;
+
+ for (int i = 0; i < sizePoses(); i++)
+ {
+ Eigen::Vector2d point = Pose(i).position();
+ double dist_to_polygon = std::numeric_limits<double>::max();
+ for (int j = 0; j < (int) vertices.size()-1; ++j)
+ {
+ dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices[j], vertices[j+1]));
+ }
+ dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices.back(), vertices.front()));
+ if (dist_to_polygon < min_dist)
+ {
+ min_dist = dist_to_polygon;
+ min_idx = i;
+ }
+ }
+
+ if (distance)
+ *distance = min_dist;
+
+ return min_idx;
+}
+
+
+int TimedElasticBand::findClosestTrajectoryPose(const Obstacle& obstacle, double* distance) const
+{
+ const PointObstacle* pobst = dynamic_cast<const PointObstacle*>(&obstacle);
+ if (pobst)
+ return findClosestTrajectoryPose(pobst->position(), distance);
+
+ const LineObstacle* lobst = dynamic_cast<const LineObstacle*>(&obstacle);
+ if (lobst)
+ return findClosestTrajectoryPose(lobst->start(), lobst->end(), distance);
+
+ const PolygonObstacle* polyobst = dynamic_cast<const PolygonObstacle*>(&obstacle);
+ if (polyobst)
+ return findClosestTrajectoryPose(polyobst->vertices(), distance);
+
+ return findClosestTrajectoryPose(obstacle.getCentroid(), distance);
+}
+
+
+void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples)
+{
+ // first and simple approach: change only start confs (and virtual start conf for inital velocity)
+ // TEST if optimizer can handle this "hard" placement
+
+ if (new_start && sizePoses()>0)
+ {
+ // find nearest state (using l2-norm) in order to prune the trajectory
+ // (remove already passed states)
+ double dist_cache = (new_start->position()- Pose(0).position()).norm();
+ double dist;
+ int lookahead = std::min<int>( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples
+
+ int nearest_idx = 0;
+ for (int i = 1; i<=lookahead; ++i)
+ {
+ dist = (new_start->position()- Pose(i).position()).norm();
+ if (dist<dist_cache)
+ {
+ dist_cache = dist;
+ nearest_idx = i;
+ }
+ else break;
+ }
+
+ // prune trajectory at the beginning (and extrapolate sequences at the end if the horizon is fixed)
+ if (nearest_idx>0)
+ {
+ // nearest_idx is equal to the number of samples to be removed (since it counts from 0 ;-) )
+ // WARNING delete starting at pose 1, and overwrite the original pose(0) with new_start, since Pose(0) is fixed during optimization!
+ deletePoses(1, nearest_idx); // delete first states such that the closest state is the new first one
+ deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
+ }
+
+ // update start
+ Pose(0) = *new_start;
+ }
+
+ if (new_goal && sizePoses()>0)
+ {
+ BackPose() = *new_goal;
+ }
+};
+
+
+bool TimedElasticBand::isTrajectoryInsideRegion(double radius, double max_dist_behind_robot, int skip_poses)
+{
+ if (sizePoses()<=0)
+ return true;
+
+ double radius_sq = radius*radius;
+ double max_dist_behind_robot_sq = max_dist_behind_robot*max_dist_behind_robot;
+ Eigen::Vector2d robot_orient = Pose(0).orientationUnitVec();
+
+ for (int i=1; i<sizePoses(); i=i+skip_poses+1)
+ {
+ Eigen::Vector2d dist_vec = Pose(i).position()-Pose(0).position();
+ double dist_sq = dist_vec.squaredNorm();
+
+ if (dist_sq > radius_sq)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot");
+ return false;
+ }
+
+ // check behind the robot with a different distance, if specified (or >=0)
+ if (max_dist_behind_robot >= 0 && dist_vec.dot(robot_orient) < 0 && dist_sq > max_dist_behind_robot_sq)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot behind");
+ return false;
+ }
+
+ }
+ return true;
+}
+
+
+
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/visualization.cpp b/src/teb_local_planner/src/visualization.cpp
new file mode 100644
index 0000000..025960d
--- /dev/null
+++ b/src/teb_local_planner/src/visualization.cpp
@@ -0,0 +1,563 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2016,
+ * TU Dortmund - Institute of Control Theory and Systems Engineering.
+ * 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 institute nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+// ros stuff
+#include "teb_local_planner/visualization.h"
+#include "teb_local_planner/optimal_planner.h"
+
+namespace teb_local_planner
+{
+
+void publishPlan(const std::vector<geometry_msgs::msg::PoseStamped>& path,
+ rclcpp::Publisher<nav_msgs::msg::Path> *pub) {
+ if(path.empty())
+ return;
+
+ nav_msgs::msg::Path gui_path;
+ gui_path.poses.resize(path.size());
+ gui_path.header.frame_id = path[0].header.frame_id;
+ gui_path.header.stamp = path[0].header.stamp;
+
+ // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+ for(unsigned int i=0; i < path.size(); i++){
+ gui_path.poses[i] = path[i];
+ }
+
+ pub->publish(gui_path);
+}
+
+TebVisualization::TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg) : nh_(nh), cfg_(&cfg), initialized_(false)
+{
+}
+
+void TebVisualization::publishGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan) const
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+ publishPlan(global_plan, global_plan_pub_.get());
+}
+
+void TebVisualization::publishLocalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& local_plan) const
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+ publishPlan(local_plan, local_plan_pub_.get());
+}
+
+void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+
+ // create path msg
+ nav_msgs::msg::Path teb_path;
+ teb_path.header.frame_id = cfg_->map_frame;
+ teb_path.header.stamp = nh_->now();
+
+ // create pose_array (along trajectory)
+ geometry_msgs::msg::PoseArray teb_poses;
+ teb_poses.header.frame_id = teb_path.header.frame_id;
+ teb_poses.header.stamp = teb_path.header.stamp;
+
+ // fill path msgs with teb configurations
+ for (int i=0; i < teb.sizePoses(); i++)
+ {
+ geometry_msgs::msg::PoseStamped pose;
+
+ pose.header.frame_id = teb_path.header.frame_id;
+ pose.header.stamp = teb_path.header.stamp;
+ teb.Pose(i).toPoseMsg(pose.pose);
+ pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*teb.getSumOfTimeDiffsUpToIdx(i);
+ teb_path.poses.push_back(pose);
+ teb_poses.poses.push_back(pose.pose);
+ }
+ local_plan_pub_->publish(teb_path);
+ teb_poses_pub_->publish(teb_poses);
+}
+
+
+
+void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns,
+ const std_msgs::msg::ColorRGBA &color)
+{
+ if ( printErrorWhenNotInitialized() )
+ return;
+
+ std::vector<visualization_msgs::msg::Marker> markers;
+ robot_model.visualizeRobot(current_pose, markers, color);
+ if (markers.empty())
+ return;
+
+ int idx = 1000000; // avoid overshadowing by obstacles
+ for (std::vector<visualization_msgs::msg::Marker>::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx)
+ {
+ marker_it->header.frame_id = cfg_->map_frame;
+ marker_it->header.stamp = nh_->now();
+ marker_it->action = visualization_msgs::msg::Marker::ADD;
+ marker_it->ns = ns;
+ marker_it->id = idx;
+ marker_it->lifetime = rclcpp::Duration(2, 0);
+ teb_marker_pub_->publish(*marker_it);
+ }
+
+}
+
+void TebVisualization::publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model)
+{
+ publishRobotFootprintModel(current_pose, robot_model, "InfeasibleRobotPoses", toColorMsg(0.5, 0.8, 0.0, 0.0));
+}
+
+
+void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
+{
+ if ( obstacles.empty() || printErrorWhenNotInitialized() )
+ return;
+
+ // Visualize point obstacles
+ {
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = "PointObstacles";
+ marker.id = 0;
+ marker.type = visualization_msgs::msg::Marker::POINTS;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.lifetime = rclcpp::Duration(2, 0);
+ marker.pose.orientation.w = 1.0;
+
+ for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
+ {
+ std::shared_ptr<PointObstacle> pobst = std::dynamic_pointer_cast<PointObstacle>(*obst);
+ if (!pobst)
+ continue;
+
+ if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = pobst->x();
+ point.y = pobst->y();
+ point.z = 0;
+ marker.points.push_back(point);
+ }
+ else // Spatiotemporally point obstacles become a line
+ {
+ marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+ geometry_msgs::msg::Point start;
+ start.x = pobst->x();
+ start.y = pobst->y();
+ start.z = 0;
+ marker.points.push_back(start);
+
+ geometry_msgs::msg::Point end;
+ double t = 20;
+ Eigen::Vector2d pred;
+ pobst->predictCentroidConstantVelocity(t, pred);
+ end.x = pred[0];
+ end.y = pred[1];
+ end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*t;
+ marker.points.push_back(end);
+ }
+ }
+
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.color.a = 1.0;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+ }
+
+ // Visualize circular obstacles
+ {
+ std::size_t idx = 0;
+ for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
+ {
+ std::shared_ptr<CircularObstacle> pobst = std::dynamic_pointer_cast<CircularObstacle>(*obst);
+ if (!pobst)
+ continue;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = "CircularObstacles";
+ marker.id = idx++;
+ marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.lifetime = rclcpp::Duration(2, 0);
+ geometry_msgs::msg::Point point;
+ point.x = pobst->x();
+ point.y = pobst->y();
+ point.z = 0;
+ marker.points.push_back(point);
+
+ marker.scale.x = pobst->radius();
+ marker.scale.y = pobst->radius();
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+ }
+ }
+
+ // Visualize line obstacles
+ {
+ std::size_t idx = 0;
+ for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
+ {
+ std::shared_ptr<LineObstacle> pobst = std::dynamic_pointer_cast<LineObstacle>(*obst);
+ if (!pobst)
+ continue;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = "LineObstacles";
+ marker.id = idx++;
+ marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.lifetime = rclcpp::Duration(2, 0);
+ marker.pose.orientation.w = 1.0;
+
+ geometry_msgs::msg::Point start;
+ start.x = pobst->start().x();
+ start.y = pobst->start().y();
+ start.z = 0;
+ marker.points.push_back(start);
+ geometry_msgs::msg::Point end;
+ end.x = pobst->end().x();
+ end.y = pobst->end().y();
+ end.z = 0;
+ marker.points.push_back(end);
+
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+ }
+ }
+
+
+ // Visualize polygon obstacles
+ {
+ std::size_t idx = 0;
+ for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
+ {
+ std::shared_ptr<PolygonObstacle> pobst = std::dynamic_pointer_cast<PolygonObstacle>(*obst);
+ if (!pobst)
+ continue;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = "PolyObstacles";
+ marker.id = idx++;
+ marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.lifetime = rclcpp::Duration(2, 0);
+ marker.pose.orientation.w = 1.0;
+
+ for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = vertex->x();
+ point.y = vertex->y();
+ point.z = 0;
+ marker.points.push_back(point);
+ }
+
+ // Also add last point to close the polygon
+ // but only if polygon has more than 2 points (it is not a line)
+ if (pobst->vertices().size() > 2)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = pobst->vertices().front().x();
+ point.y = pobst->vertices().front().y();
+ point.z = 0;
+ marker.points.push_back(point);
+ }
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.color.a = 1.0;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+ }
+ }
+}
+
+
+void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
+{
+ if ( via_points.empty() || printErrorWhenNotInitialized() )
+ return;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = ns;
+ marker.id = 0;
+ marker.type = visualization_msgs::msg::Marker::POINTS;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.lifetime = rclcpp::Duration(2, 0);
+ marker.pose.orientation.w = 1.0;
+
+ for (std::size_t i=0; i < via_points.size(); ++i)
+ {
+ geometry_msgs::msg::Point point;
+ point.x = via_points[i].x();
+ point.y = via_points[i].y();
+ point.z = 0;
+ marker.points.push_back(point);
+ }
+
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.color.a = 1.0;
+ marker.color.r = 0.0;
+ marker.color.g = 0.0;
+ marker.color.b = 1.0;
+
+ teb_marker_pub_->publish( marker );
+}
+
+void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns)
+{
+if ( printErrorWhenNotInitialized() )
+ return;
+
+ visualization_msgs::msg::Marker marker;
+ marker.header.frame_id = cfg_->map_frame;
+ marker.header.stamp = nh_->now();
+ marker.ns = ns;
+ marker.id = 0;
+ marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.pose.orientation.w = 1.0;
+
+ // Iterate through teb pose sequence
+ for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb )
+ {
+ // iterate single poses
+ PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin();
+ TimeDiffSequence::const_iterator it_timediff = it_teb->get()->teb().timediffs().begin();
+ PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end();
+ std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one.
+ double time = 0;
+
+ while (it_pose != it_pose_end)
+ {
+ geometry_msgs::msg::Point point_start;
+ point_start.x = (*it_pose)->x();
+ point_start.y = (*it_pose)->y();
+ point_start.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
+ marker.points.push_back(point_start);
+
+ time += (*it_timediff)->dt();
+
+ geometry_msgs::msg::Point point_end;
+ point_end.x = (*boost::next(it_pose))->x();
+ point_end.y = (*boost::next(it_pose))->y();
+ point_end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
+ marker.points.push_back(point_end);
+ ++it_pose;
+ ++it_timediff;
+ }
+ }
+ marker.scale.x = 0.01;
+ marker.color.a = 1.0;
+ marker.color.r = 0.5;
+ marker.color.g = 1.0;
+ marker.color.b = 0.0;
+
+ teb_marker_pub_->publish( marker );
+}
+
+void TebVisualization::publishFeedbackMessage(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planners,
+ unsigned int selected_trajectory_idx, const ObstContainer& obstacles)
+{
+ teb_msgs::msg::FeedbackMsg msg;
+ msg.header.stamp = nh_->now();
+ msg.header.frame_id = cfg_->map_frame;
+ msg.selected_trajectory_idx = selected_trajectory_idx;
+
+
+ msg.trajectories.resize(teb_planners.size());
+
+ // Iterate through teb pose sequence
+ std::size_t idx_traj = 0;
+ for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj )
+ {
+ msg.trajectories[idx_traj].header = msg.header;
+ it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory);
+ }
+
+ // add obstacles
+ msg.obstacles_msg.obstacles.resize(obstacles.size());
+ for (std::size_t i=0; i<obstacles.size(); ++i)
+ {
+ msg.obstacles_msg.header = msg.header;
+
+ // copy polygon
+ msg.obstacles_msg.obstacles[i].header = msg.header;
+ obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
+
+ // copy id
+ msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
+
+ // orientation
+ //msg.obstacles_msg.obstacles[i].orientation =; // TODO
+
+ // copy velocities
+ obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
+ }
+
+ feedback_pub_->publish(msg);
+}
+
+void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
+{
+ teb_msgs::msg::FeedbackMsg msg;
+ msg.header.stamp = nh_->now();
+ msg.header.frame_id = cfg_->map_frame;
+ msg.selected_trajectory_idx = 0;
+
+ msg.trajectories.resize(1);
+ msg.trajectories.front().header = msg.header;
+ teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
+
+ // add obstacles
+ msg.obstacles_msg.obstacles.resize(obstacles.size());
+ for (std::size_t i=0; i<obstacles.size(); ++i)
+ {
+ msg.obstacles_msg.header = msg.header;
+
+ // copy polygon
+ msg.obstacles_msg.obstacles[i].header = msg.header;
+ obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
+
+ // copy id
+ msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
+
+ // orientation
+ //msg.obstacles_msg.obstacles[i].orientation =; // TODO
+
+ // copy velocities
+ obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
+ }
+
+ feedback_pub_->publish(msg);
+}
+
+std_msgs::msg::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b)
+{
+ std_msgs::msg::ColorRGBA color;
+ color.a = a;
+ color.r = r;
+ color.g = g;
+ color.b = b;
+ return color;
+}
+
+bool TebVisualization::printErrorWhenNotInitialized() const
+{
+ if (!initialized_)
+ {
+ RCLCPP_ERROR(nh_->get_logger(), "TebVisualization class not initialized. You must call initialize or an appropriate constructor");
+ return true;
+ }
+ return false;
+}
+
+nav2_util::CallbackReturn TebVisualization::on_configure()
+{
+ // register topics
+ global_plan_pub_ = nh_->create_publisher<nav_msgs::msg::Path>("global_plan", 1);;
+ local_plan_pub_ = nh_->create_publisher<nav_msgs::msg::Path>("local_plan",1);
+ teb_poses_pub_ = nh_->create_publisher<geometry_msgs::msg::PoseArray>("teb_poses", 1);
+ teb_marker_pub_ = nh_->create_publisher<visualization_msgs::msg::Marker>("teb_markers", 1);
+ feedback_pub_ = nh_->create_publisher<teb_msgs::msg::FeedbackMsg>("teb_feedback", 1);
+
+ initialized_ = true;
+ return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+TebVisualization::on_activate()
+{
+ global_plan_pub_->on_activate();
+ local_plan_pub_->on_activate();
+ teb_poses_pub_->on_activate();
+ teb_marker_pub_->on_activate();
+ feedback_pub_->on_activate();
+ return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+TebVisualization::on_deactivate()
+{
+ global_plan_pub_->on_deactivate();
+ local_plan_pub_->on_deactivate();
+ teb_poses_pub_->on_deactivate();
+ teb_marker_pub_->on_deactivate();
+ feedback_pub_->on_deactivate();
+ return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+TebVisualization::on_cleanup()
+{
+ global_plan_pub_.reset();
+ local_plan_pub_.reset();
+ teb_poses_pub_.reset();
+ teb_marker_pub_.reset();
+ feedback_pub_.reset();
+
+ return nav2_util::CallbackReturn::SUCCESS;
+}
+
+} // namespace teb_local_planner
diff --git a/src/teb_local_planner/teb_local_planner_plugin.xml b/src/teb_local_planner/teb_local_planner_plugin.xml
new file mode 100644
index 0000000..45d0bdf
--- /dev/null
+++ b/src/teb_local_planner/teb_local_planner_plugin.xml
@@ -0,0 +1,9 @@
+<library path="teb_local_planner">
+ <class type="teb_local_planner::TebLocalPlannerROS" base_class_type="nav2_core::Controller">
+ <description>
+ The teb_local_planner package implements a plugin
+ to the base_local_planner of the 2D navigation stack.
+ </description>
+ </class>
+</library>
+
diff --git a/src/teb_local_planner/test/homotopy_class_planner_test.cpp b/src/teb_local_planner/test/homotopy_class_planner_test.cpp
new file mode 100644
index 0000000..8f9251b
--- /dev/null
+++ b/src/teb_local_planner/test/homotopy_class_planner_test.cpp
@@ -0,0 +1,82 @@
+#include "teb_local_planner/homotopy_class_planner.h"
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+class HomotopyClassPlannerTest : public teb_local_planner::HomotopyClassPlanner {
+ public:
+ void SetUp(rclcpp::Node::SharedPtr node) {
+ teb_local_planner::RobotFootprintModelPtr robot_model;
+ teb_local_planner::TebVisualizationPtr visualization;
+ teb_local_planner::HomotopyClassPlannerPtr homotopy_class_planner;
+
+ robot_model.reset(new teb_local_planner::CircularRobotFootprint(0.25));
+
+ obstacles.push_back(
+ teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 2))
+ );
+
+ obstacles.push_back(
+ teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 3))
+ );
+
+ obstacles.push_back(
+ teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 4))
+ );
+
+ visualization.reset(
+ new teb_local_planner::TebVisualization(node, cfg)
+ );
+
+ cfg.hcp.visualize_hc_graph = true;
+
+ initialize(node, cfg, &obstacles, robot_model, visualization);
+ }
+ teb_local_planner::ObstContainer obstacles;
+ teb_local_planner::TebConfig cfg;
+};
+
+TEST(test, test) {
+ HomotopyClassPlannerTest test;
+ rclcpp::Node::SharedPtr node( new rclcpp::Node("test") );
+ test.SetUp(node);
+
+ using namespace teb_local_planner;
+ PoseSE2 start(0, 0, 0);
+ PoseSE2 goal(5, 5, 0);
+ geometry_msgs::msg::Twist twist;
+ twist.linear.x = 0;
+ twist.linear.y = 0;
+ twist.angular.z = 0;
+
+ std::vector<geometry_msgs::msg::PoseStamped> initial_plan;
+
+ geometry_msgs::msg::PoseStamped start_pose;
+ geometry_msgs::msg::PoseStamped goal_pose;
+
+ start.toPoseMsg(start_pose.pose);
+ goal.toPoseMsg(goal_pose.pose);
+
+ initial_plan.push_back(start_pose);
+ initial_plan.push_back(goal_pose);
+
+ test.plan(initial_plan, &twist, false);
+ //homotopy_class_planner_->exploreEquivalenceClassesAndInitTebs(start, goal, 0.3, &twist);
+
+ rclcpp::Rate rate(10);
+ while(rclcpp::ok()) {
+ test.visualize();
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ ASSERT_TRUE(true);
+}
+
+int main(int argc, char** argv) {
+ rclcpp::init(argc, argv);
+
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/src/teb_local_planner/test/teb_basics.cpp b/src/teb_local_planner/test/teb_basics.cpp
new file mode 100644
index 0000000..b490974
--- /dev/null
+++ b/src/teb_local_planner/test/teb_basics.cpp
@@ -0,0 +1,73 @@
+#include <gtest/gtest.h>
+
+#include "teb_local_planner/timed_elastic_band.h"
+
+TEST(TEBBasic, autoResizeLargeValueAtEnd)
+{
+ double dt = 0.1;
+ double dt_hysteresis = dt/3.;
+ teb_local_planner::TimedElasticBand teb;
+
+ teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+ for (int i = 1; i < 10; ++i) {
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+ }
+ // add a pose with a large timediff as the last one
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2*dt_hysteresis);
+
+ // auto resize + test of the result
+ teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+ for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+ ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+ ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+ }
+}
+
+TEST(TEBBasic, autoResizeSmallValueAtEnd)
+{
+ double dt = 0.1;
+ double dt_hysteresis = dt/3.;
+ teb_local_planner::TimedElasticBand teb;
+
+ teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+ for (int i = 1; i < 10; ++i) {
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+ }
+ // add a pose with a small timediff as the last one
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
+
+ // auto resize + test of the result
+ teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+ for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+ ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+ ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+ }
+}
+
+TEST(TEBBasic, autoResize)
+{
+ double dt = 0.1;
+ double dt_hysteresis = dt/3.;
+ teb_local_planner::TimedElasticBand teb;
+
+ teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+ for (int i = 1; i < 10; ++i) {
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+ }
+ // modify the timediff in the middle and add a pose with a smaller timediff as the last one
+ teb.TimeDiff(5) = dt + 2*dt_hysteresis;
+ teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
+
+ // auto resize
+ teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+ for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+ ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+ ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/teb_msgs/CMakeLists.txt b/src/teb_msgs/CMakeLists.txt
new file mode 100644
index 0000000..b3ad44e
--- /dev/null
+++ b/src/teb_msgs/CMakeLists.txt
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(teb_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(costmap_converter_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(teb_msgs
+ "msg/FeedbackMsg.msg"
+ "msg/TrajectoryMsg.msg"
+ "msg/TrajectoryPointMsg.msg"
+ DEPENDENCIES builtin_interfaces costmap_converter_msgs geometry_msgs std_msgs
+)
+
+ament_package()
diff --git a/src/teb_msgs/msg/FeedbackMsg.msg b/src/teb_msgs/msg/FeedbackMsg.msg
new file mode 100644
index 0000000..aa97c96
--- /dev/null
+++ b/src/teb_msgs/msg/FeedbackMsg.msg
@@ -0,0 +1,15 @@
+# Message that contains intermediate results
+# and diagnostics of the (predictive) planner.
+
+std_msgs/Header header
+
+# The planned trajectory (or if multiple plans exist, all of them)
+teb_msgs/TrajectoryMsg[] trajectories
+
+# Index of the trajectory in 'trajectories' that is selected currently
+uint16 selected_trajectory_idx
+
+# List of active obstacles
+costmap_converter_msgs/ObstacleArrayMsg obstacles_msg
+
+
diff --git a/src/teb_msgs/msg/TrajectoryMsg.msg b/src/teb_msgs/msg/TrajectoryMsg.msg
new file mode 100644
index 0000000..14bf608
--- /dev/null
+++ b/src/teb_msgs/msg/TrajectoryMsg.msg
@@ -0,0 +1,6 @@
+# Message that contains a trajectory for mobile robot navigation
+
+std_msgs/Header header
+teb_msgs/TrajectoryPointMsg[] trajectory
+
+
diff --git a/src/teb_msgs/msg/TrajectoryPointMsg.msg b/src/teb_msgs/msg/TrajectoryPointMsg.msg
new file mode 100644
index 0000000..47b186a
--- /dev/null
+++ b/src/teb_msgs/msg/TrajectoryPointMsg.msg
@@ -0,0 +1,21 @@
+# Message that contains single point on a trajectory suited for mobile navigation.
+# The trajectory is described by a sequence of poses, velocities,
+# accelerations and temporal information.
+
+# Why this message type?
+# nav_msgs/Path describes only a path without temporal information.
+# trajectory_msgs package contains only messages for joint trajectories.
+
+# The pose of the robot
+geometry_msgs/Pose pose
+
+# Corresponding velocity
+geometry_msgs/Twist velocity
+
+# Corresponding acceleration
+geometry_msgs/Twist acceleration
+
+builtin_interfaces/Duration time_from_start
+
+
+
diff --git a/src/teb_msgs/package.xml b/src/teb_msgs/package.xml
new file mode 100644
index 0000000..9cfb7d0
--- /dev/null
+++ b/src/teb_msgs/package.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>teb_msgs</name>
+ <version>0.0.1</version>
+ <description>A package containing message definitions for teb_local_planner.</description>
+ <maintainer email="vinnam.kim@gmail.com">Vinnam Kim</maintainer>
+ <license>Apache License 2.0</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+
+ <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+ <build_depend>builtin_interfaces</build_depend>
+ <build_depend>costmap_converter_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+
+ <exec_depend>builtin_interfaces</exec_depend>
+ <exec_depend>costmap_converter_msgs</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>rosidl_default_runtime</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+ <test_depend>ament_lint_common</test_depend>
+
+ <member_of_group>rosidl_interface_packages</member_of_group>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
\ No newline at end of file
--
GitLab
From a06887545fe10b92b84d297fc9ef5b7b6c195804 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Tue, 16 Apr 2024 17:44:44 +0300
Subject: [PATCH 38/64] costmap converter added as submodule
---
.gitmodules | 4 ++++
src/costmap_converter | 1 +
2 files changed, 5 insertions(+)
create mode 160000 src/costmap_converter
diff --git a/.gitmodules b/.gitmodules
index 6d43e0e..6398ca2 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,3 +4,7 @@
[submodule "src/yolo_openvino_ros2"]
path = src/yolo_openvino_ros2
url = https://wavegit.mipt.ru/starkit/yolo_openvino_ros2.git
+[submodule "src/costmap_converter"]
+ path = src/costmap_converter
+ url = https://github.com/rst-tu-dortmund/costmap_converter.git
+ branch = ros2
diff --git a/src/costmap_converter b/src/costmap_converter
new file mode 160000
index 0000000..9565858
--- /dev/null
+++ b/src/costmap_converter
@@ -0,0 +1 @@
+Subproject commit 9565858432664e9cfe0b3556e1809336c4ab06d4
--
GitLab
From c817f58bb5f39744eb7c009048736f85e37bae69 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Tue, 16 Apr 2024 17:57:25 +0300
Subject: [PATCH 39/64] teb local planner added as submodule
---
.gitmodules | 4 +
src/teb_local_planner | 1 +
src/teb_local_planner/CHANGELOG.rst | 407 -----
src/teb_local_planner/CMakeLists.txt | 162 --
src/teb_local_planner/LICENSE | 28 -
.../cfg/TebLocalPlannerReconfigure.cfg | 420 ------
.../cfg/rviz_test_optim.rviz | 183 ---
.../cmake_modules/FindG2O.cmake | 97 --
.../cmake_modules/FindSUITESPARSE.cmake | 133 --
.../teb_local_planner/distance_calculations.h | 464 ------
.../teb_local_planner/equivalence_relations.h | 103 --
.../g2o_types/base_teb_edges.h | 278 ----
.../g2o_types/edge_acceleration.h | 732 ---------
.../g2o_types/edge_dynamic_obstacle.h | 154 --
.../g2o_types/edge_kinematics.h | 231 ---
.../g2o_types/edge_obstacle.h | 295 ----
.../g2o_types/edge_prefer_rotdir.h | 115 --
.../g2o_types/edge_shortest_path.h | 88 --
.../g2o_types/edge_time_optimal.h | 117 --
.../g2o_types/edge_velocity.h | 275 ----
.../g2o_types/edge_velocity_obstacle_ratio.h | 127 --
.../g2o_types/edge_via_point.h | 121 --
.../teb_local_planner/g2o_types/penalties.h | 193 ---
.../teb_local_planner/g2o_types/vertex_pose.h | 229 ---
.../g2o_types/vertex_timediff.h | 145 --
.../include/teb_local_planner/graph_search.h | 215 ---
.../include/teb_local_planner/h_signature.h | 432 ------
.../homotopy_class_planner.h | 591 --------
.../homotopy_class_planner.hpp | 95 --
.../include/teb_local_planner/misc.h | 210 ---
.../include/teb_local_planner/obstacles.h | 1112 --------------
.../teb_local_planner/optimal_planner.h | 736 ---------
.../teb_local_planner/planner_interface.h | 218 ---
.../include/teb_local_planner/pose_se2.h | 433 ------
.../teb_local_planner/recovery_behaviors.h | 134 --
.../teb_local_planner/robot_footprint_model.h | 684 ---------
.../include/teb_local_planner/teb_config.h | 432 ------
.../teb_local_planner/teb_local_planner_ros.h | 426 ------
.../teb_local_planner/timed_elastic_band.h | 636 --------
.../teb_local_planner/timed_elastic_band.hpp | 191 ---
.../include/teb_local_planner/visualization.h | 272 ----
.../teb_local_planner/visualization.hpp | 227 ---
src/teb_local_planner/package.xml | 53 -
src/teb_local_planner/params/teb_params.yaml | 109 --
.../scripts/cmd_vel_to_ackermann_drive.py | 60 -
.../scripts/export_to_mat.py | 112 --
.../scripts/export_to_svg.py | 244 ---
.../scripts/publish_dynamic_obstacle.py | 67 -
.../scripts/publish_test_obstacles.py | 76 -
.../scripts/publish_viapoints.py | 46 -
.../scripts/visualize_velocity_profile.py | 76 -
src/teb_local_planner/src/graph_search.cpp | 346 -----
.../src/homotopy_class_planner.cpp | 854 -----------
src/teb_local_planner/src/obstacles.cpp | 216 ---
src/teb_local_planner/src/optimal_planner.cpp | 1326 -----------------
.../src/recovery_behaviors.cpp | 118 --
src/teb_local_planner/src/teb_config.cpp | 886 -----------
.../src/teb_local_planner_ros.cpp | 1122 --------------
src/teb_local_planner/src/test_optim_node.cpp | 322 ----
.../src/timed_elastic_band.cpp | 630 --------
src/teb_local_planner/src/visualization.cpp | 563 -------
.../teb_local_planner_plugin.xml | 9 -
.../test/homotopy_class_planner_test.cpp | 82 -
src/teb_local_planner/test/teb_basics.cpp | 73 -
src/teb_msgs/CMakeLists.txt | 28 -
src/teb_msgs/msg/FeedbackMsg.msg | 15 -
src/teb_msgs/msg/TrajectoryMsg.msg | 6 -
src/teb_msgs/msg/TrajectoryPointMsg.msg | 21 -
src/teb_msgs/package.xml | 32 -
69 files changed, 5 insertions(+), 19633 deletions(-)
create mode 160000 src/teb_local_planner
delete mode 100644 src/teb_local_planner/CHANGELOG.rst
delete mode 100644 src/teb_local_planner/CMakeLists.txt
delete mode 100644 src/teb_local_planner/LICENSE
delete mode 100755 src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
delete mode 100644 src/teb_local_planner/cfg/rviz_test_optim.rviz
delete mode 100644 src/teb_local_planner/cmake_modules/FindG2O.cmake
delete mode 100644 src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
delete mode 100644 src/teb_local_planner/include/teb_local_planner/distance_calculations.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/graph_search.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/h_signature.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
delete mode 100644 src/teb_local_planner/include/teb_local_planner/misc.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/obstacles.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/optimal_planner.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/planner_interface.h
delete mode 100755 src/teb_local_planner/include/teb_local_planner/pose_se2.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/teb_config.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
delete mode 100644 src/teb_local_planner/include/teb_local_planner/visualization.h
delete mode 100644 src/teb_local_planner/include/teb_local_planner/visualization.hpp
delete mode 100644 src/teb_local_planner/package.xml
delete mode 100644 src/teb_local_planner/params/teb_params.yaml
delete mode 100755 src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
delete mode 100755 src/teb_local_planner/scripts/export_to_mat.py
delete mode 100755 src/teb_local_planner/scripts/export_to_svg.py
delete mode 100755 src/teb_local_planner/scripts/publish_dynamic_obstacle.py
delete mode 100755 src/teb_local_planner/scripts/publish_test_obstacles.py
delete mode 100755 src/teb_local_planner/scripts/publish_viapoints.py
delete mode 100755 src/teb_local_planner/scripts/visualize_velocity_profile.py
delete mode 100644 src/teb_local_planner/src/graph_search.cpp
delete mode 100644 src/teb_local_planner/src/homotopy_class_planner.cpp
delete mode 100644 src/teb_local_planner/src/obstacles.cpp
delete mode 100644 src/teb_local_planner/src/optimal_planner.cpp
delete mode 100644 src/teb_local_planner/src/recovery_behaviors.cpp
delete mode 100644 src/teb_local_planner/src/teb_config.cpp
delete mode 100644 src/teb_local_planner/src/teb_local_planner_ros.cpp
delete mode 100644 src/teb_local_planner/src/test_optim_node.cpp
delete mode 100644 src/teb_local_planner/src/timed_elastic_band.cpp
delete mode 100644 src/teb_local_planner/src/visualization.cpp
delete mode 100644 src/teb_local_planner/teb_local_planner_plugin.xml
delete mode 100644 src/teb_local_planner/test/homotopy_class_planner_test.cpp
delete mode 100644 src/teb_local_planner/test/teb_basics.cpp
delete mode 100644 src/teb_msgs/CMakeLists.txt
delete mode 100644 src/teb_msgs/msg/FeedbackMsg.msg
delete mode 100644 src/teb_msgs/msg/TrajectoryMsg.msg
delete mode 100644 src/teb_msgs/msg/TrajectoryPointMsg.msg
delete mode 100644 src/teb_msgs/package.xml
diff --git a/.gitmodules b/.gitmodules
index 6398ca2..31d07d2 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -8,3 +8,7 @@
path = src/costmap_converter
url = https://github.com/rst-tu-dortmund/costmap_converter.git
branch = ros2
+[submodule "src/teb_local_planner"]
+ path = src/teb_local_planner
+ url = https://github.com/rst-tu-dortmund/teb_local_planner.git
+ branch = ros2-master
diff --git a/src/teb_local_planner b/src/teb_local_planner
new file mode 160000
index 0000000..630a22e
--- /dev/null
+++ b/src/teb_local_planner
@@ -0,0 +1 @@
+Subproject commit 630a22e88dc9fd45be726a762edbb5b776bef231
diff --git a/src/teb_local_planner/CHANGELOG.rst b/src/teb_local_planner/CHANGELOG.rst
deleted file mode 100644
index 11b4f73..0000000
--- a/src/teb_local_planner/CHANGELOG.rst
+++ /dev/null
@@ -1,407 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package teb_local_planner
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.9.1 (2020-05-29)
-------------------
-* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang)
-* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro)
-* CMake: Removed system include to avoid compiling issues on some platforms
-* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro
-
-0.9.0 (2020-05-26)
-------------------
-* Added pill resp. stadium-shaped obstacle
-* Changed minimum CMake version to 3.1
-* Improved efficiency of 3d h-signature computation
-* Changed default value for parameter penalty_epsilon to 0.05
-* Improved efficiency of findClosedTrajectoryPose()
-* Removed obsolete method isHorizonReductionAppropriate()
-* Contributors: Christoph Rösmann, XinyuKhan
-
-0.8.4 (2019-12-02)
-------------------
-* Fixed TEB autoResize if last TimeDiff is small
-* Add a rotational threshold for identifying a warm start goal
-* Contributors: Rainer Kümmerle
-
-0.8.3 (2019-10-25)
-------------------
-* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa)
-* test_optim_node fix circular obstacles (thanks to dtaranta)
-* Fix shadow variable warning (thanks to Victor Lopez)
-* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez)
-* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle)
-* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa)
-* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer.
- Fixes `#158 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/158>`_.
-* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot)
-* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot)
-* added warning if parameter optimal_time is <= 0
-* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle.
- See `#140 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/140>`_.
-* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server
-* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta
-
-0.8.2 (2019-07-02)
-------------------
-* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash)
-* Add nonlinear part to obstacle cost to improve narrow gap behavior.
- Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term.
- The default linear behavior is achieved by setting this parameter to 1 (default).
- A value of 4 performed well in some tests and experiments (thanks to Howard Cochran).
-* Parameter `global_plan_prune_distance` added via ros parameter server.
-* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran)
-* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran)
-* On footprint visualization, avoid overshadowing by obstacles (thanks to corot)
-* Do not ignore robot model on the association stage.
- Important mostly for polygon footprint model (thanks to corot).
-* Adjustable color for footprint visualization
-* Showing (detected) infeasible robot poses in a separate marker namespace and color
-* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`)
-* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew)
-* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU)
-* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa)
-* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa).
-* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa)
-* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa)
-* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights.
- As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran).
-* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas)
-* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa)
-* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading.
- Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb
- and if a teb cannot be optimized (thanks to Marco Bassa).
- Optionally allowing the usage of the initial plan orientation when initializing new tebs.
-* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot
-
-0.8.1 (2018-08-14)
-------------------
-* bugfix in calculateHSignature. Fixes `#90 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/90>`_.
-* fixed centroid computation in a special case of polygon-obstacles
-* Contributors: Christoph Rösmann
-
-0.8.0 (2018-08-06)
-------------------
-* First melodic release
-* Updated to new g2o API
-* Migration to tf2
-* Contributors: Christoph Rösmann
-
-0.7.3 (2018-07-05)
-------------------
-* Parameter `switching_blocking_period` added to homotopy class planner parameter group.
- Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon
- as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds
- by default in order to not change the behavior of existing configurations.
-* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`.
- The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`.
- `global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name
- in the code as well as `weight_viapoint` and the ros wiki description.
- In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed.
-* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap:
- In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start)
- and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist).
- Previously, the search algorithm stopped as soon as the distance to the robot increased once.
- This caused troubles with more complex global plans, hence the new strategy checks the complete subset
- of the global plan in the local costmap for the closest distance to the robot.
-* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode)
-* Edge creation: minor performance improvement for dynamic obstacle edges
-* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory
-* Contributors: Christoph Rösmann
-
-0.7.2 (2018-06-08)
-------------------
-* Adds the possibility to provide via-points via a topic.
- Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan
- (e.g., activate the latter by setting global_plan_viapoint_sep>0 as before).
- A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node.
-* Contributors: Christoph Rösmann
-
-0.7.1 (2018-06-05)
-------------------
-* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default.
- This cost function was only intended to be active only for recovering from an oscillating robot.
- This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached.
- Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor
- (huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals).
-* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg
-* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...)
-* Update to use non deprecated pluginlib macro
-* Python scripts updated to new obstacle message definition.
-* Fixed issue when start and end are at the same location (PR #43)
-* Normalize marker quaternions in *test_optim_node*
-* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip
-
-0.7.0 (2017-09-23)
-------------------
-* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code).
- Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated.
- Note, this feature is still experimental and subject to testing.
- Motion prediction is performed using a constant velocity model.
- Dynamic obstacles might be incorporated as follows:
- * via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg).
- * via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental).
- A tutorial is going to be provided soon.
-* FeedbackMsg includes a ObstacleMsg instead of a polygon
-* ObstacleMsg removed from package since it is now part of the costmap\_converter package.
-* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now
- implemented as subclasses of more general interfaces.
-* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start.
- Old methods are marked as deprecated. This change does not affect users settings.
-* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer
-* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format
-* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true.
- We introduced the non-fast mode with the support of dynamic obstacles
- (which leads to better results in terms of x-y-t homotopy planning).
- However, we have not yet tested this mode intensively, so we keep
- the previous mode as default until we finish our tests.
-* added parameter and code to update costmap footprint if it is dynamic (#49)
-* Contributors: Franz Albers, Christoph Rösmann, procopiostein
-
-0.6.6 (2016-12-23)
-------------------
-* Strategy for recovering from oscillating local plans added (see new parameters)
-* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected
- (to avoid oscillations due to changing final orientations)
-* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true
-* Parameter max_samples added
-* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches)
-
-0.6.5 (2016-11-15)
-------------------
-* The trajectory is now initialized backwards for goals close to and behind the robot.
- Parameter 'allow_init_with_backwards_motion' added.
-* Updated the TEB selection in the HomotopyClassPlanner.
- * A new parameter is introduced to prefer the equivalence class of the initial plan
- * Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan.
-* Weight adaptation added for obstacles edges.
- Added parameter 'weight_adapt_factor'.
- Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration.
- Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions.
-* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint.
- Validation is performed by only comparing the inscribed radii of the footprints.
-* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected.
-* Changed HSignature to a generic equivalence class
-* Minor changes
-
-0.6.4 (2016-10-23)
-------------------
-* New default obstacle association strategy:
- During optimization graph creation, for each pose of the trajectory a
- relevance detection is performed before considering the obstacle
- during optimization. New parameters are introduced. The
- old strategy is kept as 'legacy' strategy (see parameters).
-* Computation of velocities, acceleration and turning radii extended:
- Added an option to compute the actual arc length
- instead of using the Euclidean distance approximation (see parameter `exact_arc_length`.
-* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy.
-* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server
-* Removed TebConfig dependency in TebVisualization
-* PolygonObstacle can now be constructed using a vertices container
-* HomotopyClassPlanner public interface extended
-* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2
-* Removed inline flags in visualization.cpp
-* Removed inline flags in timed_elastic_band.cpp.
- Fixes `#15 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/15>`_.
-* Increased bounds of many variables in dynamic_reconfigure.
- Resolves `#14 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/14>`_.
- The particular variables are maximum velocities, maximum accelerations,
- minimum turning radius,...
- Note: optimization weights and dt_ref as well as dt_hyst are not
- tuned for velocities and accelerations beyond
- the default values (e.g. >1 m/s). Just increasing the maximum velocity
- bounds without adjusting the other parameters leads to an insufficient behavior.
-* Default parameter value update: 'costmap_obstacles_behind_robot_dist'
-* Additional minor fixes.
-
-0.6.3 (2016-08-17)
-------------------
-* Changed the f0 function for calculating the H-Signature.
- The new one seems to be more robust for a much larger number of obstacles
- after some testing.
-* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again
-* Fixed distance calculation polygon-to-polygon-obstacle
-* cmake config exports now *include directories* of external packages for dependent projects
-* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13.
-
-
-0.6.2 (2016-06-15)
-------------------
-* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error.
-* Inflation mode for obstacles added.
-* The homotopy class of the global plan is now always forced to be initialized as trajectory.
-* The initial velocity of the robot is now taken into account correctly for
- all candidate trajectories.
-* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle.
- This fix addresses issue `#7 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/7>`_
-
-0.6.1 (2016-05-23)
-------------------
-* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue)
-
-
-0.6.0 (2016-05-22)
-------------------
-* Extended support to holonomic robots
-* Wrong parameter namespace for *costmap_converter* plugins fixed
-* Added the option to scale the length of the hcp sampling area
-* Compiler warnings fixed.
-* Workaround for compilation issues that are caused by a bug in boost 1.58
- concerning the graph library (missing move constructor/assignment operator
- in boost source).
-* Using *tf_listener* from *move_base* now.
-* Via-point support improved.
- Added the possibility to take the actual order of via-points into account.
- Additionally, via-points beyond start and goal are now included.
-* Obsolete include of the angles package header removed
-* Update to package.xml version 2
-* Some other minor fixes.
-
-
-0.4.0 (2016-04-19)
-------------------
-* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now.
- This allows the user to adapt the tradeoff between time-optimality and path-following.
- Check out the new tutorial: "Following the Global Plan (Via-Points)".
-* All external configuration and launch files are removed, since they are part
- of the new teb_local_planner_tutorials package.
-
-
-0.3.1 (2016-04-14)
-------------------
-* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models.
-* Trajectory selection strategy in case of multiple topologies updated:
- * The obstacle costs for selection can now be scaling separately.
- * The cost regarding time optimality can now be replaced by the actual transition time.
- * Added a hysteresis to cost comparison between a new and the previously selected trajectory.
- * In the default parameter setting the strategy is similar to release 0.3.0.
-* Warning message removed that occured if an odom message with only zeros was received.
-
-
-0.3.0 (2016-04-08)
-------------------
-* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!).
-* The new robot footprint is also visualized using the common marker topic.
-* The strategy of taking occupied costmap cells behind the robot into account has been improved.
- These changes significantly improve navigation close to walls.
-* Parameter 'max_global_plan_lookahead_dist' added.
- Previously, the complete subset of the global plan contained in the local costmap
- was taken into account for choosing the current intermediate goal point. With this parameter, the maximum
- length of the reference global plan can be limited. The actual global plan subset
- is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'.
-* Bug fixes:
- * Fixed a compilation issue on ARM architectures
- * If custom obstacles are used, the container with old obstacles is now cleared properly.
-* Parameter cleanup:
- * "weight_X_obstacle" parameters combined to single parameter "weight_obstacle".
- * "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected".
- * Deprecated parameter 'costmap_emergency_stop_dist' removed.
-* Code cleanup
-
-
-0.2.3 (2016-02-01)
-------------------
-* Marker lifetime changed
-* In case the local planner detects an infeasible trajectory it does now try to
- reduce the horizon to 50 percent of the length. The trajectory is only reduced
- if some predefined cases are detected.
- This mechanism constitutes a backup behavior.
-* Improved carlike robot support.
- Instead of commanding the robot using translational and rotational velocities,
- the robot might also be commanded using the transl. velocity and steering angle.
- Appropriate parameters are added to the config.
-* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision.
-* Some python scripts for data conversion added
-* Minor other changes
-
-0.2.2 (2016-01-11)
-------------------
-* Carlike robots (ackermann steering) are supported from now on (at least experimentally)
- by specifying a minimum bound on the turning radius.
- Currently, the output of the planner in carlike mode is still (v,omega).
- Since I don't have any real carlike robot, I would be really happy if someone could provide me with
- some feedback to further improve/extend the support.
-* Obstacle cost function modified to avoid undesired jerks in the trajectory.
-* Added a feedback message that contains current trajectory information (poses, velocities and temporal information).
- This is useful for analyzing and debugging the velocity profile e.g. at runtime.
- The message will be published only if it's activated (rosparam).
- A small python script is added to plot the velocity profile (while *test_optim_node* runs).
-* Cost functions are now taking the direction/sign of the translational velocity into account:
- Specifying a maximum backwards velocity other than forward velocity works now.
- Additionally, the change in acceleration is now computed correctly if the robot switches directions.
-* The global plan is now pruned such that already passed posses are cut off
- (relevant for global planners with *planning_rate=0*).
-* Fixed issue#1: If a global planner with *planning_rate=0* was used,
- a TF timing/extrapolation issue appeared after some time.
-* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results.
-
-
-0.2.1 (2015-12-30)
-------------------
-* This is an important bugfix release.
-* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if
- the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s).
- From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not.
-* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid).
-
-0.2.0 (2015-12-23)
-------------------
-* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes.
- The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.).
- A tutorial will explain how to activate the converter using the ros-param server.
-
-0.1.11 (2015-12-12)
--------------------
-* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal)
-
-0.1.10 (2015-08-13)
--------------------
-* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation.
-* Some bugfixes and improvements
-
-0.1.9 (2015-06-24)
-------------------
-* Fixed a segmentation fault issue. This minor update is crucial for stability.
-
-0.1.8 (2015-06-08)
-------------------
-* Custom obstacles can be included via publishing dedicated messages
-* Goal-reached-condition also checks orientation error (desired yaw) now
-* Numerical improvements of the h-signature calculation
-* Minor bugfixes
-
-0.1.7 (2015-05-22)
-------------------
-* Finally fixed saucy compilation issue by retaining compatiblity to newer distros
- (my "new" 13.10 VM helps me to stop spamming new releases for testing).
-
-0.1.6 (2015-05-22)
-------------------
-* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts.
- I am not able to test releasing on saucy, forcing me to release again and again. Sorry.
-
-0.1.5 (2015-05-21)
-------------------
-* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure.
-* Fixed a wrong default-min-max tuple in the dynamic reconfigure config.
-* Useful config and launch files are now added to cmake install.
-* Added install target for the test_optim_node executable.
-
-0.1.4 (2015-05-20)
-------------------
-* Fixed compilation errors on ROS Jade
-
-0.1.3 (2015-05-20)
-------------------
-* Fixed compilation errors on ubuntu saucy
-
-0.1.2 (2015-05-19)
-------------------
-* Removed unused include that could break compilation.
-
-0.1.1 (2015-05-19)
-------------------
-* All files added to the indigo-devel branch
-* Initial commit
-* Contributors: Christoph Rösmann
diff --git a/src/teb_local_planner/CMakeLists.txt b/src/teb_local_planner/CMakeLists.txt
deleted file mode 100644
index 43859bc..0000000
--- a/src/teb_local_planner/CMakeLists.txt
+++ /dev/null
@@ -1,162 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(teb_local_planner)
-
-# Set to Release in order to speed up the program significantly
-set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 14)
-endif()
-
-#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-# add_compile_options(-Wall -Wextra -Wpedantic)
-#endif()
-
-## Find catkin macros and libraries
-find_package(ament_cmake REQUIRED)
-find_package(costmap_converter REQUIRED)
-find_package(dwb_critics REQUIRED)
-find_package(nav_2d_utils REQUIRED)
-find_package(nav2_core REQUIRED)
-find_package(nav2_costmap_2d REQUIRED)
-find_package(nav2_util REQUIRED)
-#find_package(interactive_markers REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(teb_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_eigen REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-
-message(STATUS "System: ${CMAKE_SYSTEM}")
-## System dependencies are found with CMake's conventions
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
-message(STATUS "${CMAKE_MODULE_PATH}")
-find_package(SUITESPARSE REQUIRED)
-find_package(G2O REQUIRED)
-find_package(Boost REQUIRED)
-
-# Eigen3 FindScript Backward compatibility (ubuntu saucy)
-# Since FindEigen.cmake is deprecated starting from jade.
-if (EXISTS "FindEigen3.cmake")
- find_package(Eigen3 REQUIRED)
- set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
-elseif (EXISTS "FindEigen.cmake")
- find_package(Eigen REQUIRED)
-elseif (EXISTS "FindEigen.cmake")
- message(WARNING "No findEigen cmake script found. You must provde one of them,
- e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.")
-endif (EXISTS "FindEigen3.cmake")
-
-set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR})
-set(EXTERNAL_LIBS ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES})
-
-###########
-## Build ##
-###########
-
-include_directories(
- include
- ${Boost_INCLUDE_DIRS}
- ${EXTERNAL_INCLUDE_DIRS}
-)
-
-## Build the teb_local_planner library
-
-add_library(teb_local_planner SHARED
- src/timed_elastic_band.cpp
- src/optimal_planner.cpp
- src/obstacles.cpp
- src/visualization.cpp
- src/recovery_behaviors.cpp
- src/teb_config.cpp
- src/homotopy_class_planner.cpp
- src/teb_local_planner_ros.cpp
- src/graph_search.cpp
-)
-
-set(ament_dependencies
- costmap_converter
- dwb_critics
- nav_2d_utils
- nav2_core
- nav2_costmap_2d
- nav2_util
- geometry_msgs
- nav_msgs
- rclcpp
- rclcpp_action
- rclcpp_lifecycle
- std_msgs
- pluginlib
- teb_msgs
- tf2
- tf2_eigen
- tf2_geometry_msgs
- tf2_ros
- visualization_msgs
- builtin_interfaces
-)
-ament_target_dependencies(teb_local_planner
- ${ament_dependencies}
-)
-target_link_libraries(teb_local_planner
- ${EXTERNAL_LIBS}
-)
-
-target_compile_definitions(teb_local_planner PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
-
-
-#############
-## Install ##
-#############
-
-## Mark executables and/or libraries for installation
-install(TARGETS teb_local_planner
- DESTINATION lib
-)
-
-## Mark cpp header files for installation
-install(DIRECTORY include/
- DESTINATION include/
-)
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-install(FILES
- teb_local_planner_plugin.xml
- DESTINATION share
- )
-
-install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
-
-install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
-
-ament_export_include_directories(include)
-ament_export_libraries(teb_local_planner)
-ament_export_dependencies(${ament_dependencies})
-pluginlib_export_plugin_description_file(nav2_core teb_local_planner_plugin.xml)
-
-#############
-## Testing ##
-#############
-
-if(BUILD_TESTING)
- find_package(ament_cmake_gtest REQUIRED)
- #ament_add_gtest(homotopy_class_planner_test
- # test/homotopy_class_planner_test.cpp
- #)
- #target_link_libraries(homotopy_class_planner_test
- # teb_local_planner
- #)
-endif()
-
-ament_package()
diff --git a/src/teb_local_planner/LICENSE b/src/teb_local_planner/LICENSE
deleted file mode 100644
index d4658cc..0000000
--- a/src/teb_local_planner/LICENSE
+++ /dev/null
@@ -1,28 +0,0 @@
-Copyright (c) 2016, TU Dortmund - Lehrstuhl RST
-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 teb_local_planner nor the names of its
- contributors may be used to endorse or promote products derived from
- this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
-
diff --git a/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg b/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
deleted file mode 100755
index f403d7e..0000000
--- a/src/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg
+++ /dev/null
@@ -1,420 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-#from local_planner_limits import add_generic_localplanner_params
-
-gen = ParameterGenerator()
-
-# This unusual line allows to reuse existing parameter definitions
-# that concern all localplanners
-#add_generic_localplanner_params(gen)
-
-# For integers and doubles:
-# Name Type Reconfiguration level
-# Description
-# Default Min Max
-
-
-grp_trajectory = gen.add_group("Trajectory", type="tab")
-
-# Trajectory
-grp_trajectory.add("teb_autosize", bool_t, 0,
- "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
- True)
-
-grp_trajectory.add("dt_ref", double_t, 0,
- "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
- 0.3, 0.01, 1)
-
-grp_trajectory.add("dt_hysteresis", double_t, 0,
- "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
- 0.1, 0.002, 0.5)
-
-grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0,
- "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
- True)
-
-grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0,
- "If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
- False)
-
-grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0,
- "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
- 3.0, 0, 50.0)
-
-grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0,
- "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
- 1.0, 0.0, 10.0)
-
-grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0,
- "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
- 0.78, 0.0, 4.0)
-
-grp_trajectory.add("feasibility_check_no_poses", int_t, 0,
- "Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
- 5, -1, 50)
-
-grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0,
- "Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
- -1, -1, 20)
-
-grp_trajectory.add("exact_arc_length", bool_t, 0,
- "If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
- False)
-
-grp_trajectory.add("publish_feedback", bool_t, 0,
- "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
- False)
-
-grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0,
- "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
- 0, 0, 1)
-
-# ViaPoints
-grp_viapoints = gen.add_group("ViaPoints", type="tab")
-
-grp_viapoints.add("global_plan_viapoint_sep", double_t, 0,
- "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
- -0.1, -0.1, 5.0)
-
-grp_viapoints.add("via_points_ordered", bool_t, 0,
- "If true, the planner adheres to the order of via-points in the storage container",
- False)
-
-# Robot
-grp_robot = gen.add_group("Robot", type="tab")
-
-grp_robot.add("max_vel_x", double_t, 0,
- "Maximum translational velocity of the robot",
- 0.4, 0.01, 100)
-
-grp_robot.add("max_vel_x_backwards", double_t, 0,
- "Maximum translational velocity of the robot for driving backwards",
- 0.2, 0.01, 100)
-
-grp_robot.add("max_vel_theta", double_t, 0,
- "Maximum angular velocity of the robot",
- 0.3, 0.01, 100)
-
-grp_robot.add("acc_lim_x", double_t, 0,
- "Maximum translational acceleration of the robot",
- 0.5, 0.01, 100)
-
-grp_robot.add("acc_lim_theta", double_t, 0,
- "Maximum angular acceleration of the robot",
- 0.5, 0.01, 100)
-
-grp_robot.add("is_footprint_dynamic", bool_t, 0,
- "If true, updated the footprint before checking trajectory feasibility",
- False)
-
-grp_robot.add("use_proportional_saturation", bool_t, 0,
- "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
- False)
-grp_robot.add("transform_tolerance", double_t, 0,
- "Tolerance when querying the TF Tree for a transformation (seconds)",
- 0.5, 0.001, 20)
-
-# Robot/Carlike
-
-grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")
-
-grp_robot_carlike.add("min_turning_radius", double_t, 0,
- "Minimum turning radius of a carlike robot (diff-drive robot: zero)",
- 0.0, 0.0, 50.0)
-
-grp_robot_carlike.add("wheelbase", double_t, 0,
- "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!",
- 1.0, -10.0, 10.0)
-
-grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0,
- "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')",
- False)
-
-# Robot/Omni
-
-grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
-
-grp_robot_omni.add("max_vel_y", double_t, 0,
- "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
- 0.0, 0.0, 100)
-
-grp_robot_omni.add("acc_lim_y", double_t, 0,
- "Maximum strafing acceleration of the robot",
- 0.5, 0.01, 100)
-
-# GoalTolerance
-grp_goal = gen.add_group("GoalTolerance", type="tab")
-
-grp_goal.add("free_goal_vel", bool_t, 0,
- "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)",
- False)
-
-# Obstacles
-grp_obstacles = gen.add_group("Obstacles", type="tab")
-
-grp_obstacles.add("min_obstacle_dist", double_t, 0,
- "Minimum desired separation from obstacles",
- 0.5, 0, 10)
-
-grp_obstacles.add("inflation_dist", double_t, 0,
- "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
- 0.6, 0, 15)
-
-grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
- "Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
- 0.6, 0, 15)
-
-grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
- "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
- False)
-
-grp_obstacles.add("include_costmap_obstacles", bool_t, 0,
- "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)",
- True)
-
-grp_obstacles.add("legacy_obstacle_association", bool_t, 0,
- "If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).",
- False)
-
-grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0,
- "The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.",
- 1.5, 0.0, 100.0)
-
-grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0,
- "See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.",
- 5.0, 1.0, 100.0)
-
-grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0,
- "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)",
- 1.5, 0.0, 20.0)
-
-grp_obstacles.add("obstacle_poses_affected", int_t, 0,
- "The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well",
- 30, 0, 200)
-
-# Obstacle - Velocity ratio parameters
-grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")
-
-grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
- "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
- 1, 0, 1)
-
-grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
- "Distance to a static obstacle for which the velocity should be lower",
- 0, 0, 10)
-
-grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
- "Distance to a static obstacle for which the velocity should be higher",
- 0.5, 0, 10)
-
-# Optimization
-grp_optimization = gen.add_group("Optimization", type="tab")
-
-grp_optimization.add("no_inner_iterations", int_t, 0,
- "Number of solver iterations called in each outerloop iteration",
- 5, 1, 100)
-
-grp_optimization.add("no_outer_iterations", int_t, 0,
- "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations",
- 4, 1, 100)
-
-grp_optimization.add("optimization_activate", bool_t, 0,
- "Activate the optimization",
- True)
-
-grp_optimization.add("optimization_verbose", bool_t, 0,
- "Print verbose information",
- False)
-
-grp_optimization.add("penalty_epsilon", double_t, 0,
- "Add a small safty margin to penalty functions for hard-constraint approximations",
- 0.05, 0, 1.0)
-
-grp_optimization.add("weight_max_vel_x", double_t, 0,
- "Optimization weight for satisfying the maximum allowed translational velocity",
- 2, 0, 1000)
-
-grp_optimization.add("weight_max_vel_y", double_t, 0,
- "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)",
- 2, 0, 1000)
-
-grp_optimization.add("weight_max_vel_theta", double_t, 0,
- "Optimization weight for satisfying the maximum allowed angular velocity",
- 1, 0, 1000)
-
-grp_optimization.add("weight_acc_lim_x", double_t, 0,
- "Optimization weight for satisfying the maximum allowed translational acceleration",
- 1, 0, 1000)
-
-grp_optimization.add("weight_acc_lim_y", double_t, 0,
- "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)",
- 1, 0, 1000)
-
-grp_optimization.add("weight_acc_lim_theta", double_t, 0,
- "Optimization weight for satisfying the maximum allowed angular acceleration",
- 1, 0, 1000)
-
-grp_optimization.add("weight_kinematics_nh", double_t, 0,
- "Optimization weight for satisfying the non-holonomic kinematics",
- 1000 , 0, 10000)
-
-grp_optimization.add("weight_kinematics_forward_drive", double_t, 0,
- "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)",
- 1, 0, 10000)
-
-grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
- "Optimization weight for enforcing a minimum turning radius (carlike robots)",
- 1, 0, 1000)
-
-grp_optimization.add("weight_optimaltime", double_t, 0,
- "Optimization weight for contracting the trajectory w.r.t. transition time",
- 1, 0, 1000)
-
-grp_optimization.add("weight_shortest_path", double_t, 0,
- "Optimization weight for contracting the trajectory w.r.t. path length",
- 0, 0, 100)
-
-grp_optimization.add("weight_obstacle", double_t, 0,
- "Optimization weight for satisfying a minimum seperation from obstacles",
- 50, 0, 1000)
-
-grp_optimization.add("weight_inflation", double_t, 0,
- "Optimization weight for the inflation penalty (should be small)",
- 0.1, 0, 10)
-
-grp_optimization.add("weight_dynamic_obstacle", double_t, 0,
- "Optimization weight for satisfying a minimum seperation from dynamic obstacles",
- 50, 0, 1000)
-
-grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
- "Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
- 0.1, 0, 10)
-
-grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
- "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
- 0, 0, 1000)
-
-grp_optimization.add("weight_viapoint", double_t, 0,
- "Optimization weight for minimizing the distance to via-points",
- 1, 0, 1000)
-
-grp_optimization.add("weight_adapt_factor", double_t, 0,
- "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.",
- 2, 1, 100)
-
-grp_optimization.add("obstacle_cost_exponent", double_t, 0,
- "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
- 1, 0.01, 100)
-
-
-
-# Homotopy Class Planner
-grp_hcp = gen.add_group("HCPlanning", type="tab")
-
-grp_hcp.add("enable_multithreading", bool_t, 0,
- "Activate multiple threading for planning multiple trajectories in parallel",
- True)
-
-grp_hcp.add("max_number_classes", int_t, 0,
- "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)",
- 5, 1, 100)
-
-grp_hcp.add("max_number_plans_in_current_class", int_t, 0,
- "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
- 1, 1, 10)
-
-grp_hcp.add("selection_cost_hysteresis", double_t, 0,
- "Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)",
- 1.0, 0, 2)
-
-
-grp_hcp.add("selection_prefer_initial_plan", double_t, 0,
- "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)",
- 0.95, 0, 1)
-
-grp_hcp.add("selection_obst_cost_scale", double_t, 0,
- "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)",
- 2.0, 0, 1000)
-
-grp_hcp.add("selection_viapoint_cost_scale", double_t, 0,
- "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)",
- 1.0, 0, 100)
-
-grp_hcp.add("selection_alternative_time_cost", bool_t, 0,
- "If true, time cost is replaced by the total transition time.",
- False)
-
-grp_hcp.add("selection_dropping_probability", double_t, 0,
- "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.",
- 0.0, 0.0, 1.0)
-
-grp_hcp.add("switching_blocking_period", double_t, 0,
- "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
- 0.0, 0.0, 60)
-
-grp_hcp.add("roadmap_graph_no_samples", int_t, 0,
- "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off",
- 15, 1, 100)
-
-grp_hcp.add("roadmap_graph_area_width", double_t, 0,
- "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)",
- 5, 0.1, 20)
-
-grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0,
- "The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)",
- 1.0, 0.5, 2)
-
-grp_hcp.add("h_signature_prescaler", double_t, 0,
- "Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)",
- 1, 0.2, 1)
-
-grp_hcp.add("h_signature_threshold", double_t, 0,
- "Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold",
- 0.1, 0, 1)
-
-grp_hcp.add("obstacle_heading_threshold", double_t, 0,
- "Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)",
- 0.45, 0, 1)
-
-grp_hcp.add("viapoints_all_candidates", bool_t, 0,
- "If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).",
- True)
-
-grp_hcp.add("visualize_hc_graph", bool_t, 0,
- "Visualize the graph that is created for exploring new homotopy classes",
- False)
-
-
-# Recovery
-grp_recovery = gen.add_group("Recovery", type="tab")
-
-grp_recovery.add("shrink_horizon_backup", bool_t, 0,
- "Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
- True)
-
-grp_recovery.add("oscillation_recovery", bool_t, 0,
- "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
- True)
-
-grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
-
-grp_recovery_divergence.add(
- "divergence_detection_enable",
- bool_t,
- 0,
- "True to enable divergence detection.",
- False
-)
-
-grp_recovery_divergence.add(
- "divergence_detection_max_chi_squared",
- double_t,
- 0,
- "Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
- 10,
- 0,
- 100
-)
-
-exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))
diff --git a/src/teb_local_planner/cfg/rviz_test_optim.rviz b/src/teb_local_planner/cfg/rviz_test_optim.rviz
deleted file mode 100644
index 02ee250..0000000
--- a/src/teb_local_planner/cfg/rviz_test_optim.rviz
+++ /dev/null
@@ -1,183 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 81
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- Splitter Ratio: 0.5
- Tree Height: 562
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.588679016
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.0299999993
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 10
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 0.100000001
- Cell Size: 0.100000001
- Class: rviz/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.0299999993
- Value: Lines
- Name: Fine Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 100
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /test_optim_node/local_plan
- Unreliable: false
- Value: true
- - Alpha: 1
- Arrow Length: 0.300000012
- Axes Length: 0.300000012
- Axes Radius: 0.00999999978
- Class: rviz/PoseArray
- Color: 255; 25; 0
- Enabled: true
- Head Length: 0.0700000003
- Head Radius: 0.0299999993
- Name: PoseArray
- Shaft Length: 0.230000004
- Shaft Radius: 0.00999999978
- Shape: Arrow (Flat)
- Topic: /test_optim_node/teb_poses
- Unreliable: false
- Value: true
- - Class: rviz/Marker
- Enabled: true
- Marker Topic: /test_optim_node/teb_markers
- Name: Marker
- Namespaces:
- PointObstacles: true
- Queue Size: 100
- Value: true
- - Class: rviz/InteractiveMarkers
- Enable Transparency: true
- Enabled: true
- Name: InteractiveMarkers
- Show Axes: false
- Show Descriptions: true
- Show Visual Aids: false
- Update Topic: /marker_obstacles/update
- Value: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: odom
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz/Interact
- Hide Inactive Objects: true
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Topic: /initialpose
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
- Single click: true
- Topic: /clicked_point
- Value: true
- Views:
- Current:
- Class: rviz/Orbit
- Distance: 7.77247
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
- Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 1.56979632
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 4.71043873
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: true
- Width: 1354
- X: 137
- Y: 50
diff --git a/src/teb_local_planner/cmake_modules/FindG2O.cmake b/src/teb_local_planner/cmake_modules/FindG2O.cmake
deleted file mode 100644
index b2670d3..0000000
--- a/src/teb_local_planner/cmake_modules/FindG2O.cmake
+++ /dev/null
@@ -1,97 +0,0 @@
-# Locate the g2o libraries
-# A general framework for graph optimization.
-#
-# This module defines
-# G2O_FOUND, if false, do not try to link against g2o
-# G2O_LIBRARIES, path to the libg2o
-# G2O_INCLUDE_DIR, where to find the g2o header files
-#
-# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
-# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
-
-IF(UNIX)
-
- #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
- # in cache already
- # SET(G2O_FIND_QUIETLY TRUE)
- #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
-
- MESSAGE(STATUS "Searching for g2o ...")
- FIND_PATH(G2O_INCLUDE_DIR
- NAMES core math_groups types
- PATHS /usr/local /usr
- PATH_SUFFIXES include/g2o include)
-
- IF (G2O_INCLUDE_DIR)
- MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
- ENDIF (G2O_INCLUDE_DIR)
-
- FIND_LIBRARY(G2O_CORE_LIB
- NAMES g2o_core g2o_core_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_STUFF_LIB
- NAMES g2o_stuff g2o_stuff_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB
- NAMES g2o_types_slam2d g2o_types_slam2d_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB
- NAMES g2o_types_slam3d g2o_types_slam3d_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB
- NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_SOLVER_PCG_LIB
- NAMES g2o_solver_pcg g2o_solver_pcg_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB
- NAMES g2o_solver_csparse g2o_solver_csparse_rd
- PATHS /usr/local /usr
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_INCREMENTAL_LIB
- NAMES g2o_incremental g2o_incremental_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
- FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
- NAMES g2o_csparse_extension g2o_csparse_extension_rd
- PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
- PATH_SUFFIXES lib)
-
- SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
- ${G2O_CORE_LIB}
- ${G2O_STUFF_LIB}
- ${G2O_TYPES_SLAM2D_LIB}
- ${G2O_TYPES_SLAM3D_LIB}
- ${G2O_SOLVER_CHOLMOD_LIB}
- ${G2O_SOLVER_PCG_LIB}
- ${G2O_SOLVER_CSPARSE_LIB}
- ${G2O_INCREMENTAL_LIB}
- )
-
- IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
- SET(G2O_FOUND "YES")
- IF(NOT G2O_FIND_QUIETLY)
- MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
- ENDIF(NOT G2O_FIND_QUIETLY)
- ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
- IF(NOT G2O_LIBRARIES)
- IF(G2O_FIND_REQUIRED)
- message(FATAL_ERROR "Could not find libg2o!")
- ENDIF(G2O_FIND_REQUIRED)
- ENDIF(NOT G2O_LIBRARIES)
-
- IF(NOT G2O_INCLUDE_DIR)
- IF(G2O_FIND_REQUIRED)
- message(FATAL_ERROR "Could not find g2o include directory!")
- ENDIF(G2O_FIND_REQUIRED)
- ENDIF(NOT G2O_INCLUDE_DIR)
- ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
-
-ENDIF(UNIX)
-
diff --git a/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake b/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
deleted file mode 100644
index 101b79b..0000000
--- a/src/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake
+++ /dev/null
@@ -1,133 +0,0 @@
-# - Try to find SUITESPARSE
-# Once done this will define
-#
-# SUITESPARSE_FOUND - system has SUITESPARSE
-# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory
-# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE
-# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package)
-# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
-# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs
-# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs
-# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly
-
-IF (SUITESPARSE_INCLUDE_DIRS)
- # Already in cache, be silent
- SET(SUITESPARSE_FIND_QUIETLY TRUE)
-ENDIF (SUITESPARSE_INCLUDE_DIRS)
-
-if( WIN32 )
- # Find cholmod part of the suitesparse library collection
-
- FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
- PATHS "C:\\libs\\win32\\SuiteSparse\\Include" )
-
- # Add cholmod include directory to collection include directories
- IF ( CHOLMOD_INCLUDE_DIR )
- list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
- ENDIF( CHOLMOD_INCLUDE_DIR )
-
-
- # find path suitesparse library
- FIND_PATH( SUITESPARSE_LIBRARY_DIRS
- amd.lib
- PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
-
- # if we found the library, add it to the defined libraries
- IF ( SUITESPARSE_LIBRARY_DIRS )
- list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd )
- ENDIF( SUITESPARSE_LIBRARY_DIRS )
-
-else( WIN32 )
- IF(APPLE)
- FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
- PATHS /opt/local/include/ufsparse
- /usr/local/include )
-
- FIND_PATH( SUITESPARSE_LIBRARY_DIR
- NAMES libcholmod.a
- PATHS /opt/local/lib
- /usr/local/lib )
- ELSE(APPLE)
- FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
- PATHS /usr/local/include
- /usr/include
- /usr/include/suitesparse/
- ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
- PATH_SUFFIXES cholmod/ CHOLMOD/ )
-
- FIND_PATH( SUITESPARSE_LIBRARY_DIR
- NAMES libcholmod.so libcholmod.a
- PATHS /usr/lib
- /usr/lib64
- /usr/lib/x86_64-linux-gnu
- /usr/lib/i386-linux-gnu
- /usr/local/lib
- /usr/lib/arm-linux-gnueabihf/
- /usr/lib/aarch64-linux-gnu/
- /usr/lib/arm-linux-gnueabi/
- /usr/lib/arm-linux-gnu)
- ENDIF(APPLE)
-
- # Add cholmod include directory to collection include directories
- IF ( CHOLMOD_INCLUDE_DIR )
- list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
- ENDIF( CHOLMOD_INCLUDE_DIR )
-
- # if we found the library, add it to the defined libraries
- IF ( SUITESPARSE_LIBRARY_DIR )
-
- list ( APPEND SUITESPARSE_LIBRARIES amd)
- list ( APPEND SUITESPARSE_LIBRARIES btf)
- list ( APPEND SUITESPARSE_LIBRARIES camd)
- list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
- list ( APPEND SUITESPARSE_LIBRARIES cholmod)
- list ( APPEND SUITESPARSE_LIBRARIES colamd)
- # list ( APPEND SUITESPARSE_LIBRARIES csparse)
- list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
- list ( APPEND SUITESPARSE_LIBRARIES klu)
- # list ( APPEND SUITESPARSE_LIBRARIES spqr)
- list ( APPEND SUITESPARSE_LIBRARIES umfpack)
-
- IF (APPLE)
- list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
- ENDIF (APPLE)
-
- # Metis and spqr are optional
- FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
- NAMES metis
- PATHS ${SUITESPARSE_LIBRARY_DIR} )
- IF (SUITESPARSE_METIS_LIBRARY)
- list ( APPEND SUITESPARSE_LIBRARIES metis)
- ENDIF(SUITESPARSE_METIS_LIBRARY)
-
- if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
- SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
- else()
- SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
- endif()
-
- if(SUITESPARSE_SPQR_VALID)
- FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
- NAMES spqr
- PATHS ${SUITESPARSE_LIBRARY_DIR} )
- IF (SUITESPARSE_SPQR_LIBRARY)
- list ( APPEND SUITESPARSE_LIBRARIES spqr)
- ENDIF (SUITESPARSE_SPQR_LIBRARY)
- endif()
-
- ENDIF( SUITESPARSE_LIBRARY_DIR )
-
-endif( WIN32 )
-
-
-IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
- IF(WIN32)
- list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
- ENDIF(WIN32)
- SET(SUITESPARSE_FOUND TRUE)
- MESSAGE(STATUS "Found SuiteSparse")
-ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
- SET( SUITESPARSE_FOUND FALSE )
- MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
-ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
-
diff --git a/src/teb_local_planner/include/teb_local_planner/distance_calculations.h b/src/teb_local_planner/include/teb_local_planner/distance_calculations.h
deleted file mode 100644
index 62f6b88..0000000
--- a/src/teb_local_planner/include/teb_local_planner/distance_calculations.h
+++ /dev/null
@@ -1,464 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef DISTANCE_CALCULATIONS_H
-#define DISTANCE_CALCULATIONS_H
-
-#include <Eigen/Core>
-#include "teb_local_planner/misc.h"
-
-
-namespace teb_local_planner
-{
-
-//! Abbrev. for a container storing 2d points
-typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > Point2dContainer;
-
-
-/**
- * @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point
- * @param point 2D point
- * @param line_start 2D point representing the start of the line segment
- * @param line_end 2D point representing the end of the line segment
- * @return Closest point on the line segment
- */
-inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
-{
- Eigen::Vector2d diff = line_end - line_start;
- double sq_norm = diff.squaredNorm();
-
- if (sq_norm == 0)
- return line_start;
-
- double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm;
-
- if (u <= 0) return line_start;
- else if (u >= 1) return line_end;
-
- return line_start + u*diff;
-}
-
-/**
- * @brief Helper function to calculate the distance between a line segment and a point
- * @param point 2D point
- * @param line_start 2D point representing the start of the line segment
- * @param line_end 2D point representing the end of the line segment
- * @return minimum distance to a given line segment
- */
-inline double distance_point_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
-{
- return (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm();
-}
-
-/**
- * @brief Helper function to check whether two line segments intersects
- * @param line1_start 2D point representing the start of the first line segment
- * @param line1_end 2D point representing the end of the first line segment
- * @param line2_start 2D point representing the start of the second line segment
- * @param line2_end 2D point representing the end of the second line segment
- * @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true)
- * @return \c true if both line segments intersect
- */
-inline bool check_line_segments_intersection_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
- const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end, Eigen::Vector2d* intersection = NULL)
-{
- // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
- double s_numer, t_numer, denom, t;
- Eigen::Vector2d line1 = line1_end - line1_start;
- Eigen::Vector2d line2 = line2_end - line2_start;
-
- denom = line1.x() * line2.y() - line2.x() * line1.y();
- if (denom == 0) return false; // Collinear
- bool denomPositive = denom > 0;
-
- Eigen::Vector2d aux = line1_start - line2_start;
-
- s_numer = line1.x() * aux.y() - line1.y() * aux.x();
- if ((s_numer < 0) == denomPositive) return false; // No collision
-
- t_numer = line2.x() * aux.y() - line2.y() * aux.x();
- if ((t_numer < 0) == denomPositive) return false; // No collision
-
- if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision
-
- // Otherwise collision detected
- t = t_numer / denom;
- if (intersection)
- {
- *intersection = line1_start + t * line1;
- }
-
- return true;
-}
-
-
-/**
- * @brief Helper function to calculate the smallest distance between two line segments
- * @param line1_start 2D point representing the start of the first line segment
- * @param line1_end 2D point representing the end of the first line segment
- * @param line2_start 2D point representing the start of the second line segment
- * @param line2_end 2D point representing the end of the second line segment
- * @return smallest distance between both segments
-*/
-inline double distance_segment_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
- const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end)
-{
- // TODO more efficient implementation
-
- // check if segments intersect
- if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end))
- return 0;
-
- // check all 4 combinations
- std::array<double,4> distances;
-
- distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end);
- distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end);
- distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end);
- distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end);
-
- return *std::min_element(distances.begin(), distances.end());
-}
-
-
-/**
- * @brief Helper function to calculate the smallest distance between a point and a closed polygon
- * @param point 2D point
- * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
- * @return smallest distance between point and polygon
-*/
-inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
-{
- double dist = HUGE_VAL;
-
- // the polygon is a point
- if (vertices.size() == 1)
- {
- return (point - vertices.front()).norm();
- }
-
- // check each polygon edge
- for (int i=0; i<(int)vertices.size()-1; ++i)
- {
- double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
-// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
- if (new_dist < dist)
- dist = new_dist;
- }
-
- if (vertices.size()>2) // if not a line close polygon
- {
- double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
- if (new_dist < dist)
- return new_dist;
- }
-
- return dist;
-}
-
-/**
- * @brief Helper function to calculate the smallest distance between a line segment and a closed polygon
- * @param line_start 2D point representing the start of the line segment
- * @param line_end 2D point representing the end of the line segment
- * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
- * @return smallest distance between point and polygon
-*/
-inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices)
-{
- double dist = HUGE_VAL;
-
- // the polygon is a point
- if (vertices.size() == 1)
- {
- return distance_point_to_segment_2d(vertices.front(), line_start, line_end);
- }
-
- // check each polygon edge
- for (int i=0; i<(int)vertices.size()-1; ++i)
- {
- double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1));
-// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
- if (new_dist < dist)
- dist = new_dist;
- }
-
- if (vertices.size()>2) // if not a line close polygon
- {
- double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge
- if (new_dist < dist)
- return new_dist;
- }
-
- return dist;
-}
-
-/**
- * @brief Helper function to calculate the smallest distance between two closed polygons
- * @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)
- * @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)
- * @return smallest distance between point and polygon
-*/
-inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
-{
- double dist = HUGE_VAL;
-
- // the polygon1 is a point
- if (vertices1.size() == 1)
- {
- return distance_point_to_polygon_2d(vertices1.front(), vertices2);
- }
-
- // check each edge of polygon1
- for (int i=0; i<(int)vertices1.size()-1; ++i)
- {
- double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
- if (new_dist < dist)
- dist = new_dist;
- }
-
- if (vertices1.size()>2) // if not a line close polygon1
- {
- double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
- if (new_dist < dist)
- return new_dist;
- }
-
- return dist;
-}
-
-
-
-
-// Further distance calculations:
-
-
-// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html
-// Copyright 2001 softSurfer, 2012 Dan Sunday
-// This code may be freely used and modified for any purpose
-// providing that this copyright notice is included with it.
-// SoftSurfer makes no warranty for this code, and cannot be held
-// liable for any real or imagined damage resulting from its use.
-// Users of this code must verify correctness for their application.
-
-inline double calc_distance_line_to_line_3d(const Eigen::Ref<const Eigen::Vector3d>& x1, Eigen::Ref<const Eigen::Vector3d>& u,
- const Eigen::Ref<const Eigen::Vector3d>& x2, Eigen::Ref<const Eigen::Vector3d>& v)
-{
- Eigen::Vector3d w = x2 - x1;
- double a = u.squaredNorm(); // dot(u,u) always >= 0
- double b = u.dot(v);
- double c = v.squaredNorm(); // dot(v,v) always >= 0
- double d = u.dot(w);
- double e = v.dot(w);
- double D = a*c - b*b; // always >= 0
- double sc, tc;
-
- // compute the line parameters of the two closest points
- if (D < SMALL_NUM) { // the lines are almost parallel
- sc = 0.0;
- tc = (b>c ? d/b : e/c); // use the largest denominator
- }
- else {
- sc = (b*e - c*d) / D;
- tc = (a*e - b*d) / D;
- }
-
- // get the difference of the two closest points
- Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
-
- return dP.norm(); // return the closest distance
-}
-
-
-
-
-
-inline double calc_distance_segment_to_segment3D(const Eigen::Ref<const Eigen::Vector3d>& line1_start, Eigen::Ref<const Eigen::Vector3d>& line1_end,
- const Eigen::Ref<const Eigen::Vector3d>& line2_start, Eigen::Ref<const Eigen::Vector3d>& line2_end)
-{
- Eigen::Vector3d u = line1_end - line1_start;
- Eigen::Vector3d v = line2_end - line2_start;
- Eigen::Vector3d w = line2_start - line1_start;
- double a = u.squaredNorm(); // dot(u,u) always >= 0
- double b = u.dot(v);
- double c = v.squaredNorm(); // dot(v,v) always >= 0
- double d = u.dot(w);
- double e = v.dot(w);
- double D = a*c - b*b; // always >= 0
- double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
- double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
-
- // compute the line parameters of the two closest points
- if (D < SMALL_NUM)
- { // the lines are almost parallel
- sN = 0.0; // force using point P0 on segment S1
- sD = 1.0; // to prevent possible division by 0.0 later
- tN = e;
- tD = c;
- }
- else
- { // get the closest points on the infinite lines
- sN = (b*e - c*d);
- tN = (a*e - b*d);
- if (sN < 0.0)
- { // sc < 0 => the s=0 edge is visible
- sN = 0.0;
- tN = e;
- tD = c;
- }
- else if (sN > sD)
- { // sc > 1 => the s=1 edge is visible
- sN = sD;
- tN = e + b;
- tD = c;
- }
- }
-
- if (tN < 0.0)
- { // tc < 0 => the t=0 edge is visible
- tN = 0.0;
- // recompute sc for this edge
- if (-d < 0.0)
- sN = 0.0;
- else if (-d > a)
- sN = sD;
- else
- {
- sN = -d;
- sD = a;
- }
- }
- else if (tN > tD)
- { // tc > 1 => the t=1 edge is visible
- tN = tD;
- // recompute sc for this edge
- if ((-d + b) < 0.0)
- sN = 0;
- else if ((-d + b) > a)
- sN = sD;
- else
- {
- sN = (-d + b);
- sD = a;
- }
- }
- // finally do the division to get sc and tc
- sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
- tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
-
- // get the difference of the two closest points
- Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
-
- return dP.norm(); // return the closest distance
-}
-
-
-
-
-template <typename VectorType>
-double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2)
-{
- VectorType dv = vel1 - vel2;
-
- double dv2 = dv.squaredNorm(); // dot(v,v)
- if (dv2 < SMALL_NUM) // the tracks are almost parallel
- return 0.0; // any time is ok. Use time 0.
-
- VectorType w0 = x1 - x2;
- double cpatime = -w0.dot(dv) / dv2;
-
- return cpatime; // time of CPA
-}
-
-template <typename VectorType>
-double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0)
-{
- double ctime = calc_closest_point_to_approach_time<VectorType>(x1, vel1, x2, vel2);
- if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time;
- VectorType P1 = x1 + (ctime * vel1);
- VectorType P2 = x2 + (ctime * vel2);
-
- return (P2-P1).norm(); // distance at CPA
-}
-
-
-
-// dist_Point_to_Line(): get the distance of a point to a line
-// Input: a Point P and a Line L (in any dimension)
-// Return: the shortest distance from P to L
-template <typename VectorType>
-double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir)
-{
- VectorType w = point - line_base;
-
- double c1 = w.dot(line_dir);
- double c2 = line_dir.dot(line_dir);
- double b = c1 / c2;
-
- VectorType Pb = line_base + b * line_dir;
- return (point-Pb).norm();
-}
-//===================================================================
-
-
-// dist_Point_to_Segment(): get the distance of a point to a segment
-// Input: a Point P and a Segment S (in any dimension)
-// Return: the shortest distance from P to S
-template <typename VectorType>
-double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end)
-{
- VectorType v = line_end - line_start;
- VectorType w = point - line_start;
-
- double c1 = w.dot(v);
- if ( c1 <= 0 )
- return w.norm();
-
- double c2 = v.dot(v);
- if ( c2 <= c1 )
- return (point-line_end).norm();
-
- double b = c1 / c2;
- VectorType Pb = line_start + b * v;
- return (point-Pb).norm();
-}
-
-
-
-} // namespace teb_local_planner
-
-#endif /* DISTANCE_CALCULATIONS_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h b/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
deleted file mode 100644
index 8551152..0000000
--- a/src/teb_local_planner/include/teb_local_planner/equivalence_relations.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef EQUIVALENCE_RELATIONS_H_
-#define EQUIVALENCE_RELATIONS_H_
-
-#include <memory>
-
-namespace teb_local_planner
-{
-
-/**
- * @class EquivalenceClass
- * @brief Abstract class that defines an interface for computing and comparing equivalence classes
- *
- * Equivalence relations are utilized in order to test if two trajectories are belonging to the same
- * equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is
- * the concept of homotopy classes. All trajectories belonging to the same homotopy class
- * can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely
- * share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation
- * is defined by the concept of homology classes (e.g. refer to HSignature).
- *
- * Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object.
- *
- * @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass.
- * Call the "compute"-methods directly on the subclass.
- */
-class EquivalenceClass
-{
-public:
-
- /**
- * @brief Default constructor
- */
- EquivalenceClass() {}
-
- /**
- * @brief virtual destructor
- */
- virtual ~EquivalenceClass() {}
-
- /**
- * @brief Check if two candidate classes are equivalent
- * @param other The other equivalence class to test with
- */
- virtual bool isEqual(const EquivalenceClass& other) const = 0;
-
- /**
- * @brief Check if the equivalence value is detected correctly
- * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
- */
- virtual bool isValid() const = 0;
-
- /**
- * @brief Check if the trajectory is non-looping around an obstacle
- * @return Returns false, if the trajectory loops around an obstacle
- */
- virtual bool isReasonable() const = 0;
-
-};
-
-using EquivalenceClassPtr = std::shared_ptr<EquivalenceClass>;
-
-
-} // namespace teb_local_planner
-
-
-#endif /* EQUIVALENCE_RELATIONS_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
deleted file mode 100644
index dd35147..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h
+++ /dev/null
@@ -1,278 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef _BASE_TEB_EDGES_H_
-#define _BASE_TEB_EDGES_H_
-
-#include "teb_local_planner/teb_config.h"
-
-#include <g2o/core/base_binary_edge.h>
-#include <g2o/core/base_unary_edge.h>
-#include <g2o/core/base_multi_edge.h>
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class BaseTebUnaryEdge
- * @brief Base edge connecting a single vertex in the TEB optimization problem
- *
- * This edge defines a base edge type for the TEB optimization problem.
- * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
- * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
- * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
- * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
- */
-template <int D, typename E, typename VertexXi>
-class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
-{
-public:
-
- using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
- using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
-
- /**
- * @brief Compute and return error / cost value.
- *
- * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
- * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
- */
- ErrorVector& getError()
- {
- computeError();
- return _error;
- }
-
- /**
- * @brief Read values from input stream
- */
- virtual bool read(std::istream& is)
- {
- // TODO generic read
- return true;
- }
-
- /**
- * @brief Write values to an output stream
- */
- virtual bool write(std::ostream& os) const
- {
- // TODO generic write
- return os.good();
- }
-
- /**
- * @brief Assign the TebConfig class for parameters.
- * @param cfg TebConfig class
- */
- void setTebConfig(const TebConfig& cfg)
- {
- cfg_ = &cfg;
- }
-
-protected:
-
- using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
- using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
-
- const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/**
- * @class BaseTebBinaryEdge
- * @brief Base edge connecting two vertices in the TEB optimization problem
- *
- * This edge defines a base edge type for the TEB optimization problem.
- * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
- * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
- * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
- * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
- */
-template <int D, typename E, typename VertexXi, typename VertexXj>
-class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
-{
-public:
-
- using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
- using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
-
- /**
- * @brief Compute and return error / cost value.
- *
- * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
- * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
- */
- ErrorVector& getError()
- {
- computeError();
- return _error;
- }
-
- /**
- * @brief Read values from input stream
- */
- virtual bool read(std::istream& is)
- {
- // TODO generic read
- return true;
- }
-
- /**
- * @brief Write values to an output stream
- */
- virtual bool write(std::ostream& os) const
- {
- // TODO generic write
- return os.good();
- }
-
- /**
- * @brief Assign the TebConfig class for parameters.
- * @param cfg TebConfig class
- */
- void setTebConfig(const TebConfig& cfg)
- {
- cfg_ = &cfg;
- }
-
-protected:
-
- using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
- using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
-
- const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-/**
- * @class BaseTebMultiEdge
- * @brief Base edge connecting multiple vertices in the TEB optimization problem
- *
- * This edge defines a base edge type for the TEB optimization problem.
- * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
- * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
- * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
- * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
- */
-template <int D, typename E>
-class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
-{
-public:
-
- using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
- using g2o::BaseMultiEdge<D, E>::computeError;
-
- // Overwrites resize() from the parent class
- virtual void resize(size_t size)
- {
- g2o::BaseMultiEdge<D, E>::resize(size);
-
- for(std::size_t i=0; i<_vertices.size(); ++i)
- _vertices[i] = NULL;
- }
-
- /**
- * @brief Compute and return error / cost value.
- *
- * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
- * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
- */
- ErrorVector& getError()
- {
- computeError();
- return _error;
- }
-
- /**
- * @brief Read values from input stream
- */
- virtual bool read(std::istream& is)
- {
- // TODO generic read
- return true;
- }
-
- /**
- * @brief Write values to an output stream
- */
- virtual bool write(std::ostream& os) const
- {
- // TODO generic write
- return os.good();
- }
-
- /**
- * @brief Assign the TebConfig class for parameters.
- * @param cfg TebConfig class
- */
- void setTebConfig(const TebConfig& cfg)
- {
- cfg_ = &cfg;
- }
-
-protected:
-
- using g2o::BaseMultiEdge<D, E>::_error;
- using g2o::BaseMultiEdge<D, E>::_vertices;
-
- const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
deleted file mode 100644
index 5d3ff98..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h
+++ /dev/null
@@ -1,732 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef EDGE_ACCELERATION_H_
-#define EDGE_ACCELERATION_H_
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/vertex_timediff.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/misc.h"
-
-#include <geometry_msgs/msg/twist.hpp>
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgeAcceleration
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
- *
- * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
- * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n
- * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation() \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
- * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
- * the second one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAccelerationStart
- * @see EdgeAccelerationGoal
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values!
- */
-class EdgeAcceleration : public BaseTebMultiEdge<2, double>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAcceleration()
- {
- this->resize(5);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
- const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
- const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
- const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
-
- // VELOCITY & ACCELERATION
- const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
- const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
-
- double dist1 = diff1.norm();
- double dist2 = diff2.norm();
- const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());
- const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());
-
- if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation
- {
- if (angle_diff1 != 0)
- {
- const double radius = dist1/(2*sin(angle_diff1/2));
- dist1 = fabs( angle_diff1 * radius ); // actual arg length!
- }
- if (angle_diff2 != 0)
- {
- const double radius = dist2/(2*sin(angle_diff2/2));
- dist2 = fabs( angle_diff2 * radius ); // actual arg length!
- }
- }
-
- double vel1 = dist1 / dt1->dt();
- double vel2 = dist2 / dt2->dt();
-
-
- // consider directions
-// vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta()));
-// vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta()));
- vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) );
- vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) );
-
- const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
-
-
- _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- const double omega1 = angle_diff1 / dt1->dt();
- const double omega2 = angle_diff2 / dt2->dt();
- const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
-
- _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
- }
-
-
-
-#ifdef USE_ANALYTIC_JACOBI
-#if 0
- /*
- * @brief Jacobi matrix of the cost function specified in computeError().
- */
- void linearizeOplus()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
- const VertexPointXY* conf1 = static_cast<const VertexPointXY*>(_vertices[0]);
- const VertexPointXY* conf2 = static_cast<const VertexPointXY*>(_vertices[1]);
- const VertexPointXY* conf3 = static_cast<const VertexPointXY*>(_vertices[2]);
- const VertexTimeDiff* deltaT1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
- const VertexTimeDiff* deltaT2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
- const VertexOrientation* angle1 = static_cast<const VertexOrientation*>(_vertices[5]);
- const VertexOrientation* angle2 = static_cast<const VertexOrientation*>(_vertices[6]);
- const VertexOrientation* angle3 = static_cast<const VertexOrientation*>(_vertices[7]);
-
- Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate();
- Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate();
- double dist1 = deltaS1.norm();
- double dist2 = deltaS2.norm();
-
- double sum_time = deltaT1->estimate() + deltaT2->estimate();
- double sum_time_inv = 1 / sum_time;
- double dt1_inv = 1/deltaT1->estimate();
- double dt2_inv = 1/deltaT2->estimate();
- double aux0 = 2/sum_time_inv;
- double aux1 = dist1 * deltaT1->estimate();
- double aux2 = dist2 * deltaT2->estimate();
-
- double vel1 = dist1 * dt1_inv;
- double vel2 = dist2 * dt2_inv;
- double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv;
- double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv;
- double acc = (vel2 - vel1) * aux0;
- double omegadot = (omega2 - omega1) * aux0;
- double aux3 = -acc/2;
- double aux4 = -omegadot/2;
-
- double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
- double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
-
- _jacobianOplus[0].resize(2,2); // conf1
- _jacobianOplus[1].resize(2,2); // conf2
- _jacobianOplus[2].resize(2,2); // conf3
- _jacobianOplus[3].resize(2,1); // deltaT1
- _jacobianOplus[4].resize(2,1); // deltaT2
- _jacobianOplus[5].resize(2,1); // angle1
- _jacobianOplus[6].resize(2,1); // angle2
- _jacobianOplus[7].resize(2,1); // angle3
-
- if (aux1==0) aux1=1e-20;
- if (aux2==0) aux2=1e-20;
-
- if (dev_border_acc!=0)
- {
- // TODO: double aux = aux0 * dev_border_acc;
- // double aux123 = aux / aux1;
- _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1
- _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1
- _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2
- _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2
- _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3
- _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3
- _jacobianOplus[2](0,0) = 0;
- _jacobianOplus[2](0,1) = 0;
- _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1
- _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2
- }
- else
- {
- _jacobianOplus[0](0,0) = 0; // acc x1
- _jacobianOplus[0](0,1) = 0; // acc y1
- _jacobianOplus[1](0,0) = 0; // acc x2
- _jacobianOplus[1](0,1) = 0; // acc y2
- _jacobianOplus[2](0,0) = 0; // acc x3
- _jacobianOplus[2](0,1) = 0; // acc y3
- _jacobianOplus[3](0,0) = 0; // acc deltaT1
- _jacobianOplus[4](0,0) = 0; // acc deltaT2
- }
-
- if (dev_border_omegadot!=0)
- {
- _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1
- _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2
- _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1
- _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2
- _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3
- }
- else
- {
- _jacobianOplus[3](1,0) = 0; // omegadot deltaT1
- _jacobianOplus[4](1,0) = 0; // omegadot deltaT2
- _jacobianOplus[5](1,0) = 0; // omegadot angle1
- _jacobianOplus[6](1,0) = 0; // omegadot angle2
- _jacobianOplus[7](1,0) = 0; // omegadot angle3
- }
-
- _jacobianOplus[0](1,0) = 0; // omegadot x1
- _jacobianOplus[0](1,1) = 0; // omegadot y1
- _jacobianOplus[1](1,0) = 0; // omegadot x2
- _jacobianOplus[1](1,1) = 0; // omegadot y2
- _jacobianOplus[2](1,0) = 0; // omegadot x3
- _jacobianOplus[2](1,1) = 0; // omegadot y3
- _jacobianOplus[5](0,0) = 0; // acc angle1
- _jacobianOplus[6](0,0) = 0; // acc angle2
- _jacobianOplus[7](0,0) = 0; // acc angle3
- }
-#endif
-#endif
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-/**
- * @class EdgeAccelerationStart
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
- * and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
- * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
- * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
- * the second one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAcceleration
- * @see EdgeAccelerationGoal
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory!
- */
-class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAccelerationStart()
- {
- _measurement = NULL;
- this->resize(3);
- }
-
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
- const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- // VELOCITY & ACCELERATION
- const Eigen::Vector2d diff = pose2->position() - pose1->position();
- double dist = diff.norm();
- const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta());
- if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
- {
- const double radius = dist/(2*sin(angle_diff/2));
- dist = fabs( angle_diff * radius ); // actual arg length!
- }
-
- const double vel1 = _measurement->linear.x;
- double vel2 = dist / dt->dt();
-
- // consider directions
- //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta()));
- vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) );
-
- const double acc_lin = (vel2 - vel1) / dt->dt();
-
- _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- const double omega1 = _measurement->angular.z;
- const double omega2 = angle_diff / dt->dt();
- const double acc_rot = (omega2 - omega1) / dt->dt();
-
- _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
- }
-
- /**
- * @brief Set the initial velocity that is taken into account for calculating the acceleration
- * @param vel_start twist message containing the translational and rotational velocity
- */
- void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
- {
- _measurement = &vel_start;
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-/**
- * @class EdgeAccelerationGoal
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
- * and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
- * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation() \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
- * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
- * the second one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAcceleration
- * @see EdgeAccelerationStart
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory
- */
-class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAccelerationGoal()
- {
- _measurement = NULL;
- this->resize(3);
- }
-
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
- const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- // VELOCITY & ACCELERATION
-
- const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
- double dist = diff.norm();
- const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta());
- if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
- {
- double radius = dist/(2*sin(angle_diff/2));
- dist = fabs( angle_diff * radius ); // actual arg length!
- }
-
- double vel1 = dist / dt->dt();
- const double vel2 = _measurement->linear.x;
-
- // consider directions
- //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta()));
- vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) );
-
- const double acc_lin = (vel2 - vel1) / dt->dt();
-
- _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- const double omega1 = angle_diff / dt->dt();
- const double omega2 = _measurement->angular.z;
- const double acc_rot = (omega2 - omega1) / dt->dt();
-
- _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
- }
-
- /**
- * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
- * @param vel_goal twist message containing the translational and rotational velocity
- */
- void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
- {
- _measurement = &vel_goal;
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-/**
- * @class EdgeAccelerationHolonomic
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
- *
- * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
- * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n
- * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n
- * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation() \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
- * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir),
- * the second one the strafing acceleration and the third one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAccelerationHolonomicStart
- * @see EdgeAccelerationHolonomicGoal
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values!
- */
-class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAccelerationHolonomic()
- {
- this->resize(5);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
- const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
- const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
- const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
-
- // VELOCITY & ACCELERATION
- Eigen::Vector2d diff1 = pose2->position() - pose1->position();
- Eigen::Vector2d diff2 = pose3->position() - pose2->position();
-
- double cos_theta1 = std::cos(pose1->theta());
- double sin_theta1 = std::sin(pose1->theta());
- double cos_theta2 = std::cos(pose2->theta());
- double sin_theta2 = std::sin(pose2->theta());
-
- // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
- double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y();
- double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y();
- // transform pose3 into robot frame pose2 (inverse 2d rotation matrix)
- double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y();
- double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y();
-
- double vel1_x = p1_dx / dt1->dt();
- double vel1_y = p1_dy / dt1->dt();
- double vel2_x = p2_dx / dt2->dt();
- double vel2_y = p2_dy / dt2->dt();
-
- double dt12 = dt1->dt() + dt2->dt();
-
- double acc_x = (vel2_x - vel1_x)*2 / dt12;
- double acc_y = (vel2_y - vel1_y)*2 / dt12;
-
- _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
- double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
- double acc_rot = (omega2 - omega1)*2 / dt12;
-
- _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]);
- TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]);
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-/**
- * @class EdgeAccelerationHolonomicStart
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
- * and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
- * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n
- * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
- * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
- * the second one the strafing acceleration and the third one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAccelerationHolonomic
- * @see EdgeAccelerationHolonomicGoal
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory!
- */
-class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAccelerationHolonomicStart()
- {
- this->resize(3);
- _measurement = NULL;
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
- const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- // VELOCITY & ACCELERATION
- Eigen::Vector2d diff = pose2->position() - pose1->position();
-
- double cos_theta1 = std::cos(pose1->theta());
- double sin_theta1 = std::sin(pose1->theta());
-
- // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
- double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
- double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
-
- double vel1_x = _measurement->linear.x;
- double vel1_y = _measurement->linear.y;
- double vel2_x = p1_dx / dt->dt();
- double vel2_y = p1_dy / dt->dt();
-
- double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
- double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
-
- _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- double omega1 = _measurement->angular.z;
- double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
- double acc_rot = (omega2 - omega1) / dt->dt();
-
- _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
- TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]);
- }
-
- /**
- * @brief Set the initial velocity that is taken into account for calculating the acceleration
- * @param vel_start twist message containing the translational and rotational velocity
- */
- void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
- {
- _measurement = &vel_start;
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-/**
- * @class EdgeAccelerationHolonomicGoal
- * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
- * and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
- * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n
- * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n
- * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation() \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
- * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
- * the second one is the strafing velocity and the third one the rotational acceleration.
- * @see TebOptimalPlanner::AddEdgesAcceleration
- * @see EdgeAccelerationHolonomic
- * @see EdgeAccelerationHolonomicStart
- * @remarks Do not forget to call setTebConfig()
- * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory
- */
-class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeAccelerationHolonomicGoal()
- {
- _measurement = NULL;
- this->resize(3);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
- const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- // VELOCITY & ACCELERATION
-
- Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
-
- double cos_theta1 = std::cos(pose_pre_goal->theta());
- double sin_theta1 = std::sin(pose_pre_goal->theta());
-
- // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
- double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
- double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
-
- double vel1_x = p1_dx / dt->dt();
- double vel1_y = p1_dy / dt->dt();
- double vel2_x = _measurement->linear.x;
- double vel2_y = _measurement->linear.y;
-
- double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
- double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
-
- _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
-
- // ANGULAR ACCELERATION
- double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
- double omega2 = _measurement->angular.z;
- double acc_rot = (omega2 - omega1) / dt->dt();
-
- _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
- TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);
- TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]);
- }
-
-
- /**
- * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
- * @param vel_goal twist message containing the translational and rotational velocity
- */
- void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
- {
- _measurement = &vel_goal;
- }
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-}; // end namespace
-
-#endif /* EDGE_ACCELERATION_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
deleted file mode 100755
index 8224042..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann, Franz Albers
- *********************************************************************/
-
-#ifndef EDGE_DYNAMICOBSTACLE_H
-#define EDGE_DYNAMICOBSTACLE_H
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/vertex_timediff.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/obstacles.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/robot_footprint_model.h"
-#include "teb_local_planner/misc.h"
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgeDynamicObstacle
- * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles.
- *
- * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n
- * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n
- * @see TebOptimalPlanner::AddEdgesDynamicObstacles
- * @remarks Do not forget to call setTebConfig(), setVertexIdx() and
- * @warning Experimental
- */
-class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeDynamicObstacle() : t_(0)
- {
- }
-
- /**
- * @brief Construct edge and specify the time for its associated pose (neccessary for computeError).
- * @param t_ Estimated time until current pose is reached
- */
- EdgeDynamicObstacle(double t) : t_(t)
- {
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
- const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
-
- double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
-
- _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]);
- }
-
-
- /**
- * @brief Set Obstacle for the underlying cost function
- * @param obstacle Const pointer to an Obstacle or derived Obstacle
- */
- void setObstacle(const Obstacle* obstacle)
- {
- _measurement = obstacle;
- }
-
- /**
- * @brief Set pointer to the robot model
- * @param robot_model Robot model required for distance calculation
- */
- void setRobotModel(const BaseRobotFootprintModel* robot_model)
- {
- robot_model_ = robot_model;
- }
-
- /**
- * @brief Set all parameters at once
- * @param cfg TebConfig class
- * @param robot_model Robot model required for distance calculation
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
- {
- cfg_ = &cfg;
- robot_model_ = robot_model;
- _measurement = obstacle;
- }
-
-protected:
-
- const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
- double t_; //!< Estimated time until current pose is reached
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
deleted file mode 100755
index 012a865..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h
+++ /dev/null
@@ -1,231 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef _EDGE_KINEMATICS_H
-#define _EDGE_KINEMATICS_H
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/misc.h"
-
-#include <cmath>
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgeKinematicsDiffDrive
- * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot.
- *
- * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
- * of the non-holonomic constraint:
- * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
- *
- * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
- * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n
- * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n
- * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost,
- * the second one backward-drive cost.
- * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike
- * @remarks Do not forget to call setTebConfig()
- */
-class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeKinematicsDiffDrive()
- {
- this->setMeasurement(0.);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
-
- Eigen::Vector2d deltaS = conf2->position() - conf1->position();
-
- // non holonomic constraint
- _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
-
- // positive-drive-direction constraint
- Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );
- _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
- // epsilon=0, otherwise it pushes the first bandpoints away from start
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
- }
-
-#ifdef USE_ANALYTIC_JACOBI
-#if 1
- /**
- * @brief Jacobi matrix of the cost function specified in computeError().
- */
- void linearizeOplus()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
-
- Eigen::Vector2d deltaS = conf2->position() - conf1->position();
-
- double cos1 = cos(conf1->theta());
- double cos2 = cos(conf2->theta());
- double sin1 = sin(conf1->theta());
- double sin2 = sin(conf2->theta());
- double aux1 = sin1 + sin2;
- double aux2 = cos1 + cos2;
-
- double dd_error_1 = deltaS[0]*cos1;
- double dd_error_2 = deltaS[1]*sin1;
- double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
-
- double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] -
- ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
-
- // conf1
- _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
- _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1
- _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1
- _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1
- _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle
- _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1
-
- // conf2
- _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2
- _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2
- _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2
- _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2
- _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle
- _jacobianOplusXj(1,2) = 0; // drive-dir angle1
- }
-#endif
-#endif
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-/**
- * @class EdgeKinematicsCarlike
- * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot.
- *
- * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
- * of the non-holonomic constraint:
- * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
- *
- * The definition is identically to the one of the differential drive robot.
- * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots.
- * The turning radius is defined by \f$ r=v/omega \f$.
- *
- * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
- * The second equation enforces a minimum turning radius.
- * The \e weight can be set using setInformation(): Matrix element 2,2. \n
- * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost,
- * the second one backward-drive cost and the third one the minimum turning radius
- * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive
- * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter,
- * the user might add an extra margin to the min_turning_radius param.
- * @remarks Do not forget to call setTebConfig()
- */
-class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeKinematicsCarlike()
- {
- this->setMeasurement(0.);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
-
- Eigen::Vector2d deltaS = conf2->position() - conf1->position();
-
- // non holonomic constraint
- _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
-
- // limit minimum turning radius
- double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
- if (angle_diff == 0)
- _error[1] = 0; // straight line motion
- else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius
- _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0);
- else
- _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0);
- // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter.
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
deleted file mode 100755
index 23c8bc4..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h
+++ /dev/null
@@ -1,295 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-#ifndef EDGE_OBSTACLE_H_
-#define EDGE_OBSTACLE_H_
-
-#include "teb_local_planner/obstacles.h"
-#include "teb_local_planner/robot_footprint_model.h"
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/misc.h"
-
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgeObstacle
- * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
- *
- * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
- * \e dist2point denotes the minimum distance to the point obstacle. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
- * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
- * @remarks Do not forget to call setTebConfig() and setObstacle()
- */
-class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeObstacle()
- {
- _measurement = NULL;
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()");
- const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
-
- double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
-
- // Original obstacle cost.
- _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
-
- if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
- {
- // Optional non-linear cost. Note the max cost (before weighting) is
- // the same as the straight line version and that all other costs are
- // below the straight line (for positive exponent), so it may be
- // necessary to increase weight_obstacle and/or the inflation_weight
- // when using larger exponents.
- _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
- }
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]);
- }
-
-#ifdef USE_ANALYTIC_JACOBI
-#if 0
-
- /**
- * @brief Jacobi matrix of the cost function specified in computeError().
- */
- void linearizeOplus()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()");
- const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
-
- Eigen::Vector2d deltaS = *_measurement - bandpt->position();
- double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta();
-
- double dist_squared = deltaS.squaredNorm();
- double dist = sqrt(dist_squared);
-
- double aux0 = sin(angdiff);
- double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon);
-
- if (dev_left_border==0)
- {
- _jacobianOplusXi( 0 , 0 ) = 0;
- _jacobianOplusXi( 0 , 1 ) = 0;
- _jacobianOplusXi( 0 , 2 ) = 0;
- return;
- }
-
- double aux1 = -fabs(aux0) / dist;
- double dev_norm_x = deltaS[0]*aux1;
- double dev_norm_y = deltaS[1]*aux1;
-
- double aux2 = cos(angdiff) * g2o::sign(aux0);
- double aux3 = aux2 / dist_squared;
- double dev_proj_x = aux3 * deltaS[1] * dist;
- double dev_proj_y = -aux3 * deltaS[0] * dist;
- double dev_proj_angle = -aux2;
-
- _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x );
- _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y );
- _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle;
- }
-#endif
-#endif
-
- /**
- * @brief Set pointer to associated obstacle for the underlying cost function
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setObstacle(const Obstacle* obstacle)
- {
- _measurement = obstacle;
- }
-
- /**
- * @brief Set pointer to the robot model
- * @param robot_model Robot model required for distance calculation
- */
- void setRobotModel(const BaseRobotFootprintModel* robot_model)
- {
- robot_model_ = robot_model;
- }
-
- /**
- * @brief Set all parameters at once
- * @param cfg TebConfig class
- * @param robot_model Robot model required for distance calculation
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
- {
- cfg_ = &cfg;
- robot_model_ = robot_model;
- _measurement = obstacle;
- }
-
-protected:
-
- const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-/**
- * @class EdgeInflatedObstacle
- * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles.
- *
- * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n
- * Additional, a second penalty is provided with \n
- * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$.
- * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation.
- * \e dist2point denotes the minimum distance to the point obstacle. \n
- * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
- * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle
- * @remarks Do not forget to call setTebConfig() and setObstacle()
- */
-class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeInflatedObstacle()
- {
- _measurement = NULL;
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()");
- const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
-
- double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
-
- // Original "straight line" obstacle cost. The max possible value
- // before weighting is min_obstacle_dist
- _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
-
- if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
- {
- // Optional non-linear cost. Note the max cost (before weighting) is
- // the same as the straight line version and that all other costs are
- // below the straight line (for positive exponent), so it may be
- // necessary to increase weight_obstacle and/or the inflation_weight
- // when using larger exponents.
- _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
- }
-
- // Additional linear inflation cost
- _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0);
-
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]);
- }
-
- /**
- * @brief Set pointer to associated obstacle for the underlying cost function
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setObstacle(const Obstacle* obstacle)
- {
- _measurement = obstacle;
- }
-
- /**
- * @brief Set pointer to the robot model
- * @param robot_model Robot model required for distance calculation
- */
- void setRobotModel(const BaseRobotFootprintModel* robot_model)
- {
- robot_model_ = robot_model;
- }
-
- /**
- * @brief Set all parameters at once
- * @param cfg TebConfig class
- * @param robot_model Robot model required for distance calculation
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
- {
- cfg_ = &cfg;
- robot_model_ = robot_model;
- _measurement = obstacle;
- }
-
-protected:
-
- const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
deleted file mode 100755
index 6e5ec9e..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-#ifndef EDGE_PREFER_ROTDIR_H_
-#define EDGE_PREFER_ROTDIR_H_
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "g2o/core/base_unary_edge.h"
-#include "teb_local_planner/misc.h"
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgePreferRotDir
- * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns
- *
- * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction
- * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$)
- * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n
- * \e weight can be set using setInformation(). \n
- * @see TebOptimalPlanner::AddEdgePreferRotDir
- */
-class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgePreferRotDir()
- {
- _measurement = 1;
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
-
- _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]);
- }
-
- /**
- * @brief Specify the prefered direction of rotation
- * @param dir +1 to prefer the left side, -1 to prefer the right side
- */
- void setRotDir(double dir)
- {
- _measurement = dir;
- }
-
- /** Prefer rotations to the right */
- void preferRight() {_measurement = -1;}
-
- /** Prefer rotations to the right */
- void preferLeft() {_measurement = 1;}
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
deleted file mode 100644
index 8d5bb87..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef EDGE_SHORTEST_PATH_H_
-#define EDGE_SHORTEST_PATH_H_
-
-#include <float.h>
-
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/misc.h"
-
-#include <Eigen/Core>
-
-namespace teb_local_planner {
-
-/**
- * @class EdgeShortestPath
- * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses.
- *
- * @see TebOptimalPlanner::AddEdgesShortestPath
- */
-class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> {
-public:
- /**
- * @brief Construct edge.
- */
- EdgeShortestPath() { this->setMeasurement(0.); }
-
- /**
- * @brief Actual cost function
- */
- void computeError() {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()");
- const VertexPose *pose1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose *pose2 = static_cast<const VertexPose*>(_vertices[1]);
- _error[0] = (pose2->position() - pose1->position()).norm();
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]);
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-} // end namespace
-
-#endif /* EDGE_SHORTEST_PATH_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
deleted file mode 100644
index 0f7671b..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef EDGE_TIMEOPTIMAL_H_
-#define EDGE_TIMEOPTIMAL_H_
-
-#include <float.h>
-
-//#include <base_local_planner/BaseLocalPlannerConfig.h>
-
-#include "teb_local_planner/g2o_types/vertex_timediff.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/misc.h"
-
-#include <Eigen/Core>
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class EdgeTimeOptimal
- * @brief Edge defining the cost function for minimizing transition time of the trajectory.
- *
- * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n
- * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n
- * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n
- * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n
- * @see TebOptimalPlanner::AddEdgesTimeOptimal
- * @remarks Do not forget to call setTebConfig()
- */
-class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeTimeOptimal()
- {
- this->setMeasurement(0.);
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
- const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);
-
- _error[0] = timediff->dt();
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]);
- }
-
-#ifdef USE_ANALYTIC_JACOBI
- /**
- * @brief Jacobi matrix of the cost function specified in computeError().
- */
- void linearizeOplus()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
- _jacobianOplusXi( 0 , 0 ) = 1;
- }
-#endif
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}; // end namespace
-
-#endif /* EDGE_TIMEOPTIMAL_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
deleted file mode 100755
index 29956c0..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h
+++ /dev/null
@@ -1,275 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef EDGE_VELOCITY_H
-#define EDGE_VELOCITY_H
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/vertex_timediff.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/g2o_types/penalties.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/misc.h"
-
-#include <rclcpp/logging.hpp>
-
-#include <exception>
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class EdgeVelocity
- * @brief Edge defining the cost function for limiting the translational and rotational velocity.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n
- * \e v is calculated using the difference quotient and the position parts of both poses. \n
- * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
- * The dimension of the error / cost vector is 2: the first component represents the translational velocity and
- * the second one the rotational velocity.
- * @see TebOptimalPlanner::AddEdgesVelocity
- * @remarks Do not forget to call setTebConfig()
- */
-class EdgeVelocity : public BaseTebMultiEdge<2, double>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeVelocity()
- {
- this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
-
- double dist = deltaS.norm();
- const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
- if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
- {
- double radius = dist/(2*sin(angle_diff/2));
- dist = fabs( angle_diff * radius ); // actual arg length!
- }
- double vel = dist / deltaT->estimate();
-
-// vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
- vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
-
- const double omega = angle_diff / deltaT->estimate();
-
- _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
- }
-
-#ifdef USE_ANALYTIC_JACOBI
-#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
- // Change accordingly...
-
- /**
- * @brief Jacobi matrix of the cost function specified in computeError().
- */
- void linearizeOplus()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- Eigen::Vector2d deltaS = conf2->position() - conf1->position();
- double dist = deltaS.norm();
- double aux1 = dist*deltaT->estimate();
- double aux2 = 1/deltaT->estimate();
-
- double vel = dist * aux2;
- double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
-
- double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
- double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
-
- _jacobianOplus[0].resize(2,3); // conf1
- _jacobianOplus[1].resize(2,3); // conf2
- _jacobianOplus[2].resize(2,1); // deltaT
-
-// if (aux1==0) aux1=1e-6;
-// if (aux2==0) aux2=1e-6;
-
- if (dev_border_vel!=0)
- {
- double aux3 = dev_border_vel / aux1;
- _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1
- _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1
- _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2
- _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2
- _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT
- }
- else
- {
- _jacobianOplus[0](0,0) = 0; // vel x1
- _jacobianOplus[0](0,1) = 0; // vel y1
- _jacobianOplus[1](0,0) = 0; // vel x2
- _jacobianOplus[1](0,1) = 0; // vel y2
- _jacobianOplus[2](0,0) = 0; // vel deltaT
- }
-
- if (dev_border_omega!=0)
- {
- double aux4 = aux2 * dev_border_omega;
- _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT
- _jacobianOplus[0](1,2) = -aux4; // omega angle1
- _jacobianOplus[1](1,2) = aux4; // omega angle2
- }
- else
- {
- _jacobianOplus[2](1,0) = 0; // omega deltaT
- _jacobianOplus[0](1,2) = 0; // omega angle1
- _jacobianOplus[1](1,2) = 0; // omega angle2
- }
-
- _jacobianOplus[0](1,0) = 0; // omega x1
- _jacobianOplus[0](1,1) = 0; // omega y1
- _jacobianOplus[1](1,0) = 0; // omega x2
- _jacobianOplus[1](1,1) = 0; // omega y2
- _jacobianOplus[0](0,2) = 0; // vel angle1
- _jacobianOplus[1](0,2) = 0; // vel angle2
- }
-#endif
-#endif
-
-
-public:
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-
-
-
-/**
- * @class EdgeVelocityHolonomic
- * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta.
- *
- * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n
- * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n
- * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n
- * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
- * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis,
- * the second one w.r.t. the y-axis and the third one the rotational velocity.
- * @see TebOptimalPlanner::AddEdgesVelocity
- * @remarks Do not forget to call setTebConfig()
- */
-class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeVelocityHolonomic()
- {
- this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
- Eigen::Vector2d deltaS = conf2->position() - conf1->position();
-
- double cos_theta1 = std::cos(conf1->theta());
- double sin_theta1 = std::sin(conf1->theta());
-
- // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
- double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
- double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
-
- double vx = r_dx / deltaT->estimate();
- double vy = r_dy / deltaT->estimate();
- double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
-
- _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
- _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
- _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
- "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);
- }
-
-
-public:
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
deleted file mode 100644
index 1dff242..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h
+++ /dev/null
@@ -1,127 +0,0 @@
-#pragma once
-
-
-#include <teb_local_planner/optimal_planner.h>
-#include <teb_local_planner/g2o_types/base_teb_edges.h>
-#include <teb_local_planner/g2o_types/vertex_timediff.h>
-#include <teb_local_planner/g2o_types/vertex_pose.h>
-#include <teb_local_planner/robot_footprint_model.h>
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class EdgeVelocityObstacleRatio
- * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
- *
- * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
- * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
- * \e dist2point denotes the minimum distance to the point obstacle. \n
- * \e weight can be set using setInformation(). \n
- * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
- * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
- * @remarks Do not forget to call setTebConfig() and setObstacle()
- */
-class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeVelocityObstacleRatio() :
- robot_model_(nullptr)
- {
- // The three vertices are two poses and one time difference
- this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()");
- const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
- const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
- const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
-
- const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
-
- double dist = deltaS.norm();
- const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
- if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
- {
- double radius = dist/(2*sin(angle_diff/2));
- dist = fabs( angle_diff * radius ); // actual arg length!
- }
- double vel = dist / deltaT->estimate();
-
- vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
-
- const double omega = angle_diff / deltaT->estimate();
-
- double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement);
-
- double ratio;
- if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound)
- ratio = 0;
- else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound)
- ratio = 1;
- else
- ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) /
- (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound);
- ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel;
-
- const double max_vel_fwd = ratio * cfg_->robot.max_vel_x;
- const double max_omega = ratio * cfg_->robot.max_vel_theta;
- _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0);
- _error[1] = penaltyBoundToInterval(omega, max_omega, 0);
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::computeError() _error[0]=%f , _error[1]=%f\n",_error[0],_error[1]);
- }
-
- /**
- * @brief Set pointer to associated obstacle for the underlying cost function
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setObstacle(const Obstacle* obstacle)
- {
- _measurement = obstacle;
- }
-
- /**
- * @brief Set pointer to the robot model
- * @param robot_model Robot model required for distance calculation
- */
- void setRobotModel(const BaseRobotFootprintModel* robot_model)
- {
- robot_model_ = robot_model;
- }
-
- /**
- * @brief Set all parameters at once
- * @param cfg TebConfig class
- * @param robot_model Robot model required for distance calculation
- * @param obstacle 2D position vector containing the position of the obstacle
- */
- void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
- {
- cfg_ = &cfg;
- robot_model_ = robot_model;
- _measurement = obstacle;
- }
-
-protected:
-
- const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-} // end namespace
-
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
deleted file mode 100755
index 5575d0f..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-#ifndef EDGE_VIA_POINT_H_
-#define EDGE_VIA_POINT_H_
-
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/base_teb_edges.h"
-#include "teb_local_planner/misc.h"
-
-#include "g2o/core/base_unary_edge.h"
-
-
-namespace teb_local_planner
-{
-
-/**
- * @class EdgeViaPoint
- * @brief Edge defining the cost function for pushing a configuration towards a via point
- *
- * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
- * \f$ \min dist2point \cdot weight \f$. \n
- * \e dist2point denotes the distance to the via point. \n
- * \e weight can be set using setInformation(). \n
- * @see TebOptimalPlanner::AddEdgesViaPoints
- * @remarks Do not forget to call setTebConfig() and setViaPoint()
- */
-class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
-{
-public:
-
- /**
- * @brief Construct edge.
- */
- EdgeViaPoint()
- {
- _measurement = NULL;
- }
-
- /**
- * @brief Actual cost function
- */
- void computeError()
- {
- TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
- const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
-
- _error[0] = (bandpt->position() - *_measurement).norm();
-
- TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
- }
-
- /**
- * @brief Set pointer to associated via point for the underlying cost function
- * @param via_point 2D position vector containing the position of the via point
- */
- void setViaPoint(const Eigen::Vector2d* via_point)
- {
- _measurement = via_point;
- }
-
- /**
- * @brief Set all parameters at once
- * @param cfg TebConfig class
- * @param via_point 2D position vector containing the position of the via point
- */
- void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
- {
- cfg_ = &cfg;
- _measurement = via_point;
- }
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-} // end namespace
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
deleted file mode 100755
index 6870558..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h
+++ /dev/null
@@ -1,193 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef PENALTIES_H
-#define PENALTIES_H
-
-#include <cmath>
-#include <Eigen/Core>
-#include <g2o/stuff/misc.h>
-
-namespace teb_local_planner
-{
-
-/**
- * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
- * @param var The scalar that should be bounded
- * @param a lower and upper absolute bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundToIntervalDerivative
- * @return Penalty / cost value that is nonzero if the constraint is not satisfied
- */
-inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
-{
- if (var < -a+epsilon)
- {
- return (-var - (a - epsilon));
- }
- if (var <= a-epsilon)
- {
- return 0.;
- }
- else
- {
- return (var - (a - epsilon));
- }
-}
-
-/**
- * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
- * @param var The scalar that should be bounded
- * @param a lower bound
- * @param b upper bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundToIntervalDerivative
- * @return Penalty / cost value that is nonzero if the constraint is not satisfied
- */
-inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
-{
- if (var < a+epsilon)
- {
- return (-var + (a + epsilon));
- }
- if (var <= b-epsilon)
- {
- return 0.;
- }
- else
- {
- return (var - (b - epsilon));
- }
-}
-
-
-/**
- * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$
- * @param var The scalar that should be bounded
- * @param a lower bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundFromBelowDerivative
- * @return Penalty / cost value that is nonzero if the constraint is not satisfied
- */
-inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
-{
- if (var >= a+epsilon)
- {
- return 0.;
- }
- else
- {
- return (-var + (a+epsilon));
- }
-}
-
-/**
- * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
- * @param var The scalar that should be bounded
- * @param a lower and upper absolute bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundToInterval
- * @return Derivative of the penalty function w.r.t. \c var
- */
-inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
-{
- if (var < -a+epsilon)
- {
- return -1;
- }
- if (var <= a-epsilon)
- {
- return 0.;
- }
- else
- {
- return 1;
- }
-}
-
-/**
- * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
- * @param var The scalar that should be bounded
- * @param a lower bound
- * @param b upper bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundToInterval
- * @return Derivative of the penalty function w.r.t. \c var
- */
-inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon)
-{
- if (var < a+epsilon)
- {
- return -1;
- }
- if (var <= b-epsilon)
- {
- return 0.;
- }
- else
- {
- return 1;
- }
-}
-
-
-/**
- * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$
- * @param var The scalar that should be bounded
- * @param a lower bound
- * @param epsilon safty margin (move bound to the interior of the interval)
- * @see penaltyBoundFromBelow
- * @return Derivative of the penalty function w.r.t. \c var
- */
-inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon)
-{
- if (var >= a+epsilon)
- {
- return 0.;
- }
- else
- {
- return -1;
- }
-}
-
-
-} // namespace teb_local_planner
-
-
-#endif // PENALTIES_H
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
deleted file mode 100755
index 781a272..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h
+++ /dev/null
@@ -1,229 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef VERTEX_POSE_H_
-#define VERTEX_POSE_H_
-
-#include <g2o/config.h>
-#include <g2o/core/base_vertex.h>
-#include <g2o/core/hyper_graph_action.h>
-#include <g2o/stuff/misc.h>
-
-#include "teb_local_planner/pose_se2.h"
-
-namespace teb_local_planner
-{
-
-/**
- * @class VertexPose
- * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o
- * @see PoseSE2
- * @see VertexTimeDiff
- */
-class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
-{
-public:
-
- /**
- * @brief Default constructor
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexPose(bool fixed = false)
- {
- setToOriginImpl();
- setFixed(fixed);
- }
-
- /**
- * @brief Construct pose using a given PoseSE2
- * @param pose PoseSE2 defining the pose [x, y, angle_rad]
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexPose(const PoseSE2& pose, bool fixed = false)
- {
- _estimate = pose;
- setFixed(fixed);
- }
-
- /**
- * @brief Construct pose using a given 2D position vector and orientation
- * @param position Eigen::Vector2d containing x and y coordinates
- * @param theta yaw-angle
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed = false)
- {
- _estimate.position() = position;
- _estimate.theta() = theta;
- setFixed(fixed);
- }
-
- /**
- * @brief Construct pose using single components x, y, and the yaw angle
- * @param x x-coordinate
- * @param y y-coordinate
- * @param theta yaw angle in rad
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexPose(double x, double y, double theta, bool fixed = false)
- {
- _estimate.x() = x;
- _estimate.y() = y;
- _estimate.theta() = theta;
- setFixed(fixed);
- }
-
- /**
- * @brief Access the pose
- * @see estimate
- * @return reference to the PoseSE2 estimate
- */
- inline PoseSE2& pose() {return _estimate;}
-
- /**
- * @brief Access the pose (read-only)
- * @see estimate
- * @return const reference to the PoseSE2 estimate
- */
- inline const PoseSE2& pose() const {return _estimate;}
-
-
- /**
- * @brief Access the 2D position part
- * @see estimate
- * @return reference to the 2D position part
- */
- inline Eigen::Vector2d& position() {return _estimate.position();}
-
- /**
- * @brief Access the 2D position part (read-only)
- * @see estimate
- * @return const reference to the 2D position part
- */
- inline const Eigen::Vector2d& position() const {return _estimate.position();}
-
- /**
- * @brief Access the x-coordinate the pose
- * @return reference to the x-coordinate
- */
- inline double& x() {return _estimate.x();}
-
- /**
- * @brief Access the x-coordinate the pose (read-only)
- * @return const reference to the x-coordinate
- */
- inline const double& x() const {return _estimate.x();}
-
- /**
- * @brief Access the y-coordinate the pose
- * @return reference to the y-coordinate
- */
- inline double& y() {return _estimate.y();}
-
- /**
- * @brief Access the y-coordinate the pose (read-only)
- * @return const reference to the y-coordinate
- */
- inline const double& y() const {return _estimate.y();}
-
- /**
- * @brief Access the orientation part (yaw angle) of the pose
- * @return reference to the yaw angle
- */
- inline double& theta() {return _estimate.theta();}
-
- /**
- * @brief Access the orientation part (yaw angle) of the pose (read-only)
- * @return const reference to the yaw angle
- */
- inline const double& theta() const {return _estimate.theta();}
-
- /**
- * @brief Set the underlying estimate (2D vector) to zero.
- */
- virtual void setToOriginImpl() override
- {
- _estimate.setZero();
- }
-
- /**
- * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$.
- * A simple addition for the position.
- * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$
- * @param update increment that should be added to the previous esimate
- */
- virtual void oplusImpl(const double* update) override
- {
- _estimate.plus(update);
- }
-
- /**
- * @brief Read an estimate from an input stream.
- * First the x-coordinate followed by y and the yaw angle.
- * @param is input stream
- * @return always \c true
- */
- virtual bool read(std::istream& is) override
- {
- is >> _estimate.x() >> _estimate.y() >> _estimate.theta();
- return true;
- }
-
- /**
- * @brief Write the estimate to an output stream
- * First the x-coordinate followed by y and the yaw angle.
- * @param os output stream
- * @return \c true if the export was successful, otherwise \c false
- */
- virtual bool write(std::ostream& os) const override
- {
- os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta();
- return os.good();
- }
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}
-
-#endif // VERTEX_POSE_H_
diff --git a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h b/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
deleted file mode 100755
index 4eead6c..0000000
--- a/src/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Notes:
- * The following class is derived from a class defined by the
- * g2o-framework. g2o is licensed under the terms of the BSD License.
- * Refer to the base class source for detailed licensing information.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef VERTEX_TIMEDIFF_H
-#define VERTEX_TIMEDIFF_H
-
-
-#include "g2o/config.h"
-#include "g2o/core/base_vertex.h"
-#include "g2o/core/hyper_graph_action.h"
-
-#include <Eigen/Core>
-
-namespace teb_local_planner
-{
-
-/**
- * @class VertexTimeDiff
- * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o
- */
-class VertexTimeDiff : public g2o::BaseVertex<1, double>
-{
-public:
-
- /**
- * @brief Default constructor
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexTimeDiff(bool fixed = false)
- {
- setToOriginImpl();
- setFixed(fixed);
- }
-
- /**
- * @brief Construct the TimeDiff vertex with a value
- * @param dt time difference value of the vertex
- * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
- */
- VertexTimeDiff(double dt, bool fixed = false)
- {
- _estimate = dt;
- setFixed(fixed);
- }
-
- /**
- * @brief Access the timediff value of the vertex
- * @see estimate
- * @return reference to dt
- */
- inline double& dt() {return _estimate;}
-
- /**
- * @brief Access the timediff value of the vertex (read-only)
- * @see estimate
- * @return const reference to dt
- */
- inline const double& dt() const {return _estimate;}
-
- /**
- * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default.
- */
- virtual void setToOriginImpl() override
- {
- _estimate = 0.1;
- }
-
- /**
- * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$.
- * A simple addition implements what we want.
- * @param update increment that should be added to the previous esimate
- */
- virtual void oplusImpl(const double* update) override
- {
- _estimate += *update;
- }
-
- /**
- * @brief Read an estimate of \f$ \Delta T \f$ from an input stream
- * @param is input stream
- * @return always \c true
- */
- virtual bool read(std::istream& is) override
- {
- is >> _estimate;
- return true;
- }
-
- /**
- * @brief Write the estimate \f$ \Delta T \f$ to an output stream
- * @param os output stream
- * @return \c true if the export was successful, otherwise \c false
- */
- virtual bool write(std::ostream& os) const override
- {
- os << estimate();
- return os.good();
- }
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/graph_search.h b/src/teb_local_planner/include/teb_local_planner/graph_search.h
deleted file mode 100644
index 3c4bd7c..0000000
--- a/src/teb_local_planner/include/teb_local_planner/graph_search.h
+++ /dev/null
@@ -1,215 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2017,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Authors: Christoph Rösmann, Franz Albers
- *********************************************************************/
-
-#ifndef GRAPH_SEARCH_INTERFACE_H
-#define GRAPH_SEARCH_INTERFACE_H
-
-#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
- #include <boost/graph/adjacency_list.hpp>
-#else
- // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
- // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
- // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
- #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
- #include <boost/graph/adjacency_list.hpp>
- #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
-#endif
-
-#include <boost/graph/graph_traits.hpp>
-#include <boost/graph/depth_first_search.hpp>
-#include <boost/utility.hpp>
-#include <boost/random.hpp>
-
-#include <Eigen/Core>
-
-#include <geometry_msgs/msg/twist.hpp>
-
-#include "teb_local_planner/equivalence_relations.h"
-#include "teb_local_planner/pose_se2.h"
-#include "teb_local_planner/teb_config.h"
-
-namespace teb_local_planner
-{
-
-class HomotopyClassPlanner; // Forward declaration
-
-//! Vertex in the graph that is used to find homotopy classes (only stores 2D positions)
-struct HcGraphVertex
-{
-public:
- Eigen::Vector2d pos; // position of vertices in the map
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-//! Abbrev. for the homotopy class search-graph type @see HcGraphVertex
-typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
-//! Abbrev. for vertex type descriptors in the homotopy class search-graph
-typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
-//! Abbrev. for edge type descriptors in the homotopy class search-graph
-typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
-//! Abbrev. for the vertices iterator of the homotopy class search-graph
-typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
-//! Abbrev. for the edges iterator of the homotopy class search-graph
-typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
-//! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one
-typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;
-
-//!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors
-inline std::complex<long double> getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
-{
- return std::complex<long double>(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y());
-}
-
-//!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors
-inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
-{
- return graph[vert_descriptor].pos;
-}
-
-/**
- * @brief Base class for graph based path planning / homotopy class sampling
- */
-class GraphSearchInterface
-{
-public:
-
- virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false) = 0;
-
- /**
- * @brief Clear any existing graph of the homotopy class search
- */
- void clearGraph() {graph_.clear();}
-
- // HcGraph graph() const {return graph_;}
- // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above.
- HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes.
-
-protected:
- /**
- * @brief Protected constructor that should be called by subclasses
- */
- GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){}
-
- /**
- * @brief Depth First Search implementation to find all paths between the start and the specified goal vertex.
- *
- * Complete paths are stored to the internal path container.
- * @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/
- * @param g Graph on which the depth first should be performed
- * @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
- * @param goal Desired goal vertex
- * @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB
- * @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- */
-
- void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-
-protected:
- const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
- HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding.
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-
-class lrKeyPointGraph : public GraphSearchInterface
-{
-public:
- lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
-
- virtual ~lrKeyPointGraph(){}
-
- /**
- * @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.
- *
- * This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading. \n
- * All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n
- * This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph()
- * method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths.
- *
- * @see createProbRoadmapGraph
- * @param start Start pose from wich to start on (e.g. the current robot pose).
- * @param goal Goal pose to find paths to (e.g. the robot's goal).
- * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
- * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- */
- virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-};
-
-
-
-
-class ProbRoadmapGraph : public GraphSearchInterface
-{
-public:
- ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
-
- virtual ~ProbRoadmapGraph(){}
-
-
- /**
- * @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
- *
- * This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n
- * Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n
- * Use the sampling method for complex, non-point or huge obstacles. \n
- * You may call createGraph() instead.
- *
- * @see createGraph
- * @param start Start pose from wich to start on (e.g. the current robot pose).
- * @param goal Goal pose to find paths to (e.g. the robot's goal).
- * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
- * @param no_samples number of random samples
- * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- */
- virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-
-private:
- boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints.
-};
-} // end namespace
-
-#endif // GRAPH_SEARCH_INTERFACE_H
diff --git a/src/teb_local_planner/include/teb_local_planner/h_signature.h b/src/teb_local_planner/include/teb_local_planner/h_signature.h
deleted file mode 100644
index 8a11f0f..0000000
--- a/src/teb_local_planner/include/teb_local_planner/h_signature.h
+++ /dev/null
@@ -1,432 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2017,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Authors: Christoph Rösmann, Franz Albers
- *********************************************************************/
-
-#ifndef H_SIGNATURE_H_
-#define H_SIGNATURE_H_
-
-#include <boost/math/special_functions.hpp>
-
-#include "teb_local_planner/equivalence_relations.h"
-#include "teb_local_planner/misc.h"
-#include "teb_local_planner/obstacles.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/timed_elastic_band.h"
-
-#include <rclcpp/rclcpp.hpp>
-#include <math.h>
-#include <algorithm>
-#include <functional>
-#include <vector>
-#include <iterator>
-
-
-namespace teb_local_planner
-{
-
-/**
- * @brief The H-signature defines an equivalence relation based on homology in terms of complex calculus.
- *
- * The H-Signature depends on the obstacle configuration and can be utilized
- * to check whether two trajectores belong to the same homology class.
- * Refer to: \n
- * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
- */
-class HSignature : public EquivalenceClass
-{
-
-public:
-
- /**
- * @brief Constructor accepting a TebConfig
- * @param cfg TebConfig storing some user configuration options
- */
- HSignature(const TebConfig& cfg) : cfg_(&cfg) {}
-
-
- /**
- * @brief Calculate the H-Signature of a path
- *
- * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
- * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
- * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
- * that returns a complex value for the position (Re(*)=x, Im(*)=y).
- *
- * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
- *
- * @param path_start Iterator to the first element in the path
- * @param path_end Iterator to the last element in the path
- * @param obstacles obstacle container
- * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
- * @tparam BidirIter Bidirectional iterator type
- * @tparam Fun function of the form std::complex< long double > (const T& point_type)
- */
- template<typename BidirIter, typename Fun>
- void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles)
- {
- if (obstacles->empty())
- {
- hsignature_ = std::complex<double>(0,0);
- return;
- }
-
-
- TEB_ASSERT_MSG(cfg_->hcp.h_signature_prescaler>0.1 && cfg_->hcp.h_signature_prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
-
- // guess values for f0
- // paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
- int m = std::max( (int)obstacles->size()-1, 5 ); // for only a few obstacles we need a min threshold in order to get significantly high H-Signatures
-
- int a = (int) std::ceil(double(m)/2.0);
- int b = m-a;
-
- std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
-
- typedef std::complex<long double> cplx;
- // guess map size (only a really really coarse guess is required
- // use distance from start to goal as distance to each direction
- // TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
- cplx start = fun_cplx_point(*path_start);
- cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
- cplx delta = end-start;
- cplx normal(-delta.imag(), delta.real());
- cplx map_bottom_left;
- cplx map_top_right;
- if (std::abs(delta) < 3.0)
- { // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
- map_bottom_left = start + cplx(0, -3);
- map_top_right = start + cplx(3, 3);
- }
- else
- {
- map_bottom_left = start - normal;
- map_top_right = start + delta + normal;
- }
-
- hsignature_ = 0; // reset local signature
-
- std::vector<double> imag_proposals(5);
-
- // iterate path
- while(path_start != path_end)
- {
- cplx z1 = fun_cplx_point(*path_start);
- cplx z2 = fun_cplx_point(*std::next(path_start));
-
- for (std::size_t l=0; l<obstacles->size(); ++l) // iterate all obstacles
- {
- cplx obst_l = obstacles->at(l)->getCentroidCplx();
- //cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
- cplx f0 = (long double) cfg_->hcp.h_signature_prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
-
- // denum contains product with all obstacles exepct j==l
- cplx Al = f0;
- for (std::size_t j=0; j<obstacles->size(); ++j)
- {
- if (j==l)
- continue;
- cplx obst_j = obstacles->at(j)->getCentroidCplx();
- cplx diff = obst_l - obst_j;
- //if (diff.real()!=0 || diff.imag()!=0)
- if (std::abs(diff)<0.05) // skip really close obstacles
- continue;
- else
- Al /= diff;
- }
- // compute log value
- double diff2 = std::abs(z2-obst_l);
- double diff1 = std::abs(z1-obst_l);
- if (diff2 == 0 || diff1 == 0)
- continue;
- double log_real = std::log(diff2)-std::log(diff1);
- // complex ln has more than one solution -> choose minimum abs angle -> paper
- double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l);
- imag_proposals.at(0) = arg_diff;
- imag_proposals.at(1) = arg_diff+2*M_PI;
- imag_proposals.at(2) = arg_diff-2*M_PI;
- imag_proposals.at(3) = arg_diff+4*M_PI;
- imag_proposals.at(4) = arg_diff-4*M_PI;
- double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
- cplx log_value(log_real,log_imag);
- //cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
- hsignature_ += Al*log_value;
- }
- ++path_start;
- }
- }
-
-
- /**
- * @brief Check if two candidate classes are equivalent
- * @param other The other equivalence class to test with
- */
- virtual bool isEqual(const EquivalenceClass& other) const
- {
- const HSignature* hother = dynamic_cast<const HSignature*>(&other); // TODO: better architecture without dynamic_cast
- if (hother)
- {
- double diff_real = std::abs(hother->hsignature_.real() - hsignature_.real());
- double diff_imag = std::abs(hother->hsignature_.imag() - hsignature_.imag());
- if (diff_real<=cfg_->hcp.h_signature_threshold && diff_imag<=cfg_->hcp.h_signature_threshold)
- return true; // Found! Homotopy class already exists, therefore nothing added
- }
- else
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature equivalence classes with types other than HSignature.");
-
- return false;
- }
-
- /**
- * @brief Check if the equivalence value is detected correctly
- * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
- */
- virtual bool isValid() const
- {
- return std::isfinite(hsignature_.real()) && std::isfinite(hsignature_.imag());
- }
-
- /**
- * @brief Check if the trajectory is non-looping around an obstacle.
- * @return Returns always true, as this cannot be detected by this kind of H-Signature.
- */
- virtual bool isReasonable() const
- {
- return true;
- }
-
- /**
- * @brief Get the current value of the h-signature (read-only)
- * @return h-signature in complex-number format
- */
- const std::complex<long double>& value() const {return hsignature_;}
-
-
-private:
-
- const TebConfig* cfg_;
- std::complex<long double> hsignature_;
-};
-
-
-
-
-
-/**
- * @brief The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism.
- *
- * The H-Signature depends on the obstacle configuration and can be utilized
- * to check whether two trajectores belong to the same homology class.
- * Refer to: \n
- * - S. Bhattacharya et al.: Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D, 2011
- */
-class HSignature3d : public EquivalenceClass
-{
-public:
- /**
- * @brief Constructor accepting a TebConfig
- * @param cfg TebConfig storing some user configuration options
- */
- HSignature3d(const TebConfig& cfg) : cfg_(&cfg) {}
-
-
- /**
- * @brief Calculate the H-Signature of a path
- *
- * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
- * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
- * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
- * that returns a complex value for the position (Re(*)=x, Im(*)=y).
- *
- * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
- *
- * @param path_start Iterator to the first element in the path
- * @param path_end Iterator to the last element in the path
- * @param obstacles obstacle container
- * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
- * @tparam BidirIter Bidirectional iterator type
- * @tparam Fun function of the form std::complex< long double > (const T& point_type)
- */
- template<typename BidirIter, typename Fun>
- void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
- boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
- {
- hsignature3d_.resize(obstacles->size());
-
- std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points)
-
- constexpr int num_int_steps_per_segment = 10;
-
- for (std::size_t l = 0; l < obstacles->size(); ++l) // iterate all obstacles
- {
- double H = 0;
- double transition_time = 0;
- double next_transition_time = 0;
- BidirIter path_iter;
- TimeDiffSequence::iterator timediff_iter;
-
- Eigen::Vector3d s1 (obstacles->at(l)->getCentroid()(0), obstacles->at(l)->getCentroid()(1), 0);
- double t = 120; // some large value for defining the end point of the obstacle/"conductor" model
- Eigen::Vector3d s2;
- obstacles->at(l)->predictCentroidConstantVelocity(t, s2.head(2));
- s2[2] = t;
- Eigen::Vector3d ds = s2 - s1;
- double ds_sq_norm = ds.squaredNorm(); // by definition not zero as t > 0 (3rd component)
-
- // iterate path
- for (path_iter = path_start, timediff_iter = timediff_start.get(); path_iter != path_end; ++path_iter, ++timediff_iter)
- {
- std::complex<long double> z1 = fun_cplx_point(*path_iter);
- std::complex<long double> z2 = fun_cplx_point(*std::next(path_iter));
-
- transition_time = next_transition_time;
- if (timediff_start == boost::none || timediff_end == boost::none) // if no time information is provided yet, approximate transition time
- next_transition_time += std::abs(z2 - z1) / cfg_->robot.max_vel_x; // Approximate the time, if no time is known
- else // otherwise use the time information from the teb trajectory
- {
- if (std::distance(path_iter, path_end) != std::distance(timediff_iter, timediff_end.get())) {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Size of poses and timediff vectors does not match. This is a bug.");
- }
- next_transition_time += (*timediff_iter)->dt();
- }
-
- Eigen::Vector3d direction_vec;
- direction_vec[0] = z2.real() - z1.real();
- direction_vec[1] = z2.imag() - z1.imag();
- direction_vec[2] = next_transition_time - transition_time;
-
- if(direction_vec.norm() < 1e-15) // Coincident poses
- continue;
-
- Eigen::Vector3d r(z1.real(), z1.imag(), transition_time);
- Eigen::Vector3d dl = 1.0/static_cast<double>(num_int_steps_per_segment) * direction_vec; // Integrate with multiple steps between each pose
- Eigen::Vector3d p1, p2, d, phi;
- for (int i = 0; i < num_int_steps_per_segment; ++i, r += dl)
- {
- p1 = s1 - r;
- p2 = s2 - r;
- d = (ds.cross(p1.cross(p2))) / ds_sq_norm;
- phi = 1.0 / d.squaredNorm() * ((d.cross(p2) / p2.norm()) - (d.cross(p1) / p1.norm()));
- H += phi.dot(dl);
- }
- }
-
- // normalize to 1
- hsignature3d_.at(l) = H/(4.0*M_PI);
- }
- }
-
- /**
- * @brief Check if two candidate classes are equivalent
- *
- * If the absolute value of the H-Signature is equal or greater than 1, a loop (in x-y) around the obstacle is indicated.
- * Positive H-Signature: Obstacle lies on the left hand side of the planned trajectory
- * Negative H-Signature: Obstacle lies on the right hand side of the planned trajectory
- * H-Signature equals zero: Obstacle lies in infinity, has no influence on trajectory
- *
- * @param other The other equivalence class to test with
- */
- virtual bool isEqual(const EquivalenceClass& other) const
- {
- const HSignature3d* hother = dynamic_cast<const HSignature3d*>(&other); // TODO: better architecture without dynamic_cast
- if (hother)
- {
- if (hsignature3d_.size() == hother->hsignature3d_.size())
- {
- for(size_t i = 0; i < hsignature3d_.size(); ++i)
- {
- // If the H-Signature for one obstacle is below this threshold, that obstacle is far away from the planned trajectory,
- // and therefore ignored in the homotopy class planning
- if (std::abs(hother->hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold || std::abs(hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold)
- continue;
-
- if (boost::math::sign(hother->hsignature3d_.at(i)) != boost::math::sign(hsignature3d_.at(i)))
- return false; // Signatures are not equal, new homotopy class
- }
- return true; // Found! Homotopy class already exists, therefore nothing added
- }
- }
- else {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature3d equivalence classes with types other than HSignature3d.");
- }
-
- return false;
- }
-
- /**
- * @brief Check if the equivalence value is detected correctly
- * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
- */
- virtual bool isValid() const
- {
- for(const double& value : hsignature3d_)
- {
- if (!std::isfinite(value))
- return false;
- }
- return true;
- }
-
- /**
- * @brief Check if the trajectory is non-looping around any obstacle. Values greater than 1 indicate a looping trajectory.
- * @return Returns false, if the trajectory loops around an obstacle
- */
- virtual bool isReasonable() const
- {
- for(const double& value : hsignature3d_)
- {
- if (value > 1.0)
- return false;
- }
- return true;
- }
-
- /**
- * @brief Get the current h-signature (read-only)
- * @return h-signature in complex-number format
- */
- const std::vector<double>& values() const {return hsignature3d_;}
-
-private:
- const TebConfig* cfg_;
- std::vector<double> hsignature3d_;
-};
-
-
-} // namespace teb_local_planner
-
-
-#endif /* H_SIGNATURE_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
deleted file mode 100644
index 841e630..0000000
--- a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h
+++ /dev/null
@@ -1,591 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef HOMOTOPY_CLASS_PLANNER_H_
-#define HOMOTOPY_CLASS_PLANNER_H_
-
-#include <math.h>
-
-#include <algorithm>
-#include <functional>
-#include <iterator>
-#include <memory>
-#include <vector>
-#include <iterator>
-#include <random>
-
-#include <visualization_msgs/msg/marker.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <std_msgs/msg/color_rgba.hpp>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include "teb_local_planner/planner_interface.h"
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/obstacles.h"
-#include "teb_local_planner/optimal_planner.h"
-#include "teb_local_planner/visualization.h"
-#include "teb_local_planner/robot_footprint_model.h"
-#include "teb_local_planner/equivalence_relations.h"
-#include "teb_local_planner/graph_search.h"
-
-
-namespace teb_local_planner
-{
-
-//!< Inline function used for calculateHSignature() in combination with VertexPose pointers
-inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
-{
- return std::complex<long double>(pose->x(), pose->y());
-};
-
-
-//!< Inline function used for calculateHSignature() in combination with geometry_msgs::msg::PoseStamped
-inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::msg::PoseStamped& pose)
-{
- return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
-};
-
-/**
- * @class HomotopyClassPlanner
- * @brief Local planner that explores alternative homotopy classes, create a plan for each alternative
- * and finally return the robot controls for the current best path (repeated in each sampling interval)
- *
- * Equivalence classes (e.g. homotopy) are explored using the help of a search-graph. \n
- * A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate
- * per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant: \n
- * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
- * - C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
- *
- * Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory
- * optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand). \n
- * Depending on the config parameters, the optimization is performed in parallel. \n
- * After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the
- * cost already contains important features like clearance from obstacles and transition time. \n
- *
- * Everyhting is performed by calling one of the overloaded plan() methods. \n
- * Afterwards the velocity command to control the robot is obtained from the "best" candidate
- * via getVelocityCommand(). \n
- *
- * All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories
- * are preferred against new path initilizations in order to improve the hot-starting capability.
- */
-class HomotopyClassPlanner : public PlannerInterface
-{
-public:
-
- using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
-
- /**
- * @brief Default constructor
- */
- HomotopyClassPlanner();
-
- /**
- * @brief Construct and initialize the HomotopyClassPlanner
- * @param node Shared pointer for rclcpp::Node
- * @param cfg Const reference to the TebConfig class for internal parameters
- * @param obstacles Container storing all relevant obstacles (see Obstacle)
- * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
- * @param visualization Shared pointer to the TebVisualization class (optional)
- * @param via_points Container storing via-points (optional)
- */
- HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
- TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
-
- /**
- * @brief Destruct the HomotopyClassPlanner.
- */
- virtual ~HomotopyClassPlanner();
-
- /**
- * @brief Initialize the HomotopyClassPlanner
- * @param node Shared pointer for rclcpp::Node
- * @param cfg Const reference to the TebConfig class for internal parameters
- * @param obstacles Container storing all relevant obstacles (see Obstacle)
- * @param visualization Shared pointer to the TebVisualization class (optional)
- * @param via_points Container storing via-points (optional)
- */
- void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
- TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
-
- /** @name Plan a trajectory */
- //@{
-
- /**
- * @brief Plan a trajectory based on an initial reference plan.
- *
- * Provide this method to create and optimize a trajectory that is initialized
- * according to an initial reference plan (given as a container of poses).
- * @warning The current implementation extracts only the start and goal pose and calls the overloaded plan()
- * @param initial_plan vector of geometry_msgs::msg::PoseStamped (must be valid until clearPlanner() is called!)
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
- /**
- * @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
- *
- * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
- * @param start tf::Pose containing the start pose of the trajectory
- * @param goal tf::Pose containing the goal pose of the trajectory
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- // tf2 doesn't have tf::Pose
-// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
- /**
- * @brief Plan a trajectory between a given start and goal pose.
- *
- * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
- * @param start PoseSE2 containing the start pose of the trajectory
- * @param goal PoseSE2 containing the goal pose of the trajectory
- * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
- /**
- * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
- * @warning Call plan() first and check if the generated plan is feasible.
- * @param[out] vx translational velocity [m/s]
- * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
- * @param[out] omega rotational velocity [rad/s]
- * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
- * @return \c true if command is valid, \c false otherwise
- */
- virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
-
- /**
- * @brief Access current best trajectory candidate (that relates to the "best" homotopy class).
- *
- * If no trajectory is available, the pointer will be empty.
- * If only a single trajectory is available, return it.
- * Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods).
- * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
- */
- TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
-
- /**
- * @brief Check whether the planned trajectory is feasible or not.
- *
- * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
- * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
- * @param costmap_model Pointer to the costmap model
- * @param footprint_spec The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
- * @return \c true, if the robot footprint along the first part of the trajectory intersects with
- * any obstacle in the costmap, \c false otherwise.
- */
- virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
- double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
-
- /**
- * @brief In case of empty best teb, scores again the available plans to find the best one.
- * The best_teb_ variable is updated consequently.
- * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
- * An empty pointer is returned if no plan is available.
- */
- TebOptimalPlannerPtr findBestTeb();
-
- /**
- * @brief Removes the specified teb and the corresponding homotopy class from the list of available ones.
- * @param pointer to the teb Band to be removed
- * @return Iterator to the next valid teb if available, else to the end of the tebs container.
- */
- TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);
-
- //@}
-
- /** @name Visualization */
- //@{
-
- /**
- * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
- * @param visualization shared pointer to a TebVisualization instance
- * @see visualizeTeb
- */
- void setVisualization(const TebVisualizationPtr & visualization) override;
-
- /**
- * @brief Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
- *
- * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
- * @see setVisualization
- */
- virtual void visualize();
-
- //@}
-
- /** @name Important steps that are called during planning */
- //@{
-
-
- /**
- * @brief Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.
- *
- * This "all-in-one" method creates a graph with position keypoints from which
- * feasible paths (with clearance from obstacles) are extracted. \n
- * All obtained paths are filted to only keep a single path for each equivalence class. \n
- * Each time a new equivalence class is explored (by means of \b no previous trajectory/TEB remain in that equivalence class),
- * a new trajectory/TEB will be initialized. \n
- *
- * Everything is prepared now for the optimization step: see optimizeAllTEBs().
- * @param start Current start pose (e.g. pose of the robot)
- * @param goal Goal pose (e.g. robot's goal)
- * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
- * @param @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- */
- void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel = false);
-
- /**
- * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path
- *
- * Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters.
- * @param path_start start iterator of a generic 2d path
- * @param path_end end iterator of a generic 2d path
- * @param fun_position unary function that returns the Eigen::Vector2d object
- * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
- * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- * @tparam BidirIter Bidirectional iterator type
- * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
- * @return Shared pointer to the newly created teb optimal planner
- */
- template<typename BidirIter, typename Fun>
- TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-
- /**
- * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal
- * @param start start pose
- * @param goal goal pose
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- * @return Shared pointer to the newly created teb optimal planner
- */
- TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-
- /**
- * @brief Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container
- * @param initial_plan container of poses (start and goal orientation should be valid!)
- * @param start_velocity start velocity (optional)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
- * @return Shared pointer to the newly created teb optimal planner
- */
- TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
-
- /**
- * @brief Update TEBs with new pose, goal and current velocity.
- * @param start New start pose (optional)
- * @param goal New goal pose (optional)
- * @param start_velocity start velocity (optional)
- */
- void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity);
-
-
- /**
- * @brief Optimize all available trajectories by invoking the optimizer on each one.
- *
- * Depending on the configuration parameters, the optimization is performed either single or multi threaded.
- * @param iter_innerloop Number of inner iterations (see TebOptimalPlanner::optimizeTEB())
- * @param iter_outerloop Number of outer iterations (see TebOptimalPlanner::optimizeTEB())
- */
- void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
-
- /**
- * @brief Returns a shared pointer to the TEB related to the initial plan
- * @return A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty.
- */
- TebOptimalPlannerPtr getInitialPlanTEB();
-
- /**
- * @brief In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
- *
- * The trajectory cost includes features such as transition time and clearance from obstacles. \n
- * The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations.
- * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
- */
- TebOptimalPlannerPtr selectBestTeb();
-
- //@}
-
- /**
- * @brief Reset the planner.
- *
- * Clear all previously found H-signatures, paths, tebs and the hcgraph.
- */
- virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
-
-
- /**
- * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
- *
- * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
- * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
- * Initial means that the penalty is applied only to the first few poses of the trajectory.
- * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
- */
- virtual void setPreferredTurningDir(RotType dir);
-
- /**
- * @brief Calculate the equivalence class of a path
- *
- * Currently, only the H-signature (refer to HSignature) is implemented.
- *
- * @param path_start Iterator to the first element in the path
- * @param path_end Iterator to the last element in the path
- * @param obstacles obstacle container
- * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
- * @tparam BidirIter Bidirectional iterator type
- * @tparam Fun function of the form std::complex< long double > (const T& point_type)
- * @return pointer to the equivalence class base type
- */
- template<typename BidirIter, typename Fun>
- EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
- boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);
-
- /**
- * @brief Read-only access to the internal trajectory container.
- * @return read-only reference to the teb container.
- */
- const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;}
-
- bool hasDiverged() const override;
-
- /**
- * Compute and return the cost of the current optimization graph (supports multiple trajectories)
- * @param[out] cost current cost value for each trajectory
- * [for a planner with just a single trajectory: size=1, vector will not be cleared]
- * @param obst_cost_scale Specify extra scaling for obstacle costs
- * @param viapoint_cost_scale Specify extra scaling for via points.
- * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
- */
- virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
-
- /**
- * @brief Check if two h-signatures are similar (w.r.t. a certain threshold)
- * @param h1 first h-signature
- * @param h2 second h-signature
- * @return \c true if both h-signatures are similar, false otherwise.
- */
- inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
- {
- double diff_real = std::abs(h2.real() - h1.real());
- double diff_imag = std::abs(h2.imag() - h1.imag());
- if (diff_real<=threshold && diff_imag<=threshold)
- return true; // Found! Homotopy class already exists, therefore nothing added
- return false;
- }
- /**
- * @brief Checks if the orientation of the computed trajectories differs from that of the best plan of more than the
- * specified threshold and eventually deletes them.
- * Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration).
- * @param orient_threshold: Threshold paramter for allowed orientation changes in radians
- * @param len_orientation_vector: length of the vector used to compute the start orientation
- */
- void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
- /**
- * @brief Given a plan, computes its start orientation using a vector of length >= len_orientation_vector
- * starting from the initial pose.
- * @param plan: Teb to be analyzed
- * @param len_orientation_vector: min length of the vector used to compute the start orientation
- * @param orientation: computed start orientation
- * @return: Could the vector for the orientation check be computed? (False if the plan has no pose with a distance
- * > len_orientation_vector from the start poseq)
- */
- bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);
-
-
- /**
- * @brief Access config (read-only)
- * @return const pointer to the config instance
- */
- const TebConfig* config() const {return cfg_;}
-
- /**
- * @brief Access current obstacle container (read-only)
- * @return const pointer to the obstacle container instance
- */
- const ObstContainer* obstacles() const {return obstacles_;}
-
- /**
- * @brief Returns true if the planner is initialized
- */
- bool isInitialized() const {return initialized_;}
-
- /**
- * @brief Clear any existing graph of the homotopy class search
- */
- void clearGraph() {if(graph_search_) graph_search_->clearGraph();}
-
- /**
- * @brief find the index of the currently best TEB in the container
- * @remarks bestTeb() should be preferred whenever possible
- * @return index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1.
- */
- int bestTebIdx() const;
-
-
- /**
- * @brief Internal helper function that adds a new equivalence class to the list of known classes only if it is unique.
- * @param eq_class equivalence class that should be tested
- * @param lock if \c true, exclude the H-signature from deletion.
- * @return \c true if the h-signature was added and no duplicate was found, \c false otherwise
- */
- bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
-
- /**
- * @brief Return the current set of equivalence erelations (read-only)
- * @return reference to the internal set of currently tracked equivalence relations
- */
- const EquivalenceClassContainer& getEquivalenceClassRef() const {return equivalence_classes_;}
-
- bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const;
-
- int numTebsInClass(const EquivalenceClassPtr& eq_class) const;
-
- int numTebsInBestTebClass() const;
-
- /**
- * @brief Randomly drop non-optimal TEBs to so we can explore other alternatives
- *
- * The HCP has a tendency to become "fixated" once its tebs_ list becomes
- * fully populated, repeatedly refining and evaluating paths from the same
- * few homotopy classes until the robot moves far enough for a teb to become
- * invalid. As a result, it can fail to discover a more optimal path. This
- * function alleviates this problem by randomly dropping TEBs other than the
- * current "best" one with a probability controlled by
- * selection_dropping_probability parameter.
- */
- void randomlyDropTebs();
-
-protected:
-
- /** @name Explore new paths and keep only a single one for each homotopy class */
- //@{
-
- /**
- * @brief Check if a h-signature exists already.
- * @param eq_class equivalence class that should be tested
- * @return \c true if the h-signature is found, \c false otherwise
- */
- bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
-
-
- /**
- * @brief Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.
- *
- * Calling this method in each new planning interval is really important.
- * First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed).
- * Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures.
- * Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths.
- * @param delete_detours if this param is \c true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards().
- */
- void renewAndAnalyzeOldTebs(bool delete_detours);
-
- /**
- * @brief Associate trajectories with via-points
- *
- * If \c all_trajectories is true, all trajectory candidates are connected with the set of via_points,
- * otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed)
- * @remarks Requires that the plan method is called with an initial plan provided and that via-points are enabled (config)
- * @param all_trajectories see method description
- */
- void updateReferenceTrajectoryViaPoints(bool all_trajectories);
-
- //@}
-
- // external objects (store weak pointers)
- const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
- ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
- const ViaPointContainer* via_points_; //!< Store the current list of via-points
-
- // internal objects (memory management owned)
- TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
- TebOptimalPlannerPtr best_teb_; //!< Store the current best teb.
- EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb
- RobotFootprintModelPtr robot_model_; //!< Robot model shared instance
-
- const std::vector<geometry_msgs::msg::PoseStamped>* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization
- EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan
- TebOptimalPlannerPtr initial_plan_teb_; //!< Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.)
-
- TebOptPlannerContainer tebs_; //!< Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs
-
- EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
- // The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
-
- std::shared_ptr<GraphSearchInterface> graph_search_;
-
- rclcpp::Time last_eq_class_switching_time_; //!< Store the time at which the equivalence class changed recently
-
- std::default_random_engine random_;
- bool initialized_; //!< Keeps track about the correct initialization of this class
-
- TebOptimalPlannerPtr last_best_teb_; //!< Points to the plan used in the previous control cycle
-
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-
-};
-
-//! Abbrev. for a shared pointer of a HomotopyClassPlanner instance.
-typedef std::shared_ptr<HomotopyClassPlanner> HomotopyClassPlannerPtr;
-
-
-} // namespace teb_local_planner
-
-// include template implementations / definitions
-#include "teb_local_planner/homotopy_class_planner.hpp"
-
-#endif /* HOMOTOPY_CLASS_PLANNER_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp b/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
deleted file mode 100644
index db7cdef..0000000
--- a/src/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2017,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/homotopy_class_planner.h"
-#include "teb_local_planner/h_signature.h"
-
-namespace teb_local_planner
-{
-
-
-template<typename BidirIter, typename Fun>
-EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
- boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
-{
- if(cfg_->obstacles.include_dynamic_obstacles)
- {
- HSignature3d* H = new HSignature3d(*cfg_);
- H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end);
- return EquivalenceClassPtr(H);
- }
- else
- {
- HSignature* H = new HSignature(*cfg_);
- H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles);
- return EquivalenceClassPtr(H);
- }
-}
-
-
-template<typename BidirIter, typename Fun>
-TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_));
-
- candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
- cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
- cfg_->trajectory.allow_init_with_backwards_motion);
-
- if (start_velocity)
- candidate->setVelocityStart(*start_velocity);
-
- EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
- candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
-
-
- if (free_goal_vel)
- candidate->setVelocityGoalFree();
-
- if(addEquivalenceClassIfNew(H))
- {
- tebs_.push_back(candidate);
- return tebs_.back();
- }
-
- // If the candidate constitutes no new equivalence class, return a null pointer
- return TebOptimalPlannerPtr();
-}
-
-} // namespace teb_local_planner
-
diff --git a/src/teb_local_planner/include/teb_local_planner/misc.h b/src/teb_local_planner/include/teb_local_planner/misc.h
deleted file mode 100644
index 67f5d90..0000000
--- a/src/teb_local_planner/include/teb_local_planner/misc.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef MISC_H
-#define MISC_H
-
-#include <builtin_interfaces/msg/duration.hpp>
-
-#include <Eigen/Core>
-
-#include <exception>
-#include <type_traits>
-
-#include <rclcpp/logging.hpp>
-
-namespace teb_local_planner
-{
-#define SMALL_NUM 0.00000001
-
-//! Symbols for left/none/right rotations
-enum class RotType { left, none, right };
-
-/**
- * @brief Check whether two variables (double) are close to each other
- * @param a the first value to compare
- * @param b the second value to compare
- * @param epsilon precision threshold
- * @return \c true if |a-b| < epsilon, false otherwise
- */
-inline bool is_close(double a, double b, double epsilon = 1e-4)
-{
- return std::fabs(a - b) < epsilon;
-}
-
-/**
- * @brief Return the average angle of an arbitrary number of given angles [rad]
- * @param angles vector containing all angles
- * @return average / mean angle, that is normalized to [-pi, pi]
- */
-inline double average_angles(const std::vector<double>& angles)
-{
- double x=0, y=0;
- for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
- {
- x += cos(*it);
- y += sin(*it);
- }
- if(x == 0 && y == 0)
- return 0;
- else
- return std::atan2(y, x);
-}
-
-/** @brief Small helper function: check if |a|<|b| */
-inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
-
-
-/**
- * @brief Calculate a fast approximation of a sigmoid function
- * @details The following function is implemented: \f$ x / (1 + |x|) \f$
- * @param x the argument of the function
-*/
-inline double fast_sigmoid(double x)
-{
- return x / (1 + fabs(x));
-}
-
-/**
- * @brief Calculate Euclidean distance between two 2D point datatypes
- * @param point1 object containing fields x and y
- * @param point2 object containing fields x and y
- * @return Euclidean distance: ||point2-point1||
-*/
-template <typename P1, typename P2>
-inline double distance_points2d(const P1& point1, const P2& point2)
-{
- return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
-}
-
-
-/**
- * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
- * @param v1 object containing public methods x() and y()
- * @param v2 object containing fields x() and y()
- * @return magnitude that would result in the 3D case (along the z-axis)
-*/
-template <typename V1, typename V2>
-inline double cross2d(const V1& v1, const V2& v2)
-{
- return v1.x()*v2.y() - v2.x()*v1.y();
-}
-
-/**
- * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
- *
- * Return a constant reference for boths input variants (pointer or reference).
- * @remarks Makes only sense in combination with the overload getConstReference(const T& val).
- * @param ptr pointer of type T
- * @tparam T arbitrary type
- * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
- */
-template<typename T>
-inline const T& get_const_reference(const T* ptr) {return *ptr;}
-
-/**
- * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
- *
- * Return a constant reference for boths input variants (pointer or reference).
- * @remarks Makes only sense in combination with the overload getConstReference(const T* val).
- * @param val
- * @param dummy SFINAE helper variable
- * @tparam T arbitrary type
- * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
- */
-template<typename T>
-inline const T& get_const_reference(const T& val, typename std::enable_if_t<!std::is_pointer<T>::value, T>* dummy = nullptr) {return val;}
-
-inline builtin_interfaces::msg::Duration durationFromSec(double t_sec)
-{
- int32_t sec;
- uint32_t nsec;
- sec = static_cast<int32_t>(floor(t_sec));
- nsec = static_cast<int32_t>(std::round((t_sec - sec) * 1e9));
- // avoid rounding errors
- sec += (nsec / 1000000000l);
- nsec %= 1000000000l;
-
- builtin_interfaces::msg::Duration duration;
- duration.sec = sec;
- duration.nanosec = nsec;
- return duration;
-}
-
-struct TebAssertionFailureException : public std::runtime_error
-{
- TebAssertionFailureException(const std::string &msg)
- : std::runtime_error(msg)
- {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), msg.c_str());
- }
-};
-
-#define TEB_ASSERT_MSG_IMPL(...) \
- { \
- char arg_string[1024]; \
- std::sprintf(arg_string, __VA_ARGS__); \
- const std::string msg(arg_string); \
- throw TebAssertionFailureException(msg); \
- }
-
-template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_arithmetic<T>::value>* = nullptr>
-void teb_assert_msg_impl(const T expression, ARGS ...args) {
- if(expression == 0) {
- char arg_string[1024];
- std::sprintf(arg_string, args..., "");
- const std::string msg(arg_string);
- throw TebAssertionFailureException(msg);
- }
-}
-
-template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_pointer<T>::value>* = nullptr>
-void teb_assert_msg_impl(const T expression, ARGS ...args) {
- if(expression == nullptr) {
- char arg_string[1024];
- std::sprintf(arg_string, args..., "");
- const std::string msg(arg_string);
- throw TebAssertionFailureException(msg);
- }
-}
-
-#define TEB_ASSERT_MSG(expression, ...) teb_assert_msg_impl(expression, __VA_ARGS__)
-
-} // namespace teb_local_planner
-
-#endif /* MISC_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/obstacles.h b/src/teb_local_planner/include/teb_local_planner/obstacles.h
deleted file mode 100644
index 6893589..0000000
--- a/src/teb_local_planner/include/teb_local_planner/obstacles.h
+++ /dev/null
@@ -1,1112 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-
-#ifndef OBSTACLES_H
-#define OBSTACLES_H
-
-#include <Eigen/Core>
-#include <Eigen/StdVector>
-#include <Eigen/Geometry>
-
-#include <complex>
-
-#include <geometry_msgs/msg/polygon.hpp>
-#include <geometry_msgs/msg/twist_with_covariance.hpp>
-#include <geometry_msgs/msg/quaternion_stamped.hpp>
-
-#include "teb_local_planner/distance_calculations.h"
-
-
-namespace teb_local_planner
-{
-
-/**
- * @class Obstacle
- * @brief Abstract class that defines the interface for modelling obstacles
- */
-class Obstacle
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- */
- Obstacle() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero())
- {
- }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~Obstacle()
- {
- }
-
-
- /** @name Centroid coordinates (abstract, obstacle type depending) */
- //@{
-
- /**
- * @brief Get centroid coordinates of the obstacle
- * @return Eigen::Vector2d containing the centroid
- */
- virtual const Eigen::Vector2d& getCentroid() const = 0;
-
- /**
- * @brief Get centroid coordinates of the obstacle as complex number
- * @return std::complex containing the centroid coordinate
- */
- virtual std::complex<double> getCentroidCplx() const = 0;
-
- //@}
-
-
- /** @name Collision checking and distance calculations (abstract, obstacle type depending) */
- //@{
-
- /**
- * @brief Check if a given point collides with the obstacle
- * @param position 2D reference position that should be checked
- * @param min_dist Minimum distance allowed to the obstacle to be collision free
- * @return \c true if position is inside the region of the obstacle or if the minimum distance is lower than min_dist
- */
- virtual bool checkCollision(const Eigen::Vector2d& position, double min_dist) const = 0;
-
- /**
- * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
- * @param line_start 2D point for the end of the reference line
- * @param line_end 2D point for the end of the reference line
- * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
- * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
- */
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const = 0;
-
- /**
- * @brief Get the minimum euclidean distance to the obstacle (point as reference)
- * @param position 2d reference position
- * @return The nearest possible distance to the obstacle
- */
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const = 0;
-
- /**
- * @brief Get the minimum euclidean distance to the obstacle (line as reference)
- * @param line_start 2d position of the begin of the reference line
- * @param line_end 2d position of the end of the reference line
- * @return The nearest possible distance to the obstacle
- */
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const = 0;
-
- /**
- * @brief Get the minimum euclidean distance to the obstacle (polygon as reference)
- * @param polygon Vertices (2D points) describing a closed polygon
- * @return The nearest possible distance to the obstacle
- */
- virtual double getMinimumDistance(const Point2dContainer& polygon) const = 0;
-
- /**
- * @brief Get the closest point on the boundary of the obstacle w.r.t. a specified reference position
- * @param position reference 2d position
- * @return closest point on the obstacle boundary
- */
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const = 0;
-
- //@}
-
-
-
- /** @name Velocity related methods for non-static, moving obstacles */
- //@{
-
- /**
- * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference)
- * @param position 2d reference position
- * @param t time, for which the minimum distance to the obstacle is estimated
- * @return The nearest possible distance to the obstacle at time t
- */
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const = 0;
-
- /**
- * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference)
- * @param line_start 2d position of the begin of the reference line
- * @param line_end 2d position of the end of the reference line
- * @param t time, for which the minimum distance to the obstacle is estimated
- * @return The nearest possible distance to the obstacle at time t
- */
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const = 0;
-
- /**
- * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference)
- * @param polygon Vertices (2D points) describing a closed polygon
- * @param t time, for which the minimum distance to the obstacle is estimated
- * @return The nearest possible distance to the obstacle at time t
- */
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const = 0;
-
- /**
- * @brief Predict position of the centroid assuming a constant velocity model
- * @param[in] t time in seconds for the prediction (t>=0)
- * @param[out] position predicted 2d position of the centroid
- */
- virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
- {
- position = getCentroid() + t * getCentroidVelocity();
- }
-
- /**
- * @brief Check if the obstacle is a moving with a (non-zero) velocity
- * @return \c true if the obstacle is not marked as static, \c false otherwise
- */
- bool isDynamic() const {return dynamic_;}
-
- /**
- * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
- * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
- * @param vel 2D vector containing the velocities of the centroid in x and y directions
- */
- void setCentroidVelocity(const Eigen::Ref<const Eigen::Vector2d>& vel) {centroid_velocity_ = vel; dynamic_=true;}
-
- /**
- * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid
- * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic)
- * @param velocity geometry_msgs::msg::TwistWithCovariance containing the velocity of the obstacle
- * @param orientation geometry_msgs::msg::QuaternionStamped containing the orientation of the obstacle
- */
- void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity,
- const geometry_msgs::msg::Quaternion& orientation)
- {
- // Set velocity, if obstacle is moving
- Eigen::Vector2d vel;
- vel.coeffRef(0) = velocity.twist.linear.x;
- vel.coeffRef(1) = velocity.twist.linear.y;
-
- // If norm of velocity is less than 0.001, consider obstacle as not dynamic
- // TODO: Get rid of constant
- if (vel.norm() < 0.001)
- return;
-
- // currently velocity published by stage is already given in the map frame
-// double yaw = tf::getYaw(orientation.quaternion);
-// ROS_INFO("Yaw: %f", yaw);
-// Eigen::Rotation2Dd rot(yaw);
-// vel = rot * vel;
- setCentroidVelocity(vel);
- }
-
- void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity,
- const geometry_msgs::msg::QuaternionStamped& orientation)
- {
- setCentroidVelocity(velocity, orientation.quaternion);
- }
-
- /**
- * @brief Get the obstacle velocity (vx, vy) (w.r.t. to the centroid)
- * @returns 2D vector containing the velocities of the centroid in x and y directions
- */
- const Eigen::Vector2d& getCentroidVelocity() const {return centroid_velocity_;}
-
- //@}
-
-
-
- /** @name Helper Functions */
- //@{
-
- /**
- * @brief Convert the obstacle to a polygon message
- *
- * Convert the obstacle to a corresponding polygon msg.
- * Point obstacles have one vertex, lines have two vertices
- * and polygons might are implictly closed such that the start vertex must not be repeated.
- * @param[out] polygon the polygon message
- */
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) = 0;
-
- virtual void toTwistWithCovarianceMsg(geometry_msgs::msg::TwistWithCovariance& twistWithCovariance)
- {
- if (dynamic_)
- {
- twistWithCovariance.twist.linear.x = centroid_velocity_(0);
- twistWithCovariance.twist.linear.y = centroid_velocity_(1);
- }
- else
- {
- twistWithCovariance.twist.linear.x = 0;
- twistWithCovariance.twist.linear.y = 0;
- }
-
- // TODO:Covariance
- }
-
- //@}
-
-protected:
-
- bool dynamic_; //!< Store flag if obstacle is dynamic (resp. a moving obstacle)
- Eigen::Vector2d centroid_velocity_; //!< Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is \c true)
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-//! Abbrev. for shared obstacle pointers
-typedef std::shared_ptr<Obstacle> ObstaclePtr;
-//! Abbrev. for shared obstacle const pointers
-typedef std::shared_ptr<const Obstacle> ObstacleConstPtr;
-//! Abbrev. for containers storing multiple obstacles
-typedef std::vector<ObstaclePtr> ObstContainer;
-
-
-
-/**
- * @class PointObstacle
- * @brief Implements a 2D point obstacle
- */
-class PointObstacle : public Obstacle
-{
-public:
-
- /**
- * @brief Default constructor of the point obstacle class
- */
- PointObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
- {}
-
- /**
- * @brief Construct PointObstacle using a 2d position vector
- * @param position 2d position that defines the current obstacle position
- */
- PointObstacle(const Eigen::Ref< const Eigen::Vector2d>& position) : Obstacle(), pos_(position)
- {}
-
- /**
- * @brief Construct PointObstacle using x- and y-coordinates
- * @param x x-coordinate
- * @param y y-coordinate
- */
- PointObstacle(double x, double y) : Obstacle(), pos_(Eigen::Vector2d(x,y))
- {}
-
-
- // implements checkCollision() of the base class
- virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
- {
- return getMinimumDistance(point) < min_dist;
- }
-
-
- // implements checkLineIntersection() of the base class
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
- {
- // Distance Line - Circle
- // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
- Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
- Eigen::Vector2d b = pos_-line_start; // b=m-x
-
- // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
- double t = a.dot(b)/a.dot(a);
- if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
- else if (t>1) t=1;
- Eigen::Vector2d nearest_point = line_start + a*t;
-
- // check collision
- return checkCollision(nearest_point, min_dist);
- }
-
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const
- {
- return (position-pos_).norm();
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
- {
- return distance_point_to_segment_2d(pos_, line_start, line_end);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Point2dContainer& polygon) const
- {
- return distance_point_to_polygon_2d(pos_, polygon);
- }
-
- // implements getMinimumDistanceVec() of the base class
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
- {
- return pos_;
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
- {
- return (pos_ + t*centroid_velocity_ - position).norm();
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
- {
- return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
- {
- return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon);
- }
-
- // implements predictCentroidConstantVelocity() of the base class
- virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
- {
- position = pos_ + t*centroid_velocity_;
- }
-
- // implements getCentroid() of the base class
- virtual const Eigen::Vector2d& getCentroid() const
- {
- return pos_;
- }
-
- // implements getCentroidCplx() of the base class
- virtual std::complex<double> getCentroidCplx() const
- {
- return std::complex<double>(pos_[0],pos_[1]);
- }
-
- // Accessor methods
- const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
- Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
- double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
- const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
- double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
- const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)
-
- // implements toPolygonMsg() of the base class
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
- {
- polygon.points.resize(1);
- polygon.points.front().x = pos_.x();
- polygon.points.front().y = pos_.y();
- polygon.points.front().z = 0;
- }
-
-protected:
-
- Eigen::Vector2d pos_; //!< Store the position of the PointObstacle
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/**
- * @class CircularObstacle
- * @brief Implements a 2D circular obstacle (point obstacle plus radius)
- */
-class CircularObstacle : public Obstacle
-{
-public:
-
- /**
- * @brief Default constructor of the circular obstacle class
- */
- CircularObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
- {}
-
- /**
- * @brief Construct CircularObstacle using a 2d center position vector and radius
- * @param position 2d position that defines the current obstacle position
- * @param radius radius of the obstacle
- */
- CircularObstacle(const Eigen::Ref< const Eigen::Vector2d>& position, double radius) : Obstacle(), pos_(position), radius_(radius)
- {}
-
- /**
- * @brief Construct CircularObstacle using x- and y-center-coordinates and radius
- * @param x x-coordinate
- * @param y y-coordinate
- * @param radius radius of the obstacle
- */
- CircularObstacle(double x, double y, double radius) : Obstacle(), pos_(Eigen::Vector2d(x,y)), radius_(radius)
- {}
-
-
- // implements checkCollision() of the base class
- virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
- {
- return getMinimumDistance(point) < min_dist;
- }
-
-
- // implements checkLineIntersection() of the base class
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
- {
- // Distance Line - Circle
- // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
- Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
- Eigen::Vector2d b = pos_-line_start; // b=m-x
-
- // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
- double t = a.dot(b)/a.dot(a);
- if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
- else if (t>1) t=1;
- Eigen::Vector2d nearest_point = line_start + a*t;
-
- // check collision
- return checkCollision(nearest_point, min_dist);
- }
-
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const
- {
- return (position-pos_).norm() - radius_;
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
- {
- return distance_point_to_segment_2d(pos_, line_start, line_end) - radius_;
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Point2dContainer& polygon) const
- {
- return distance_point_to_polygon_2d(pos_, polygon) - radius_;
- }
-
- // implements getMinimumDistanceVec() of the base class
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
- {
- return pos_ + radius_*(position-pos_).normalized();
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
- {
- return (pos_ + t*centroid_velocity_ - position).norm() - radius_;
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
- {
- return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end) - radius_;
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
- {
- return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon) - radius_;
- }
-
- // implements predictCentroidConstantVelocity() of the base class
- virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
- {
- position = pos_ + t*centroid_velocity_;
- }
-
- // implements getCentroid() of the base class
- virtual const Eigen::Vector2d& getCentroid() const
- {
- return pos_;
- }
-
- // implements getCentroidCplx() of the base class
- virtual std::complex<double> getCentroidCplx() const
- {
- return std::complex<double>(pos_[0],pos_[1]);
- }
-
- // Accessor methods
- const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only)
- Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle
- double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle
- const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only)
- double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle
- const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only)
- double& radius() {return radius_;} //!< Return the current radius of the obstacle
- const double& radius() const {return radius_;} //!< Return the current radius of the obstacle
-
- // implements toPolygonMsg() of the base class
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
- {
- // TODO(roesmann): the polygon message type cannot describe a "perfect" circle
- // We could switch to ObstacleMsg if required somewhere...
- polygon.points.resize(1);
- polygon.points.front().x = pos_.x();
- polygon.points.front().y = pos_.y();
- polygon.points.front().z = 0;
- }
-
-protected:
-
- Eigen::Vector2d pos_; //!< Store the center position of the CircularObstacle
- double radius_ = 0.0; //!< Radius of the obstacle
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/**
-* @class LineObstacle
-* @brief Implements a 2D line obstacle
-*/
-
-class LineObstacle : public Obstacle
-{
-public:
- //! Abbrev. for a container storing vertices (2d points defining the edge points of the polygon)
- typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > VertexContainer;
-
- /**
- * @brief Default constructor of the point obstacle class
- */
- LineObstacle() : Obstacle()
- {
- start_.setZero();
- end_.setZero();
- centroid_.setZero();
- }
-
- /**
- * @brief Construct LineObstacle using 2d position vectors as start and end of the line
- * @param line_start 2d position that defines the start of the line obstacle
- * @param line_end 2d position that defines the end of the line obstacle
- */
- LineObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end)
- : Obstacle(), start_(line_start), end_(line_end)
- {
- calcCentroid();
- }
-
- /**
- * @brief Construct LineObstacle using start and end coordinates
- * @param x1 x-coordinate of the start of the line
- * @param y1 y-coordinate of the start of the line
- * @param x2 x-coordinate of the end of the line
- * @param y2 y-coordinate of the end of the line
- */
- LineObstacle(double x1, double y1, double x2, double y2) : Obstacle()
- {
- start_.x() = x1;
- start_.y() = y1;
- end_.x() = x2;
- end_.y() = y2;
- calcCentroid();
- }
-
- // implements checkCollision() of the base class
- virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
- {
- return getMinimumDistance(point) <= min_dist;
- }
-
- // implements checkLineIntersection() of the base class
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
- {
- return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const
- {
- return distance_point_to_segment_2d(position, start_, end_);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
- {
- return distance_segment_to_segment_2d(start_, end_, line_start, line_end);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Point2dContainer& polygon) const
- {
- return distance_segment_to_polygon_2d(start_, end_, polygon);
- }
-
- // implements getMinimumDistanceVec() of the base class
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
- {
- return closest_point_on_line_segment_2d(position, start_, end_);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_point_to_segment_2d(position, start_ + offset, end_ + offset);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon);
- }
-
- // implements getCentroid() of the base class
- virtual const Eigen::Vector2d& getCentroid() const
- {
- return centroid_;
- }
-
- // implements getCentroidCplx() of the base class
- virtual std::complex<double> getCentroidCplx() const
- {
- return std::complex<double>(centroid_.x(), centroid_.y());
- }
-
- // Access or modify line
- const Eigen::Vector2d& start() const {return start_;}
- void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
- const Eigen::Vector2d& end() const {return end_;}
- void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}
-
- // implements toPolygonMsg() of the base class
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
- {
- polygon.points.resize(2);
- polygon.points.front().x = start_.x();
- polygon.points.front().y = start_.y();
-
- polygon.points.back().x = end_.x();
- polygon.points.back().y = end_.y();
- polygon.points.back().z = polygon.points.front().z = 0;
- }
-
-protected:
- void calcCentroid() { centroid_ = 0.5*(start_ + end_); }
-
-private:
- Eigen::Vector2d start_;
- Eigen::Vector2d end_;
-
- Eigen::Vector2d centroid_;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-/**
-* @class PillObstacle
-* @brief Implements a 2D pill/stadium/capsular-shaped obstacle (line + distance/radius)
-*/
-
-class PillObstacle : public Obstacle
-{
-public:
-
- /**
- * @brief Default constructor of the point obstacle class
- */
- PillObstacle() : Obstacle()
- {
- start_.setZero();
- end_.setZero();
- centroid_.setZero();
- }
-
- /**
- * @brief Construct LineObstacle using 2d position vectors as start and end of the line
- * @param line_start 2d position that defines the start of the line obstacle
- * @param line_end 2d position that defines the end of the line obstacle
- */
- PillObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end, double radius)
- : Obstacle(), start_(line_start), end_(line_end), radius_(radius)
- {
- calcCentroid();
- }
-
- /**
- * @brief Construct LineObstacle using start and end coordinates
- * @param x1 x-coordinate of the start of the line
- * @param y1 y-coordinate of the start of the line
- * @param x2 x-coordinate of the end of the line
- * @param y2 y-coordinate of the end of the line
- */
- PillObstacle(double x1, double y1, double x2, double y2, double radius) : Obstacle(), radius_(radius)
- {
- start_.x() = x1;
- start_.y() = y1;
- end_.x() = x2;
- end_.y() = y2;
- calcCentroid();
- }
-
- // implements checkCollision() of the base class
- virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
- {
- return getMinimumDistance(point) <= min_dist;
- }
-
- // implements checkLineIntersection() of the base class
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
- {
- return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const
- {
- return distance_point_to_segment_2d(position, start_, end_) - radius_;
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
- {
- return distance_segment_to_segment_2d(start_, end_, line_start, line_end) - radius_;
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Point2dContainer& polygon) const
- {
- return distance_segment_to_polygon_2d(start_, end_, polygon) - radius_;
- }
-
- // implements getMinimumDistanceVec() of the base class
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
- {
- Eigen::Vector2d closed_point_line = closest_point_on_line_segment_2d(position, start_, end_);
- return closed_point_line + radius_*(position-closed_point_line).normalized();
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_point_to_segment_2d(position, start_ + offset, end_ + offset) - radius_;
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end) - radius_;
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
- {
- Eigen::Vector2d offset = t*centroid_velocity_;
- return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon) - radius_;
- }
-
- // implements getCentroid() of the base class
- virtual const Eigen::Vector2d& getCentroid() const
- {
- return centroid_;
- }
-
- // implements getCentroidCplx() of the base class
- virtual std::complex<double> getCentroidCplx() const
- {
- return std::complex<double>(centroid_.x(), centroid_.y());
- }
-
- // Access or modify line
- const Eigen::Vector2d& start() const {return start_;}
- void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
- const Eigen::Vector2d& end() const {return end_;}
- void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}
-
- // implements toPolygonMsg() of the base class
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
- {
- // Currently, we only export the line
- // TODO(roesmann): export whole pill
- polygon.points.resize(2);
- polygon.points.front().x = start_.x();
- polygon.points.front().y = start_.y();
-
- polygon.points.back().x = end_.x();
- polygon.points.back().y = end_.y();
- polygon.points.back().z = polygon.points.front().z = 0;
- }
-
-protected:
- void calcCentroid() { centroid_ = 0.5*(start_ + end_); }
-
-private:
- Eigen::Vector2d start_;
- Eigen::Vector2d end_;
- double radius_ = 0.0;
-
- Eigen::Vector2d centroid_;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/**
- * @class PolygonObstacle
- * @brief Implements a polygon obstacle with an arbitrary number of vertices
- * @details If the polygon has only 2 vertices, than it is considered as a line,
- * otherwise the polygon will always be closed (a connection between the first and the last vertex
- * is included automatically).
- */
-class PolygonObstacle : public Obstacle
-{
-public:
-
- /**
- * @brief Default constructor of the polygon obstacle class
- */
- PolygonObstacle() : Obstacle(), finalized_(false)
- {
- centroid_.setConstant(NAN);
- }
-
- /**
- * @brief Construct polygon obstacle with a list of vertices
- */
- PolygonObstacle(const Point2dContainer& vertices) : Obstacle(), vertices_(vertices)
- {
- finalizePolygon();
- }
-
-
- /* FIXME Not working at the moment due to the aligned allocator version of std::vector
- * And it is C++11 code that is disabled atm to ensure compliance with ROS indigo/jade
- template <typename... Vector2dType>
- PolygonObstacle(const Vector2dType&... vertices) : _vertices({vertices...})
- {
- calcCentroid();
- _finalized = true;
- }
- */
-
-
- // implements checkCollision() of the base class
- virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
- {
- // line case
- if (noVertices()==2)
- return getMinimumDistance(point) <= min_dist;
-
- // check if point is in the interior of the polygon
- // point in polygon test - raycasting (http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html)
- // using the following algorithm we may obtain false negatives on edge-cases, but that's ok for our purposes
- int i, j;
- bool c = false;
- for (i = 0, j = noVertices()-1; i < noVertices(); j = i++)
- {
- if ( ((vertices_.at(i).y()>point.y()) != (vertices_.at(j).y()>point.y())) &&
- (point.x() < (vertices_.at(j).x()-vertices_.at(i).x()) * (point.y()-vertices_.at(i).y()) / (vertices_.at(j).y()-vertices_.at(i).y()) + vertices_.at(i).x()) )
- c = !c;
- }
- if (c>0) return true;
-
- // If this statement is reached, the point lies outside the polygon or maybe on its edges
- // Let us check the minium distance as well
- return min_dist == 0 ? false : getMinimumDistance(point) < min_dist;
- }
-
-
- /**
- * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist)
- * @param line_start 2D point for the end of the reference line
- * @param line_end 2D point for the end of the reference line
- * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free
- * @remarks we ignore \c min_dist here
- * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist
- */
- virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const;
-
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& position) const
- {
- return distance_point_to_polygon_2d(position, vertices_);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
- {
- return distance_segment_to_polygon_2d(line_start, line_end, vertices_);
- }
-
- // implements getMinimumDistance() of the base class
- virtual double getMinimumDistance(const Point2dContainer& polygon) const
- {
- return distance_polygon_to_polygon_2d(polygon, vertices_);
- }
-
- // implements getMinimumDistanceVec() of the base class
- virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const;
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
- {
- Point2dContainer pred_vertices;
- predictVertices(t, pred_vertices);
- return distance_point_to_polygon_2d(position, pred_vertices);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
- {
- Point2dContainer pred_vertices;
- predictVertices(t, pred_vertices);
- return distance_segment_to_polygon_2d(line_start, line_end, pred_vertices);
- }
-
- // implements getMinimumSpatioTemporalDistance() of the base class
- virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
- {
- Point2dContainer pred_vertices;
- predictVertices(t, pred_vertices);
- return distance_polygon_to_polygon_2d(polygon, pred_vertices);
- }
-
- virtual void predictVertices(double t, Point2dContainer& pred_vertices) const
- {
- // Predict obstacle (polygon) at time t
- pred_vertices.resize(vertices_.size());
- Eigen::Vector2d offset = t*centroid_velocity_;
- for (std::size_t i = 0; i < vertices_.size(); i++)
- {
- pred_vertices[i] = vertices_[i] + offset;
- }
- }
-
- // implements getCentroid() of the base class
- virtual const Eigen::Vector2d& getCentroid() const
- {
- assert(finalized_ && "Finalize the polygon after all vertices are added.");
- return centroid_;
- }
-
- // implements getCentroidCplx() of the base class
- virtual std::complex<double> getCentroidCplx() const
- {
- assert(finalized_ && "Finalize the polygon after all vertices are added.");
- return std::complex<double>(centroid_.coeffRef(0), centroid_.coeffRef(1));
- }
-
- // implements toPolygonMsg() of the base class
- virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon);
-
-
- /** @name Define the polygon */
- ///@{
-
- // Access or modify polygon
- const Point2dContainer& vertices() const {return vertices_;} //!< Access vertices container (read-only)
- Point2dContainer& vertices() {return vertices_;} //!< Access vertices container
-
- /**
- * @brief Add a vertex to the polygon (edge-point)
- * @remarks You do not need to close the polygon (do not repeat the first vertex)
- * @warning Do not forget to call finalizePolygon() after adding all vertices
- * @param vertex 2D point defining a new polygon edge
- */
- void pushBackVertex(const Eigen::Ref<const Eigen::Vector2d>& vertex)
- {
- vertices_.push_back(vertex);
- finalized_ = false;
- }
-
- /**
- * @brief Add a vertex to the polygon (edge-point)
- * @remarks You do not need to close the polygon (do not repeat the first vertex)
- * @warning Do not forget to call finalizePolygon() after adding all vertices
- * @param x x-coordinate of the new vertex
- * @param y y-coordinate of the new vertex
- */
- void pushBackVertex(double x, double y)
- {
- vertices_.push_back(Eigen::Vector2d(x,y));
- finalized_ = false;
- }
-
- /**
- * @brief Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods
- */
- void finalizePolygon()
- {
- fixPolygonClosure();
- calcCentroid();
- finalized_ = true;
- }
-
- /**
- * @brief Clear all vertices (Afterwards the polygon is not valid anymore)
- */
- void clearVertices() {vertices_.clear(); finalized_ = false;}
-
- /**
- * @brief Get the number of vertices defining the polygon (the first vertex is counted once)
- */
- int noVertices() const {return (int)vertices_.size();}
-
-
- ///@}
-
-protected:
-
- void fixPolygonClosure(); //!< Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one.
-
- void calcCentroid(); //!< Compute the centroid of the polygon (called inside finalizePolygon())
-
-
- Point2dContainer vertices_; //!< Store vertices defining the polygon (@see pushBackVertex)
- Eigen::Vector2d centroid_; //!< Store the centroid coordinates of the polygon (@see calcCentroid)
-
- bool finalized_; //!< Flat that keeps track if the polygon was finalized after adding all vertices
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-} // namespace teb_local_planner
-
-#endif /* OBSTACLES_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/optimal_planner.h b/src/teb_local_planner/include/teb_local_planner/optimal_planner.h
deleted file mode 100644
index f76efe8..0000000
--- a/src/teb_local_planner/include/teb_local_planner/optimal_planner.h
+++ /dev/null
@@ -1,736 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef OPTIMAL_PLANNER_H_
-#define OPTIMAL_PLANNER_H_
-
-#include <math.h>
-
-
-// teb stuff
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/misc.h"
-#include "teb_local_planner/timed_elastic_band.h"
-#include "teb_local_planner/planner_interface.h"
-#include "teb_local_planner/visualization.h"
-#include "teb_local_planner/robot_footprint_model.h"
-
-// g2o lib stuff
-#include "g2o/core/sparse_optimizer.h"
-#include "g2o/core/block_solver.h"
-#include "g2o/core/factory.h"
-#include "g2o/core/optimization_algorithm_gauss_newton.h"
-#include "g2o/core/optimization_algorithm_levenberg.h"
-#include "g2o/solvers/csparse/linear_solver_csparse.h"
-#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
-
-// g2o custom edges and vertices for the TEB planner
-#include "teb_local_planner/g2o_types/edge_velocity.h"
-#include "teb_local_planner/g2o_types/edge_acceleration.h"
-#include "teb_local_planner/g2o_types/edge_kinematics.h"
-#include "teb_local_planner/g2o_types/edge_time_optimal.h"
-#include "teb_local_planner/g2o_types/edge_shortest_path.h"
-#include "teb_local_planner/g2o_types/edge_obstacle.h"
-#include "teb_local_planner/g2o_types/edge_dynamic_obstacle.h"
-#include "teb_local_planner/g2o_types/edge_via_point.h"
-#include "teb_local_planner/g2o_types/edge_prefer_rotdir.h"
-
-// messages
-#include <nav_msgs/msg/path.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <teb_msgs/msg/trajectory_msg.hpp>
-#include <teb_msgs/msg/trajectory_point_msg.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-
-#include <dwb_critics/obstacle_footprint.hpp>
-
-#include <tf2/transform_datatypes.h>
-
-#include <limits.h>
-
-namespace teb_local_planner
-{
-
-//! Typedef for the block solver utilized for optimization
-typedef g2o::BlockSolverX TEBBlockSolver;
-
-//! Typedef for the linear solver utilized for optimization
-typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
-//typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
-
-//! Typedef for a container storing via-points
-typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ViaPointContainer;
-
-
-/**
- * @class TebOptimalPlanner
- * @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
- *
- * For an introduction and further details about the TEB optimization problem refer to:
- * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
- * - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013.
- * - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011.
- *
- * @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions.
- * @todo: We introduced the non-fast mode with the support of dynamic obstacles
- * (which leads to better results in terms of x-y-t homotopy planning).
- * However, we have not tested this mode intensively yet, so we keep
- * the legacy fast mode as default until we finish our tests.
- */
-class TebOptimalPlanner : public PlannerInterface
-{
-public:
-
- /**
- * @brief Default constructor
- */
- TebOptimalPlanner();
-
- /**
- * @brief Construct and initialize the TEB optimal planner.
- * @param cfg Const reference to the TebConfig class for internal parameters
- * @param obstacles Container storing all relevant obstacles (see Obstacle)
- * @param visual Shared pointer to the TebVisualization class (optional)
- * @param via_points Container storing via-points (optional)
- */
- TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
- TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
-
- /**
- * @brief Destruct the optimal planner.
- */
- virtual ~TebOptimalPlanner();
-
- /**
- * @brief Initializes the optimal planner
- * @param node Shared pointer for rclcpp::Node
- * @param cfg Const reference to the TebConfig class for internal parameters
- * @param obstacles Container storing all relevant obstacles (see Obstacle)
- * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
- * @param visual Shared pointer to the TebVisualization class (optional)
- * @param via_points Container storing via-points (optional)
- */
- void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
- TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
-
- /** @name Plan a trajectory */
- //@{
-
- /**
- * @brief Plan a trajectory based on an initial reference plan.
- *
- * Call this method to create and optimize a trajectory that is initialized
- * according to an initial reference plan (given as a container of poses). \n
- * The method supports hot-starting from previous solutions, if avaiable: \n
- * - If no trajectory exist yet, a new trajectory is initialized based on the initial plan,
- * see TimedElasticBand::initTEBtoGoal
- * - If a previous solution is avaiable, update the trajectory based on the initial plan,
- * see bool TimedElasticBand::updateAndPruneTEB
- * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
- * @param initial_plan vector of geometry_msgs::msg::PoseStamped
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
- /**
- * @brief Plan a trajectory between a given start and goal pose (tf::Pose version)
- *
- * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
- * The method supports hot-starting from previous solutions, if avaiable: \n
- * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses,
- * see TimedElasticBand::initTEBtoGoal
- * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
- * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
- * @param start tf::Pose containing the start pose of the trajectory
- * @param goal tf::Pose containing the goal pose of the trajectory
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- // tf2 doesn't have tf::Pose
-// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
- /**
- * @brief Plan a trajectory between a given start and goal pose
- *
- * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
- * The method supports hot-starting from previous solutions, if avaiable: \n
- * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses
- * @see TimedElasticBand::initTEBtoGoal
- * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
- * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
- * @param start PoseSE2 containing the start pose of the trajectory
- * @param goal PoseSE2 containing the goal pose of the trajectory
- * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
-
-
- /**
- * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
- * @warning Call plan() first and check if the generated plan is feasible.
- * @param[out] vx translational velocity [m/s]
- * @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s]
- * @param[out] omega rotational velocity [rad/s]
- * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
- * @return \c true if command is valid, \c false otherwise
- */
- virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
-
-
- /**
- * @brief Optimize a previously initialized trajectory (actual TEB optimization loop).
- *
- * optimizeTEB implements the main optimization loop. \n
- * It consist of two nested loops:
- * - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize().
- * Afterwards the internal method optimizeGraph() is called that constitutes the innerloop.
- * - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified
- * number of optimization calls (\c iterations_innerloop).
- *
- * The outer loop is repeated \c iterations_outerloop times. \n
- * The ratio of inner and outer loop iterations significantly defines the contraction behavior
- * and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n
- * The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n
- * Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost().
- * @remarks This method is usually called from a plan() method
- * @param iterations_innerloop Number of iterations for the actual solver loop
- * @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop.
- * @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(),
- * the vector can be accessed afterwards using getCurrentCost().
- * @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true)
- * @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true)
- * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
- * (only used if \c compute_cost_afterwards is true).
- * @return \c true if the optimization terminates successfully, \c false otherwise
- */
- bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false,
- double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
-
- //@}
-
-
- /** @name Desired initial and final velocity */
- //@{
-
-
- /**
- * @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload].
- * @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method
- * @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used,
- * for holonomic robots also linear.y)
- */
- void setVelocityStart(const geometry_msgs::msg::Twist& vel_start);
-
- /**
- * @brief Set the desired final velocity at the trajectory's goal pose.
- * @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan()
- * @param vel_goal twist message containing the translational and angular final velocity
- */
- void setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal);
-
- /**
- * @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit
- * @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan()
- */
- void setVelocityGoalFree() {vel_goal_.first = false;}
-
- //@}
-
-
- /** @name Take obstacles into account */
- //@{
-
-
- /**
- * @brief Assign a new set of obstacles
- * @param obst_vector pointer to an obstacle container (can also be a nullptr)
- * @remarks This method overrids the obstacle container optinally assigned in the constructor.
- */
- void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;}
-
- /**
- * @brief Access the internal obstacle container.
- * @return Const reference to the obstacle container
- */
- const ObstContainer& getObstVector() const {return *obstacles_;}
-
- //@}
-
- /** @name Take via-points into account */
- //@{
-
-
- /**
- * @brief Assign a new set of via-points
- * @param via_points pointer to a via_point container (can also be a nullptr)
- * @details Any previously set container will be overwritten.
- */
- void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;}
-
- /**
- * @brief Access the internal via-point container.
- * @return Const reference to the via-point container
- */
- const ViaPointContainer& getViaPoints() const {return *via_points_;}
-
- //@}
-
-
- /** @name Visualization */
- //@{
-
- /**
- * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
- * @param visualization shared pointer to a TebVisualization instance
- * @see visualize
- */
- void setVisualization(const TebVisualizationPtr & visualization) override;
-
- /**
- * @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).
- *
- * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
- * @see setVisualization
- */
- virtual void visualize();
-
- //@}
-
-
- /** @name Utility methods and more */
- //@{
-
- /**
- * @brief Reset the planner by clearing the internal graph and trajectory.
- */
- virtual void clearPlanner()
- {
- clearGraph();
- teb_.clearTimedElasticBand();
- }
-
- /**
- * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
- *
- * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
- * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
- * Initial means that the penalty is applied only to the first few poses of the trajectory.
- * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
- */
- virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;}
-
- /**
- * @brief Register the vertices and edges defined for the TEB to the g2o::Factory.
- *
- * This allows the user to export the internal graph to a text file for instance.
- * Access the optimizer() for more details.
- */
- static void registerG2OTypes();
-
- /**
- * @brief Access the internal TimedElasticBand trajectory.
- * @warning In general, the underlying teb must not be modified directly. Use with care...
- * @return reference to the teb
- */
- TimedElasticBand& teb() {return teb_;};
-
- /**
- * @brief Access the internal TimedElasticBand trajectory (read-only).
- * @return const reference to the teb
- */
- const TimedElasticBand& teb() const {return teb_;};
-
- /**
- * @brief Access the internal g2o optimizer.
- * @warning In general, the underlying optimizer must not be modified directly. Use with care...
- * @return const shared pointer to the g2o sparse optimizer
- */
- std::shared_ptr<g2o::SparseOptimizer> optimizer() {return optimizer_;};
-
- /**
- * @brief Access the internal g2o optimizer (read-only).
- * @return const shared pointer to the g2o sparse optimizer
- */
- std::shared_ptr<const g2o::SparseOptimizer> optimizer() const {return optimizer_;};
-
- /**
- * @brief Check if last optimization was successful
- * @return \c true if the last optimization returned without errors,
- * otherwise \c false (also if no optimization has been called before).
- */
- bool isOptimized() const {return optimized_;};
-
- /**
- * @brief Returns true if the planner has diverged.
- */
- bool hasDiverged() const override;
-
- /**
- * @brief Compute the cost vector of a given optimization problen (hyper-graph must exist).
- *
- * Use this method to obtain information about the current edge errors / costs (local cost functions). \n
- * The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n
- * Refer to the method declaration for the detailed composition. \n
- * The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single
- * squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional
- * or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n
- * Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the
- * implemented definition, if the value is scaled to match the magnitude of other cost values.
- *
- * @todo Remove the scaling term for the alternative time cost.
- * @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself?
- * @see getCurrentCost
- * @see optimizeTEB
- * @param obst_cost_scale Specify extra scaling for obstacle costs.
- * @param viapoint_cost_scale Specify extra scaling for via points.
- * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time.
- * @return TebCostVec containing the cost values
- */
- void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
-
- /**
- * Compute and return the cost of the current optimization graph (supports multiple trajectories)
- * @param[out] cost current cost value for each trajectory
- * [for a planner with just a single trajectory: size=1, vector will not be cleared]
- * @param obst_cost_scale Specify extra scaling for obstacle costs
- * @param viapoint_cost_scale Specify extra scaling for via points.
- * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
- */
- virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
- {
- computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
- cost.push_back( getCurrentCost() );
- }
-
- /**
- * @brief Access the cost vector.
- *
- * The accumulated cost value previously calculated using computeCurrentCost
- * or by calling optimizeTEB with enabled cost flag.
- * @return const reference to the TebCostVec.
- */
- double getCurrentCost() const {return cost_;}
-
-
- /**
- * @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots)
- *
- * The velocity is extracted using finite differences.
- * The direction of the translational velocity is also determined.
- * @param pose1 pose at time k
- * @param pose2 consecutive pose at time k+1
- * @param dt actual time difference between k and k+1 (must be >0 !!!)
- * @param[out] vx translational velocity
- * @param[out] vy strafing velocity which can be nonzero for holonomic robots
- * @param[out] omega rotational velocity
- */
- inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const;
-
- /**
- * @brief Compute the velocity profile of the trajectory
- *
- * This method computes the translational and rotational velocity for the complete
- * planned trajectory.
- * The first velocity is the one that is provided as initial velocity (fixed).
- * Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k.
- * The last velocity is the final velocity (fixed).
- * The number of Twist objects is therefore sizePoses()+1;
- * In summary:
- * v[0] = v_start,
- * v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt,
- * v(end) = v_goal
- * It can be used for evaluation and debugging purposes or
- * for open-loop control. For computing the velocity required for controlling the robot
- * to the next step refer to getVelocityCommand().
- * @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1
- */
- void getVelocityProfile(std::vector<geometry_msgs::msg::Twist>& velocity_profile) const;
-
- /**
- * @brief Return the complete trajectory including poses, velocity profiles and temporal information
- *
- * It is useful for evaluation and debugging purposes or for open-loop control.
- * Since the velocity obtained using difference quotients is the mean velocity between consecutive poses,
- * the velocity at each pose at time stamp k is obtained by taking the average between both velocities.
- * The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off).
- * See getVelocityProfile() for the list of velocities between consecutive points.
- * @todo The acceleration profile is not added at the moment.
- * @param[out] trajectory the resulting trajectory
- */
- void getFullTrajectory(std::vector<teb_msgs::msg::TrajectoryPointMsg>& trajectory) const;
-
- /**
- * @brief Check whether the planned trajectory is feasible or not.
- *
- * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
- * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
- * @param costmap_model Pointer to the costmap model
- * @param footprint_spec The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
- * @return \c true, if the robot footprint along the first part of the trajectory intersects with
- * any obstacle in the costmap, \c false otherwise.
- */
- virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec, double inscribed_radius = 0.0,
- double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1);
-
- /**
- * @brief Check whether the footprint of the robot at the pose touches an obstacle or not.
- *
- * @param pose2d Pose to check
- * @param costmap_model Pointer to the costmap model
- * @param footprint_spec The specification of the footprint of the robot in world coordinates
- * @return \c true, if the robot pose is valid, \c false otherwise.
- */
- virtual bool isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model,
- const std::vector<geometry_msgs::msg::Point>& footprint_spec);
-
- //@}
-
-protected:
-
- /** @name Hyper-Graph creation and optimization */
- //@{
-
- /**
- * @brief Build the hyper-graph representing the TEB optimization problem.
- *
- * This method creates the optimization problem according to the hyper-graph formulation. \n
- * For more details refer to the literature cited in the TebOptimalPlanner class description.
- * @see optimizeGraph
- * @see clearGraph
- * @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph
- * This might be used for weight adapation strategies.
- * Currently, only obstacle collision weights are considered.
- * @return \c true, if the graph was created successfully, \c false otherwise.
- */
- bool buildGraph(double weight_multiplier=1.0);
-
- /**
- * @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB.
- *
- * This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n
- * The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered
- * by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description.
- * @see buildGraph
- * @see clearGraph
- * @param no_iterations Number of solver iterations
- * @param clear_after Clear the graph after optimization.
- * @return \c true, if optimization terminates successfully, \c false otherwise.
- */
- bool optimizeGraph(int no_iterations, bool clear_after=true);
-
- /**
- * @brief Clear an existing internal hyper-graph.
- * @see buildGraph
- * @see optimizeGraph
- */
- void clearGraph();
-
- /**
- * @brief Add all relevant vertices to the hyper-graph as optimizable variables.
- *
- * Vertices (if unfixed) represent the variables that will be optimized. \n
- * In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n
- * The order of insertion of vertices (to the graph) is important for efficiency,
- * since it affect the sparsity pattern of the underlying hessian computed for optimization.
- * @see VertexPose
- * @see VertexTimeDiff
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddTEBVertices();
-
- /**
- * @brief Add all edges (local cost functions) for limiting the translational and angular velocity.
- * @see EdgeVelocity
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesVelocity();
-
- /**
- * @brief Add all edges (local cost functions) for limiting the translational and angular acceleration.
- * @see EdgeAcceleration
- * @see EdgeAccelerationStart
- * @see EdgeAccelerationGoal
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesAcceleration();
-
- /**
- * @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)
- * @see EdgeTimeOptimal
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesTimeOptimal();
-
- /**
- * @brief Add all edges (local cost functions) for minimizing the path length
- * @see EdgeShortestPath
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesShortestPath();
-
- /**
- * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles
- * @warning do not combine with AddEdgesInflatedObstacles
- * @see EdgeObstacle
- * @see buildGraph
- * @see optimizeGraph
- * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
- */
- void AddEdgesObstacles(double weight_multiplier=1.0);
-
- /**
- * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)
- * @see EdgeObstacle
- * @see buildGraph
- * @see optimizeGraph
- * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
- */
- void AddEdgesObstaclesLegacy(double weight_multiplier=1.0);
-
- /**
- * @brief Add all edges (local cost functions) related to minimizing the distance to via-points
- * @see EdgeViaPoint
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesViaPoints();
-
- /**
- * @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.
- * @warning experimental
- * @todo Should we also add neighbors to decrease jiggling/oscillations
- * @see EdgeDynamicObstacle
- * @see buildGraph
- * @see optimizeGraph
- * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
-
- */
- void AddEdgesDynamicObstacles(double weight_multiplier=1.0);
-
- /**
- * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot
- * @warning do not combine with AddEdgesKinematicsCarlike()
- * @see AddEdgesKinematicsCarlike
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesKinematicsDiffDrive();
-
- /**
- * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot
- * @warning do not combine with AddEdgesKinematicsDiffDrive()
- * @see AddEdgesKinematicsDiffDrive
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesKinematicsCarlike();
-
- /**
- * @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesPreferRotDir();
-
- /**
- * @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles
- * @see buildGraph
- * @see optimizeGraph
- */
- void AddEdgesVelocityObstacleRatio();
-
- //@}
-
-
- /**
- * @brief Initialize and configure the g2o sparse optimizer.
- * @return shared pointer to the g2o::SparseOptimizer instance
- */
- std::shared_ptr<g2o::SparseOptimizer> initOptimizer();
-
-
- // external objects (store weak pointers)
- const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
- ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
- const ViaPointContainer* via_points_; //!< Store via points for planning
- std::vector<ObstContainer> obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices
-
- double cost_; //!< Store cost value of the current hyper-graph
- RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)
-
- // internal objects (memory management owned)
- TebVisualizationPtr visualization_; //!< Instance of the visualization class
- TimedElasticBand teb_; //!< Actual trajectory object
- RobotFootprintModelPtr robot_model_; //!< Robot model
- std::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
- std::pair<bool, geometry_msgs::msg::Twist> vel_start_; //!< Store the initial velocity at the start pose
- std::pair<bool, geometry_msgs::msg::Twist> vel_goal_; //!< Store the final velocity at the goal pose
-
- bool initialized_; //!< Keeps track about the correct initialization of this class
- bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-//! Abbrev. for shared instances of the TebOptimalPlanner
-typedef std::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;
-//! Abbrev. for shared const TebOptimalPlanner pointers
-typedef std::shared_ptr<const TebOptimalPlanner> TebOptimalPlannerConstPtr;
-//! Abbrev. for containers storing multiple teb optimal planners
-typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;
-
-} // namespace teb_local_planner
-
-#endif /* OPTIMAL_PLANNER_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/planner_interface.h b/src/teb_local_planner/include/teb_local_planner/planner_interface.h
deleted file mode 100644
index 268ba84..0000000
--- a/src/teb_local_planner/include/teb_local_planner/planner_interface.h
+++ /dev/null
@@ -1,218 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef PLANNER_INTERFACE_H_
-#define PLANNER_INTERFACE_H_
-
-#include<memory>
-
-// ros
-#include <dwb_critics/obstacle_footprint.hpp>
-
-#include <rclcpp/node.hpp>
-// this package
-#include <teb_local_planner/pose_se2.h>
-#include <teb_local_planner/robot_footprint_model.h>
-
-#include <tf2/transform_datatypes.h>
-
-#include <nav2_costmap_2d/costmap_2d.hpp>
-
-#include <nav2_util/lifecycle_node.hpp>
-
-// messages
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-
-// this package
-#include "teb_local_planner/pose_se2.h"
-#include "teb_local_planner/visualization.h"
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class PlannerInterface
- * @brief This abstract class defines an interface for local planners
- */
-class PlannerInterface
-{
-public:
-
- /**
- * @brief Default constructor
- */
- PlannerInterface()
- {
- }
- /**
- * @brief Virtual destructor.
- */
- virtual ~PlannerInterface()
- {
- }
-
-
- /** @name Plan a trajectory */
- //@{
-
- /**
- * @brief Plan a trajectory based on an initial reference plan.
- *
- * Provide this method to create and optimize a trajectory that is initialized
- * according to an initial reference plan (given as a container of poses).
- * @param initial_plan vector of geometry_msgs::msg::PoseStamped
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
-
- /**
- * @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
- *
- * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
- * @param start tf::Pose containing the start pose of the trajectory
- * @param goal tf::Pose containing the goal pose of the trajectory
- * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- // tf2 doesn't have tf2::Pose
- //virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
-
- /**
- * @brief Plan a trajectory between a given start and goal pose.
- *
- * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
- * @param start PoseSE2 containing the start pose of the trajectory
- * @param goal PoseSE2 containing the goal pose of the trajectory
- * @param start_vel Initial velocity at the start pose (twist msg containing the translational and angular velocity).
- * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
- * otherwise the final velocity will be zero (default: false)
- * @return \c true if planning was successful, \c false otherwise
- */
- virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
-
- /**
- * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
- * @warning Call plan() first and check if the generated plan is feasible.
- * @param[out] vx translational velocity [m/s]
- * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
- * @param[out] omega rotational velocity [rad/s]
- * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
- * @return \c true if command is valid, \c false otherwise
- */
- virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const = 0;
-
- //@}
-
-
- /**
- * @brief Reset the planner.
- */
- virtual void clearPlanner() = 0;
-
- /**
- * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
- *
- * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
- * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
- * Initial means that the penalty is applied only to the first few poses of the trajectory.
- * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
- */
- virtual void setPreferredTurningDir(RotType dir) {
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "setPreferredTurningDir() not implemented for this planner.");}
-
- /**
- * @brief Visualize planner specific stuff.
- * Overwrite this method to provide an interface to perform all planner related visualizations at once.
- */
- virtual void visualize()
- {
- }
-
- virtual void setVisualization(const TebVisualizationPtr & visualization) = 0;
-
- /**
- * @brief Check whether the planned trajectory is feasible or not.
- *
- * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
- * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
- * @param costmap_model Pointer to the costmap model
- * @param footprint_spec The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
- * @return \c true, if the robot footprint along the first part of the trajectory intersects with
- * any obstacle in the costmap, \c false otherwise.
- */
- virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
- double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0) = 0;
-
- /**
- * Compute and return the cost of the current optimization graph (supports multiple trajectories)
- * @param[out] cost current cost value for each trajectory
- * [for a planner with just a single trajectory: size=1, vector will not be cleared]
- * @param obst_cost_scale Specify extra scaling for obstacle costs
- * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
- */
- virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
- {
- }
-
- /**
- * @brief Returns true if the planner has diverged.
- */
- virtual bool hasDiverged() const = 0;
-
- nav2_util::LifecycleNode::SharedPtr node_{nullptr};
-};
-
-//! Abbrev. for shared instances of PlannerInterface or it's subclasses
-typedef std::shared_ptr<PlannerInterface> PlannerInterfacePtr;
-
-
-} // namespace teb_local_planner
-
-#endif /* PLANNER_INTERFACE_H__ */
diff --git a/src/teb_local_planner/include/teb_local_planner/pose_se2.h b/src/teb_local_planner/include/teb_local_planner/pose_se2.h
deleted file mode 100755
index 55888c7..0000000
--- a/src/teb_local_planner/include/teb_local_planner/pose_se2.h
+++ /dev/null
@@ -1,433 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef POSE_SE2_H_
-#define POSE_SE2_H_
-
-#include <g2o/stuff/misc.h>
-
-#include <Eigen/Core>
-#include "teb_local_planner/misc.h"
-#include <geometry_msgs/msg/pose.hpp>
-#include <geometry_msgs/msg/pose2_d.hpp>
-
-#include <tf2/convert.h>
-#include <tf2/utils.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-
-namespace teb_local_planner
-{
-
-/**
- * @class PoseSE2
- * @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$
- * The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].
- */
-class PoseSE2
-{
-public:
-
- /** @name Construct PoseSE2 instances */
- ///@{
-
- /**
- * @brief Default constructor
- */
- PoseSE2()
- {
- setZero();
- }
-
- /**
- * @brief Construct pose given a position vector and an angle theta
- * @param position 2D position vector
- * @param theta angle given in rad
- */
- PoseSE2(const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
- {
- _position = position;
- _theta = theta;
- }
-
- /**
- * @brief Construct pose using single components x, y, and the yaw angle
- * @param x x-coordinate
- * @param y y-coordinate
- * @param theta yaw angle in rad
- */
- PoseSE2(double x, double y, double theta)
- {
- _position.coeffRef(0) = x;
- _position.coeffRef(1) = y;
- _theta = theta;
- }
-
- /**
- * @brief Construct pose using a geometry_msgs::msg::Pose
- * @param pose geometry_msgs::msg::Pose object
- */
- PoseSE2(const geometry_msgs::msg::PoseStamped& pose)
- :PoseSE2(pose.pose)
- {
-
- }
-
- /**
- * @brief Construct pose using a geometry_msgs::msg::Pose
- * @param pose geometry_msgs::msg::Pose object
- */
- PoseSE2(const geometry_msgs::msg::Pose& pose)
- {
- _position.coeffRef(0) = pose.position.x;
- _position.coeffRef(1) = pose.position.y;
- _theta = tf2::getYaw( pose.orientation );
- }
-
- /**
- * @brief Construct pose using a geometry_msgs::msg::Pose2D
- * @param pose geometry_msgs::msg::Pose2D object
- */
- PoseSE2(const geometry_msgs::msg::Pose2D& pose)
- {
- _position.coeffRef(0) = pose.x;
- _position.coeffRef(1) = pose.y;
- _theta = pose.theta;
- }
-
- /**
- * @brief Copy constructor
- * @param pose PoseSE2 instance
- */
- PoseSE2(const PoseSE2& pose)
- {
- _position = pose._position;
- _theta = pose._theta;
- }
-
- ///@}
-
-
- /**
- * @brief Destructs the PoseSE2
- */
- ~PoseSE2() {}
-
-
- /** @name Access and modify values */
- ///@{
-
- /**
- * @brief Access the 2D position part
- * @see estimate
- * @return reference to the 2D position part
- */
- Eigen::Vector2d& position() {return _position;}
-
- /**
- * @brief Access the 2D position part (read-only)
- * @see estimate
- * @return const reference to the 2D position part
- */
- const Eigen::Vector2d& position() const {return _position;}
-
- /**
- * @brief Access the x-coordinate the pose
- * @return reference to the x-coordinate
- */
- double& x() {return _position.coeffRef(0);}
-
- /**
- * @brief Access the x-coordinate the pose (read-only)
- * @return const reference to the x-coordinate
- */
- const double& x() const {return _position.coeffRef(0);}
-
- /**
- * @brief Access the y-coordinate the pose
- * @return reference to the y-coordinate
- */
- double& y() {return _position.coeffRef(1);}
-
- /**
- * @brief Access the y-coordinate the pose (read-only)
- * @return const reference to the y-coordinate
- */
- const double& y() const {return _position.coeffRef(1);}
-
- /**
- * @brief Access the orientation part (yaw angle) of the pose
- * @return reference to the yaw angle
- */
- double& theta() {return _theta;}
-
- /**
- * @brief Access the orientation part (yaw angle) of the pose (read-only)
- * @return const reference to the yaw angle
- */
- const double& theta() const {return _theta;}
-
- /**
- * @brief Set pose to [0,0,0]
- */
- void setZero()
- {
- _position.setZero();
- _theta = 0;
- }
-
- /**
- * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose
- * @param[out] pose Pose message
- */
- void toPoseMsg(geometry_msgs::msg::Pose& pose) const
- {
- pose.position.x = _position.x();
- pose.position.y = _position.y();
- pose.position.z = 0;
- tf2::Quaternion q;
- q.setRPY(0, 0, _theta);
- pose.orientation = tf2::toMsg(q);
- }
-
- /**
- * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose2D
- * @param[out] pose Pose message
- */
- void toPoseMsg(geometry_msgs::msg::Pose2D& pose) const
- {
- pose.x = _position.x();
- pose.y = _position.y();
- pose.theta = _theta;
- }
-
- /**
- * @brief Return the unit vector of the current orientation
- * @returns [cos(theta), sin(theta))]^T
- */
- Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
-
- ///@}
-
-
- /** @name Arithmetic operations for which operators are not always reasonable */
- ///@{
-
- /**
- * @brief Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]
- * @param factor scale factor
- */
- void scale(double factor)
- {
- _position *= factor;
- _theta = g2o::normalize_theta( _theta*factor );
- }
-
- /**
- * @brief Increment the pose by adding a double[3] array
- * The angle is normalized afterwards
- * @param pose_as_array 3D double array [x, y, theta]
- */
- void plus(const double* pose_as_array)
- {
- _position.coeffRef(0) += pose_as_array[0];
- _position.coeffRef(1) += pose_as_array[1];
- _theta = g2o::normalize_theta( _theta + pose_as_array[2] );
- }
-
- /**
- * @brief Get the mean / average of two poses and store it in the caller class
- * For the position part: 0.5*(x1+x2)
- * For the angle: take the angle of the mean direction vector
- * @param pose1 first pose to consider
- * @param pose2 second pose to consider
- */
- void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
- {
- _position = (pose1._position + pose2._position)/2;
- _theta = g2o::average_angle(pose1._theta, pose2._theta);
- }
-
- /**
- * @brief Get the mean / average of two poses and return the result (static)
- * For the position part: 0.5*(x1+x2)
- * For the angle: take the angle of the mean direction vector
- * @param pose1 first pose to consider
- * @param pose2 second pose to consider
- * @return mean / average of \c pose1 and \c pose2
- */
- static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
- {
- return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
- }
-
- /**
- * @brief Rotate pose globally
- *
- * Compute [pose_x, pose_y] = Rot(\c angle) * [pose_x, pose_y].
- * if \c adjust_theta, pose_theta is also rotated by \c angle
- * @param angle the angle defining the 2d rotation
- * @param adjust_theta if \c true, the orientation theta is also rotated
- */
- void rotateGlobal(double angle, bool adjust_theta=true)
- {
- double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y();
- double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y();
- _position.x() = new_x;
- _position.y() = new_y;
- if (adjust_theta)
- _theta = g2o::normalize_theta(_theta+angle);
- }
-
- ///@}
-
-
- /** @name Operator overloads / Allow some arithmetic operations */
- ///@{
-
- /**
- * @brief Asignment operator
- * @param rhs PoseSE2 instance
- * @todo exception safe version of the assignment operator
- */
- PoseSE2& operator=( const PoseSE2& rhs )
- {
- if (&rhs != this)
- {
- _position = rhs._position;
- _theta = rhs._theta;
- }
- return *this;
- }
-
- /**
- * @brief Compound assignment operator (addition)
- * @param rhs addend
- */
- PoseSE2& operator+=(const PoseSE2& rhs)
- {
- _position += rhs._position;
- _theta = g2o::normalize_theta(_theta + rhs._theta);
- return *this;
- }
-
- /**
- * @brief Arithmetic operator overload for additions
- * @param lhs First addend
- * @param rhs Second addend
- */
- friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs)
- {
- return lhs += rhs;
- }
-
- /**
- * @brief Compound assignment operator (subtraction)
- * @param rhs value to subtract
- */
- PoseSE2& operator-=(const PoseSE2& rhs)
- {
- _position -= rhs._position;
- _theta = g2o::normalize_theta(_theta - rhs._theta);
- return *this;
- }
-
- /**
- * @brief Arithmetic operator overload for subtractions
- * @param lhs First term
- * @param rhs Second term
- */
- friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs)
- {
- return lhs -= rhs;
- }
-
- /**
- * @brief Multiply pose with scalar and return copy without normalizing theta
- * This operator is useful for calculating velocities ...
- * @param pose pose to scale
- * @param scalar factor to multiply with
- * @warning theta is not normalized after multiplying
- */
- friend PoseSE2 operator*(PoseSE2 pose, double scalar)
- {
- pose._position *= scalar;
- pose._theta *= scalar;
- return pose;
- }
-
- /**
- * @brief Multiply pose with scalar and return copy without normalizing theta
- * This operator is useful for calculating velocities ...
- * @param scalar factor to multiply with
- * @param pose pose to scale
- * @warning theta is not normalized after multiplying
- */
- friend PoseSE2 operator*(double scalar, PoseSE2 pose)
- {
- pose._position *= scalar;
- pose._theta *= scalar;
- return pose;
- }
-
- /**
- * @brief Output stream operator
- * @param stream output stream
- * @param pose to be used
- */
- friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)
- {
- stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta;
- return stream;
- }
-
- ///@}
-
-
-private:
-
- Eigen::Vector2d _position;
- double _theta;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-} // namespace teb_local_planner
-
-#endif // POSE_SE2_H_
diff --git a/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h b/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
deleted file mode 100644
index 2e7607d..0000000
--- a/src/teb_local_planner/include/teb_local_planner/recovery_behaviors.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef RECOVERY_BEHAVIORS_H__
-#define RECOVERY_BEHAVIORS_H__
-
-
-#include <boost/circular_buffer.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-namespace teb_local_planner
-{
-
-
-/**
- * @class FailureDetector
- * @brief This class implements methods in order to detect if the robot got stucked or is oscillating
- *
- * The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot
- * might got stucked or oscillates between left/right/forward/backwards motions.
- */
-class FailureDetector
-{
-public:
-
- /**
- * @brief Default constructor
- */
- FailureDetector() {}
-
- /**
- * @brief destructor.
- */
- ~FailureDetector() {}
-
- /**
- * @brief Set buffer length (measurement history)
- * @param length number of measurements to be kept
- */
- void setBufferLength(int length) {buffer_.set_capacity(length);}
-
- /**
- * @brief Add a new twist measurement to the internal buffer and compute a new decision
- * @param twist geometry_msgs::msg::Twist velocity information
- * @param v_max maximum forward translational velocity
- * @param v_backwards_max maximum backward translational velocity
- * @param omega_max maximum angular velocity
- * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
- * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
- */
- void update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps);
-
- /**
- * @brief Check if the robot got stucked
- *
- * This call does not compute the actual decision,
- * since it is computed within each update() invocation.
- * @return true if the robot got stucked, false otherwise.
- */
- bool isOscillating() const;
-
- /**
- * @brief Clear the current internal state
- *
- * This call also resets the internal buffer
- */
- void clear();
-
-protected:
-
- /** Variables to be monitored */
- struct VelMeasurement
- {
- double v = 0;
- double omega = 0;
- };
-
- /**
- * @brief Detect if the robot got stucked based on the current buffer content
- *
- * Afterwards the status might be checked using gotStucked();
- * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
- * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
- * @return true if the robot got stucked, false otherwise
- */
- bool detect(double v_eps, double omega_eps);
-
-private:
-
- boost::circular_buffer<VelMeasurement> buffer_; //!< Circular buffer to store the last measurements @see setBufferLength
- bool oscillating_ = false; //!< Current state: true if robot is oscillating
-
-};
-
-
-} // namespace teb_local_planner
-
-#endif /* RECOVERY_BEHAVIORS_H__ */
diff --git a/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h b/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
deleted file mode 100644
index 65200f2..0000000
--- a/src/teb_local_planner/include/teb_local_planner/robot_footprint_model.h
+++ /dev/null
@@ -1,684 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-
-#ifndef ROBOT_FOOTPRINT_MODEL_H
-#define ROBOT_FOOTPRINT_MODEL_H
-
-#include "teb_local_planner/pose_se2.h"
-#include "teb_local_planner/obstacles.h"
-#include <visualization_msgs/msg/marker.hpp>
-
-namespace teb_local_planner
-{
-
-/**
- * @class BaseRobotFootprintModel
- * @brief Abstract class that defines the interface for robot footprint/contour models
- *
- * The robot model class is currently used in optimization only, since
- * taking the navigation stack footprint into account might be
- * inefficient. The footprint is only used for checking feasibility.
- */
-class BaseRobotFootprintModel
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- */
- BaseRobotFootprintModel()
- {
- }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~BaseRobotFootprintModel()
- {
- }
-
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const = 0;
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const = 0;
-
- /**
- * @brief Visualize the robot using a markers
- *
- * Fill a marker message with all necessary information (type, pose, scale and color).
- * The header, namespace, id and marker lifetime will be overwritten.
- * @param current_pose Current robot pose
- * @param[out] markers container of marker messages describing the robot shape
- * @param color Color of the footprint
- */
- virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const {}
-
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius() = 0;
-
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-//! Abbrev. for shared obstacle pointers
-typedef std::shared_ptr<BaseRobotFootprintModel> RobotFootprintModelPtr;
-//! Abbrev. for shared obstacle const pointers
-typedef std::shared_ptr<const BaseRobotFootprintModel> RobotFootprintModelConstPtr;
-
-
-
-/**
- * @class PointRobotShape
- * @brief Class that defines a point-robot
- *
- * Instead of using a CircularRobotFootprint this class might
- * be utitilzed and the robot radius can be added to the mininum distance
- * parameter. This avoids a subtraction of zero each time a distance is calculated.
- */
-class PointRobotFootprint : public BaseRobotFootprintModel
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- */
- PointRobotFootprint() {}
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~PointRobotFootprint() {}
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
- {
- return obstacle->getMinimumDistance(current_pose.position());
- }
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
- {
- return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t);
- }
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius() {return 0.0;}
-
-};
-
-
-/**
- * @class CircularRobotFootprint
- * @brief Class that defines the a robot of circular shape
- */
-class CircularRobotFootprint : public BaseRobotFootprintModel
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- * @param radius radius of the robot
- */
- CircularRobotFootprint(double radius) : radius_(radius) { }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~CircularRobotFootprint() { }
-
- /**
- * @brief Set radius of the circular robot
- * @param radius radius of the robot
- */
- void setRadius(double radius) {radius_ = radius;}
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
- {
- return obstacle->getMinimumDistance(current_pose.position()) - radius_;
- }
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
- {
- return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t) - radius_;
- }
-
- /**
- * @brief Visualize the robot using a markers
- *
- * Fill a marker message with all necessary information (type, pose, scale and color).
- * The header, namespace, id and marker lifetime will be overwritten.
- * @param current_pose Current robot pose
- * @param[out] markers container of marker messages describing the robot shape
- * @param color Color of the footprint
- */
- virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
- {
- markers.resize(1);
- visualization_msgs::msg::Marker& marker = markers.back();
- marker.type = visualization_msgs::msg::Marker::CYLINDER;
- current_pose.toPoseMsg(marker.pose);
- marker.scale.x = marker.scale.y = 2*radius_; // scale = diameter
- marker.scale.z = 0.05;
- marker.color = color;
- }
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius() {return radius_;}
-
-private:
-
- double radius_;
-};
-
-
-/**
- * @class TwoCirclesRobotFootprint
- * @brief Class that approximates the robot with two shifted circles
- */
-class TwoCirclesRobotFootprint : public BaseRobotFootprintModel
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
- * @param front_radius radius of the front circle
- * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
- * @param rear_radius radius of the front circle
- */
- TwoCirclesRobotFootprint(double front_offset, double front_radius, double rear_offset, double rear_radius)
- : front_offset_(front_offset), front_radius_(front_radius), rear_offset_(rear_offset), rear_radius_(rear_radius) { }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~TwoCirclesRobotFootprint() { }
-
- /**
- * @brief Set parameters of the contour/footprint
- * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
- * @param front_radius radius of the front circle
- * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
- * @param rear_radius radius of the front circle
- */
- void setParameters(double front_offset, double front_radius, double rear_offset, double rear_radius)
- {front_offset_=front_offset; front_radius_=front_radius; rear_offset_=rear_offset; rear_radius_=rear_radius;}
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
- {
- Eigen::Vector2d dir = current_pose.orientationUnitVec();
- double dist_front = obstacle->getMinimumDistance(current_pose.position() + front_offset_*dir) - front_radius_;
- double dist_rear = obstacle->getMinimumDistance(current_pose.position() - rear_offset_*dir) - rear_radius_;
- return std::min(dist_front, dist_rear);
- }
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
- {
- Eigen::Vector2d dir = current_pose.orientationUnitVec();
- double dist_front = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() + front_offset_*dir, t) - front_radius_;
- double dist_rear = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() - rear_offset_*dir, t) - rear_radius_;
- return std::min(dist_front, dist_rear);
- }
-
- /**
- * @brief Visualize the robot using a markers
- *
- * Fill a marker message with all necessary information (type, pose, scale and color).
- * The header, namespace, id and marker lifetime will be overwritten.
- * @param current_pose Current robot pose
- * @param[out] markers container of marker messages describing the robot shape
- * @param color Color of the footprint
- */
- virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
- {
- Eigen::Vector2d dir = current_pose.orientationUnitVec();
- if (front_radius_>0)
- {
- markers.push_back(visualization_msgs::msg::Marker());
- visualization_msgs::msg::Marker& marker1 = markers.front();
- marker1.type = visualization_msgs::msg::Marker::CYLINDER;
- current_pose.toPoseMsg(marker1.pose);
- marker1.pose.position.x += front_offset_*dir.x();
- marker1.pose.position.y += front_offset_*dir.y();
- marker1.scale.x = marker1.scale.y = 2*front_radius_; // scale = diameter
-// marker1.scale.z = 0.05;
- marker1.color = color;
-
- }
- if (rear_radius_>0)
- {
- markers.push_back(visualization_msgs::msg::Marker());
- visualization_msgs::msg::Marker& marker2 = markers.back();
- marker2.type = visualization_msgs::msg::Marker::CYLINDER;
- current_pose.toPoseMsg(marker2.pose);
- marker2.pose.position.x -= rear_offset_*dir.x();
- marker2.pose.position.y -= rear_offset_*dir.y();
- marker2.scale.x = marker2.scale.y = 2*rear_radius_; // scale = diameter
-// marker2.scale.z = 0.05;
- marker2.color = color;
- }
- }
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius()
- {
- double min_longitudinal = std::min(rear_offset_ + rear_radius_, front_offset_ + front_radius_);
- double min_lateral = std::min(rear_radius_, front_radius_);
- return std::min(min_longitudinal, min_lateral);
- }
-
-private:
-
- double front_offset_;
- double front_radius_;
- double rear_offset_;
- double rear_radius_;
-
-};
-
-
-
-/**
- * @class LineRobotFootprint
- * @brief Class that approximates the robot with line segment (zero-width)
- */
-class LineRobotFootprint : public BaseRobotFootprintModel
-{
-public:
-
- /**
- * @brief Default constructor of the abstract obstacle class
- * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
- * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
- */
- LineRobotFootprint(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end)
- {
- setLine(line_start, line_end);
- }
-
- /**
- * @brief Default constructor of the abstract obstacle class (Eigen Version)
- * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
- * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
- */
- LineRobotFootprint(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
- {
- setLine(line_start, line_end);
- }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~LineRobotFootprint() { }
-
- /**
- * @brief Set vertices of the contour/footprint
- * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
- */
- void setLine(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end)
- {
- line_start_.x() = line_start.x;
- line_start_.y() = line_start.y;
- line_end_.x() = line_end.x;
- line_end_.y() = line_end.y;
- }
-
- /**
- * @brief Set vertices of the contour/footprint (Eigen version)
- * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
- */
- void setLine(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
- {
- line_start_ = line_start;
- line_end_ = line_end;
- }
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
- {
- Eigen::Vector2d line_start_world;
- Eigen::Vector2d line_end_world;
- transformToWorld(current_pose, line_start_world, line_end_world);
- return obstacle->getMinimumDistance(line_start_world, line_end_world);
- }
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
- {
- Eigen::Vector2d line_start_world;
- Eigen::Vector2d line_end_world;
- transformToWorld(current_pose, line_start_world, line_end_world);
- return obstacle->getMinimumSpatioTemporalDistance(line_start_world, line_end_world, t);
- }
-
- /**
- * @brief Visualize the robot using a markers
- *
- * Fill a marker message with all necessary information (type, pose, scale and color).
- * The header, namespace, id and marker lifetime will be overwritten.
- * @param current_pose Current robot pose
- * @param[out] markers container of marker messages describing the robot shape
- * @param color Color of the footprint
- */
- virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
- {
- markers.push_back(visualization_msgs::msg::Marker());
- visualization_msgs::msg::Marker& marker = markers.front();
- marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
- current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
-
- // line
- geometry_msgs::msg::Point line_start_world;
- line_start_world.x = line_start_.x();
- line_start_world.y = line_start_.y();
- line_start_world.z = 0;
- marker.points.push_back(line_start_world);
-
- geometry_msgs::msg::Point line_end_world;
- line_end_world.x = line_end_.x();
- line_end_world.y = line_end_.y();
- line_end_world.z = 0;
- marker.points.push_back(line_end_world);
-
- marker.scale.x = 0.05;
- marker.color = color;
- }
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius()
- {
- return 0.0; // lateral distance = 0.0
- }
-
-private:
-
- /**
- * @brief Transforms a line to the world frame manually
- * @param current_pose Current robot pose
- * @param[out] line_start line_start_ in the world frame
- * @param[out] line_end line_end_ in the world frame
- */
- void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const
- {
- double cos_th = std::cos(current_pose.theta());
- double sin_th = std::sin(current_pose.theta());
- line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y();
- line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y();
- line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y();
- line_end_world.y() = current_pose.y() + sin_th * line_end_.x() + cos_th * line_end_.y();
- }
-
- Eigen::Vector2d line_start_;
- Eigen::Vector2d line_end_;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-};
-
-
-
-/**
- * @class PolygonRobotFootprint
- * @brief Class that approximates the robot with a closed polygon
- */
-class PolygonRobotFootprint : public BaseRobotFootprintModel
-{
-public:
-
-
-
- /**
- * @brief Default constructor of the abstract obstacle class
- * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
- */
- PolygonRobotFootprint(const Point2dContainer& vertices) : vertices_(vertices) { }
-
- /**
- * @brief Virtual destructor.
- */
- virtual ~PolygonRobotFootprint() { }
-
- /**
- * @brief Set vertices of the contour/footprint
- * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
- */
- void setVertices(const Point2dContainer& vertices) {vertices_ = vertices;}
-
- /**
- * @brief Calculate the distance between the robot and an obstacle
- * @param current_pose Current robot pose
- * @param obstacle Pointer to the obstacle
- * @return Euclidean distance to the robot
- */
- virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
- {
- Point2dContainer polygon_world(vertices_.size());
- transformToWorld(current_pose, polygon_world);
- return obstacle->getMinimumDistance(polygon_world);
- }
-
- /**
- * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
- * @param current_pose robot pose, from which the distance to the obstacle is estimated
- * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
- * @param t time, for which the predicted distance to the obstacle is calculated
- * @return Euclidean distance to the robot
- */
- virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
- {
- Point2dContainer polygon_world(vertices_.size());
- transformToWorld(current_pose, polygon_world);
- return obstacle->getMinimumSpatioTemporalDistance(polygon_world, t);
- }
-
- /**
- * @brief Visualize the robot using a markers
- *
- * Fill a marker message with all necessary information (type, pose, scale and color).
- * The header, namespace, id and marker lifetime will be overwritten.
- * @param current_pose Current robot pose
- * @param[out] markers container of marker messages describing the robot shape
- * @param color Color of the footprint
- */
- virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::msg::Marker>& markers, const std_msgs::msg::ColorRGBA& color) const
- {
- if (vertices_.empty())
- return;
-
- markers.push_back(visualization_msgs::msg::Marker());
- visualization_msgs::msg::Marker& marker = markers.front();
- marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
- current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
-
- for (std::size_t i = 0; i < vertices_.size(); ++i)
- {
- geometry_msgs::msg::Point point;
- point.x = vertices_[i].x();
- point.y = vertices_[i].y();
- point.z = 0;
- marker.points.push_back(point);
- }
- // add first point again in order to close the polygon
- geometry_msgs::msg::Point point;
- point.x = vertices_.front().x();
- point.y = vertices_.front().y();
- point.z = 0;
- marker.points.push_back(point);
-
- marker.scale.x = 0.025;
- marker.color = color;
-
- }
-
- /**
- * @brief Compute the inscribed radius of the footprint model
- * @return inscribed radius
- */
- virtual double getInscribedRadius()
- {
- double min_dist = std::numeric_limits<double>::max();
- Eigen::Vector2d center(0.0, 0.0);
-
- if (vertices_.size() <= 2)
- return 0.0;
-
- for (int i = 0; i < (int)vertices_.size() - 1; ++i)
- {
- // compute distance from the robot center point to the first vertex
- double vertex_dist = vertices_[i].norm();
- double edge_dist = distance_point_to_segment_2d(center, vertices_[i], vertices_[i+1]);
- min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
- }
-
- // we also need to check the last vertex and the first vertex
- double vertex_dist = vertices_.back().norm();
- double edge_dist = distance_point_to_segment_2d(center, vertices_.back(), vertices_.front());
- return std::min(min_dist, std::min(vertex_dist, edge_dist));
- }
-
-private:
-
- /**
- * @brief Transforms a polygon to the world frame manually
- * @param current_pose Current robot pose
- * @param[out] polygon_world polygon in the world frame
- */
- void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const
- {
- double cos_th = std::cos(current_pose.theta());
- double sin_th = std::sin(current_pose.theta());
- for (std::size_t i=0; i<vertices_.size(); ++i)
- {
- polygon_world[i].x() = current_pose.x() + cos_th * vertices_[i].x() - sin_th * vertices_[i].y();
- polygon_world[i].y() = current_pose.y() + sin_th * vertices_[i].x() + cos_th * vertices_[i].y();
- }
- }
-
- Point2dContainer vertices_;
-
-};
-
-
-
-
-
-} // namespace teb_local_planner
-
-#endif /* ROBOT_FOOTPRINT_MODEL_H */
diff --git a/src/teb_local_planner/include/teb_local_planner/teb_config.h b/src/teb_local_planner/include/teb_local_planner/teb_config.h
deleted file mode 100644
index 9a70daa..0000000
--- a/src/teb_local_planner/include/teb_local_planner/teb_config.h
+++ /dev/null
@@ -1,432 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef TEB_CONFIG_H_
-#define TEB_CONFIG_H_
-
-#include <nav2_util/lifecycle_node.hpp>
-#include <memory>
-#include <rclcpp/rclcpp.hpp>
-#include <Eigen/Core>
-#include <Eigen/StdVector>
-#include <nav_2d_utils/parameters.hpp>
-#include "teb_local_planner/robot_footprint_model.h"
-#include <nav2_costmap_2d/footprint.hpp>
-
-// Definitions
-#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi
-
-namespace teb_local_planner
-{
-/**
- * @class TebConfig
- * @brief Config class for the teb_local_planner and its components.
- */
-class TebConfig
-{
-public:
- using UniquePtr = std::unique_ptr<TebConfig>;
-
- std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator
- std::string map_frame; //!< Global planning frame
- std::string node_name; //!< node name used for parameter event callback
-
- RobotFootprintModelPtr robot_model;
- std::string model_name;
- double radius;
- std::vector<double> line_start, line_end;
- double front_offset, front_radius, rear_offset, rear_radius;
- std::string footprint_string;
-
- //! Trajectory related parameters
- struct Trajectory
- {
- double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended)
- double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate)
- double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
- int min_samples; //!< Minimum number of samples (should be always greater than 2)
- int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore.
- bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner
- bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)
- double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled)
- bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container
- double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!]
- double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning
- bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
- double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
- double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting)
- int feasibility_check_no_poses; //!< Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.
- double feasibility_check_lookahead_distance; //!< Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.
- bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
- double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]
- int control_look_ahead_poses; //! Index of the pose used to extract the velocity command
- } trajectory; //!< Trajectory related parameters
-
- //! Robot related parameters
- struct Robot
- {
- double base_max_vel_x; //!< Maximum translational velocity of the robot before speed limit is applied
- double base_max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards before speed limit is applied
- double base_max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) before speed limit is applied
- double base_max_vel_theta; //!< Maximum angular velocity of the robot before speed limit is applied
- double max_vel_x; //!< Maximum translational velocity of the robot
- double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
- double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
- double max_vel_theta; //!< Maximum angular velocity of the robot
- double acc_lim_x; //!< Maximum translational acceleration of the robot
- double acc_lim_y; //!< Maximum strafing acceleration of the robot
- double acc_lim_theta; //!< Maximum angular acceleration of the robot
- double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero);
- double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
- bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
- bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
- bool use_proportional_saturation; //<! If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually
- double transform_tolerance = 0.5; //<! Tolerance when querying the TF Tree for a transformation (seconds)
- } robot; //!< Robot related parameters
-
- //! Goal tolerance related parameters
- struct GoalTolerance
- {
- double xy_goal_tolerance; //!< Allowed final euclidean distance to the goal position
- bool free_goal_vel; //!< Allow the robot's velocity to be nonzero (usally max_vel) for planning purposes
- } goal_tolerance; //!< Goal tolerance related parameters
-
- //! Obstacle related parameters
- struct Obstacles
- {
- double min_obstacle_dist; //!< Minimum desired separation from obstacles
- double inflation_dist; //!< buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
- double dynamic_obstacle_inflation_dist; //!< Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
- bool include_dynamic_obstacles; //!< Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also effects homotopy class planning); If false, all obstacles are considered to be static.
- bool include_costmap_obstacles; //!< Specify whether the obstacles in the costmap should be taken into account directly
- double costmap_obstacles_behind_robot_dist; //!< Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
- int obstacle_poses_affected; //!< The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
- bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only "relevant" obstacles).
- double obstacle_association_force_inclusion_factor; //!< The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
- double obstacle_association_cutoff_factor; //!< See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
- std::string costmap_converter_plugin; //!< Define a plugin name of the costmap_converter package (costmap cells are converted to points/lines/polygons)
- bool costmap_converter_spin_thread; //!< If \c true, the costmap converter invokes its callback queue in a different thread
- int costmap_converter_rate; //!< The rate that defines how often the costmap_converter plugin processes the current costmap (the value should not be much higher than the costmap update rate)
- double obstacle_proximity_ratio_max_vel; //!< Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to a static obstacles
- double obstacle_proximity_lower_bound; //!< Distance to a static obstacle for which the velocity should be lower
- double obstacle_proximity_upper_bound; //!< Distance to a static obstacle for which the velocity should be higher
- } obstacles; //!< Obstacle related parameters
-
-
- //! Optimization related parameters
- struct Optimization
- {
- int no_inner_iterations; //!< Number of solver iterations called in each outerloop iteration
- int no_outer_iterations; //!< Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
-
- bool optimization_activate; //!< Activate the optimization
- bool optimization_verbose; //!< Print verbose information
-
- double penalty_epsilon; //!< Add a small safety margin to penalty functions for hard-constraint approximations
-
- double weight_max_vel_x; //!< Optimization weight for satisfying the maximum allowed translational velocity
- double weight_max_vel_y; //!< Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
- double weight_max_vel_theta; //!< Optimization weight for satisfying the maximum allowed angular velocity
- double weight_acc_lim_x; //!< Optimization weight for satisfying the maximum allowed translational acceleration
- double weight_acc_lim_y; //!< Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
- double weight_acc_lim_theta; //!< Optimization weight for satisfying the maximum allowed angular acceleration
- double weight_kinematics_nh; //!< Optimization weight for satisfying the non-holonomic kinematics
- double weight_kinematics_forward_drive; //!< Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
- double weight_kinematics_turning_radius; //!< Optimization weight for enforcing a minimum turning radius (carlike robots)
- double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t. transition time
- double weight_shortest_path; //!< Optimization weight for contracting the trajectory w.r.t. path length
- double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles
- double weight_inflation; //!< Optimization weight for the inflation penalty (should be small)
- double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles
- double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles (should be small)
- double weight_velocity_obstacle_ratio; //!< Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle
- double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points
- double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently only activated if an oscillation is detected, see 'oscillation_recovery'
-
- double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
- double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)
- } optim; //!< Optimization related parameters
-
-
- struct HomotopyClasses
- {
- bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).
- bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel.
- bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.
- int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
- int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima)
- double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).
- double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.
- double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate.
- double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' candidate.
- bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time.
- double selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.
- double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed
-
- int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off.
- double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.
- double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!
- double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1).
- double h_signature_threshold; //!< Two h-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.
-
- double obstacle_keypoint_offset; //!< If simple_exploration is turned on, this parameter determines the distance on the left and right side of the obstacle at which a new keypoint will be cretead (in addition to min_obstacle_dist).
- double obstacle_heading_threshold; //!< Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration [0,1]
-
- bool viapoints_all_candidates; //!< If true, all trajectories of different topologies are attached to the current set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan.
-
- bool visualize_hc_graph; //!< Visualize the graph that is created for exploring new homotopy classes.
- double visualize_with_time_as_z_axis_scale; //!< If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.
- bool delete_detours_backwards; //!< If enabled, the planner will discard the plans detouring backwards with respect to the best plan
- double detours_orientation_tolerance; //!< A plan is considered a detour if its start orientation differs more than this from the best plan
- double length_start_orientation_vector; //!< Length of the vector used to compute the start orientation of a plan
- double max_ratio_detours_duration_best_duration; //!< Detours are discarted if their execution time / the execution time of the best teb is > this
- } hcp;
-
- //! Recovery/backup related parameters
- struct Recovery
- {
- bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.
- double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected.
- bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards)
- double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
- double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
- double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected.
- double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations
- bool divergence_detection_enable; //!< True to enable divergence detection.
- int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.
- } recovery; //!< Parameters related to recovery and backup strategies
-
-
- /**
- * @brief Construct the TebConfig using default values.
- * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used,
- * the default variables will be overwritten: \n
- * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a
- * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications.
- * All parameters considered by the dynamic_reconfigure server (and their \b default values) are
- * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n
- * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file.
- * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters.
- * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n
- * <b>TebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults</b>
- */
- TebConfig()
- {
-
- odom_topic = "odom";
- map_frame = "odom";
-
- // Trajectory
-
- trajectory.teb_autosize = true;
- trajectory.dt_ref = 0.3;
- trajectory.dt_hysteresis = 0.1;
- trajectory.min_samples = 3;
- trajectory.max_samples = 500;
- trajectory.global_plan_overwrite_orientation = true;
- trajectory.allow_init_with_backwards_motion = false;
- trajectory.global_plan_viapoint_sep = -1;
- trajectory.via_points_ordered = false;
- trajectory.max_global_plan_lookahead_dist = 1;
- trajectory.global_plan_prune_distance = 1;
- trajectory.exact_arc_length = false;
- trajectory.force_reinit_new_goal_dist = 1;
- trajectory.force_reinit_new_goal_angular = 0.5 * M_PI;
- trajectory.feasibility_check_no_poses = 5;
- trajectory.feasibility_check_lookahead_distance = -1;
- trajectory.publish_feedback = false;
- trajectory.min_resolution_collision_check_angular = M_PI;
- trajectory.control_look_ahead_poses = 1;
-
- // Robot
-
- robot.max_vel_x = 0.4;
- robot.max_vel_x_backwards = 0.2;
- robot.max_vel_y = 0.0;
- robot.max_vel_theta = 0.3;
- robot.base_max_vel_x = robot.max_vel_x;
- robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
- robot.base_max_vel_y = robot.base_max_vel_y;
- robot.base_max_vel_theta = robot.base_max_vel_theta;
- robot.acc_lim_x = 0.5;
- robot.acc_lim_y = 0.5;
- robot.acc_lim_theta = 0.5;
- robot.min_turning_radius = 0;
- robot.wheelbase = 1.0;
- robot.cmd_angle_instead_rotvel = false;
- robot.is_footprint_dynamic = false;
- robot.use_proportional_saturation = false;
-
- // GoalTolerance
-
- goal_tolerance.xy_goal_tolerance = 0.2;
- goal_tolerance.free_goal_vel = false;
-
- // Obstacles
-
- obstacles.min_obstacle_dist = 0.5;
- obstacles.inflation_dist = 0.6;
- obstacles.dynamic_obstacle_inflation_dist = 0.6;
- obstacles.include_dynamic_obstacles = true;
- obstacles.include_costmap_obstacles = true;
- obstacles.costmap_obstacles_behind_robot_dist = 1.5;
- obstacles.obstacle_poses_affected = 25;
- obstacles.legacy_obstacle_association = false;
- obstacles.obstacle_association_force_inclusion_factor = 1.5;
- obstacles.obstacle_association_cutoff_factor = 5;
- obstacles.costmap_converter_plugin = "";
- obstacles.costmap_converter_spin_thread = true;
- obstacles.costmap_converter_rate = 5;
- obstacles.obstacle_proximity_ratio_max_vel = 1;
- obstacles.obstacle_proximity_lower_bound = 0;
- obstacles.obstacle_proximity_upper_bound = 0.5;
-
- // Optimization
-
- optim.no_inner_iterations = 5;
- optim.no_outer_iterations = 4;
- optim.optimization_activate = true;
- optim.optimization_verbose = false;
- optim.penalty_epsilon = 0.05;
- optim.weight_max_vel_x = 2; //1
- optim.weight_max_vel_y = 2;
- optim.weight_max_vel_theta = 1;
- optim.weight_acc_lim_x = 1;
- optim.weight_acc_lim_y = 1;
- optim.weight_acc_lim_theta = 1;
- optim.weight_kinematics_nh = 1000;
- optim.weight_kinematics_forward_drive = 1;
- optim.weight_kinematics_turning_radius = 1;
- optim.weight_optimaltime = 1;
- optim.weight_shortest_path = 0;
- optim.weight_obstacle = 50;
- optim.weight_inflation = 0.1;
- optim.weight_dynamic_obstacle = 50;
- optim.weight_dynamic_obstacle_inflation = 0.1;
- optim.weight_velocity_obstacle_ratio = 0;
- optim.weight_viapoint = 1;
- optim.weight_prefer_rotdir = 50;
-
- optim.weight_adapt_factor = 2.0;
- optim.obstacle_cost_exponent = 1.0;
-
- // Homotopy Class Planner
-
- hcp.enable_homotopy_class_planning = true;
- hcp.enable_multithreading = true;
- hcp.simple_exploration = false;
- hcp.max_number_classes = 5;
- hcp.selection_cost_hysteresis = 1.0;
- hcp.selection_prefer_initial_plan = 0.95;
- hcp.selection_obst_cost_scale = 100.0;
- hcp.selection_viapoint_cost_scale = 1.0;
- hcp.selection_alternative_time_cost = false;
- hcp.selection_dropping_probability = 0.0;
-
- hcp.obstacle_keypoint_offset = 0.1;
- hcp.obstacle_heading_threshold = 0.45;
- hcp.roadmap_graph_no_samples = 15;
- hcp.roadmap_graph_area_width = 6; // [m]
- hcp.roadmap_graph_area_length_scale = 1.0;
- hcp.h_signature_prescaler = 1;
- hcp.h_signature_threshold = 0.1;
- hcp.switching_blocking_period = 0.0;
-
- hcp.viapoints_all_candidates = true;
-
- hcp.visualize_hc_graph = false;
- hcp.visualize_with_time_as_z_axis_scale = 0.0;
- hcp.delete_detours_backwards = true;
- hcp.detours_orientation_tolerance = M_PI / 2.0;
- hcp.length_start_orientation_vector = 0.4;
- hcp.max_ratio_detours_duration_best_duration = 3.0;
-
- // Recovery
-
- recovery.shrink_horizon_backup = true;
- recovery.shrink_horizon_min_duration = 10;
- recovery.oscillation_recovery = true;
- recovery.oscillation_v_eps = 0.1;
- recovery.oscillation_omega_eps = 0.1;
- recovery.oscillation_recovery_min_duration = 10;
- recovery.oscillation_filter_duration = 10;
- recovery.divergence_detection_enable = false;
- recovery.divergence_detection_max_chi_squared = 10;
- }
-
- void declareParameters(const nav2_util::LifecycleNode::SharedPtr, const std::string name);
-
- /**
- * @brief Load parmeters from the ros param server.
- * @param nh const reference to the local rclcpp::Node::SharedPtr
- */
- void loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name);
-
- /**
- * @brief Callback executed when a paramter change is detected
- * @param parameters list of changed parameters
- */
- rcl_interfaces::msg::SetParametersResult
- dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
- /**
- * @brief Check parameters and print warnings in case of discrepancies
- *
- * Call this method whenever parameters are changed using public interfaces to inform the user
- * about some improper uses.
- */
- void checkParameters() const;
-
- /**
- * @brief Check if some deprecated parameters are found and print warnings
- * @param nh const reference to the local rclcpp::Node::SharedPtr
- */
- void checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const;
-
- /**
- * @brief Return the internal config mutex
- */
- std::mutex& configMutex() {return config_mutex_;}
-
-private:
- std::mutex config_mutex_; //!< Mutex for config accesses and changes
- rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
-};
-} // namespace teb_local_planner
-
-#endif
diff --git a/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h b/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
deleted file mode 100644
index 5e67792..0000000
--- a/src/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
+++ /dev/null
@@ -1,426 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef TEB_LOCAL_PLANNER_ROS_H_
-#define TEB_LOCAL_PLANNER_ROS_H_
-
-#include <pluginlib/class_loader.hpp>
-
-#include <rclcpp/rclcpp.hpp>
-
-// Navigation2 local planner base class and utilities
-#include <nav2_core/controller.hpp>
-
-// timed-elastic-band related classes
-#include "teb_local_planner/optimal_planner.h"
-#include "teb_local_planner/homotopy_class_planner.h"
-#include "teb_local_planner/visualization.h"
-#include "teb_local_planner/recovery_behaviors.h"
-
-// message types
-#include <nav_msgs/msg/path.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <costmap_converter_msgs/msg/obstacle_msg.hpp>
-
-// transforms
-#include <tf2_ros/transform_listener.h>
-#include <tf2/transform_datatypes.h>
-
-// costmap
-#include <costmap_converter/costmap_converter_interface.h>
-#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
-
-#include <nav2_util/lifecycle_node.hpp>
-#include <nav2_costmap_2d/costmap_2d_ros.hpp>
-#include <nav_2d_utils/parameters.hpp>
-#include "rcl_interfaces/msg/set_parameters_result.hpp"
-// dynamic reconfigure
-//#include "teb_local_planner/TebLocalPlannerReconfigureConfig.h>
-//#include <dynamic_reconfigure/server.h>
-
-
-namespace teb_local_planner
-{
-using TFBufferPtr = std::shared_ptr<tf2_ros::Buffer>;
-using CostmapROSPtr = std::shared_ptr<nav2_costmap_2d::Costmap2DROS>;
-
-/**
- * @class TebLocalPlannerROS
- * @brief Implements the actual abstract navigation stack routines of the teb_local_planner plugin
- * @todo Escape behavior, more efficient obstacle handling
- */
-class TebLocalPlannerROS : public nav2_core::Controller
-{
-
-public:
- /**
- * @brief Constructor of the teb plugin
- */
- TebLocalPlannerROS();
-
- /**
- * @brief Destructor of the plugin
- */
- ~TebLocalPlannerROS();
-
- /**
- * @brief Configure the teb plugin
- *
- * @param node The node of the instance
- * @param tf Pointer to a transform listener
- * @param costmap_ros Cost map representing occupied and free space
- */
- void configure(
- const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
- std::string name,
- std::shared_ptr<tf2_ros::Buffer> tf,
- std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
- void activate() override;
- void deactivate() override;
- void cleanup() override;
-
- /**
- * @brief Initializes the teb plugin
- */
- void initialize(nav2_util::LifecycleNode::SharedPtr node);
-
- /**
- * @brief Set the plan that the teb local planner is following
- * @param orig_global_plan The plan to pass to the local planner
- * @return
- */
- void setPlan(const nav_msgs::msg::Path & orig_global_plan) override;
-
- /**
- * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
- * @param pose is the current position
- * @param velocity is the current velocity
- * @return velocity commands to send to the base
- */
- geometry_msgs::msg::TwistStamped computeVelocityCommands(
- const geometry_msgs::msg::PoseStamped &pose,
- const geometry_msgs::msg::Twist &velocity,
- nav2_core::GoalChecker * goal_checker);
-
-
- /** @name Public utility functions/methods */
- //@{
-
- /**
- * @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.
- *
- * Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).
- * @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)
- * @return Translational and angular velocity combined into an Eigen::Vector2d
- */
-// static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
-
- /**
- * @brief Get the current robot footprint/contour model
- * @param nh const reference to the local rclcpp::Node::SharedPtr
- * @return Robot footprint model used for optimization
- */
- RobotFootprintModelPtr getRobotFootprintFromParamServer(nav2_util::LifecycleNode::SharedPtr node);
-
- /**
- * @brief Set the footprint from the given XmlRpcValue.
- * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
- * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point
- * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the
- * sub-arrays should all have exactly 2 elements (x and y coordinates).
- * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
- * It is used only for reporting errors.
- * @return container of vertices describing the polygon
- */
-// Using ROS2 parameter server
-// static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
-
- /**
- * @brief Get a number from the given XmlRpcValue.
- * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
- * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point
- * @param value double value type
- * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
- * It is used only for reporting errors.
- * @returns double value
- */
-// Using ROS2 parameter server
-// static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
-
- //@}
-
-protected:
-
- /**
- * @brief Update internal obstacle vector based on occupied costmap cells
- * @remarks All occupied cells will be added as point obstacles.
- * @remarks All previous obstacles are cleared.
- * @sa updateObstacleContainerWithCostmapConverter
- * @todo Include temporal coherence among obstacle msgs (id vector)
- * @todo Include properties for dynamic obstacles (e.g. using constant velocity model)
- */
- void updateObstacleContainerWithCostmap();
-
- /**
- * @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin
- * @remarks Requires a loaded costmap_converter plugin.
- * @remarks All previous obstacles are cleared.
- * @sa updateObstacleContainerWithCostmap
- */
- void updateObstacleContainerWithCostmapConverter();
-
- /**
- * @brief Update internal obstacle vector based on custom messages received via subscriber
- * @remarks All previous obstacles are NOT cleared. Call this method after other update methods.
- * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter
- */
- void updateObstacleContainerWithCustomObstacles();
-
-
- /**
- * @brief Update internal via-point container based on the current reference plan
- * @remarks All previous via-points will be cleared.
- * @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame)
- * @param min_separation minimum separation between two consecutive via-points
- */
- void updateViaPointsContainer(const std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, double min_separation);
-
-
- /**
- * @brief Callback for the dynamic_reconfigure node.
- *
- * This callback allows to modify parameters dynamically at runtime without restarting the node
- * @param config Reference to the dynamic reconfigure config
- * @param level Dynamic reconfigure level
- */
- // TODO : dynamic reconfigure is not supported on ROS2
-// void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
-
-
- /**
- * @brief Callback for custom obstacles that are not obtained from the costmap
- * @param obst_msg pointer to the message containing a list of polygon shaped obstacles
- */
- void customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg);
-
- /**
- * @brief Callback for custom via-points
- * @param via_points_msg pointer to the message containing a list of via-points
- */
- void customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg);
-
- /**
- * @brief Prune global plan such that already passed poses are cut off
- *
- * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform.
- * If no valid transformation can be found, the method returns \c false.
- * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot.
- * If no pose within the specified treshold \c dist_behind_robot can be found,
- * nothing will be pruned and the method returns \c false.
- * @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.
- * @param global_pose The global pose of the robot
- * @param[in,out] global_plan The plan to be transformed
- * @param dist_behind_robot Distance behind the robot that should be kept [meters]
- * @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold
- */
- bool pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose,
- std::vector<geometry_msgs::msg::PoseStamped>& global_plan, double dist_behind_robot=1);
-
- /**
- * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
- *
- * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
- * such that the index of the current goal pose is returned as well as
- * the transformation between the global plan and the planning frame.
- * @param global_plan The plan to be transformed
- * @param global_pose The global pose of the robot
- * @param costmap A reference to the costmap being used so the window size for transforming can be computed
- * @param global_frame The frame to transform the plan to
- * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
- * @param[out] transformed_plan Populated with the transformed plan
- * @param[out] current_goal_idx Index of the current (local) goal pose in the global plan
- * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
- * @return \c true if the global plan is transformed, \c false otherwise
- */
- bool transformGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan,
- const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap,
- const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan,
- int* current_goal_idx = NULL, geometry_msgs::msg::TransformStamped* tf_plan_to_global = NULL) const;
-
- /**
- * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.
- *
- * If the current (local) goal point is not the final one (global)
- * substitute the goal orientation by the angle of the direction vector between
- * the local goal and the subsequent pose of the global plan.
- * This is often helpful, if the global planner does not consider orientations. \n
- * A moving average filter is utilized to smooth the orientation.
- * @param global_plan The global plan
- * @param local_goal Current local goal
- * @param current_goal_idx Index of the current (local) goal pose in the global plan
- * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
- * @param moving_average_length number of future poses of the global plan to be taken into account
- * @return orientation (yaw-angle) estimate
- */
- double estimateLocalGoalOrientation(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan, const geometry_msgs::msg::PoseStamped& local_goal,
- int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length=3) const;
-
-
- /**
- * @brief Saturate the translational and angular velocity to given limits.
- *
- * The limit of the translational velocity for backwards driving can be changed independently.
- * Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for
- * penalizing backwards driving instead.
- * @param[in,out] vx The translational velocity that should be saturated.
- * @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots
- * @param[in,out] omega The angular velocity that should be saturated.
- * @param max_vel_x Maximum translational velocity for forward driving
- * @param max_vel_y Maximum strafing velocity (for holonomic robots)
- * @param max_vel_theta Maximum (absolute) angular velocity
- * @param max_vel_x_backwards Maximum translational velocity for backwards driving
- */
- void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
- double max_vel_theta, double max_vel_x_backwards) const;
-
-
- /**
- * @brief Convert translational and rotational velocities to a steering angle of a carlike robot
- *
- * The conversion is based on the following equations:
- * - The turning radius is defined by \f$ R = v/omega \f$
- * - For a car like robot withe a distance L between both axles, the relation is: \f$ tan(\phi) = L/R \f$
- * - phi denotes the steering angle.
- * @remarks You might provide distances instead of velocities, since the temporal information is not required.
- * @param v translational velocity [m/s]
- * @param omega rotational velocity [rad/s]
- * @param wheelbase distance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots
- * @param min_turning_radius Specify a lower bound on the turning radius
- * @return Resulting steering angle in [rad] inbetween [-pi/2, pi/2]
- */
- double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
-
- /**
- * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint
- *
- * This method prints warnings if validation fails.
- * @remarks Currently, we only validate the inscribed radius of the footprints
- * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization
- * @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap
- * @param min_obst_dist desired distance to obstacles
- */
- void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
-
-
- void configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int& goal_idx);
-
- /**
- * @brief Limits the maximum linear speed of the robot.
- * @param speed_limit expressed in absolute value (in m/s)
- * or in percentage from maximum robot speed.
- * @param percentage Setting speed limit in percentage if true
- * or in absolute values in false case.
- */
- void setSpeedLimit(const double & speed_limit, const bool & percentage);
-
-private:
- // Definition of member variables
- rclcpp_lifecycle::LifecycleNode::WeakPtr nh_;
- rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
- rclcpp::Clock::SharedPtr clock_;
- rclcpp::Node::SharedPtr intra_proc_node_;
- // external objects (store weak pointers)
- CostmapROSPtr costmap_ros_; //!< Pointer to the costmap ros wrapper, received from the navigation stack
- nav2_costmap_2d::Costmap2D* costmap_; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper)
- TFBufferPtr tf_; //!< pointer to Transform Listener
- TebConfig::UniquePtr cfg_; //!< Config class that stores and manages all related parameters
-
- // internal objects (memory management owned)
- PlannerInterfacePtr planner_; //!< Instance of the underlying optimal planner class
- ObstContainer obstacles_; //!< Obstacle vector that should be considered during local trajectory optimization
- ViaPointContainer via_points_; //!< Container of via-points that should be considered during local trajectory optimization
- TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
- std::shared_ptr<dwb_critics::ObstacleFootprintCritic> costmap_model_;
- FailureDetector failure_detector_; //!< Detect if the robot got stucked
-
- std::vector<geometry_msgs::msg::PoseStamped> global_plan_; //!< Store the current global plan
-
- pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> costmap_converter_loader_; //!< Load costmap converter plugins at runtime
- std::shared_ptr<costmap_converter::BaseCostmapToPolygons> costmap_converter_; //!< Store the current costmap_converter
-
- //std::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
- rclcpp::Subscription<costmap_converter_msgs::msg::ObstacleArrayMsg>::SharedPtr custom_obst_sub_; //!< Subscriber for custom obstacles received via a ObstacleMsg.
- std::mutex custom_obst_mutex_; //!< Mutex that locks the obstacle array (multi-threaded)
- costmap_converter_msgs::msg::ObstacleArrayMsg custom_obstacle_msg_; //!< Copy of the most recent obstacle message
-
- rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr via_points_sub_; //!< Subscriber for custom via-points received via a Path msg.
- bool custom_via_points_active_; //!< Keep track whether valid via-points have been received from via_points_sub_
- std::mutex via_point_mutex_; //!< Mutex that locks the via_points container (multi-threaded)
-
- PoseSE2 robot_pose_; //!< Store current robot pose
- PoseSE2 robot_goal_; //!< Store current robot goal
- geometry_msgs::msg::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega)
- rclcpp::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected
- int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan.
- rclcpp::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected
- RotType last_preferred_rotdir_; //!< Store recent preferred turning direction
- geometry_msgs::msg::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands()
-
- std::vector<geometry_msgs::msg::Point> footprint_spec_; //!< Store the footprint of the robot
- double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible)
- double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot
-
- // flags
- bool initialized_; //!< Keeps track about the correct initialization of this class
- std::string name_; //!< Name of plugin ID
-
-protected:
- // Dynamic parameters handler
- rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-}; // end namespace teb_local_planner
-
-#endif // TEB_LOCAL_PLANNER_ROS_H_
-
-
diff --git a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
deleted file mode 100644
index f720906..0000000
--- a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.h
+++ /dev/null
@@ -1,636 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef TIMED_ELASTIC_BAND_H_
-#define TIMED_ELASTIC_BAND_H_
-#include <boost/optional.hpp>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <geometry_msgs/msg/pose2_d.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <tf2/buffer_core.h>
-
-#include <cassert>
-#include <complex>
-#include <iterator>
-
-#include "teb_local_planner/obstacles.h"
-
-// G2O Types
-#include "teb_local_planner/g2o_types/vertex_pose.h"
-#include "teb_local_planner/g2o_types/vertex_timediff.h"
-
-
-namespace teb_local_planner
-{
-
-//! Container of poses that represent the spatial part of the trajectory
-typedef std::vector<VertexPose*> PoseSequence;
-//! Container of time differences that define the temporal of the trajectory
-typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
-
-
-/**
- * @class TimedElasticBand
- * @brief Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.
- *
- * All trajectory related methods (initialization, modifying, ...) are implemented inside this class. \n
- * Let \f$ Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \f$ be a sequence of poses, \n
- * in which \f$ \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \f$ denotes a single pose of the robot. \n
- * The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between
- * two consecutive poses, resuting in a sequence of \c n-1 time intervals \f$ \Delta T_i \f$: \n
- * \f$ \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \f$. \n
- * Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration.
- * The tuple of both sequences defines the underlying trajectory.
- *
- * Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. \n
- * TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.
- *
- * @todo Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.
- */
-class TimedElasticBand
-{
-public:
-
- /**
- * @brief Construct the class
- */
- TimedElasticBand();
-
- /**
- * @brief Destruct the class
- */
- virtual ~TimedElasticBand();
-
-
-
- /** @name Access pose and timediff sequences */
- //@{
-
- /**
- * @brief Access the complete pose sequence
- * @return reference to the pose sequence
- */
- PoseSequence& poses() {return pose_vec_;};
-
- /**
- * @brief Access the complete pose sequence (read-only)
- * @return const reference to the pose sequence
- */
- const PoseSequence& poses() const {return pose_vec_;};
-
- /**
- * @brief Access the complete timediff sequence
- * @return reference to the dimediff sequence
- */
- TimeDiffSequence& timediffs() {return timediff_vec_;};
-
- /**
- * @brief Access the complete timediff sequence
- * @return reference to the dimediff sequence
- */
- const TimeDiffSequence& timediffs() const {return timediff_vec_;};
-
- /**
- * @brief Access the time difference at pos \c index of the time sequence
- * @param index element position inside the internal TimeDiffSequence
- * @return reference to the time difference at pos \c index
- */
- double& TimeDiff(int index)
- {
- assert(index<sizeTimeDiffs());
- return timediff_vec_.at(index)->dt();
- }
-
- /**
- * @brief Access the time difference at pos \c index of the time sequence (read-only)
- * @param index element position inside the internal TimeDiffSequence
- * @return const reference to the time difference at pos \c index
- */
- const double& TimeDiff(int index) const
- {
- assert(index<sizeTimeDiffs());
- return timediff_vec_.at(index)->dt();
- }
-
- /**
- * @brief Access the pose at pos \c index of the pose sequence
- * @param index element position inside the internal PoseSequence
- * @return reference to the pose at pos \c index
- */
- PoseSE2& Pose(int index)
- {
- assert(index<sizePoses());
- return pose_vec_.at(index)->pose();
- }
-
- /**
- * @brief Access the pose at pos \c index of the pose sequence (read-only)
- * @param index element position inside the internal PoseSequence
- * @return const reference to the pose at pos \c index
- */
- const PoseSE2& Pose(int index) const
- {
- assert(index<sizePoses());
- return pose_vec_.at(index)->pose();
- }
-
- /**
- * @brief Access the last PoseSE2 in the pose sequence
- */
- PoseSE2& BackPose() {return pose_vec_.back()->pose(); }
-
- /**
- * @brief Access the last PoseSE2 in the pose sequence (read-only)
- */
- const PoseSE2& BackPose() const {return pose_vec_.back()->pose();}
-
- /**
- * @brief Access the last TimeDiff in the time diff sequence
- */
- double& BackTimeDiff() {return timediff_vec_.back()->dt(); }
-
- /**
- * @brief Access the last TimeDiff in the time diff sequence (read-only)
- */
- const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); }
-
- /**
- * @brief Access the vertex of a pose at pos \c index for optimization purposes
- * @param index element position inside the internal PoseSequence
- * @return Weak raw pointer to the pose vertex at pos \c index
- */
- VertexPose* PoseVertex(int index)
- {
- assert(index<sizePoses());
- return pose_vec_.at(index);
- }
-
- /**
- * @brief Access the vertex of a time difference at pos \c index for optimization purposes
- * @param index element position inside the internal TimeDiffSequence
- * @return Weak raw pointer to the timediff vertex at pos \c index
- */
- VertexTimeDiff* TimeDiffVertex(int index)
- {
- assert(index<sizeTimeDiffs());
- return timediff_vec_.at(index);
- }
-
- //@}
-
-
-
- /** @name Append new elements to the pose and timediff sequences */
- //@{
-
- /**
- * @brief Append a new pose vertex to the back of the pose sequence
- * @param pose PoseSE2 to push back on the internal PoseSequence
- * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
- */
- void addPose(const PoseSE2& pose, bool fixed=false);
-
- /**
- * @brief Append a new pose vertex to the back of the pose sequence
- * @param position 2D vector representing the position part
- * @param theta yaw angle representing the orientation part
- * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
- */
- void addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed=false);
-
- /**
- * @brief Append a new pose vertex to the back of the pose sequence
- * @param x x-coordinate of the position part
- * @param y y-coordinate of the position part
- * @param theta yaw angle representing the orientation part
- * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
- */
- void addPose(double x, double y, double theta, bool fixed=false);
-
- /**
- * @brief Append a new time difference vertex to the back of the time diff sequence
- * @param dt time difference value to push back on the internal TimeDiffSequence
- * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
- */
- void addTimeDiff(double dt, bool fixed=false);
-
- /**
- * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
- * @param pose PoseSE2 to push back on the internal PoseSequence
- * @param dt time difference value to push back on the internal TimeDiffSequence
- * @warning Since the timediff is defined to connect two consecutive poses, this call is only
- * allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...):
- * therefore add a single pose using addPose() first!
- */
- void addPoseAndTimeDiff(const PoseSE2& pose, double dt);
-
- /**
- * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
- * @param position 2D vector representing the position part
- * @param theta yaw angle representing the orientation part
- * @param dt time difference value to push back on the internal TimeDiffSequence
- * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
- */
- void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt);
-
- /**
- * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
- * @param x x-coordinate of the position part
- * @param y y-coordinate of the position part
- * @param theta yaw angle representing the orientation part
- * @param dt time difference value to push back on the internal TimeDiffSequence
- * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
- */
- void addPoseAndTimeDiff(double x, double y, double theta, double dt);
-
- //@}
-
-
- /** @name Insert new elements and remove elements of the pose and timediff sequences */
- //@{
-
- /**
- * @brief Insert a new pose vertex at pos. \c index to the pose sequence
- * @param index element position inside the internal PoseSequence
- * @param pose PoseSE2 element to insert into the internal PoseSequence
- */
- void insertPose(int index, const PoseSE2& pose);
-
- /**
- * @brief Insert a new pose vertex at pos. \c index to the pose sequence
- * @param index element position inside the internal PoseSequence
- * @param position 2D vector representing the position part
- * @param theta yaw-angle representing the orientation part
- */
- void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta);
-
- /**
- * @brief Insert a new pose vertex at pos. \c index to the pose sequence
- * @param index element position inside the internal PoseSequence
- * @param x x-coordinate of the position part
- * @param y y-coordinate of the position part
- * @param theta yaw-angle representing the orientation part
- */
- void insertPose(int index, double x, double y, double theta);
-
- /**
- * @brief Insert a new timediff vertex at pos. \c index to the timediff sequence
- * @param index element position inside the internal TimeDiffSequence
- * @param dt timediff value
- */
- void insertTimeDiff(int index, double dt);
-
- /**
- * @brief Delete pose at pos. \c index in the pose sequence
- * @param index element position inside the internal PoseSequence
- */
- void deletePose(int index);
-
- /**
- * @brief Delete multiple (\c number) poses starting at pos. \c index in the pose sequence
- * @param index first element position inside the internal PoseSequence
- * @param number number of elements that should be deleted
- */
- void deletePoses(int index, int number);
-
- /**
- * @brief Delete pose at pos. \c index in the timediff sequence
- * @param index element position inside the internal TimeDiffSequence
- */
- void deleteTimeDiff(int index);
-
- /**
- * @brief Delete multiple (\c number) time differences starting at pos. \c index in the timediff sequence
- * @param index first element position inside the internal TimeDiffSequence
- * @param number number of elements that should be deleted
- */
- void deleteTimeDiffs(int index, int number);
-
- //@}
-
-
- /** @name Init the trajectory */
- //@{
-
- /**
- * @brief Initialize a trajectory between a given start and goal pose.
- *
- * The implemented algorithm subsamples the straight line between
- * start and goal using a given discretiziation width. \n
- * The discretization width can be defined in the euclidean space
- * using the \c diststep parameter. Each time difference between two consecutive
- * poses is initialized to \c timestep. \n
- * If the \c diststep is chosen to be zero,
- * the resulting trajectory contains the start and goal pose only.
- * @param start PoseSE2 defining the start of the trajectory
- * @param goal PoseSE2 defining the goal of the trajectory (final pose)
- * @param diststep euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
- * @param max_vel_x maximum translational velocity used for determining time differences
- * @param min_samples Minimum number of samples that should be initialized at least
- * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
- * @return true if everything was fine, false otherwise
- */
- bool initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double max_vel_x=0.5, int min_samples = 3, bool guess_backwards_motion = false);
-
-
- /**
- * @brief Initialize a trajectory from a generic 2D reference path.
- *
- * The temporal information is determined using a given maximum velocity
- * (2D vector containing the translational and angular velocity). \n
- * A constant velocity profile is implemented. \n
- * A possible maximum acceleration is considered if \c max_acceleration param is provided.
- *
- * Since the orientation is not included in the reference path, it can be provided seperately
- * (e.g. from the robot pose and robot goal). \n
- * Otherwise the goal heading will be used as start and goal orientation. \n
- * The orientation along the trajectory will be determined using the connection vector
- * between two consecutive positions of the reference path.
- *
- * The reference path is provided using a start and end iterator to a container class.
- * You must provide a unary function that accepts the dereferenced iterator and returns
- * a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.
- *
- * @param path_start start iterator of a generic 2d path
- * @param path_end end iterator of a generic 2d path
- * @param fun_position unary function that returns the Eigen::Vector2d object
- * @param max_vel_x maximum translational velocity used for determining time differences
- * @param max_vel_theta maximum angular velocity used for determining time differences
- * @param max_acc_x specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
- * @param max_acc_theta specify to satisfy a maxmimum angular acceleration and decceleration (optional)
- * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
- * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
- * @param min_samples Minimum number of samples that should be initialized at least
- * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
- * @tparam BidirIter Bidirectional iterator type
- * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
- * @return true if everything was fine, false otherwise
- * @remarks Use \c boost::none to skip optional arguments.
- */
- template<typename BidirIter, typename Fun>
- bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
- boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
- boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false);
-
- /**
- * @brief Initialize a trajectory from a reference pose sequence (positions and orientations).
- *
- * This method initializes the timed elastic band using a pose container
- * (e.g. as local plan from the ros navigation stack). \n
- * The initial time difference between two consecutive poses can be uniformly set
- * via the argument \c dt.
- * @param plan vector of geometry_msgs::msg::PoseStamped
- * @param max_vel_x maximum translational velocity used for determining time differences
- * @param max_vel_theta maximum rotational velocity used for determining time differences
- * @param estimate_orient if \c true, calculate orientation using the straight line distance vector between consecutive poses
- * (only copy start and goal orientation; recommended if no orientation data is available).
- * @param min_samples Minimum number of samples that should be initialized at least
- * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if \c estimate_orient is enabled.
- * @return true if everything was fine, false otherwise
- */
- bool initTrajectoryToGoal(const std::vector<geometry_msgs::msg::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false);
-
- //@}
-
- /** @name Update and modify the trajectory */
- //@{
-
-
- /**
- * @brief Hot-Start from an existing trajectory with updated start and goal poses.
- *
- * This method updates a previously optimized trajectory with a new start and/or a new goal pose. \n
- * The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start. \n
- * Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed.
- * The optimizer has to smooth the trajectory in TebOptimalPlanner. \n
- *
- * @todo Smooth the trajectory here and test the performance improvement of the optimization.
- * @todo Implement a updateAndPruneTEB based on a new reference path / pose sequence.
- *
- * @param new_start New start pose (optional)
- * @param new_goal New goal pose (optional)
- * @param min_samples Specify the minimum number of samples that should at least remain in the trajectory
- */
- void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3);
-
-
- /**
- * @brief Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.
- *
- * Resizing the trajectory is helpful e.g. for the following scenarios:
- *
- * - Obstacles requires the teb to be extended in order to
- * satisfy the given discritization width (plan resolution)
- * and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$
- * After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.
- * - If the distance to the goal state is getting smaller,
- * dt is decreasing as well. This leads to a heavily
- * fine-grained discretization in combination with many
- * discrete poses. Thus, the computation time will
- * be/remain high and in addition numerical instabilities
- * can appear (e.g. due to the division by a small \f$ \Delta T_i \f$).
- *
- * The implemented strategy checks all timediffs \f$ \Delta T_i \f$ and
- *
- * - inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$
- * - removes a sample if \f$ \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \f$
- *
- * Each call only one new sample (pose-dt-pair) is inserted or removed.
- * @param dt_ref reference temporal resolution
- * @param dt_hysteresis hysteresis to avoid oscillations
- * @param min_samples minimum number of samples that should be remain in the trajectory after resizing
- * @param max_samples maximum number of samples that should not be exceeded during resizing
- * @param fast_mode if true, the trajectory is iterated once to insert or erase points; if false the trajectory
- * is repeatedly iterated until no poses are added or removed anymore
- */
- void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false);
-
- /**
- * @brief Set a pose vertex at pos \c index of the pose sequence to be fixed or unfixed during optimization.
- * @param index index to the pose vertex
- * @param status if \c true, the vertex will be fixed, otherwise unfixed
- */
- void setPoseVertexFixed(int index, bool status);
-
- /**
- * @brief Set a timediff vertex at pos \c index of the timediff sequence to be fixed or unfixed during optimization.
- * @param index index to the timediff vertex
- * @param status if \c true, the vertex will be fixed, otherwise unfixed
- */
- void setTimeDiffVertexFixed(int index, bool status);
-
- /**
- * @brief clear all poses and timediffs from the trajectory.
- * The pose and timediff sequences will be empty and isInit() will return \c false
- */
- void clearTimedElasticBand();
-
- //@}
-
-
- /** @name Utility and status methods */
- //@{
-
- /**
- * @brief Find the closest point on the trajectory w.r.t. to a provided reference point.
- *
- * This function can be useful to find the part of a trajectory that is close to an obstacle.
- *
- * @todo implement a more efficient version that first performs a coarse search.
- * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
- * Allows simple comparisons starting from the middle of the trajectory.
- *
- * @param ref_point reference point (2D position vector)
- * @param[out] distance [optional] the resulting minimum distance
- * @param begin_idx start search at this pose index
- * @return Index to the closest pose in the pose sequence
- */
- int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance = NULL, int begin_idx=0) const;
-
- /**
- * @brief Find the closest point on the trajectory w.r.t. to a provided reference line.
- *
- * This function can be useful to find the part of a trajectory that is close to an (line) obstacle.
- *
- * @todo implement a more efficient version that first performs a coarse search.
- * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
- * Allows simple comparisons starting from the middle of the trajectory.
- *
- * @param ref_line_start start of the reference line (2D position vector)
- * @param ref_line_end end of the reference line (2D position vector)
- * @param[out] distance [optional] the resulting minimum distance
- * @return Index to the closest pose in the pose sequence
- */
- int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance = NULL) const;
-
- /**
- * @brief Find the closest point on the trajectory w.r.t. to a provided reference polygon.
- *
- * This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.
- *
- * @todo implement a more efficient version that first performs a coarse search.
- * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
- * Allows simple comparisons starting from the middle of the trajectory.
- *
- * @param vertices vertex container containing Eigen::Vector2d points (the last and first point are connected)
- * @param[out] distance [optional] the resulting minimum distance
- * @return Index to the closest pose in the pose sequence
- */
- int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const;
-
- /**
- * @brief Find the closest point on the trajectory w.r.t to a provided obstacle type
- *
- * This function can be useful to find the part of a trajectory that is close to an obstacle.
- * The method is calculates appropriate distance metrics for point, line and polygon obstacles.
- * For all unknown obstacles the centroid is used.
- *
- * @param obstacle Subclass of the Obstacle base class
- * @param[out] distance [optional] the resulting minimum distance
- * @return Index to the closest pose in the pose sequence
- */
- int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const;
-
-
- /**
- * @brief Get the length of the internal pose sequence
- */
- int sizePoses() const {return (int)pose_vec_.size();};
-
- /**
- * @brief Get the length of the internal timediff sequence
- */
- int sizeTimeDiffs() const {return (int)timediff_vec_.size();};
-
- /**
- * @brief Check whether the trajectory is initialized (nonzero pose and timediff sequences)
- */
- bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}
-
- /**
- * @brief Calculate the total transition time (sum over all time intervals of the timediff sequence)
- */
- double getSumOfAllTimeDiffs() const;
-
- /**
- * @brief Calculate the estimated transition time up to the pose denoted by index
- * @param index Index of the pose up to which the transition times are summed up
- * @return Estimated transition time up to pose index
- */
- double getSumOfTimeDiffsUpToIdx(int index) const;
-
- /**
- * @brief Calculate the length (accumulated euclidean distance) of the trajectory
- */
- double getAccumulatedDistance() const;
-
- /**
- * @brief Check if all trajectory points are contained in a specific region
- *
- * The specific region is a circle around the current robot position (Pose(0)) with given radius \c radius.
- * This method investigates a different radius for points behind the robot if \c max_dist_behind_robot >= 0.
- * @param radius radius of the region with the robot position (Pose(0)) as center
- * @param max_dist_behind_robot A separate radius for trajectory points behind the robot, activated if 0 or positive
- * @param skip_poses If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested.
- * @return \c true, if all tested trajectory points are inside the specified region, \c false otherwise.
- */
- bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0);
-
-
-
- //@}
-
-protected:
- PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices
- TimeDiffSequence timediff_vec_; //!< Internal container storing the sequence of optimzable timediff vertices
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-} // namespace teb_local_planner
-
-
-// include template implementations / definitions
-#include "teb_local_planner/timed_elastic_band.hpp"
-
-
-#endif /* TIMED_ELASTIC_BAND_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp b/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
deleted file mode 100644
index 9db4a25..0000000
--- a/src/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp
+++ /dev/null
@@ -1,191 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/timed_elastic_band.h"
-
-#include <iterator>
-
-namespace teb_local_planner
-{
-template<typename BidirIter, typename Fun>
-bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
- boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
- boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples, bool guess_backwards_motion)
-{
- Eigen::Vector2d start_position = fun_position( *path_start );
- Eigen::Vector2d goal_position = fun_position( *std::prev(path_end) );
-
- bool backwards = false;
-
- double start_orient, goal_orient;
- if (start_orientation)
- {
- start_orient = *start_orientation;
-
- // check if the goal is behind the start pose (w.r.t. start orientation)
- if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0)
- backwards = true;
- }
- else
- {
- Eigen::Vector2d start2goal = goal_position - start_position;
- start_orient = atan2(start2goal[1],start2goal[0]);
- }
-
- double timestep = 1; // TODO: time
-
- if (goal_orientation)
- {
- goal_orient = *goal_orientation;
- }
- else
- {
- goal_orient = start_orient;
- }
-
- if (!isInit())
- {
- addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization
-
- // we insert middle points now (increase start by 1 and decrease goal by 1)
- std::advance(path_start,1);
- std::advance(path_end,-1);
- int idx=0;
- for (; path_start != path_end; ++path_start) // insert middle-points
- {
- //Eigen::Vector2d point_to_goal = path.back()-*it;
- //double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
- // Alternative: Direction from last path
- Eigen::Vector2d curr_point = fun_position(*path_start);
- Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use std::prev(*path_start) for those cases,
- // where fun_position() does not return a reference or is expensive.
- double diff_norm = diff_last.norm();
-
- double timestep_vel = diff_norm/max_vel_x; // constant velocity
- double timestep_acc;
-
- if (max_acc_x)
- {
- timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
- if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
- else timestep = timestep_vel;
- }
- else timestep = timestep_vel;
-
- if (timestep<=0) timestep=0.2; // TODO: this is an assumption
-
- double yaw = atan2(diff_last[1],diff_last[0]);
- if (backwards)
- yaw = g2o::normalize_theta(yaw + M_PI);
- addPoseAndTimeDiff(curr_point, yaw ,timestep);
-
- /*
- // TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time.
-
- Eigen::Vector2d diff_next = fun_position(*std::next(path_start))-curr_point; // TODO maybe store the std::next for the following iteration
- double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
- -atan2(diff_last[1],diff_last[0]) ) );
-
- timestep_vel = ang_diff/max_vel_theta; // constant velocity
- if (max_acc_theta)
- {
- timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
- if (timestep_vel < timestep_acc) timestep = timestep_acc;
- else timestep = timestep_vel;
- }
- else timestep = timestep_vel;
-
- if (timestep<=0) timestep=0.2; // TODO: this is an assumption
-
- yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished
- if (backwards)
- yaw = g2o::normalize_theta(yaw + M_PI);
- addPoseAndTimeDiff(curr_point, yaw ,timestep);
-
- */
-
- ++idx;
- }
- Eigen::Vector2d diff = goal_position-Pose(idx).position();
- double diff_norm = diff.norm();
- double timestep_vel = diff_norm/max_vel_x; // constant velocity
- if (max_acc_x)
- {
- double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
- if (timestep_vel < timestep_acc) timestep = timestep_acc;
- else timestep = timestep_vel;
- }
- else timestep = timestep_vel;
-
-
- PoseSE2 goal(goal_position, goal_orient);
-
- // if number of samples is not larger than min_samples, insert manually
- if ( sizePoses() < min_samples-1 )
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
- while (sizePoses() < min_samples-1) // subtract goal point that will be added later
- {
- // Each inserted point bisects the remaining distance. Thus the timestep is also bisected.
- timestep /= 2;
- // simple strategy: interpolate between the current pose and the goal
- addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization
- }
- }
-
- // now add goal
- addPoseAndTimeDiff(goal, timestep); // add goal point
- setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
- }
- else // size!=0
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
- return false;
- }
- return true;
-}
-
-
-} // namespace teb_local_planner
-
-
-
diff --git a/src/teb_local_planner/include/teb_local_planner/visualization.h b/src/teb_local_planner/include/teb_local_planner/visualization.h
deleted file mode 100644
index c46dbe8..0000000
--- a/src/teb_local_planner/include/teb_local_planner/visualization.h
+++ /dev/null
@@ -1,272 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#ifndef VISUALIZATION_H_
-#define VISUALIZATION_H_
-
-
-
-// teb stuff
-#include "teb_local_planner/teb_config.h"
-#include "teb_local_planner/timed_elastic_band.h"
-#include "teb_local_planner/robot_footprint_model.h"
-
-#include <teb_msgs/msg/feedback_msg.hpp>
-
-// boost
-#include <boost/graph/adjacency_list.hpp>
-#include <boost/graph/graph_traits.hpp>
-
-// std
-#include <iterator>
-
-#include <nav2_util/lifecycle_node.hpp>
-
-#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
-
-// messages
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <nav_msgs/msg/path.hpp>
-#include <std_msgs/msg/color_rgba.hpp>
-#include <tf2/transform_datatypes.h>
-#include <visualization_msgs/msg/marker.hpp>
-
-namespace teb_local_planner
-{
-
-class TebOptimalPlanner; //!< Forward Declaration
-
-
-/**
- * @class TebVisualization
- * @brief Visualize stuff from the teb_local_planner
- */
-class TebVisualization
-{
-public:
- /**
- * @brief Constructor that initializes the class and registers topics
- * @param nh local rclcpp::Node::SharedPtr
- * @param cfg const reference to the TebConfig class for parameters
- */
- TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg);
-
- /** @name Publish to topics */
- //@{
-
- /**
- * @brief Publish a given global plan to the ros topic \e ../../global_plan
- * @param global_plan Pose array describing the global plan
- */
- void publishGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan) const;
-
- /**
- * @brief Publish a given local plan to the ros topic \e ../../local_plan
- * @param local_plan Pose array describing the local plan
- */
- void publishLocalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& local_plan) const;
-
- /**
- * @brief Publish Timed_Elastic_Band related stuff (local plan, pose sequence).
- *
- * Given a Timed_Elastic_Band instance, publish the local plan to \e ../../local_plan
- * and the pose sequence to \e ../../teb_poses.
- * @param teb const reference to a Timed_Elastic_Band
- */
- void publishLocalPlanAndPoses(const TimedElasticBand& teb) const;
-
- /**
- * @brief Publish the visualization of the robot model
- *
- * @param current_pose Current pose of the robot
- * @param robot_model Subclass of BaseRobotFootprintModel
- * @param ns Namespace for the marker objects
- * @param color Color of the footprint
- */
- void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel",
- const std_msgs::msg::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0));
-
- /**
- * @brief Publish the robot footprints related to infeasible poses
- *
- * @param current_pose Current pose of the robot
- * @param robot_model Subclass of BaseRobotFootprintModel
- */
- void publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model);
-
- /**
- * @brief Publish obstacle positions to the ros topic \e ../../teb_markers
- * @todo Move filling of the marker message to polygon class in order to avoid checking types.
- * @param obstacles Obstacle container
- */
- void publishObstacles(const ObstContainer& obstacles) const;
-
- /**
- * @brief Publish via-points to the ros topic \e ../../teb_markers
- * @param via_points via-point container
- */
- void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns = "ViaPoints") const;
-
- /**
- * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers.
- * @remarks Make sure that vertices of the graph contain a member \c pos as \c Eigen::Vector2d type
- * to query metric position values.
- * @param graph Const reference to the boost::adjacency_list (graph)
- * @param ns_prefix Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended)
- * @tparam GraphType boost::graph object in which vertices has the field/member \c pos.
- */
- template <typename GraphType>
- void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph");
-
- /**
- * @brief Publish multiple 2D paths (each path given as a point sequence) from a container class.
- *
- * Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist
- * and std::vector could be individually substituded by std::list / std::deque /...
- *
- * A common point-type for object T could be Eigen::Vector2d.
- *
- * T could be also a raw pointer std::vector< std::vector< T* > >.
- *
- * @code
- * typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > PathType; // could be a list or deque as well ...
- * std::vector<PathType> path_container(2); // init 2 empty paths; the container could be a list or deque as well ...
- * // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here
- * // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here
- * publishPathContainer( path_container.begin(), path_container.end() );
- * @endcode
- *
- * @remarks Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence.
- * Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.\n
- * @param first Bidirectional iterator pointing to the begin of the path
- * @param last Bidirectional iterator pointing to the end of the path
- * @param ns Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended)
- * @tparam BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container
- */
- template <typename BidirIter>
- void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer");
-
- /**
- * @brief Publish multiple Tebs from a container class (publish as marker message).
- *
- * @param teb_planner Container of std::shared_ptr< TebOptPlannerPtr >
- * @param ns Namespace for the marker objects
- */
- void publishTebContainer(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planner, const std::string& ns = "TebContainer");
-
- /**
- * @brief Publish a feedback message (multiple trajectory version)
- *
- * The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on).
- * Each trajectory is composed of the sequence of poses, the velocity profile and temporal information.
- * The feedback message also contains a list of active obstacles.
- * @param teb_planners container with multiple tebs (resp. their planner instances)
- * @param selected_trajectory_idx Idx of the currently selected trajectory in \c teb_planners
- * @param obstacles Container of obstacles
- */
- void publishFeedbackMessage(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles);
-
- /**
- * @brief Publish a feedback message (single trajectory overload)
- *
- * The feedback message contains the planned trajectory
- * that is composed of the sequence of poses, the velocity profile and temporal information.
- * The feedback message also contains a list of active obstacles.
- * @param teb_planner the planning instance
- * @param obstacles Container of obstacles
- */
- void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles);
-
- nav2_util::CallbackReturn on_configure();
- nav2_util::CallbackReturn on_activate();
- nav2_util::CallbackReturn on_deactivate();
- nav2_util::CallbackReturn on_cleanup();
-
- //@}
-
- /**
- * @brief Helper function to generate a color message from single values
- * @param a Alpha value
- * @param r Red value
- * @param g Green value
- * @param b Blue value
- * @return Color message
- */
- static std_msgs::msg::ColorRGBA toColorMsg(double a, double r, double g, double b);
-
-protected:
-
- /**
- * @brief Small helper function that checks if initialize() has been called and prints an error message if not.
- * @return \c true if not initialized, \c false if everything is ok
- */
- bool printErrorWhenNotInitialized() const;
-
- nav2_util::LifecycleNode::SharedPtr nh_;
-
- rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr global_plan_pub_; //!< Publisher for the global plan
- rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr local_plan_pub_; //!< Publisher for the local plan
- rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr teb_poses_pub_; //!< Publisher for the trajectory pose sequence
- rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr teb_marker_pub_; //!< Publisher for visualization markers
- rclcpp_lifecycle::LifecyclePublisher<teb_msgs::msg::FeedbackMsg>::SharedPtr feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes
-
- const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
-
- bool initialized_; //!< Keeps track about the correct initialization of this class
-
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-//! Abbrev. for shared instances of the TebVisualization
-typedef std::shared_ptr<TebVisualization> TebVisualizationPtr;
-
-//! Abbrev. for shared instances of the TebVisualization (read-only)
-typedef std::shared_ptr<const TebVisualization> TebVisualizationConstPtr;
-
-
-} // namespace teb_local_planner
-
-
-// Include template method implementations / definitions
-#include "teb_local_planner/visualization.hpp"
-
-#endif /* VISUALIZATION_H_ */
diff --git a/src/teb_local_planner/include/teb_local_planner/visualization.hpp b/src/teb_local_planner/include/teb_local_planner/visualization.hpp
deleted file mode 100644
index 9b45a54..0000000
--- a/src/teb_local_planner/include/teb_local_planner/visualization.hpp
+++ /dev/null
@@ -1,227 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/visualization.h"
-
-#include <boost/utility.hpp>
-#include <iterator>
-
-namespace teb_local_planner
-{
-
-
-template <typename GraphType>
-void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix)
-{
- if ( printErrorWhenNotInitialized() )
- return;
-
- typedef typename boost::graph_traits<GraphType>::vertex_iterator GraphVertexIterator;
- typedef typename boost::graph_traits<GraphType>::edge_iterator GraphEdgeIterator;
-
- // Visualize Edges
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = ns_prefix + "Edges";
- marker.id = 0;
- marker.pose.orientation.w = 1.0;
-// #define TRIANGLE
-#ifdef TRIANGLE
- marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
-#else
- marker.type = visualization_msgs::msg::Marker::LINE_LIST;
-#endif
- marker.action = visualization_msgs::msg::Marker::ADD;
-
- GraphEdgeIterator it_edge, end_edges;
- for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge)
- {
-#ifdef TRIANGLE
- geometry_msgs::msg::Point point_start1;
- point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05;
- point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05;
- point_start1.z = 0;
- marker.points.push_back(point_start1);
- geometry_msgs::msg::Point point_start2;
- point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05;
- point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05;
- point_start2.z = 0;
- marker.points.push_back(point_start2);
-
-#else
- geometry_msgs::msg::Point point_start;
- point_start.x = graph[boost::source(*it_edge,graph)].pos[0];
- point_start.y = graph[boost::source(*it_edge,graph)].pos[1];
- point_start.z = 0;
- marker.points.push_back(point_start);
-#endif
- geometry_msgs::msg::Point point_end;
- point_end.x = graph[boost::target(*it_edge,graph)].pos[0];
- point_end.y = graph[boost::target(*it_edge,graph)].pos[1];
- point_end.z = 0;
- marker.points.push_back(point_end);
-
- // add color
- std_msgs::msg::ColorRGBA color;
- color.a = 1.0;
- color.r = 0;
- color.g = 0;
- color.b = 1;
- marker.colors.push_back(color);
- marker.colors.push_back(color);
-#ifdef TRIANGLE
- marker.colors.push_back(color);
-#endif
- }
-
-#ifdef TRIANGLE
- marker.scale.x = 1;
- marker.scale.y = 1;
- marker.scale.z = 1;
-#else
- marker.scale.x = 0.01;
-#endif
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- // Now publish edge markers
- teb_marker_pub_->publish( marker );
-
- // Visualize vertices
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = ns_prefix + "Vertices";
- marker.id = 0;
- marker.type = visualization_msgs::msg::Marker::POINTS;
- marker.action = visualization_msgs::msg::Marker::ADD;
-
- GraphVertexIterator it_vert, end_vert;
- for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert)
- {
- geometry_msgs::msg::Point point;
- point.x = graph[*it_vert].pos[0];
- point.y = graph[*it_vert].pos[1];
- point.z = 0;
- marker.points.push_back(point);
- // add color
-
- std_msgs::msg::ColorRGBA color;
- color.a = 1.0;
- if (it_vert==end_vert-1)
- {
- color.r = 1;
- color.g = 0;
- color.b = 0;
- }
- else
- {
- color.r = 0;
- color.g = 1;
- color.b = 0;
- }
- marker.colors.push_back(color);
- }
- // set first color (start vertix) to blue
- if (!marker.colors.empty())
- {
- marker.colors.front().b = 1;
- marker.colors.front().g = 0;
- }
-
- marker.scale.x = 0.1;
- marker.scale.y = 0.1;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- // Now publish vertex markers
- teb_marker_pub_->publish( marker );
-}
-
-template <typename BidirIter>
-void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns)
-{
- if ( printErrorWhenNotInitialized() )
- return;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = ns;
- marker.id = 0;
- marker.type = visualization_msgs::msg::Marker::LINE_LIST;
- marker.action = visualization_msgs::msg::Marker::ADD;
-
- typedef typename std::iterator_traits<BidirIter>::value_type PathType; // Get type of the path (point container)
-
- // Iterate through path container
- while(first != last)
- {
- // iterate single path points
- typename PathType::const_iterator it_point, end_point;
- for (it_point = first->begin(), end_point = std::prev(first->end()); it_point != end_point; ++it_point)
- {
- geometry_msgs::msg::Point point_start;
- point_start.x = get_const_reference(*it_point).x();
- point_start.y = get_const_reference(*it_point).y();
- point_start.z = 0;
- marker.points.push_back(point_start);
-
- geometry_msgs::msg::Point point_end;
- point_end.x = get_const_reference(*std::next(it_point)).x();
- point_end.y = get_const_reference(*std::next(it_point)).y();
- point_end.z = 0;
- marker.points.push_back(point_end);
- }
- ++first;
- }
- marker.scale.x = 0.01;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
-}
-
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/package.xml b/src/teb_local_planner/package.xml
deleted file mode 100644
index 770a32a..0000000
--- a/src/teb_local_planner/package.xml
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>teb_local_planner</name>
-
- <version>0.9.1</version>
-
- <description>
- The teb_local_planner package implements a plugin
- to the base_local_planner of the 2D navigation stack.
- The underlying method called Timed Elastic Band locally optimizes
- the robot's trajectory with respect to trajectory execution time,
- separation from obstacles and compliance with kinodynamic constraints at runtime.
- </description>
-
- <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
-
- <license>BSD</license>
-
- <url type="website">http://wiki.ros.org/teb_local_planner</url>
-
- <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
-
- <buildtool_depend>ament_cmake</buildtool_depend>
-
- <depend>costmap_converter</depend>
- <depend>costmap_converter_msgs</depend>
-
- <depend>geometry_msgs</depend>
- <depend>libg2o</depend>
- <depend>dwb_critics</depend>
- <depend>nav2_core</depend>
- <depend>nav2_costmap_2d</depend>
- <depend>nav2_msgs</depend>
- <depend>nav2_util</depend>
- <depend>pluginlib</depend>
- <depend>rclcpp</depend>
- <depend>rclcpp_action</depend>
- <depend>rclcpp_lifecycle</depend>
- <depend>std_msgs</depend>
- <depend>teb_msgs</depend>
- <depend>tf2</depend>
- <depend>tf2_eigen</depend>
- <depend>visualization_msgs</depend>
- <depend>builtin_interfaces</depend>
- <exec_depend>nav2_bringup</exec_depend>
-
- <test_depend>ament_cmake_gtest</test_depend>
-
- <export>
- <build_type>ament_cmake</build_type>
- <nav2_core plugin="${prefix}/teb_local_planner_plugin.xml" />
- </export>
-</package>
diff --git a/src/teb_local_planner/params/teb_params.yaml b/src/teb_local_planner/params/teb_params.yaml
deleted file mode 100644
index 4fc447b..0000000
--- a/src/teb_local_planner/params/teb_params.yaml
+++ /dev/null
@@ -1,109 +0,0 @@
-controller_server:
- ros__parameters:
- odom_topic: /odom
- use_sim_time: True
- controller_frequency: 5.0
- controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"]
- controller_plugins: ["FollowPath"]
- FollowPath:
- plugin: teb_local_planner::TebLocalPlannerROS
-
- teb_autosize: 1.0
- dt_ref: 0.3
- dt_hysteresis: 0.1
- max_samples: 500
- global_plan_overwrite_orientation: False
- allow_init_with_backwards_motion: False
- max_global_plan_lookahead_dist: 3.0
- global_plan_viapoint_sep: 0.3
- global_plan_prune_distance: 1.0
- exact_arc_length: False
- feasibility_check_no_poses: 2
- publish_feedback: False
-
- # Robot
-
- max_vel_x: 0.26
- max_vel_theta: 1.0
- acc_lim_x: 2.5
- acc_lim_theta: 3.2
-
- footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
- type: "circular"
- radius: 0.17 # for type "circular"
-
- # GoalTolerance
-
- free_goal_vel: False
-
- # Obstacles
-
- min_obstacle_dist: 0.27
- inflation_dist: 0.6
- include_costmap_obstacles: True
- costmap_obstacles_behind_robot_dist: 1.0
- obstacle_poses_affected: 15
-
- dynamic_obstacle_inflation_dist: 0.6
- include_dynamic_obstacles: True
-
- costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
- costmap_converter_spin_thread: True
- costmap_converter_rate: 5
-
- # Optimization
-
- no_inner_iterations: 5
- no_outer_iterations: 4
- optimization_activate: True
- optimization_verbose: False
- penalty_epsilon: 0.1
- obstacle_cost_exponent: 4.0
- weight_max_vel_x: 0.5
- weight_max_vel_theta: 0.5
- weight_acc_lim_x: 0.5
- weight_acc_lim_theta: 10.5
- weight_kinematics_nh: 1000.0
- weight_kinematics_forward_drive: 3.0
- weight_kinematics_turning_radius: 1.0
- weight_optimaltime: 1.0 # must be > 0
- weight_shortest_path: 0.0
- weight_obstacle: 100.0
- weight_inflation: 0.2
- weight_dynamic_obstacle: 10.0 # not in use yet
- weight_dynamic_obstacle_inflation: 0.2
- weight_viapoint: 50.0
- weight_adapt_factor: 2.0
-
- # Homotopy Class Planner
-
- enable_homotopy_class_planning: True
- enable_multithreading: True
- max_number_classes: 4
- selection_cost_hysteresis: 5.0
- selection_prefer_initial_plan: 1.0
- selection_obst_cost_scale: 1.0
- selection_alternative_time_cost: True
-
- roadmap_graph_no_samples: 15
- roadmap_graph_area_width: 5.0
- roadmap_graph_area_length_scale: 1.0
- h_signature_prescaler: 0.5
- h_signature_threshold: 0.1
- obstacle_heading_threshold: 0.45
- switching_blocking_period: 0.0
- viapoints_all_candidates: True
- delete_detours_backwards: True
- max_ratio_detours_duration_best_duration: 3.0
- visualize_hc_graph: False
- visualize_with_time_as_z_axis_scale: 0.0
-
- # Recovery
-
- shrink_horizon_backup: True
- shrink_horizon_min_duration: 10.0
- oscillation_recovery: True
- oscillation_v_eps: 0.1
- oscillation_omega_eps: 0.1
- oscillation_recovery_min_duration: 10.0
- oscillation_filter_duration: 10.0
diff --git a/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py b/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
deleted file mode 100755
index d8c5b5f..0000000
--- a/src/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py
+++ /dev/null
@@ -1,60 +0,0 @@
-#!/usr/bin/env python3
-
-# Author: christoph.roesmann@tu-dortmund.de
-
-import rclpy, math
-from geometry_msgs.msg import Twist
-from ackermann_msgs.msg import AckermannDriveStamped
-
-
-def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
- if omega == 0 or v == 0:
- return 0
-
- radius = v / omega
- return math.atan(wheelbase / radius)
-
-
-def cmd_callback(data):
- global wheelbase
- global ackermann_cmd_topic
- global frame_id
- global pub
- global cmd_angle_instead_rotvel
-
- v = data.linear.x
- # if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node
- # in this case this script only needs to do the msg conversion from twist to Ackermann drive
- if cmd_angle_instead_rotvel:
- steering = data.angular.z
- else:
- steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
-
- msg = AckermannDriveStamped()
- msg.header.stamp = node.get_clock().now().to_msg()
- msg.header.frame_id = frame_id
- msg.drive.steering_angle = float(steering)
- msg.drive.speed = float(v)
-
- pub.publish(msg)
-
-
-
-
-
-if __name__ == '__main__':
- rclpy.init()
- global node
- node = rclpy.create_node('cmd_vel_to_ackermann_drive')
-
- twist_cmd_topic = node.declare_parameter("twist_cmd_topic", "/cmd_vel").value
- ackermann_cmd_topic = node.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd").value
- wheelbase = node.declare_parameter("wheelbase", 1.0).value
- frame_id = node.declare_parameter('frame_id', 'odom').value
- cmd_angle_instead_rotvel = node.declare_parameter('cmd_angle_instead_rotvel', False).value
-
- node.create_subscription(Twist, twist_cmd_topic, cmd_callback, 1)
- pub = node.create_publisher(AckermannDriveStamped, ackermann_cmd_topic, 1)
-
- rclpy.spin(node)
-
diff --git a/src/teb_local_planner/scripts/export_to_mat.py b/src/teb_local_planner/scripts/export_to_mat.py
deleted file mode 100755
index 1cc3351..0000000
--- a/src/teb_local_planner/scripts/export_to_mat.py
+++ /dev/null
@@ -1,112 +0,0 @@
-#!/usr/bin/env python
-
-# This small script subscribes to the FeedbackMsg message of teb_local_planner
-# and exports data to a mat file.
-# publish_feedback must be turned on such that the planner publishes this information.
-# Author: christoph.roesmann@tu-dortmund.de
-
-import rospy, math
-from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
-from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
-from tf.transformations import euler_from_quaternion
-import numpy as np
-import scipy.io as sio
-import time
-
-def feedback_callback(data):
- global got_data
-
- if not data.trajectories: # empty
- trajectory = []
- return
-
- if got_data:
- return
-
- got_data = True
-
- # copy trajectory
- trajectories = []
- for traj in data.trajectories:
- trajectory = []
-# # store as struct and cell array
-# for point in traj.trajectory:
-# (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
-# pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
-# velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
-# time_from_start = point.time_from_start.to_sec()
-# trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
-
- # store as all-in-one mat
- arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
- for index, point in enumerate(traj.trajectory):
- arr[0,index] = point.pose.position.x
- arr[1,index] = point.pose.position.y
- (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
- arr[2,index] = yaw
- arr[3,index] = point.velocity.linear.x
- arr[4,index] = point.velocity.angular.z
- arr[5,index] = point.time_from_start.to_sec()
-
-# trajectories.append({'raw': trajectory, 'mat': arr})
- trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
-
- # copy obstacles
- obstacles = []
- for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
- #polygon = []
- #for point in obst.polygon.points:
- # polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
- obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
- for index, point in enumerate(obst.polygon.points):
- obst_arr[0, index] = point.x
- obst_arr[1, index] = point.y
- obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
- obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
-
- #obstacles.append(polygon)
- obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
-
-
- # create main struct:
- mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
-
- timestr = time.strftime("%Y%m%d_%H%M%S")
- filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
-
- rospy.loginfo("Saving mat-file '%s'.", filename)
- sio.savemat(filename, mat)
-
-
-
-
-
-def feedback_exporter():
- global got_data
-
- rospy.init_node("export_to_mat", anonymous=True)
-
-
- topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
-
- rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
-
- rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
-
- r = rospy.Rate(2) # define rate here
- while not rospy.is_shutdown():
-
- if got_data:
- rospy.loginfo("Data export completed.")
- return
-
- r.sleep()
-
-if __name__ == '__main__':
- try:
- global got_data
- got_data = False
- feedback_exporter()
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/teb_local_planner/scripts/export_to_svg.py b/src/teb_local_planner/scripts/export_to_svg.py
deleted file mode 100755
index 8d5edf2..0000000
--- a/src/teb_local_planner/scripts/export_to_svg.py
+++ /dev/null
@@ -1,244 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-"""
-========================================================================================
-# This small script subscribes to the FeedbackMsg message of teb_local_planner
-# and converts the current scene to a svg-image
-# publish_feedback must be turned on such that the planner publishes this information.
-# Author: christoph.roesmann@tu-dortmund.de
-
-It is recommendable to start this node after initialization of TEB is completed.
-
-Requirements:
-svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
-=======================================================================================
-"""
-import roslib;
-import rospy
-import svgwrite
-import math
-import sys
-import time
-import random
-from svgwrite import cm, mm
-from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
-from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
-
-
-# ================= PARAMETERS ==================
-# TODO: In case of a more general node, change parameter to ros-parameter
-# Drawing parameters:
-SCALE = 200 # Overall scaling: 100 pixel = 1 m
-MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
-SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
-GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction.
-GRID_X_MAX = 2
-GRID_Y_MIN = -2
-GRID_Y_MAX = 1
-
-# TEB parameters:
-OBSTACLE_DIST = 50 *SCALE/100 # cm
-
-
-# ================= FUNCTIONS ===================
-
-def sign(number):
- """
- Signum function: get sign of a number
-
- @param number: get sign of this number
- @type number: numeric type (eg. integer)
- @return: sign of number
- @rtype: integer {1, -1, 0}
- """
- return cmp(number,0)
-
-def arrowMarker(color='green', orientation='auto'):
- """
- Create an arrow marker with svgwrite
-
- @return: arrow marker
- @rtype: svg_write marker object
- """
- arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
- arrow.viewbox(width=10, height=10)
- arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
- svg.defs.add(arrow)
- return arrow
-
-def quaternion2YawDegree(orientation):
- """
- Get yaw angle [degree] from quaternion representation
-
- @param orientation: orientation in quaternions to read from
- @type orientation: geometry_msgs/Quaternion
- @return: yaw angle [degree]
- @rtype: float
- """
- yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
- return yawRad*180/math.pi
-
-
-def feedback_callback(data):
- """
- Callback for receiving TEB and obstacle information
-
- @param data: Received feedback message
- @type data: visualization_msgs/Marker
-
- @globalparam tebList: Received TEB List
- @globaltype tebList: teb_local_planner/FeedbackMsg
- """
- # TODO: Remove global variables
- global feedbackMsg
-
- if not feedbackMsg:
- feedbackMsg = data
- rospy.loginfo("TEB feedback message received...")
-
-
-# ================ MAIN FUNCTION ================
-
-if __name__ == '__main__':
- rospy.init_node('export_to_svg', anonymous=True)
-
- topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
-
- rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
-
- rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
-
- rate = rospy.Rate(10.0)
- feedbackMsg = []
-
- timestr = time.strftime("%Y%m%d_%H%M%S")
- filename_string = "teb_svg_" + timestr + '.svg'
-
- rospy.loginfo("SVG will be written to '%s'.", filename_string)
-
- random.seed(0)
-
- svg=svgwrite.Drawing(filename=filename_string, debug=True)
-
- # Create viewbox -> this box defines the size of the visible drawing
- svg.viewbox(GRID_X_MIN*SCALE-1*SCALE,GRID_Y_MIN*SCALE-1*SCALE,GRID_X_MAX*SCALE-GRID_X_MIN*SCALE+2*SCALE,GRID_Y_MAX*SCALE-GRID_Y_MIN*SCALE+2*SCALE)
-
- # Draw grid:
- hLines = svg.add(svg.g(id='hLines', stroke='black'))
- hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
- for y in range(GRID_Y_MAX):
- hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
- for y in range(-GRID_Y_MIN):
- hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
- vLines = svg.add(svg.g(id='vline', stroke='black'))
- vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
- for x in range(GRID_X_MAX):
- vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
- for x in range(-GRID_X_MIN):
- vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
-
-
- # Draw legend:
- legend = svg.g(id='legend', font_size=25)
- stringGeometry = "Geometry: 1 Unit = 1.0m"
- legendGeometry = svg.text(stringGeometry)
- legend.add(legendGeometry)
- legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
- svg.add(legend)
-
-
- #arrow = arrowMarker() # Init arrow marker
-
- rospy.loginfo("Initialization completed.\nWaiting for feedback message...")
-
- # -------------------- WAIT FOR CALLBACKS --------------------------
- while not rospy.is_shutdown():
- if feedbackMsg:
- break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
- rate.sleep()
- # ------------------------------------------------------------------
-
- if not feedbackMsg.trajectories:
- rospy.loginfo("Received message does not contain trajectories. Shutting down...")
- sys.exit()
-
- if len(feedbackMsg.trajectories[0].trajectory) < 2:
- rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
- sys.exit()
-
- # iterate trajectories
- for index, traj in enumerate(feedbackMsg.trajectories):
-
- #color
- traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
-
- # Iterate through TEB positions -> Draw Paths
- points = []
- for point in traj.trajectory:
- points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
- # svgwrite rotates clockwise!
-
-
- if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
- line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
- stroke_linejoin='round', opacity=1.0 ) )
- else:
- line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
- stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
- #marker_points = points[::7]
- #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
- #arrow = arrowMarker(traj_color)
- #markerline.set_markers( (arrow, arrow, arrow) )
- #line.set_markers( (arrow, arrow, arrow) )
- #line['marker-start'] = arrow.get_funciri()
-
-
- # Add Start and Goal Point
- start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
- goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
- start_position = start_pose.position
- goal_position = goal_pose.position
- svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
- svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
- svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
- svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
-
- # draw start arrow
- start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
- start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
- start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) )
- start_arrow.scale(3)
- svg.add(start_arrow)
-
- # draw goal arrow
- goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
- goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
- goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) )
- goal_arrow.scale(3)
- svg.add(goal_arrow)
-
- # Draw obstacles
- for obstacle in feedbackMsg.obstacles:
- if len(obstacle.polygon.points) == 1: # point obstacle
- point = obstacle.polygon.points[0]
- svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
- svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
- svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
- if len(obstacle.polygon.points) == 2: # line obstacle
- line_start = obstacle.polygon.points[0]
- line_end = obstacle.polygon.points[1]
- svg.add(svg.line(start=(line_start.x*SCALE,-line_start.y*SCALE), end=(line_end.x*SCALE,-line_end.y*SCALE), stroke='black', fill='gray', stroke_width=1, opacity=1.0))
- svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
- if len(obstacle.polygon.points) > 2: # polygon obstacle
- vertices = []
- for point in obstacle.polygon.points:
- vertices.append((point.x*SCALE, -point.y*SCALE))
- svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
- svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
-
-
-
- # Save svg to file (svg_output.svg) and exit node
- svg.save()
-
- rospy.loginfo("Drawing completed.")
diff --git a/src/teb_local_planner/scripts/publish_dynamic_obstacle.py b/src/teb_local_planner/scripts/publish_dynamic_obstacle.py
deleted file mode 100755
index 0cc16d4..0000000
--- a/src/teb_local_planner/scripts/publish_dynamic_obstacle.py
+++ /dev/null
@@ -1,67 +0,0 @@
-#!/usr/bin/env python
-
-# Author: franz.albers@tu-dortmund.de
-
-import rospy, math, tf
-from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
-from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance
-from tf.transformations import quaternion_from_euler
-
-
-def publish_obstacle_msg():
- pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
- #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
- rospy.init_node("test_obstacle_msg")
-
- y_0 = -3.0
- vel_x = 0.0
- vel_y = 0.3
- range_y = 6.0
-
- obstacle_msg = ObstacleArrayMsg()
- obstacle_msg.header.stamp = rospy.Time.now()
- obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
-
- # Add point obstacle
- obstacle_msg.obstacles.append(ObstacleMsg())
- obstacle_msg.obstacles[0].id = 99
- obstacle_msg.obstacles[0].polygon.points = [Point32()]
- obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
- obstacle_msg.obstacles[0].polygon.points[0].y = 0
- obstacle_msg.obstacles[0].polygon.points[0].z = 0
-
- yaw = math.atan2(vel_y, vel_x)
- q = tf.transformations.quaternion_from_euler(0,0,yaw)
- obstacle_msg.obstacles[0].orientation = Quaternion(*q)
-
- obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
- obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
- obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
- obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
- obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
- obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
-
- r = rospy.Rate(10) # 10hz
- t = 0.0
- while not rospy.is_shutdown():
-
- # Vary y component of the point obstacle
- if (vel_y >= 0):
- obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
- else:
- obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y
-
- t = t + 0.1
-
- pub.publish(obstacle_msg)
-
- r.sleep()
-
-
-
-if __name__ == '__main__':
- try:
- publish_obstacle_msg()
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/teb_local_planner/scripts/publish_test_obstacles.py b/src/teb_local_planner/scripts/publish_test_obstacles.py
deleted file mode 100755
index 800803d..0000000
--- a/src/teb_local_planner/scripts/publish_test_obstacles.py
+++ /dev/null
@@ -1,76 +0,0 @@
-#!/usr/bin/env python
-
-# Author: christoph.roesmann@tu-dortmund.de
-
-import rospy, math
-from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
-from geometry_msgs.msg import PolygonStamped, Point32
-
-
-def publish_obstacle_msg():
- pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
- #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
- rospy.init_node("test_obstacle_msg")
-
-
- obstacle_msg = ObstacleArrayMsg()
- obstacle_msg.header.stamp = rospy.Time.now()
- obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
-
- # Add point obstacle
- obstacle_msg.obstacles.append(ObstacleMsg())
- obstacle_msg.obstacles[0].id = 0
- obstacle_msg.obstacles[0].polygon.points = [Point32()]
- obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
- obstacle_msg.obstacles[0].polygon.points[0].y = 0
- obstacle_msg.obstacles[0].polygon.points[0].z = 0
-
-
- # Add line obstacle
- obstacle_msg.obstacles.append(ObstacleMsg())
- obstacle_msg.obstacles[1].id = 1
- line_start = Point32()
- line_start.x = -2.5
- line_start.y = 0.5
- #line_start.y = -3
- line_end = Point32()
- line_end.x = -2.5
- line_end.y = 2
- #line_end.y = -4
- obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
-
- # Add polygon obstacle
- obstacle_msg.obstacles.append(ObstacleMsg())
- obstacle_msg.obstacles[1].id = 2
- v1 = Point32()
- v1.x = -1
- v1.y = -1
- v2 = Point32()
- v2.x = -0.5
- v2.y = -1.5
- v3 = Point32()
- v3.x = 0
- v3.y = -1
- obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
-
-
- r = rospy.Rate(10) # 10hz
- t = 0.0
- while not rospy.is_shutdown():
-
- # Vary y component of the point obstacle
- obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
- t = t + 0.1
-
- pub.publish(obstacle_msg)
-
- r.sleep()
-
-
-
-if __name__ == '__main__':
- try:
- publish_obstacle_msg()
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/teb_local_planner/scripts/publish_viapoints.py b/src/teb_local_planner/scripts/publish_viapoints.py
deleted file mode 100755
index 7bd0c7e..0000000
--- a/src/teb_local_planner/scripts/publish_viapoints.py
+++ /dev/null
@@ -1,46 +0,0 @@
-#!/usr/bin/env python
-
-# Author: christoph.roesmann@tu-dortmund.de
-
-import rospy, math
-from geometry_msgs.msg import PoseStamped
-from nav_msgs.msg import Path
-
-
-def publish_via_points_msg():
- pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1)
- rospy.init_node("test_via_points_msg")
-
-
- via_points_msg = Path()
- via_points_msg.header.stamp = rospy.Time.now()
- via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
-
- # Add via-points
- point1 = PoseStamped()
- point1.pose.position.x = 0.0;
- point1.pose.position.y = 1.5;
-
- point2 = PoseStamped()
- point2.pose.position.x = 2.0;
- point2.pose.position.y = -0.5;
-
-
- via_points_msg.poses = [point1, point2]
-
- r = rospy.Rate(5) # 10hz
- t = 0.0
- while not rospy.is_shutdown():
-
- pub.publish(via_points_msg)
-
- r.sleep()
-
-
-
-if __name__ == '__main__':
- try:
- publish_via_points_msg()
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/teb_local_planner/scripts/visualize_velocity_profile.py b/src/teb_local_planner/scripts/visualize_velocity_profile.py
deleted file mode 100755
index b9b6d78..0000000
--- a/src/teb_local_planner/scripts/visualize_velocity_profile.py
+++ /dev/null
@@ -1,76 +0,0 @@
-#!/usr/bin/env python
-
-# This small script subscribes to the FeedbackMsg message of teb_local_planner
-# and plots the current velocity.
-# publish_feedback must be turned on such that the planner publishes this information.
-# Author: christoph.roesmann@tu-dortmund.de
-
-import rospy, math
-from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
-from geometry_msgs.msg import PolygonStamped, Point32
-import numpy as np
-import matplotlib.pyplot as plotter
-
-def feedback_callback(data):
- global trajectory
-
- if not data.trajectories: # empty
- trajectory = []
- return
- trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
-
-
-def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
- ax_v.cla()
- ax_v.grid()
- ax_v.set_ylabel('Trans. velocity [m/s]')
- ax_v.plot(t, v, '-bx')
- ax_omega.cla()
- ax_omega.grid()
- ax_omega.set_ylabel('Rot. velocity [rad/s]')
- ax_omega.set_xlabel('Time [s]')
- ax_omega.plot(t, omega, '-bx')
- fig.canvas.draw()
-
-
-
-def velocity_plotter():
- global trajectory
- rospy.init_node("visualize_velocity_profile", anonymous=True)
-
- topic_name = "/test_optim_node/teb_feedback"
- topic_name = rospy.get_param('~feedback_topic', topic_name)
- rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
-
- rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name)
- rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
-
- # two subplots sharing the same t axis
- fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
- plotter.ion()
- plotter.show()
-
-
- r = rospy.Rate(2) # define rate here
- while not rospy.is_shutdown():
-
- t = []
- v = []
- omega = []
-
- for point in trajectory:
- t.append(point.time_from_start.to_sec())
- v.append(point.velocity.linear.x)
- omega.append(point.velocity.angular.z)
-
- plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
-
- r.sleep()
-
-if __name__ == '__main__':
- try:
- trajectory = []
- velocity_plotter()
- except rospy.ROSInterruptException:
- pass
-
diff --git a/src/teb_local_planner/src/graph_search.cpp b/src/teb_local_planner/src/graph_search.cpp
deleted file mode 100644
index f2d1218..0000000
--- a/src/teb_local_planner/src/graph_search.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2017,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Authors: Christoph Rösmann, Franz Albers
- *********************************************************************/
-
-#include "teb_local_planner/graph_search.h"
-#include "teb_local_planner/homotopy_class_planner.h"
-
-namespace teb_local_planner
-{
-
-void GraphSearchInterface::DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation,
- double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- // see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths
-
- if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
- return; // We do not need to search for further possible alternative homotopy classes.
-
- HcGraphVertexType back = visited.back();
-
- /// Examine adjacent nodes
- HcGraphAdjecencyIterator it, end;
- for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
- {
- if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() )
- continue; // already visited
-
- if ( *it == goal ) // goal reached
- {
- visited.push_back(*it);
-
- // Add new TEB, if this path belongs to a new homotopy class
- hcp_->addAndInitNewTeb(visited.begin(), visited.end(), std::bind(getVector2dFromHcGraph, std::placeholders::_1, boost::cref(graph_)),
- start_orientation, goal_orientation, start_velocity);
-
- visited.pop_back();
- break;
- }
- }
-
- /// Recursion for all adjacent vertices
- for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
- {
- if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() || *it == goal)
- continue; // already visited || goal reached
-
-
- visited.push_back(*it);
-
- // recursion step
- DepthFirst(g, visited, goal, start_orientation, goal_orientation, start_velocity, free_goal_vel);
-
- visited.pop_back();
- }
-}
-
-
-
-void lrKeyPointGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- // Clear existing graph and paths
- clearGraph();
- if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
- return;
- // Direction-vector between start and goal and normal-vector:
- Eigen::Vector2d diff = goal.position()-start.position();
-
- if (diff.norm()<cfg_->goal_tolerance.xy_goal_tolerance)
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
- if (hcp_->getTrajectoryContainer().empty())
- {
- RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
- hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
- }
- return;
- }
-
- Eigen::Vector2d normal(-diff[1],diff[0]); // normal-vector
- normal.normalize();
- normal = normal*dist_to_obst; // scale with obstacle_distance;
-
- // Insert Vertices
- HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
- graph_[start_vtx].pos = start.position();
- diff.normalize();
-
- // store nearest obstacle keypoints -> only used if limit_obstacle_heading is enabled
- std::pair<HcGraphVertexType,HcGraphVertexType> nearest_obstacle; // both vertices are stored
- double min_dist = DBL_MAX;
-
- if (hcp_->obstacles()!=NULL)
- {
- for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
- {
- // check if obstacle is placed in front of start point
- Eigen::Vector2d start2obst = (*it_obst)->getCentroid() - start.position();
- double dist = start2obst.norm();
- if (start2obst.dot(diff)/dist<0.1)
- continue;
-
- // Add Keypoints
- HcGraphVertexType u = boost::add_vertex(graph_);
- graph_[u].pos = (*it_obst)->getCentroid() + normal;
- HcGraphVertexType v = boost::add_vertex(graph_);
- graph_[v].pos = (*it_obst)->getCentroid() - normal;
-
- // store nearest obstacle
- if (obstacle_heading_threshold && dist<min_dist)
- {
- min_dist = dist;
- nearest_obstacle.first = u;
- nearest_obstacle.second = v;
- }
- }
- }
-
- HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
- graph_[goal_vtx].pos = goal.position();
-
- // Insert Edges
- HcGraphVertexIterator it_i, end_i, it_j, end_j;
- for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=end_i-1; ++it_i) // ignore goal in this loop
- {
- for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
- {
- if (it_i==it_j)
- continue;
- // TODO: make use of knowing in which order obstacles are inserted and that for each obstacle 2 vertices are added,
- // therefore we must only check one of them.
- Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
- distij.normalize();
- // Check if the direction is backwards:
- if (distij.dot(diff)<=obstacle_heading_threshold)
- continue;
-
-
- // Check start angle to nearest obstacle
- if (obstacle_heading_threshold && *it_i==start_vtx && min_dist!=DBL_MAX)
- {
- if (*it_j == nearest_obstacle.first || *it_j == nearest_obstacle.second)
- {
- Eigen::Vector2d keypoint_dist = graph_[*it_j].pos-start.position();
- keypoint_dist.normalize();
- Eigen::Vector2d start_orient_vec( cos(start.theta()), sin(start.theta()) ); // already normalized
- // check angle
- if (start_orient_vec.dot(keypoint_dist) <= obstacle_heading_threshold)
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "createGraph() - deleted edge: limit_obstacle_heading");
- continue;
- }
- }
- }
-
- // Collision Check
-
- if (hcp_->obstacles()!=NULL)
- {
- bool collision = false;
- for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
- {
- if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, 0.5*dist_to_obst) )
- {
- collision = true;
- break;
- }
- }
- if (collision)
- continue;
- }
-
- // Create Edge
- boost::add_edge(*it_i,*it_j,graph_);
- }
- }
-
-
- // Find all paths between start and goal!
- std::vector<HcGraphVertexType> visited;
- visited.push_back(start_vtx);
- DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
-}
-
-
-void ProbRoadmapGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- // Clear existing graph and paths
- clearGraph();
- if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
- return;
- // Direction-vector between start and goal and normal-vector:
- Eigen::Vector2d diff = goal.position()-start.position();
- double start_goal_dist = diff.norm();
-
- if (start_goal_dist<cfg_->goal_tolerance.xy_goal_tolerance)
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
- if (hcp_->getTrajectoryContainer().empty())
- {
- RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
- hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
- }
- return;
- }
- Eigen::Vector2d normal(-diff.coeffRef(1),diff.coeffRef(0)); // normal-vector
- normal.normalize();
-
- // Now sample vertices between start, goal and a specified width between both sides
- // Let's start with a square area between start and goal (maybe change it later to something like a circle or whatever)
-
- double area_width = cfg_->hcp.roadmap_graph_area_width;
-
- boost::random::uniform_real_distribution<double> distribution_x(0, start_goal_dist * cfg_->hcp.roadmap_graph_area_length_scale);
- boost::random::uniform_real_distribution<double> distribution_y(0, area_width);
-
- double phi = atan2(diff.coeffRef(1),diff.coeffRef(0)); // rotate area by this angle
- Eigen::Rotation2D<double> rot_phi(phi);
-
- Eigen::Vector2d area_origin;
- if (cfg_->hcp.roadmap_graph_area_length_scale != 1.0)
- area_origin = start.position() + 0.5*(1.0-cfg_->hcp.roadmap_graph_area_length_scale)*start_goal_dist*diff.normalized() - 0.5*area_width*normal; // bottom left corner of the origin
- else
- area_origin = start.position() - 0.5*area_width*normal; // bottom left corner of the origin
-
- // Insert Vertices
- HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
- graph_[start_vtx].pos = start.position();
- diff.normalize(); // normalize in place
-
-
- // Start sampling
- for (int i=0; i < cfg_->hcp.roadmap_graph_no_samples; ++i)
- {
- Eigen::Vector2d sample;
-// bool coll_free;
-// do // sample as long as a collision free sample is found
-// {
- // Sample coordinates
- sample = area_origin + rot_phi*Eigen::Vector2d(distribution_x(rnd_generator_), distribution_y(rnd_generator_));
-
- // Test for collision
- // we do not care for collision checking here to improve efficiency, since we perform resampling repeatedly.
- // occupied vertices are ignored in the edge insertion state since they always violate the edge-obstacle collision check.
-// coll_free = true;
-// for (ObstContainer::const_iterator it_obst = obstacles_->begin(); it_obst != obstacles_->end(); ++it_obst)
-// {
-// if ( (*it_obst)->checkCollision(sample, dist_to_obst)) // TODO really keep dist_to_obst here?
-// {
-// coll_free = false;
-// break;
-// }
-// }
-//
-// } while (!coll_free && ros::ok());
-
- // Add new vertex
- HcGraphVertexType v = boost::add_vertex(graph_);
- graph_[v].pos = sample;
- }
-
- // Now add goal vertex
- HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
- graph_[goal_vtx].pos = goal.position();
-
-
- // Insert Edges
- HcGraphVertexIterator it_i, end_i, it_j, end_j;
- for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=boost::prior(end_i); ++it_i) // ignore goal in this loop
- {
- for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
- {
- if (it_i==it_j) // same vertex found
- continue;
-
- Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
- distij.normalize(); // normalize in place
-
- // Check if the direction is backwards:
- if (distij.dot(diff)<=obstacle_heading_threshold)
- continue; // diff is already normalized
-
-
- // Collision Check
- bool collision = false;
- for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
- {
- if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, dist_to_obst) )
- {
- collision = true;
- break;
- }
- }
- if (collision)
- continue;
-
- // Create Edge
- boost::add_edge(*it_i,*it_j,graph_);
- }
- }
-
- /// Find all paths between start and goal!
- std::vector<HcGraphVertexType> visited;
- visited.push_back(start_vtx);
- DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
-}
-
-} // end namespace
diff --git a/src/teb_local_planner/src/homotopy_class_planner.cpp b/src/teb_local_planner/src/homotopy_class_planner.cpp
deleted file mode 100644
index 6fbf995..0000000
--- a/src/teb_local_planner/src/homotopy_class_planner.cpp
+++ /dev/null
@@ -1,854 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/homotopy_class_planner.h"
-
-#include <limits>
-
-namespace teb_local_planner
-{
-
-HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), robot_model_(new PointRobotFootprint()), initial_plan_(NULL), initialized_(false)
-{
-}
-
-HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
- TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL)
-{
- initialize(node, cfg, obstacles, visual, via_points);
-}
-
-HomotopyClassPlanner::~HomotopyClassPlanner()
-{
-}
-
-void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles,
- TebVisualizationPtr visual, const ViaPointContainer* via_points)
-{
- node_ = node;
- cfg_ = &cfg;
- obstacles_ = obstacles;
- via_points_ = via_points;
-
- if (cfg_->hcp.simple_exploration)
- graph_search_ = std::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
- else
- graph_search_ = std::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));
-
- std::random_device rd;
- random_.seed(rd());
-
- // This is needed to prevent different time sources error
- last_eq_class_switching_time_ = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
-
- initialized_ = true;
-
- setVisualization(visual);
-}
-
-void HomotopyClassPlanner::setVisualization(const TebVisualizationPtr& visualization)
-{
- visualization_ = visualization;
-}
-
-
-
-bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-{
- TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
-
- // store initial plan for further initializations (must be valid for the lifetime of this object or clearPlanner() is called!)
- initial_plan_ = &initial_plan;
-
- PoseSE2 start(initial_plan.front().pose);
- PoseSE2 goal(initial_plan.back().pose);
-
- return plan(start, goal, start_vel, free_goal_vel);
-}
-
-// tf2 doesn't have tf::Pose
-//bool HomotopyClassPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-//{
-// TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
-// PoseSE2 start_pose(start);
-// PoseSE2 goal_pose(goal);
-// return plan(start_pose, goal_pose, start_vel, free_goal_vel);
-//}
-
-bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-{
- TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
-
- // Update old TEBs with new start, goal and velocity
- updateAllTEBs(&start, &goal, start_vel);
-
- // Init new TEBs based on newly explored homotopy classes
- exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel, free_goal_vel);
- // update via-points if activated
- updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
- // Optimize all trajectories in alternative homotopy classes
- optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
- // Select which candidate (based on alternative homotopy classes) should be used
- selectBestTeb();
-
- initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
- return true;
-}
-
-bool HomotopyClassPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
-{
- TebOptimalPlannerConstPtr best_teb = bestTeb();
- if (!best_teb)
- {
- vx = 0;
- vy = 0;
- omega = 0;
- return false;
- }
-
- return best_teb->getVelocityCommand(vx, vy, omega, look_ahead_poses);
-}
-
-
-
-
-void HomotopyClassPlanner::visualize()
-{
- if (visualization_)
- {
- // Visualize graph
- if (cfg_->hcp.visualize_hc_graph && graph_search_)
- visualization_->publishGraph(graph_search_->graph_);
-
- // Visualize active tebs as marker
- visualization_->publishTebContainer(tebs_);
-
- // Visualize best teb and feedback message if desired
- TebOptimalPlannerConstPtr best_teb = bestTeb();
- if (best_teb)
- {
- visualization_->publishLocalPlanAndPoses(best_teb->teb());
-
- if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.
- visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *cfg_->robot_model);
-
- // feedback message
- if (cfg_->trajectory.publish_feedback)
- {
- int best_idx = bestTebIdx();
- if (best_idx>=0)
- visualization_->publishFeedbackMessage(tebs_, (unsigned int) best_idx, *obstacles_);
- }
- }
- }
- else RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
-}
-
-
-
-bool HomotopyClassPlanner::hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const
-{
- // iterate existing h-signatures and check if there is an existing H-Signature similar the candidate
- for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
- {
- if (eq_class->isEqual(*eqrel.first))
- return true; // Found! Homotopy class already exists, therefore nothing added
- }
- return false;
-}
-
-bool HomotopyClassPlanner::addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock)
-{
- if (!eq_class)
- return false;
-
- if (!eq_class->isValid())
- {
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner: Ignoring invalid H-signature");
- return false;
- }
-
- if (hasEquivalenceClass(eq_class))
- {
- // Allow up to configured number of Tebs that are in the same homotopy
- // class as the current (best) Teb to avoid being stuck in a local minimum
- if (!isInBestTebClass(eq_class) || numTebsInBestTebClass() >= cfg_->hcp.max_number_plans_in_current_class)
- return false;
- }
-
- // Homotopy class not found -> Add to class-list, return that the h-signature is new
- equivalence_classes_.push_back(std::make_pair(eq_class,lock));
- return true;
-}
-
-
-void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours)
-{
- // clear old h-signatures (since they could be changed due to new obstacle positions.
- equivalence_classes_.clear();
-
- // Adding the equivalence class of the latest best_teb_ first
- TebOptPlannerContainer::iterator it_best_teb = best_teb_ ? std::find(tebs_.begin(), tebs_.end(), best_teb_) : tebs_.end();
- bool has_best_teb = it_best_teb != tebs_.end();
- if (has_best_teb)
- {
- std::iter_swap(tebs_.begin(), it_best_teb); // Putting the last best teb at the beginning of the container
- best_teb_eq_class_ = calculateEquivalenceClass(best_teb_->teb().poses().begin(),
- best_teb_->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
- best_teb_->teb().timediffs().begin(), best_teb_->teb().timediffs().end());
- addEquivalenceClassIfNew(best_teb_eq_class_);
- }
- // Collect h-signatures for all existing TEBs and store them together with the corresponding iterator / pointer:
-// typedef std::list< std::pair<TebOptPlannerContainer::iterator, std::complex<long double> > > TebCandidateType;
-// TebCandidateType teb_candidates;
-
- // get new homotopy classes and delete multiple TEBs per homotopy class. Skips the best teb if available (added before).
- TebOptPlannerContainer::iterator it_teb = has_best_teb ? std::next(tebs_.begin(), 1) : tebs_.begin();
- while(it_teb != tebs_.end())
- {
- // calculate equivalence class for the current candidate
- EquivalenceClassPtr equivalence_class = calculateEquivalenceClass(it_teb->get()->teb().poses().begin(), it_teb->get()->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
- it_teb->get()->teb().timediffs().begin(), it_teb->get()->teb().timediffs().end());
-
-// teb_candidates.push_back(std::make_pair(it_teb,H));
-
- // WORKAROUND until the commented code below works
- // Here we do not compare cost values. Just first come first serve...
- bool new_flag = addEquivalenceClassIfNew(equivalence_class);
- if (!new_flag)
- {
- it_teb = tebs_.erase(it_teb);
- continue;
- }
-
- ++it_teb;
- }
- if(delete_detours)
- deletePlansDetouringBackwards(cfg_->hcp.detours_orientation_tolerance, cfg_->hcp.length_start_orientation_vector);
-
- // Find multiple candidates and delete the one with higher cost
- // TODO: this code needs to be adpated. Erasing tebs from the teb container_ could make iteratores stored in the candidate list invalid!
-// TebCandidateType::reverse_iterator cand_i = teb_candidates.rbegin();
-// int test_idx = 0;
-// while (cand_i != teb_candidates.rend())
-// {
-//
-// TebCandidateType::reverse_iterator cand_j = std::find_if(boost::next(cand_i),teb_candidates.rend(), boost::bind(compareH,_1,cand_i->second));
-// if (cand_j != teb_candidates.rend() && cand_j != cand_i)
-// {
-// TebOptimalPlannerPtr pt1 = *(cand_j->first);
-// TebOptimalPlannerPtr pt2 = *(cand_i->first);
-// assert(pt1);
-// assert(pt2);
-// if ( cand_j->first->get()->getCurrentCost().sum() > cand_i->first->get()->getCurrentCost().sum() )
-// {
-// // found one that has higher cost, therefore erase cand_j
-// tebs_.erase(cand_j->first);
-// teb_candidates.erase(cand_j);
-// }
-// else // otherwise erase cand_i
-// {
-// tebs_.erase(cand_i->first);
-// cand_i = teb_candidates.erase(cand_i);
-// }
-// }
-// else
-// {
-// ROS_WARN_STREAM("increase cand_i");
-// ++cand_i;
-// }
-// }
-
- // now add the h-signatures to the internal lookup-table (but only if there is no existing duplicate)
-// for (TebCandidateType::iterator cand=teb_candidates.begin(); cand!=teb_candidates.end(); ++cand)
-// {
-// bool new_flag = addNewHSignatureIfNew(cand->second, cfg_->hcp.h_signature_threshold);
-// if (!new_flag)
-// {
-// // ROS_ERROR_STREAM("getAndFilterHomotopyClassesTEB() - This schould not be happen.");
-// tebs_.erase(cand->first);
-// }
-// }
-
-}
-
-void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories)
-{
- if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0)
- return;
-
- if(equivalence_classes_.size() < tebs_.size())
- {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::updateReferenceTrajectoryWithViaPoints(): Number of h-signatures does not match number of trajectories.");
- return;
- }
-
- if (all_trajectories)
- {
- // enable via-points for all tebs
- for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
- {
- tebs_[i]->setViaPoints(via_points_);
- }
- }
- else
- {
- // enable via-points for teb in the same hommotopy class as the initial_plan and deactivate it for all other ones
- for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
- {
- if(initial_plan_eq_class_->isEqual(*equivalence_classes_[i].first))
- tebs_[i]->setViaPoints(via_points_);
- else
- tebs_[i]->setViaPoints(NULL);
- }
- }
-}
-
-
-void HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-{
- // first process old trajectories
- renewAndAnalyzeOldTebs(cfg_->hcp.delete_detours_backwards);
- randomlyDropTebs();
-
- // inject initial plan if available and not yet captured
- if (initial_plan_)
- {
- initial_plan_teb_ = addAndInitNewTeb(*initial_plan_, start_vel, free_goal_vel);
- }
- else
- {
- initial_plan_teb_.reset();
- initial_plan_teb_ = getInitialPlanTEB(); // this method searches for initial_plan_eq_class_ in the teb container (-> if !initial_plan_teb_)
- }
-
- // now explore new homotopy classes and initialize tebs if new ones are found. The appropriate createGraph method is chosen via polymorphism.
- graph_search_->createGraph(start,goal,dist_to_obst,cfg_->hcp.obstacle_heading_threshold, start_vel, free_goal_vel);
-}
-
-
-TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- if(tebs_.size() >= cfg_->hcp.max_number_classes)
- return TebOptimalPlannerPtr();
- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
-
- candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
-
- if (start_velocity)
- candidate->setVelocityStart(*start_velocity);
-
- EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
- candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
-
- if (free_goal_vel)
- candidate->setVelocityGoalFree();
-
- if(addEquivalenceClassIfNew(H))
- {
- tebs_.push_back(candidate);
- return tebs_.back();
- }
-
- // If the candidate constitutes no new equivalence class, return a null pointer
- return TebOptimalPlannerPtr();
-}
-
-
-bool HomotopyClassPlanner::isInBestTebClass(const EquivalenceClassPtr& eq_class) const
-{
- bool answer = false;
- if (best_teb_eq_class_)
- answer = best_teb_eq_class_->isEqual(*eq_class);
- return answer;
-}
-
-int HomotopyClassPlanner::numTebsInClass(const EquivalenceClassPtr& eq_class) const
-{
- int count = 0;
- for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
- {
- if (eq_class->isEqual(*eqrel.first))
- ++count;
- }
- return count;
-}
-
-int HomotopyClassPlanner::numTebsInBestTebClass() const
-{
- int count = 0;
- if (best_teb_eq_class_)
- count = numTebsInClass(best_teb_eq_class_);
- return count;
-}
-
-TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
-{
- if(tebs_.size() >= cfg_->hcp.max_number_classes)
- return TebOptimalPlannerPtr();
- TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_));
-
- candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
- cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
-
- if (start_velocity)
- candidate->setVelocityStart(*start_velocity);
-
- if (free_goal_vel)
- candidate->setVelocityGoalFree();
-
- // store the h signature of the initial plan to enable searching a matching teb later.
- initial_plan_eq_class_ = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
- candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
-
- if(addEquivalenceClassIfNew(initial_plan_eq_class_, true)) // also prevent candidate from deletion
- {
- tebs_.push_back(candidate);
- return tebs_.back();
- }
-
- // If the candidate constitutes no new equivalence class, return a null pointer
- return TebOptimalPlannerPtr();
-}
-
-void HomotopyClassPlanner::updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity)
-{
- // If new goal is too far away, clear all existing trajectories to let them reinitialize later.
- // Since all Tebs are sharing the same fixed goal pose, just take the first candidate:
- if (!tebs_.empty()
- && ((goal->position() - tebs_.front()->teb().BackPose().position()).norm() >= cfg_->trajectory.force_reinit_new_goal_dist
- || fabs(g2o::normalize_theta(goal->theta() - tebs_.front()->teb().BackPose().theta())) >= cfg_->trajectory.force_reinit_new_goal_angular))
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
- tebs_.clear();
- equivalence_classes_.clear();
- }
-
- // hot-start from previous solutions
- for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- it_teb->get()->teb().updateAndPruneTEB(*start, *goal);
- if (start_velocity)
- it_teb->get()->setVelocityStart(*start_velocity);
- }
-}
-
-
-void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
-{
- // optimize TEBs in parallel since they are independend of each other
- if (cfg_->hcp.enable_multithreading)
- {
- std::vector<std::thread> teb_threads;
-
- for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- auto functor = [&, it_teb]() {
- it_teb->get()->optimizeTEB(iter_innerloop, iter_outerloop,
- true, cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
- cfg_->hcp.selection_alternative_time_cost);
- };
-
- teb_threads.emplace_back(functor);
- }
-
- for(auto & thread : teb_threads) {
- if(thread.joinable()) {
- thread.join();
- }
- }
- }
- else
- {
- for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- it_teb->get()->optimizeTEB(iter_innerloop,iter_outerloop, true, cfg_->hcp.selection_obst_cost_scale,
- cfg_->hcp.selection_viapoint_cost_scale, cfg_->hcp.selection_alternative_time_cost); // compute cost as well inside optimizeTEB (last argument = true)
- }
- }
-}
-
-TebOptimalPlannerPtr HomotopyClassPlanner::getInitialPlanTEB()
-{
- // first check stored teb object
- if (initial_plan_teb_)
- {
- // check if the teb is still part of the teb container
- if ( std::find(tebs_.begin(), tebs_.end(), initial_plan_teb_ ) != tebs_.end() )
- return initial_plan_teb_;
- else
- {
- initial_plan_teb_.reset(); // reset pointer for next call
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "initial teb not found, trying to find a match according to the cached equivalence class");
- }
- }
-
- // reset the locked state for equivalence classes // TODO: this might be adapted if not only the plan containing the initial plan is locked!
- for (int i=0; i<equivalence_classes_.size(); ++i)
- {
- equivalence_classes_[i].second = false;
- }
-
- // otherwise check if the stored reference equivalence class exist in the list of known classes
- if (initial_plan_eq_class_ && initial_plan_eq_class_->isValid())
- {
- if (equivalence_classes_.size() == tebs_.size())
- {
- for (int i=0; i<equivalence_classes_.size(); ++i)
- {
- if (equivalence_classes_[i].first->isEqual(*initial_plan_eq_class_))
- {
- equivalence_classes_[i].second = true;
- return tebs_[i];
- }
- }
- }
- else
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::getInitialPlanTEB(): number of equivalence classes (%lu) and number of trajectories (%lu) does not match.", equivalence_classes_.size(), tebs_.size());
- }
- else
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::getInitialPlanTEB(): initial TEB not found in the set of available trajectories.");
-
- return TebOptimalPlannerPtr();
-}
-
-void HomotopyClassPlanner::randomlyDropTebs()
-{
- if (cfg_->hcp.selection_dropping_probability == 0.0)
- {
- return;
- }
- // interate both vectors in parallel
- auto it_eqrel = equivalence_classes_.begin();
- auto it_teb = tebs_.begin();
- while (it_teb != tebs_.end() && it_eqrel != equivalence_classes_.end())
- {
- if (it_teb->get() != best_teb_.get() // Always preserve the "best" teb
- && (random_() <= cfg_->hcp.selection_dropping_probability * random_.max()))
- {
- it_teb = tebs_.erase(it_teb);
- it_eqrel = equivalence_classes_.erase(it_eqrel);
- }
- else
- {
- ++it_teb;
- ++it_eqrel;
- }
- }
-}
-
-TebOptimalPlannerPtr HomotopyClassPlanner::selectBestTeb()
-{
- double min_cost = std::numeric_limits<double>::max(); // maximum cost
- double min_cost_last_best = std::numeric_limits<double>::max();
- double min_cost_initial_plan_teb = std::numeric_limits<double>::max();
- TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
-
- // check if last best_teb is still a valid candidate
- if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end())
- {
- // get cost of this candidate
- min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis
- last_best_teb_ = best_teb_;
- }
- else
- {
- last_best_teb_.reset();
- }
-
- if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB()
- {
- // get cost of this candidate
- min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis
- }
-
-
- best_teb_.reset(); // reset pointer
-
- for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- // check if the related TEB leaves the local costmap region
-// if (tebs_.size()>1 && !(*it_teb)->teb().isTrajectoryInsideRegion(20, -1, 1))
-// {
-// ROS_INFO("HomotopyClassPlanner::selectBestTeb(): skipping trajectories that are not inside the local costmap");
-// continue;
-// }
-
- double teb_cost;
-
- if (*it_teb == last_best_teb_)
- teb_cost = min_cost_last_best; // skip already known cost value of the last best_teb
- else if (*it_teb == initial_plan_teb)
- teb_cost = min_cost_initial_plan_teb;
- else
- teb_cost = it_teb->get()->getCurrentCost();
-
- if (teb_cost < min_cost)
- {
- // check if this candidate is currently not selected
- best_teb_ = *it_teb;
- min_cost = teb_cost;
- }
- }
-
-
- // in case we haven't found any teb due to some previous checks, investigate list again
-// if (!best_teb_ && !tebs_.empty())
-// {
-// RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "all " << tebs_.size() << " tebs rejected previously");
-// if (tebs_.size()==1)
-// best_teb_ = tebs_.front();
-// else // if multiple TEBs are available:
-// {
-// // try to use the one that relates to the initial plan
-// TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
-// if (initial_plan_teb)
-// best_teb_ = initial_plan_teb;
-// else
-// {
-// // now compute the cost for the rest (we haven't computed it before)
-// for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
-// {
-// double teb_cost = it_teb->get()->getCurrentCost();
-// if (teb_cost < min_cost)
-// {
-// // check if this candidate is currently not selected
-// best_teb_ = *it_teb;
-// min_cost = teb_cost;
-// }
-// }
-// }
-// }
-// }
-
- // check if we are allowed to change
- if (last_best_teb_ && best_teb_ != last_best_teb_)
- {
- rclcpp::Time now = node_->now();
- if ((now - last_eq_class_switching_time_).seconds() > cfg_->hcp.switching_blocking_period)
- {
- last_eq_class_switching_time_ = now;
- }
- else
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "HomotopyClassPlanner::selectBestTeb(): Switching equivalence classes blocked (check parameter switching_blocking_period.");
- // block switching, so revert best_teb_
- best_teb_ = last_best_teb_;
- }
-
- }
-
-
- return best_teb_;
-}
-
-int HomotopyClassPlanner::bestTebIdx() const
-{
- if (tebs_.size() == 1)
- return 0;
-
- if (!best_teb_)
- return -1;
-
- int idx = 0;
- for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb, ++idx)
- {
- if (*it_teb == best_teb_)
- return idx;
- }
- return -1;
-}
-
-bool HomotopyClassPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
- double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
-{
- bool feasible = false;
- while(rclcpp::ok() && !feasible && tebs_.size() > 0)
- {
- TebOptimalPlannerPtr best = findBestTeb();
- if (!best)
- {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Couldn't retrieve the best plan");
- return false;
- }
- feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx, feasibility_check_lookahead_distance);
- if(!feasible)
- {
- removeTeb(best);
- if(last_best_teb_ && (last_best_teb_ == best)) // Same plan as before.
- return feasible; // Not failing could result in oscillations between trajectories.
- }
- }
- return feasible;
-}
-
-TebOptimalPlannerPtr HomotopyClassPlanner::findBestTeb()
-{
- if(tebs_.empty())
- return TebOptimalPlannerPtr();
- if (!best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end())
- best_teb_ = selectBestTeb();
- return best_teb_;
-}
-
-TebOptPlannerContainer::iterator HomotopyClassPlanner::removeTeb(TebOptimalPlannerPtr& teb)
-{
- TebOptPlannerContainer::iterator return_iterator = tebs_.end();
- if(equivalence_classes_.size() != tebs_.size())
- {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "removeTeb: size of eq classes != size of tebs");
- return return_iterator;
- }
- auto it_eq_classes = equivalence_classes_.begin();
- for(auto it = tebs_.begin(); it != tebs_.end(); ++it)
- {
- if(*it == teb)
- {
- return_iterator = tebs_.erase(it);
- equivalence_classes_.erase(it_eq_classes);
- break;
- }
- ++it_eq_classes;
- }
- return return_iterator;
-}
-
-void HomotopyClassPlanner::setPreferredTurningDir(RotType dir)
-{
- // set preferred turning dir for all TEBs
- for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- (*it_teb)->setPreferredTurningDir(dir);
- }
-}
-
-bool HomotopyClassPlanner::hasDiverged() const
-{
- // Early return if there is no best trajectory initialized
- if (!best_teb_)
- return false;
-
- return best_teb_->hasDiverged();
-}
-
-void HomotopyClassPlanner::computeCurrentCost(std::vector<double>& cost, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
-{
- for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
- {
- it_teb->get()->computeCurrentCost(cost, obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
- }
-}
-
-void HomotopyClassPlanner::deletePlansDetouringBackwards(const double orient_threshold,
- const double len_orientation_vector)
-{
- if (tebs_.size() < 2 || !best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end() ||
- best_teb_->teb().sizePoses() < 2)
- {
- return; // A moving direction wasn't chosen yet
- }
- double current_movement_orientation;
- double best_plan_duration = std::max(best_teb_->teb().getSumOfAllTimeDiffs(), 1.0);
- if(!computeStartOrientation(best_teb_, len_orientation_vector, current_movement_orientation))
- return; // The plan is shorter than len_orientation_vector
- for(auto it_teb = tebs_.begin(); it_teb != tebs_.end();)
- {
- if(*it_teb == best_teb_) // The current plan should not be considered a detour
- {
- ++it_teb;
- continue;
- }
- if((*it_teb)->teb().sizePoses() < 2)
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Discarding a plan with less than 2 poses");
- it_teb = removeTeb(*it_teb);
- continue;
- }
- double plan_orientation;
- if(!computeStartOrientation(*it_teb, len_orientation_vector, plan_orientation))
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Failed to compute the start orientation for one of the tebs, likely close to the target");
- it_teb = removeTeb(*it_teb);
- continue;
- }
- if(fabs(g2o::normalize_theta(plan_orientation - current_movement_orientation)) > orient_threshold)
- {
- it_teb = removeTeb(*it_teb); // Plan detouring backwards
- continue;
- }
- if(!it_teb->get()->isOptimized())
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's not optimized");
- it_teb = removeTeb(*it_teb); // Deletes tebs that cannot be optimized (last optim call failed)
- continue;
- }
- if(it_teb->get()->teb().getSumOfAllTimeDiffs() / best_plan_duration > cfg_->hcp.max_ratio_detours_duration_best_duration)
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's duration is much longer than that of the best teb");
- it_teb = removeTeb(*it_teb);
- continue;
- }
- ++it_teb;
- }
-}
-
-bool HomotopyClassPlanner::computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector,
- double& orientation)
-{
- VertexPose start_pose = plan->teb().Pose(0);
- bool second_pose_found = false;
- Eigen::Vector2d start_vector;
- for(auto& pose : plan->teb().poses())
- {
- start_vector = start_pose.position() - pose->position();
- if(start_vector.norm() > len_orientation_vector)
- {
- second_pose_found = true;
- break;
- }
- }
- if(!second_pose_found) // The current plan is too short to make assumptions on the start orientation
- return false;
- orientation = std::atan2(start_vector[1], start_vector[0]);
- return true;
-}
-
-
-} // end namespace
diff --git a/src/teb_local_planner/src/obstacles.cpp b/src/teb_local_planner/src/obstacles.cpp
deleted file mode 100644
index 95253e3..0000000
--- a/src/teb_local_planner/src/obstacles.cpp
+++ /dev/null
@@ -1,216 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/obstacles.h"
-// #include "teb_local_planner/misc.h"
-#include <rclcpp/logging.hpp>
-#include <rclcpp/logger.hpp>
-
-namespace teb_local_planner
-{
-
-
-void PolygonObstacle::fixPolygonClosure()
-{
- if (vertices_.size()<2)
- return;
-
- if (vertices_.front().isApprox(vertices_.back()))
- vertices_.pop_back();
-}
-
-void PolygonObstacle::calcCentroid()
-{
- if (vertices_.empty())
- {
- centroid_.setConstant(NAN);
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs.");
- return;
- }
-
- // if polygon is a point
- if (noVertices()==1)
- {
- centroid_ = vertices_.front();
- return;
- }
-
- // if polygon is a line:
- if (noVertices()==2)
- {
- centroid_ = 0.5*(vertices_.front() + vertices_.back());
- return;
- }
-
- // otherwise:
-
- centroid_.setZero();
-
- // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon)
- double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i)
- for (int i=0; i < noVertices()-1; ++i)
- {
- A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1);
- }
- A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1);
- A *= 0.5;
-
- if (A!=0)
- {
- for (int i=0; i < noVertices()-1; ++i)
- {
- double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1));
- centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux;
- }
- double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1));
- centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux;
- centroid_ /= (6*A);
- }
- else // A == 0 -> all points are placed on a 'perfect' line
- {
- // seach for the two outer points of the line with the maximum distance inbetween
- int i_cand = 0;
- int j_cand = 0;
- double max_dist = 0;
- for (int i=0; i< noVertices(); ++i)
- {
- for (int j=i+1; j< noVertices(); ++j) // start with j=i+1
- {
- double dist = (vertices_[j] - vertices_[i]).norm();
- if (dist > max_dist)
- {
- max_dist = dist;
- i_cand = i;
- j_cand = j;
- }
- }
- }
- // calc centroid of that line
- centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]);
- }
-}
-
-
-
-Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const
-{
- // the polygon is a point
- if (noVertices() == 1)
- {
- return vertices_.front();
- }
-
- if (noVertices() > 1)
- {
-
- Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1));
-
- if (noVertices() > 2) // real polygon and not a line
- {
- double dist = (new_pt-position).norm();
- Eigen::Vector2d closest_pt = new_pt;
-
- // check each polygon edge
- for (int i=1; i<noVertices()-1; ++i) // skip the first one, since we already checked it (new_pt)
- {
- new_pt = closest_point_on_line_segment_2d(position, vertices_.at(i), vertices_.at(i+1));
- double new_dist = (new_pt-position).norm();
- if (new_dist < dist)
- {
- dist = new_dist;
- closest_pt = new_pt;
- }
- }
- // afterwards we check the edge between goal and start (close polygon)
- new_pt = closest_point_on_line_segment_2d(position, vertices_.back(), vertices_.front());
- double new_dist = (new_pt-position).norm();
- if (new_dist < dist)
- return new_pt;
- else
- return closest_pt;
- }
- else
- {
- return new_pt; // closest point on line segment
- }
- }
-
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "PolygonObstacle::getClosestPoint() cannot find any closest point. Polygon ill-defined?");
- return Eigen::Vector2d::Zero(); // todo: maybe boost::optional?
-}
-
-
-bool PolygonObstacle::checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist) const
-{
- // Simple strategy, check all edge-line intersections until an intersection is found...
- // check each polygon edge
- for (int i=0; i<noVertices()-1; ++i)
- {
- if ( check_line_segments_intersection_2d(line_start, line_end, vertices_.at(i), vertices_.at(i+1)) )
- return true;
- }
- if (noVertices()==2) // if polygon is a line
- return false;
-
- return check_line_segments_intersection_2d(line_start, line_end, vertices_.back(), vertices_.front()); //otherwise close polygon
-}
-
-
-
-// implements toPolygonMsg() of the base class
-void PolygonObstacle::toPolygonMsg(geometry_msgs::msg::Polygon& polygon)
-{
- polygon.points.resize(vertices_.size());
- for (std::size_t i=0; i<vertices_.size(); ++i)
- {
- polygon.points[i].x = vertices_[i].x();
- polygon.points[i].y = vertices_[i].y();
- polygon.points[i].z = 0;
- }
-}
-
-
-
-
-
-
-
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/optimal_planner.cpp b/src/teb_local_planner/src/optimal_planner.cpp
deleted file mode 100644
index 1ab883f..0000000
--- a/src/teb_local_planner/src/optimal_planner.cpp
+++ /dev/null
@@ -1,1326 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include <tf2/time.h>
-#include <tf2_ros/buffer_interface.h>
-
-#include "teb_local_planner/optimal_planner.h"
-
-#include <map>
-// g2o custom edges and vertices for the TEB planner
-#include <teb_local_planner/g2o_types/edge_velocity.h>
-#include <teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h>
-#include <teb_local_planner/g2o_types/edge_acceleration.h>
-#include <teb_local_planner/g2o_types/edge_kinematics.h>
-#include <teb_local_planner/g2o_types/edge_time_optimal.h>
-#include <teb_local_planner/g2o_types/edge_shortest_path.h>
-#include <teb_local_planner/g2o_types/edge_obstacle.h>
-#include <teb_local_planner/g2o_types/edge_dynamic_obstacle.h>
-#include <teb_local_planner/g2o_types/edge_via_point.h>
-#include <teb_local_planner/g2o_types/edge_prefer_rotdir.h>
-
-#include <memory>
-#include <limits>
-
-namespace teb_local_planner
-{
-
-// ============== Implementation ===================
-
-TebOptimalPlanner::TebOptimalPlanner() : cfg_(nullptr), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none),
- initialized_(false), optimized_(false)
-{
-}
-
-TebOptimalPlanner::TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points)
-{
- initialize(node, cfg, obstacles, visual, via_points);
-}
-
-TebOptimalPlanner::~TebOptimalPlanner()
-{
- clearGraph();
- // free dynamically allocated memory
- //if (optimizer_)
- // g2o::Factory::destroy();
- //g2o::OptimizationAlgorithmFactory::destroy();
- //g2o::HyperGraphActionLibrary::destroy();
-}
-
-void TebOptimalPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points)
-{
- node_ = node;
- // init optimizer (set solver and block ordering settings)
- optimizer_ = initOptimizer();
-
- cfg_ = &cfg;
- obstacles_ = obstacles;
- via_points_ = via_points;
- cost_ = HUGE_VAL;
- prefer_rotdir_ = RotType::none;
-
- vel_start_.first = true;
- vel_start_.second.linear.x = 0;
- vel_start_.second.linear.y = 0;
- vel_start_.second.angular.z = 0;
-
- vel_goal_.first = true;
- vel_goal_.second.linear.x = 0;
- vel_goal_.second.linear.y = 0;
- vel_goal_.second.angular.z = 0;
- initialized_ = true;
-
- setVisualization(visual);
-}
-
-void TebOptimalPlanner::setVisualization(const TebVisualizationPtr& visualization)
-{
- visualization_ = visualization;
-}
-
-void TebOptimalPlanner::visualize()
-{
- if (!visualization_)
- return;
-
- visualization_->publishLocalPlanAndPoses(teb_);
-
- if (teb_.sizePoses() > 0)
- visualization_->publishRobotFootprintModel(teb_.Pose(0), *cfg_->robot_model);
-
- if (cfg_->trajectory.publish_feedback)
- visualization_->publishFeedbackMessage(*this, *obstacles_);
-
-}
-
-/*
- * registers custom vertices and edges in g2o framework
- */
-void TebOptimalPlanner::registerG2OTypes()
-{
- g2o::Factory* factory = g2o::Factory::instance();
- factory->registerType("VERTEX_POSE", std::make_shared<g2o::HyperGraphElementCreator<VertexPose>>());
- factory->registerType("VERTEX_TIMEDIFF", std::make_shared<g2o::HyperGraphElementCreator<VertexTimeDiff>>());
- factory->registerType("EDGE_TIME_OPTIMAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeTimeOptimal>>());
- factory->registerType("EDGE_SHORTEST_PATH", std::make_shared<g2o::HyperGraphElementCreator<EdgeShortestPath>>());
- factory->registerType("EDGE_VELOCITY", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocity>>());
- factory->registerType("EDGE_VELOCITY_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>>());
- factory->registerType("EDGE_ACCELERATION", std::make_shared<g2o::HyperGraphElementCreator<EdgeAcceleration>>());
- factory->registerType("EDGE_ACCELERATION_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationStart>>());
- factory->registerType("EDGE_ACCELERATION_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationGoal>>());
- factory->registerType("EDGE_ACCELERATION_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>>());
- factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>>());
- factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>>());
- factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>>());
- factory->registerType("EDGE_KINEMATICS_CARLIKE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>>());
- factory->registerType("EDGE_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeObstacle>>());
- factory->registerType("EDGE_INFLATED_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeInflatedObstacle>>());
- factory->registerType("EDGE_DYNAMIC_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeDynamicObstacle>>());
- factory->registerType("EDGE_VIA_POINT", std::make_shared<g2o::HyperGraphElementCreator<EdgeViaPoint>>());
- factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
- return;
-}
-
-/*
- * initialize g2o optimizer. Set solver settings here.
- * Return: pointer to new SparseOptimizer Object.
- */
-std::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
-{
- // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
- static std::once_flag flag;
- std::call_once(flag, [this](){this->registerG2OTypes();});
-
- // allocating the optimizer
- std::shared_ptr<g2o::SparseOptimizer> optimizer = std::make_shared<g2o::SparseOptimizer>();
- auto linearSolver = std::make_unique<TEBLinearSolver>(); // see typedef in optimization.h
- linearSolver->setBlockOrdering(true);
- auto blockSolver = std::make_unique<TEBBlockSolver>(std::move(linearSolver));
- g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
-
- optimizer->setAlgorithm(solver);
-
- optimizer->initMultiThreading(); // required for >Eigen 3.1
-
- return optimizer;
-}
-
-
-bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,
- double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
-{
- if (cfg_->optim.optimization_activate==false)
- return false;
-
- bool success = false;
- optimized_ = false;
-
- double weight_multiplier = 1.0;
-
- // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles
- // (which leads to better results in terms of x-y-t homotopy planning).
- // however, we have not tested this mode intensively yet, so we keep
- // the legacy fast mode as default until we finish our tests.
- bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;
-
- for(int i=0; i<iterations_outerloop; ++i)
- {
- if (cfg_->trajectory.teb_autosize)
- {
- //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
- teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);
-
- }
-
- success = buildGraph(weight_multiplier);
- if (!success)
- {
- clearGraph();
- return false;
- }
- success = optimizeGraph(iterations_innerloop, false);
- if (!success)
- {
- clearGraph();
- return false;
- }
- optimized_ = true;
-
- if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
- computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
-
- clearGraph();
-
- weight_multiplier *= cfg_->optim.weight_adapt_factor;
- }
-
- return true;
-}
-
-void TebOptimalPlanner::setVelocityStart(const geometry_msgs::msg::Twist& vel_start)
-{
- vel_start_.first = true;
- vel_start_.second.linear.x = vel_start.linear.x;
- vel_start_.second.linear.y = vel_start.linear.y;
- vel_start_.second.angular.z = vel_start.angular.z;
-}
-
-void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal)
-{
- vel_goal_.first = true;
- vel_goal_.second = vel_goal;
-}
-
-bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-{
- TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
- if (!teb_.isInit())
- {
- teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
- cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
- }
- else // warm start
- {
- PoseSE2 start_(initial_plan.front().pose);
- PoseSE2 goal_(initial_plan.back().pose);
- if (teb_.sizePoses()>0
- && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
- && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
- teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
- else // goal too far away -> reinit
- {
- RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
- teb_.clearTimedElasticBand();
- teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,
- cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
- }
- }
- if (start_vel)
- setVelocityStart(*start_vel);
- if (free_goal_vel)
- setVelocityGoalFree();
- else
- vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
-
- // now optimize
- return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
-}
-
-
-//bool TebOptimalPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-//{
-// PoseSE2 start_(start);
-// PoseSE2 goal_(goal);
-// return plan(start_, goal_, start_vel);
-//}
-
-bool TebOptimalPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel)
-{
- TEB_ASSERT_MSG(initialized_, "Call initialize() first.");
- if (!teb_.isInit())
- {
- // init trajectory
- teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization
- }
- else // warm start
- {
- if (teb_.sizePoses() > 0
- && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist
- && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
- teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples);
- else // goal too far away -> reinit
- {
- RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
- teb_.clearTimedElasticBand();
- teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
- }
- }
- if (start_vel)
- setVelocityStart(*start_vel);
- if (free_goal_vel)
- setVelocityGoalFree();
- else
- vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
-
- // now optimize
- return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations, true);
-}
-
-
-bool TebOptimalPlanner::buildGraph(double weight_multiplier)
-{
- if (!optimizer_->edges().empty() || !optimizer_->vertices().empty())
- {
- RCLCPP_WARN(node_->get_logger(), "Cannot build graph, because it is not empty. Call graphClear()!");
- return false;
- }
-
- optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);
-
- // add TEB vertices
- AddTEBVertices();
-
- // add Edges (local cost functions)
- if (cfg_->obstacles.legacy_obstacle_association)
- AddEdgesObstaclesLegacy(weight_multiplier);
- else
- AddEdgesObstacles(weight_multiplier);
-
- if (cfg_->obstacles.include_dynamic_obstacles)
- AddEdgesDynamicObstacles();
-
- AddEdgesViaPoints();
-
- AddEdgesVelocity();
-
- AddEdgesAcceleration();
-
- AddEdgesTimeOptimal();
-
- AddEdgesShortestPath();
-
- if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
- AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
- else
- AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.
-
- AddEdgesPreferRotDir();
-
- if (cfg_->optim.weight_velocity_obstacle_ratio > 0)
- AddEdgesVelocityObstacleRatio();
-
- return true;
-}
-
-bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)
-{
- if (cfg_->robot.max_vel_x<0.01)
- {
- RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted...");
- if (clear_after) clearGraph();
- return false;
- }
-
- if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
- {
- RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
- if (clear_after) clearGraph();
- return false;
- }
-
- optimizer_->setVerbose(cfg_->optim.optimization_verbose);
- optimizer_->initializeOptimization();
-
- int iter = optimizer_->optimize(no_iterations);
-
- // Save Hessian for visualization
- // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*> (optimizer_->solver());
- // lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");
-
- if(!iter)
- {
- RCLCPP_ERROR(node_->get_logger(), "optimizeGraph(): Optimization failed! iter=%i", iter);
- return false;
- }
-
- if (clear_after) clearGraph();
-
- return true;
-}
-
-void TebOptimalPlanner::clearGraph()
-{
- // clear optimizer states
- if (optimizer_)
- {
- // we will delete all edges but keep the vertices.
- // before doing so, we will delete the link from the vertices to the edges.
- auto& vertices = optimizer_->vertices();
- for(auto& v : vertices)
- v.second->edges().clear();
-
- optimizer_->vertices().clear(); // necessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!)
- optimizer_->clear();
- }
-}
-
-
-
-void TebOptimalPlanner::AddTEBVertices()
-{
- // add vertices to graph
- RCLCPP_DEBUG_EXPRESSION(node_->get_logger(), cfg_->optim.optimization_verbose, "Adding TEB vertices ...");
- unsigned int id_counter = 0; // used for vertices ids
- obstacles_per_vertex_.resize(teb_.sizePoses());
- auto iter_obstacle = obstacles_per_vertex_.begin();
- for (int i=0; i<teb_.sizePoses(); ++i)
- {
- teb_.PoseVertex(i)->setId(id_counter++);
- optimizer_->addVertex(teb_.PoseVertex(i));
- if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs())
- {
- teb_.TimeDiffVertex(i)->setId(id_counter++);
- optimizer_->addVertex(teb_.TimeDiffVertex(i));
- }
- iter_obstacle->clear();
- (iter_obstacle++)->reserve(obstacles_->size());
- }
-}
-
-
-void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
-{
- if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr )
- return; // if weight equals zero skip adding edges!
-
-
- bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
-
- Eigen::Matrix<double,1,1> information;
- information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
-
- Eigen::Matrix<double,2,2> information_inflated;
- information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
- information_inflated(1,1) = cfg_->optim.weight_inflation;
- information_inflated(0,1) = information_inflated(1,0) = 0;
-
- auto iter_obstacle = obstacles_per_vertex_.begin();
-
- auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) {
- if (inflated)
- {
- EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
- dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
- dist_bandpt_obst->setInformation(information_inflated);
- dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle);
- optimizer_->addEdge(dist_bandpt_obst);
- }
- else
- {
- EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
- dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
- dist_bandpt_obst->setInformation(information);
- dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle);
- optimizer_->addEdge(dist_bandpt_obst);
- };
- };
-
- // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too
- const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0;
- for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i)
- {
- double left_min_dist = std::numeric_limits<double>::max();
- double right_min_dist = std::numeric_limits<double>::max();
- ObstaclePtr left_obstacle;
- ObstaclePtr right_obstacle;
-
- const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();
-
- // iterate obstacles
- for (const ObstaclePtr& obst : *obstacles_)
- {
- // we handle dynamic obstacles differently below
- if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
- continue;
-
- // calculate distance to robot model
- double dist = cfg_->robot_model->calculateDistance(teb_.Pose(i), obst.get());
-
- // force considering obstacle if really close to the current pose
- if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor)
- {
- iter_obstacle->push_back(obst);
- continue;
- }
- // cut-off distance
- if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)
- continue;
-
- // determine side (left or right) and assign obstacle if closer than the previous one
- if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
- {
- if (dist < left_min_dist)
- {
- left_min_dist = dist;
- left_obstacle = obst;
- }
- }
- else
- {
- if (dist < right_min_dist)
- {
- right_min_dist = dist;
- right_obstacle = obst;
- }
- }
- }
-
- if (left_obstacle)
- iter_obstacle->push_back(left_obstacle);
- if (right_obstacle)
- iter_obstacle->push_back(right_obstacle);
-
- // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
- if (i == 0)
- {
- ++iter_obstacle;
- continue;
- }
-
- // create obstacle edges
- for (const ObstaclePtr obst : *iter_obstacle)
- create_edge(i, obst.get());
- ++iter_obstacle;
- }
-}
-
-
-void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier)
-{
- if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr)
- return; // if weight equals zero skip adding edges!
-
- Eigen::Matrix<double,1,1> information;
- information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
-
- Eigen::Matrix<double,2,2> information_inflated;
- information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
- information_inflated(1,1) = cfg_->optim.weight_inflation;
- information_inflated(0,1) = information_inflated(1,0) = 0;
-
- bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;
-
- for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
- {
- if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below
- continue;
-
- int index;
-
- if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses())
- index = teb_.sizePoses() / 2;
- else
- index = teb_.findClosestTrajectoryPose(*(obst->get()));
-
-
- // check if obstacle is outside index-range between start and goal
- if ( (index <= 1) || (index > teb_.sizePoses()-2) ) // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range
- continue;
-
- if (inflated)
- {
- EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
- dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
- dist_bandpt_obst->setInformation(information_inflated);
- dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst);
- }
- else
- {
- EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
- dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
- dist_bandpt_obst->setInformation(information);
- dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst);
- }
-
- for (int neighbourIdx=0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected/2); neighbourIdx++)
- {
- if (index+neighbourIdx < teb_.sizePoses())
- {
- if (inflated)
- {
- EdgeInflatedObstacle* dist_bandpt_obst_n_r = new EdgeInflatedObstacle;
- dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx));
- dist_bandpt_obst_n_r->setInformation(information_inflated);
- dist_bandpt_obst_n_r->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst_n_r);
- }
- else
- {
- EdgeObstacle* dist_bandpt_obst_n_r = new EdgeObstacle;
- dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx));
- dist_bandpt_obst_n_r->setInformation(information);
- dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst_n_r);
- }
- }
- if ( index - neighbourIdx >= 0) // needs to be casted to int to allow negative values
- {
- if (inflated)
- {
- EdgeInflatedObstacle* dist_bandpt_obst_n_l = new EdgeInflatedObstacle;
- dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx));
- dist_bandpt_obst_n_l->setInformation(information_inflated);
- dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst_n_l);
- }
- else
- {
- EdgeObstacle* dist_bandpt_obst_n_l = new EdgeObstacle;
- dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx));
- dist_bandpt_obst_n_l->setInformation(information);
- dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
- optimizer_->addEdge(dist_bandpt_obst_n_l);
- }
- }
- }
-
- }
-}
-
-
-void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier)
-{
- if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==NULL )
- return; // if weight equals zero skip adding edges!
-
- Eigen::Matrix<double,2,2> information;
- information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
- information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation;
- information(0,1) = information(1,0) = 0;
-
- for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst)
- {
- if (!(*obst)->isDynamic())
- continue;
-
- // Skip first and last pose, as they are fixed
- double time = teb_.TimeDiff(0);
- for (int i=1; i < teb_.sizePoses() - 1; ++i)
- {
- EdgeDynamicObstacle* dynobst_edge = new EdgeDynamicObstacle(time);
- dynobst_edge->setVertex(0,teb_.PoseVertex(i));
- dynobst_edge->setInformation(information);
- dynobst_edge->setParameters(*cfg_, cfg_->robot_model.get(), obst->get());
- optimizer_->addEdge(dynobst_edge);
- time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1".
- }
- }
-}
-
-void TebOptimalPlanner::AddEdgesViaPoints()
-{
- if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() )
- return; // if weight equals zero skip adding edges!
-
- int start_pose_idx = 0;
-
- int n = teb_.sizePoses();
- if (n<3) // we do not have any degrees of freedom for reaching via-points
- return;
-
- for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it)
- {
-
- int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx);
- if (cfg_->trajectory.via_points_ordered)
- start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points
-
- // check if point conicides with goal or is located behind it
- if ( index > n-2 )
- index = n-2; // set to a pose before the goal, since we can move it away!
- // check if point coincides with start or is located before it
- if ( index < 1)
- {
- if (cfg_->trajectory.via_points_ordered)
- {
- index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later.
- }
- else
- {
- RCLCPP_DEBUG(node_->get_logger(), "TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
- continue; // skip via points really close or behind the current robot pose
- }
- }
- Eigen::Matrix<double,1,1> information;
- information.fill(cfg_->optim.weight_viapoint);
-
- EdgeViaPoint* edge_viapoint = new EdgeViaPoint;
- edge_viapoint->setVertex(0,teb_.PoseVertex(index));
- edge_viapoint->setInformation(information);
- edge_viapoint->setParameters(*cfg_, &(*vp_it));
- optimizer_->addEdge(edge_viapoint);
- }
-}
-
-void TebOptimalPlanner::AddEdgesVelocity()
-{
- if (cfg_->robot.max_vel_y == 0) // non-holonomic robot
- {
- if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0)
- return; // if weight equals zero skip adding edges!
-
- int n = teb_.sizePoses();
- Eigen::Matrix<double,2,2> information;
- information(0,0) = cfg_->optim.weight_max_vel_x;
- information(1,1) = cfg_->optim.weight_max_vel_theta;
- information(0,1) = 0.0;
- information(1,0) = 0.0;
-
- for (int i=0; i < n - 1; ++i)
- {
- EdgeVelocity* velocity_edge = new EdgeVelocity;
- velocity_edge->setVertex(0,teb_.PoseVertex(i));
- velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
- velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
- velocity_edge->setInformation(information);
- velocity_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(velocity_edge);
- }
- }
- else // holonomic-robot
- {
- if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_y==0 && cfg_->optim.weight_max_vel_theta==0)
- return; // if weight equals zero skip adding edges!
-
- int n = teb_.sizePoses();
- Eigen::Matrix<double,3,3> information;
- information.fill(0);
- information(0,0) = cfg_->optim.weight_max_vel_x;
- information(1,1) = cfg_->optim.weight_max_vel_y;
- information(2,2) = cfg_->optim.weight_max_vel_theta;
-
- for (int i=0; i < n - 1; ++i)
- {
- EdgeVelocityHolonomic* velocity_edge = new EdgeVelocityHolonomic;
- velocity_edge->setVertex(0,teb_.PoseVertex(i));
- velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
- velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
- velocity_edge->setInformation(information);
- velocity_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(velocity_edge);
- }
-
- }
-}
-
-void TebOptimalPlanner::AddEdgesAcceleration()
-{
- if (cfg_->optim.weight_acc_lim_x==0 && cfg_->optim.weight_acc_lim_theta==0)
- return; // if weight equals zero skip adding edges!
-
- int n = teb_.sizePoses();
-
- if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot
- {
- Eigen::Matrix<double,2,2> information;
- information.fill(0);
- information(0,0) = cfg_->optim.weight_acc_lim_x;
- information(1,1) = cfg_->optim.weight_acc_lim_theta;
-
- // check if an initial velocity should be taken into accound
- if (vel_start_.first)
- {
- EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
- acceleration_edge->setVertex(0,teb_.PoseVertex(0));
- acceleration_edge->setVertex(1,teb_.PoseVertex(1));
- acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
- acceleration_edge->setInitialVelocity(vel_start_.second);
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
-
- // now add the usual acceleration edge for each tuple of three teb poses
- for (int i=0; i < n - 2; ++i)
- {
- EdgeAcceleration* acceleration_edge = new EdgeAcceleration;
- acceleration_edge->setVertex(0,teb_.PoseVertex(i));
- acceleration_edge->setVertex(1,teb_.PoseVertex(i+1));
- acceleration_edge->setVertex(2,teb_.PoseVertex(i+2));
- acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i));
- acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1));
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
-
- // check if a goal velocity should be taken into accound
- if (vel_goal_.first)
- {
- EdgeAccelerationGoal* acceleration_edge = new EdgeAccelerationGoal;
- acceleration_edge->setVertex(0,teb_.PoseVertex(n-2));
- acceleration_edge->setVertex(1,teb_.PoseVertex(n-1));
- acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 ));
- acceleration_edge->setGoalVelocity(vel_goal_.second);
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
- }
- else // holonomic robot
- {
- Eigen::Matrix<double,3,3> information;
- information.fill(0);
- information(0,0) = cfg_->optim.weight_acc_lim_x;
- information(1,1) = cfg_->optim.weight_acc_lim_y;
- information(2,2) = cfg_->optim.weight_acc_lim_theta;
-
- // check if an initial velocity should be taken into accound
- if (vel_start_.first)
- {
- EdgeAccelerationHolonomicStart* acceleration_edge = new EdgeAccelerationHolonomicStart;
- acceleration_edge->setVertex(0,teb_.PoseVertex(0));
- acceleration_edge->setVertex(1,teb_.PoseVertex(1));
- acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
- acceleration_edge->setInitialVelocity(vel_start_.second);
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
-
- // now add the usual acceleration edge for each tuple of three teb poses
- for (int i=0; i < n - 2; ++i)
- {
- EdgeAccelerationHolonomic* acceleration_edge = new EdgeAccelerationHolonomic;
- acceleration_edge->setVertex(0,teb_.PoseVertex(i));
- acceleration_edge->setVertex(1,teb_.PoseVertex(i+1));
- acceleration_edge->setVertex(2,teb_.PoseVertex(i+2));
- acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i));
- acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1));
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
-
- // check if a goal velocity should be taken into accound
- if (vel_goal_.first)
- {
- EdgeAccelerationHolonomicGoal* acceleration_edge = new EdgeAccelerationHolonomicGoal;
- acceleration_edge->setVertex(0,teb_.PoseVertex(n-2));
- acceleration_edge->setVertex(1,teb_.PoseVertex(n-1));
- acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 ));
- acceleration_edge->setGoalVelocity(vel_goal_.second);
- acceleration_edge->setInformation(information);
- acceleration_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(acceleration_edge);
- }
- }
-}
-
-
-
-void TebOptimalPlanner::AddEdgesTimeOptimal()
-{
- if (cfg_->optim.weight_optimaltime==0)
- return; // if weight equals zero skip adding edges!
-
- Eigen::Matrix<double,1,1> information;
- information.fill(cfg_->optim.weight_optimaltime);
-
- for (int i=0; i < teb_.sizeTimeDiffs(); ++i)
- {
- EdgeTimeOptimal* timeoptimal_edge = new EdgeTimeOptimal;
- timeoptimal_edge->setVertex(0,teb_.TimeDiffVertex(i));
- timeoptimal_edge->setInformation(information);
- timeoptimal_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(timeoptimal_edge);
- }
-}
-
-void TebOptimalPlanner::AddEdgesShortestPath()
-{
- if (cfg_->optim.weight_shortest_path==0)
- return; // if weight equals zero skip adding edges!
-
- Eigen::Matrix<double,1,1> information;
- information.fill(cfg_->optim.weight_shortest_path);
-
- for (int i=0; i < teb_.sizePoses()-1; ++i)
- {
- EdgeShortestPath* shortest_path_edge = new EdgeShortestPath;
- shortest_path_edge->setVertex(0,teb_.PoseVertex(i));
- shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1));
- shortest_path_edge->setInformation(information);
- shortest_path_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(shortest_path_edge);
- }
-}
-
-
-
-void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
-{
- if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0)
- return; // if weight equals zero skip adding edges!
-
- // create edge for satisfiying kinematic constraints
- Eigen::Matrix<double,2,2> information_kinematics;
- information_kinematics.fill(0.0);
- information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
- information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive;
-
- for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only
- {
- EdgeKinematicsDiffDrive* kinematics_edge = new EdgeKinematicsDiffDrive;
- kinematics_edge->setVertex(0,teb_.PoseVertex(i));
- kinematics_edge->setVertex(1,teb_.PoseVertex(i+1));
- kinematics_edge->setInformation(information_kinematics);
- kinematics_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(kinematics_edge);
- }
-}
-
-void TebOptimalPlanner::AddEdgesKinematicsCarlike()
-{
- if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0)
- return; // if weight equals zero skip adding edges!
-
- // create edge for satisfiying kinematic constraints
- Eigen::Matrix<double,2,2> information_kinematics;
- information_kinematics.fill(0.0);
- information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh;
- information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius;
-
- for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only
- {
- EdgeKinematicsCarlike* kinematics_edge = new EdgeKinematicsCarlike;
- kinematics_edge->setVertex(0,teb_.PoseVertex(i));
- kinematics_edge->setVertex(1,teb_.PoseVertex(i+1));
- kinematics_edge->setInformation(information_kinematics);
- kinematics_edge->setTebConfig(*cfg_);
- optimizer_->addEdge(kinematics_edge);
- }
-}
-
-
-void TebOptimalPlanner::AddEdgesPreferRotDir()
-{
- //TODO(roesmann): Note, these edges can result in odd predictions, in particular
- // we can observe a substantional mismatch between open- and closed-loop planning
- // leading to a poor control performance.
- // At the moment, we keep these functionality for oscillation recovery:
- // Activating the edge for a short time period might not be crucial and
- // could move the robot to a new oscillation-free state.
- // This needs to be analyzed in more detail!
- if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir==0)
- return; // if weight equals zero skip adding edges!
-
- if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left)
- {
- RCLCPP_WARN(node_->get_logger(), "TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation.");
- return;
- }
-
- // create edge for satisfiying kinematic constraints
- Eigen::Matrix<double,1,1> information_rotdir;
- information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);
-
- for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) // currently: apply to first 3 rotations
- {
- EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir;
- rotdir_edge->setVertex(0,teb_.PoseVertex(i));
- rotdir_edge->setVertex(1,teb_.PoseVertex(i+1));
- rotdir_edge->setInformation(information_rotdir);
-
- if (prefer_rotdir_ == RotType::left)
- rotdir_edge->preferLeft();
- else if (prefer_rotdir_ == RotType::right)
- rotdir_edge->preferRight();
-
- optimizer_->addEdge(rotdir_edge);
- }
-}
-
-void TebOptimalPlanner::AddEdgesVelocityObstacleRatio()
-{
- Eigen::Matrix<double,2,2> information;
- information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio;
- information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio;
- information(0,1) = information(1,0) = 0;
-
- auto iter_obstacle = obstacles_per_vertex_.begin();
-
- for (int index = 0; index < teb_.sizePoses() - 1; ++index)
- {
- for (const ObstaclePtr obstacle : (*iter_obstacle++))
- {
- EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio;
- edge->setVertex(0,teb_.PoseVertex(index));
- edge->setVertex(1,teb_.PoseVertex(index + 1));
- edge->setVertex(2,teb_.TimeDiffVertex(index));
- edge->setInformation(information);
- edge->setParameters(*cfg_, cfg_->robot_model.get(), obstacle.get());
- optimizer_->addEdge(edge);
- }
- }
-}
-
-bool TebOptimalPlanner::hasDiverged() const
-{
- // Early returns if divergence detection is not active
- if (!cfg_->recovery.divergence_detection_enable)
- return false;
-
- auto stats_vector = optimizer_->batchStatistics();
-
- // No statistics yet
- if (stats_vector.empty())
- return false;
-
- // Grab the statistics of the final iteration
- const auto last_iter_stats = stats_vector.back();
-
- return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared;
-}
-
-void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
-{
- // check if graph is empty/exist -> important if function is called between buildGraph and optimizeGraph/clearGraph
- bool graph_exist_flag(false);
- if (optimizer_->edges().empty() && optimizer_->vertices().empty())
- {
- // here the graph is build again, for time efficiency make sure to call this function
- // between buildGraph and Optimize (deleted), but it depends on the application
- buildGraph();
- optimizer_->initializeOptimization();
- }
- else
- {
- graph_exist_flag = true;
- }
-
- optimizer_->computeInitialGuess();
-
- cost_ = 0;
-
- if (alternative_time_cost)
- {
- cost_ += teb_.getSumOfAllTimeDiffs();
- // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
- // since we are using an AutoResize Function with hysteresis.
- }
-
- // now we need pointers to all edges -> calculate error for each edge-type
- // since we aren't storing edge pointers, we need to check every edge
- for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++)
- {
- double cur_cost = (*it)->chi2();
-
- if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
- || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
- || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
- {
- cur_cost *= obst_cost_scale;
- }
- else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
- {
- cur_cost *= viapoint_cost_scale;
- }
- else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
- {
- continue; // skip these edges if alternative_time_cost is active
- }
- cost_ += cur_cost;
- }
-
- // delete temporary created graph
- if (!graph_exist_flag)
- clearGraph();
-}
-
-
-void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const
-{
- if (dt == 0)
- {
- vx = 0;
- vy = 0;
- omega = 0;
- return;
- }
-
- Eigen::Vector2d deltaS = pose2.position() - pose1.position();
-
- if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
- {
- Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
- // translational velocity
- double dir = deltaS.dot(conf1dir);
- vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
- vy = 0;
- }
- else // holonomic robot
- {
- // transform pose 2 into the current robot frame (pose1)
- // for velocities only the rotation of the direction vector is necessary.
- // (map->pose1-frame: inverse 2d rotation matrix)
- double cos_theta1 = std::cos(pose1.theta());
- double sin_theta1 = std::sin(pose1.theta());
- double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
- double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
- vx = p1_dx / dt;
- vy = p1_dy / dt;
- }
-
- // rotational velocity
- double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
- omega = orientdiff/dt;
-}
-
-bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
-{
- if (teb_.sizePoses()<2)
- {
- RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist.");
- vx = 0;
- vy = 0;
- omega = 0;
- return false;
- }
- look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
- double dt = 0.0;
- for(int counter = 0; counter < look_ahead_poses; ++counter)
- {
- dt += teb_.TimeDiff(counter);
- if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?
- {
- look_ahead_poses = counter + 1;
- break;
- }
- }
- if (dt<=0)
- {
- RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
- vx = 0;
- vy = 0;
- omega = 0;
- return false;
- }
-
- // Get velocity from the first two configurations
- extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
- return true;
-}
-
-void TebOptimalPlanner::getVelocityProfile(std::vector<geometry_msgs::msg::Twist>& velocity_profile) const
-{
- int n = teb_.sizePoses();
- velocity_profile.resize( n+1 );
-
- // start velocity
- velocity_profile.front().linear.z = 0;
- velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0;
- velocity_profile.front().linear.x = vel_start_.second.linear.x;
- velocity_profile.front().linear.y = vel_start_.second.linear.y;
- velocity_profile.front().angular.z = vel_start_.second.angular.z;
-
- for (int i=1; i<n; ++i)
- {
- velocity_profile[i].linear.z = 0;
- velocity_profile[i].angular.x = velocity_profile[i].angular.y = 0;
- extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), velocity_profile[i].linear.x, velocity_profile[i].linear.y, velocity_profile[i].angular.z);
- }
-
- // goal velocity
- velocity_profile.back().linear.z = 0;
- velocity_profile.back().angular.x = velocity_profile.back().angular.y = 0;
- velocity_profile.back().linear.x = vel_goal_.second.linear.x;
- velocity_profile.back().linear.y = vel_goal_.second.linear.y;
- velocity_profile.back().angular.z = vel_goal_.second.angular.z;
-}
-
-void TebOptimalPlanner::getFullTrajectory(std::vector<teb_msgs::msg::TrajectoryPointMsg>& trajectory) const
-{
- int n = teb_.sizePoses();
-
- trajectory.resize(n);
-
- if (n == 0)
- return;
-
- double curr_time = 0;
-
- // start
- teb_msgs::msg::TrajectoryPointMsg& start = trajectory.front();
- teb_.Pose(0).toPoseMsg(start.pose);
- start.velocity.linear.z = 0;
- start.velocity.angular.x = start.velocity.angular.y = 0;
- start.velocity.linear.x = vel_start_.second.linear.x;
- start.velocity.linear.y = vel_start_.second.linear.y;
- start.velocity.angular.z = vel_start_.second.angular.z;
- start.time_from_start = durationFromSec(curr_time);
-
- curr_time += teb_.TimeDiff(0);
-
- // intermediate points
- for (int i=1; i < n-1; ++i)
- {
- teb_msgs::msg::TrajectoryPointMsg& point = trajectory[i];
- teb_.Pose(i).toPoseMsg(point.pose);
- point.velocity.linear.z = 0;
- point.velocity.angular.x = point.velocity.angular.y = 0;
- double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2;
- extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), vel1_x, vel1_y, omega1);
- extractVelocity(teb_.Pose(i), teb_.Pose(i+1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2);
- point.velocity.linear.x = 0.5*(vel1_x+vel2_x);
- point.velocity.linear.y = 0.5*(vel1_y+vel2_y);
- point.velocity.angular.z = 0.5*(omega1+omega2);
- point.time_from_start = durationFromSec(curr_time);
-
- curr_time += teb_.TimeDiff(i);
- }
-
- // goal
- teb_msgs::msg::TrajectoryPointMsg& goal = trajectory.back();
- teb_.BackPose().toPoseMsg(goal.pose);
- goal.velocity.linear.z = 0;
- goal.velocity.angular.x = goal.velocity.angular.y = 0;
- goal.velocity.linear.x = vel_goal_.second.linear.x;
- goal.velocity.linear.y = vel_goal_.second.linear.y;
- goal.velocity.angular.z = vel_goal_.second.angular.z;
- goal.time_from_start = durationFromSec(curr_time);
-}
-
-
-bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
- double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
-{
- if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses())
- look_ahead_idx = teb().sizePoses() - 1;
-
- if (feasibility_check_lookahead_distance > 0){
- for (int i=1; i < teb().sizePoses(); ++i){
- double pose_distance=std::hypot(teb().Pose(i).x()-teb().Pose(0).x(), teb().Pose(i).y()-teb().Pose(0).y());
- if(pose_distance > feasibility_check_lookahead_distance){
- look_ahead_idx = i - 1;
- break;
- }
- }
- }
-
- geometry_msgs::msg::Pose2D pose2d;
- for (int i=0; i <= look_ahead_idx; ++i)
- {
- teb().Pose(i).toPoseMsg(pose2d);
- if (!isPoseValid(pose2d, costmap_model, footprint_spec)){
- if (visualization_)
- {
- visualization_->publishInfeasibleRobotPose(teb().Pose(i), *cfg_->robot_model);
- }
- return false;
- }
- // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold
- // and interpolates in that case.
- // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)!
- if (i<look_ahead_idx)
- {
- double delta_rot = g2o::normalize_theta(g2o::normalize_theta(teb().Pose(i+1).theta()) -
- g2o::normalize_theta(teb().Pose(i).theta()));
- Eigen::Vector2d delta_dist = teb().Pose(i+1).position()-teb().Pose(i).position();
- if(fabs(delta_rot) > cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius)
- {
- int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular),
- std::ceil(delta_dist.norm() / inscribed_radius)) - 1;
- PoseSE2 intermediate_pose = teb().Pose(i);
- for(int step = 0; step < n_additional_samples; ++step)
- {
- intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0);
- intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() +
- delta_rot / (n_additional_samples + 1.0));
- intermediate_pose.toPoseMsg(pose2d);
-
- if (!isPoseValid(pose2d, costmap_model, footprint_spec)){
- if (visualization_)
- {
- visualization_->publishInfeasibleRobotPose(intermediate_pose, *cfg_->robot_model);
- }
- return false;
- }
- }
- }
- }
- }
- return true;
-}
-
-bool TebOptimalPlanner::isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model,
- const std::vector<geometry_msgs::msg::Point>& footprint_spec)
-{
- try {
- if ( costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)) < 0 ) {
- return false;
- }
- } catch (...) {
- return false;
- }
- return true;
-}
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/recovery_behaviors.cpp b/src/teb_local_planner/src/recovery_behaviors.cpp
deleted file mode 100644
index eaeca56..0000000
--- a/src/teb_local_planner/src/recovery_behaviors.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/recovery_behaviors.h"
-#include <rclcpp/rclcpp.hpp>
-#include <limits>
-#include <functional>
-#include <numeric>
-#include <g2o/stuff/misc.h>
-
-namespace teb_local_planner
-{
-
-// ============== FailureDetector Implementation ===================
-
-void FailureDetector::update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
-{
- if (buffer_.capacity() == 0)
- return;
-
- VelMeasurement measurement;
- measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now
- measurement.omega = twist.angular.z;
-
- if (measurement.v > 0 && v_max>0)
- measurement.v /= v_max;
- else if (measurement.v < 0 && v_backwards_max > 0)
- measurement.v /= v_backwards_max;
-
- if (omega_max > 0)
- measurement.omega /= omega_max;
-
- buffer_.push_back(measurement);
-
- // immediately compute new state
- detect(v_eps, omega_eps);
-}
-
-void FailureDetector::clear()
-{
- buffer_.clear();
- oscillating_ = false;
-}
-
-bool FailureDetector::isOscillating() const
-{
- return oscillating_;
-}
-
-bool FailureDetector::detect(double v_eps, double omega_eps)
-{
- oscillating_ = false;
-
- if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half
- return false;
-
- double n = (double)buffer_.size();
-
- // compute mean for v and omega
- double v_mean=0;
- double omega_mean=0;
- int omega_zero_crossings = 0;
- for (int i=0; i < n; ++i)
- {
- v_mean += buffer_[i].v;
- omega_mean += buffer_[i].omega;
- if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
- ++omega_zero_crossings;
- }
- v_mean /= n;
- omega_mean /= n;
-
- if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 )
- {
- oscillating_ = true;
- }
-// ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings);
- return oscillating_;
-}
-
-
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/teb_config.cpp b/src/teb_local_planner/src/teb_config.cpp
deleted file mode 100644
index 8123efd..0000000
--- a/src/teb_local_planner/src/teb_config.cpp
+++ /dev/null
@@ -1,886 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/teb_config.h"
-
-using nav2_util::declare_parameter_if_not_declared;
-
-namespace teb_local_planner
-{
-
-void TebConfig::declareParameters(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) {
- node_name = name;
-
- declare_parameter_if_not_declared(nh, name + "." + "odom_topic", rclcpp::ParameterValue(odom_topic));
-
- // Trajectory
- declare_parameter_if_not_declared(nh, name + "." + "teb_autosize", rclcpp::ParameterValue(trajectory.teb_autosize));
- declare_parameter_if_not_declared(nh, name + "." + "dt_ref", rclcpp::ParameterValue(trajectory.dt_ref));
- declare_parameter_if_not_declared(nh, name + "." + "dt_hysteresis", rclcpp::ParameterValue(trajectory.dt_hysteresis));
- declare_parameter_if_not_declared(nh, name + "." + "min_samples", rclcpp::ParameterValue(trajectory.min_samples));
- declare_parameter_if_not_declared(nh, name + "." + "max_samples", rclcpp::ParameterValue(trajectory.max_samples));
- declare_parameter_if_not_declared(nh, name + "." + "global_plan_overwrite_orientation", rclcpp::ParameterValue(trajectory.global_plan_overwrite_orientation));
- declare_parameter_if_not_declared(nh, name + "." + "allow_init_with_backwards_motion", rclcpp::ParameterValue(trajectory.allow_init_with_backwards_motion));
- declare_parameter_if_not_declared(nh, name + "." + "global_plan_viapoint_sep", rclcpp::ParameterValue(trajectory.global_plan_viapoint_sep));
- declare_parameter_if_not_declared(nh, name + "." + "via_points_ordered", rclcpp::ParameterValue(trajectory.via_points_ordered));
- declare_parameter_if_not_declared(nh, name + "." + "max_global_plan_lookahead_dist", rclcpp::ParameterValue(trajectory.max_global_plan_lookahead_dist));
- declare_parameter_if_not_declared(nh, name + "." + "global_plan_prune_distance", rclcpp::ParameterValue(trajectory.global_plan_prune_distance));
- declare_parameter_if_not_declared(nh, name + "." + "exact_arc_length", rclcpp::ParameterValue(trajectory.exact_arc_length));
- declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_dist", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_dist));
- declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_angular", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_angular));
- declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_no_poses", rclcpp::ParameterValue(trajectory.feasibility_check_no_poses));
- declare_parameter_if_not_declared(nh, name + "." + "publish_feedback", rclcpp::ParameterValue(trajectory.publish_feedback));
- declare_parameter_if_not_declared(nh, name + "." + "min_resolution_collision_check_angular", rclcpp::ParameterValue(trajectory.min_resolution_collision_check_angular));
- declare_parameter_if_not_declared(nh, name + "." + "control_look_ahead_poses", rclcpp::ParameterValue(trajectory.control_look_ahead_poses));
- declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_lookahead_distance", rclcpp::ParameterValue(trajectory.feasibility_check_lookahead_distance));
-
- // Robot
- declare_parameter_if_not_declared(nh, name + "." + "max_vel_x", rclcpp::ParameterValue(robot.max_vel_x));
- declare_parameter_if_not_declared(nh, name + "." + "max_vel_x_backwards", rclcpp::ParameterValue(robot.max_vel_x_backwards));
- declare_parameter_if_not_declared(nh, name + "." + "max_vel_y", rclcpp::ParameterValue(robot.max_vel_y));
- declare_parameter_if_not_declared(nh, name + "." + "max_vel_theta", rclcpp::ParameterValue(robot.max_vel_theta));
- declare_parameter_if_not_declared(nh, name + "." + "acc_lim_x", rclcpp::ParameterValue(robot.acc_lim_x));
- declare_parameter_if_not_declared(nh, name + "." + "acc_lim_y", rclcpp::ParameterValue(robot.acc_lim_y));
- declare_parameter_if_not_declared(nh, name + "." + "acc_lim_theta", rclcpp::ParameterValue(robot.acc_lim_theta));
- declare_parameter_if_not_declared(nh, name + "." + "min_turning_radius", rclcpp::ParameterValue(robot.min_turning_radius));
- declare_parameter_if_not_declared(nh, name + "." + "wheelbase", rclcpp::ParameterValue(robot.wheelbase));
- declare_parameter_if_not_declared(nh, name + "." + "cmd_angle_instead_rotvel", rclcpp::ParameterValue(robot.cmd_angle_instead_rotvel));
- declare_parameter_if_not_declared(nh, name + "." + "is_footprint_dynamic", rclcpp::ParameterValue(robot.is_footprint_dynamic));
-
- // GoalTolerance
- declare_parameter_if_not_declared(nh, name + "." + "free_goal_vel", rclcpp::ParameterValue(goal_tolerance.free_goal_vel));
-
- // Obstacles
- declare_parameter_if_not_declared(nh, name + "." + "min_obstacle_dist", rclcpp::ParameterValue(obstacles.min_obstacle_dist));
- declare_parameter_if_not_declared(nh, name + "." + "inflation_dist", rclcpp::ParameterValue(obstacles.inflation_dist));
- declare_parameter_if_not_declared(nh, name + "." + "dynamic_obstacle_inflation_dist", rclcpp::ParameterValue(obstacles.dynamic_obstacle_inflation_dist));
- declare_parameter_if_not_declared(nh, name + "." + "include_dynamic_obstacles", rclcpp::ParameterValue(obstacles.include_dynamic_obstacles));
- declare_parameter_if_not_declared(nh, name + "." + "include_costmap_obstacles", rclcpp::ParameterValue(obstacles.include_costmap_obstacles));
- declare_parameter_if_not_declared(nh, name + "." + "costmap_obstacles_behind_robot_dist", rclcpp::ParameterValue(obstacles.costmap_obstacles_behind_robot_dist));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_poses_affected", rclcpp::ParameterValue(obstacles.obstacle_poses_affected));
- declare_parameter_if_not_declared(nh, name + "." + "legacy_obstacle_association", rclcpp::ParameterValue(obstacles.legacy_obstacle_association));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_force_inclusion_factor", rclcpp::ParameterValue(obstacles.obstacle_association_force_inclusion_factor));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_cutoff_factor", rclcpp::ParameterValue(obstacles.obstacle_association_cutoff_factor));
- declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_plugin", rclcpp::ParameterValue(obstacles.costmap_converter_plugin));
- declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_spin_thread", rclcpp::ParameterValue(obstacles.costmap_converter_spin_thread));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_ratio_max_vel", rclcpp::ParameterValue(obstacles.obstacle_proximity_ratio_max_vel));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_lower_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_lower_bound));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_upper_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_upper_bound));
-
- // Optimization
- declare_parameter_if_not_declared(nh, name + "." + "no_inner_iterations", rclcpp::ParameterValue(optim.no_inner_iterations));
- declare_parameter_if_not_declared(nh, name + "." + "no_outer_iterations", rclcpp::ParameterValue(optim.no_outer_iterations));
- declare_parameter_if_not_declared(nh, name + "." + "optimization_activate", rclcpp::ParameterValue(optim.optimization_activate));
- declare_parameter_if_not_declared(nh, name + "." + "optimization_verbose", rclcpp::ParameterValue(optim.optimization_verbose));
- declare_parameter_if_not_declared(nh, name + "." + "penalty_epsilon", rclcpp::ParameterValue(optim.penalty_epsilon));
- declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_x", rclcpp::ParameterValue(optim.weight_max_vel_x));
- declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_y", rclcpp::ParameterValue(optim.weight_max_vel_y));
- declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_theta", rclcpp::ParameterValue(optim.weight_max_vel_theta));
- declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_x", rclcpp::ParameterValue(optim.weight_acc_lim_x));
- declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_y", rclcpp::ParameterValue(optim.weight_acc_lim_y));
- declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_theta", rclcpp::ParameterValue(optim.weight_acc_lim_theta));
- declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_nh", rclcpp::ParameterValue(optim.weight_kinematics_nh));
- declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_forward_drive", rclcpp::ParameterValue(optim.weight_kinematics_forward_drive));
- declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_turning_radius", rclcpp::ParameterValue(optim.weight_kinematics_turning_radius));
- declare_parameter_if_not_declared(nh, name + "." + "weight_optimaltime", rclcpp::ParameterValue(optim.weight_optimaltime));
- declare_parameter_if_not_declared(nh, name + "." + "weight_shortest_path", rclcpp::ParameterValue(optim.weight_shortest_path));
- declare_parameter_if_not_declared(nh, name + "." + "weight_obstacle", rclcpp::ParameterValue(optim.weight_obstacle));
- declare_parameter_if_not_declared(nh, name + "." + "weight_inflation", rclcpp::ParameterValue(optim.weight_inflation));
- declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle", rclcpp::ParameterValue(optim.weight_dynamic_obstacle));
- declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle_inflation", rclcpp::ParameterValue(optim.weight_dynamic_obstacle_inflation));
- declare_parameter_if_not_declared(nh, name + "." + "weight_viapoint", rclcpp::ParameterValue(optim.weight_viapoint));
- declare_parameter_if_not_declared(nh, name + "." + "weight_prefer_rotdir", rclcpp::ParameterValue(optim.weight_prefer_rotdir));
- declare_parameter_if_not_declared(nh, name + "." + "weight_adapt_factor", rclcpp::ParameterValue(optim.weight_adapt_factor));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_cost_exponent", rclcpp::ParameterValue(optim.obstacle_cost_exponent));
- declare_parameter_if_not_declared(nh, name + "." + "weight_velocity_obstacle_ratio", rclcpp::ParameterValue(optim.weight_velocity_obstacle_ratio));
-
- // Homotopy Class Planner
- declare_parameter_if_not_declared(nh, name + "." + "enable_homotopy_class_planning", rclcpp::ParameterValue(hcp.enable_homotopy_class_planning));
- declare_parameter_if_not_declared(nh, name + "." + "enable_multithreading", rclcpp::ParameterValue(hcp.enable_multithreading));
- declare_parameter_if_not_declared(nh, name + "." + "simple_exploration", rclcpp::ParameterValue(hcp.simple_exploration));
- declare_parameter_if_not_declared(nh, name + "." + "max_number_classes", rclcpp::ParameterValue(hcp.max_number_classes));
- declare_parameter_if_not_declared(nh, name + "." + "selection_obst_cost_scale", rclcpp::ParameterValue(hcp.selection_obst_cost_scale));
- declare_parameter_if_not_declared(nh, name + "." + "selection_prefer_initial_plan", rclcpp::ParameterValue(hcp.selection_prefer_initial_plan));
- declare_parameter_if_not_declared(nh, name + "." + "selection_viapoint_cost_scale", rclcpp::ParameterValue(hcp.selection_viapoint_cost_scale));
- declare_parameter_if_not_declared(nh, name + "." + "selection_cost_hysteresis", rclcpp::ParameterValue(hcp.selection_cost_hysteresis));
- declare_parameter_if_not_declared(nh, name + "." + "selection_alternative_time_cost", rclcpp::ParameterValue(hcp.selection_alternative_time_cost));
- declare_parameter_if_not_declared(nh, name + "." + "switching_blocking_period", rclcpp::ParameterValue(hcp.switching_blocking_period));
- declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_samples", rclcpp::ParameterValue(hcp.roadmap_graph_no_samples));
- declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_width", rclcpp::ParameterValue(hcp.roadmap_graph_area_width));
- declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_length_scale", rclcpp::ParameterValue(hcp.roadmap_graph_area_length_scale));
- declare_parameter_if_not_declared(nh, name + "." + "h_signature_prescaler", rclcpp::ParameterValue(hcp.h_signature_prescaler));
- declare_parameter_if_not_declared(nh, name + "." + "h_signature_threshold", rclcpp::ParameterValue(hcp.h_signature_threshold));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_keypoint_offset", rclcpp::ParameterValue(hcp.obstacle_keypoint_offset));
- declare_parameter_if_not_declared(nh, name + "." + "obstacle_heading_threshold", rclcpp::ParameterValue(hcp.obstacle_heading_threshold));
- declare_parameter_if_not_declared(nh, name + "." + "viapoints_all_candidates", rclcpp::ParameterValue(hcp.viapoints_all_candidates));
- declare_parameter_if_not_declared(nh, name + "." + "visualize_hc_graph", rclcpp::ParameterValue(hcp.visualize_hc_graph));
- declare_parameter_if_not_declared(nh, name + "." + "visualize_with_time_as_z_axis_scale", rclcpp::ParameterValue(hcp.visualize_with_time_as_z_axis_scale));
- declare_parameter_if_not_declared(nh, name + "." + "delete_detours_backwards", rclcpp::ParameterValue(hcp.delete_detours_backwards));
- declare_parameter_if_not_declared(nh, name + "." + "detours_orientation_tolerance", rclcpp::ParameterValue(hcp.detours_orientation_tolerance));
- declare_parameter_if_not_declared(nh, name + "." + "length_start_orientation_vector", rclcpp::ParameterValue(hcp.length_start_orientation_vector));
- declare_parameter_if_not_declared(nh, name + "." + "max_ratio_detours_duration_best_duration", rclcpp::ParameterValue(hcp.max_ratio_detours_duration_best_duration));
- declare_parameter_if_not_declared(nh, name + "." + "selection_dropping_probability", rclcpp::ParameterValue(hcp.selection_dropping_probability));
-
- // Recovery
- declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_backup", rclcpp::ParameterValue(recovery.shrink_horizon_backup));
- declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_min_duration", rclcpp::ParameterValue(recovery.shrink_horizon_min_duration));
- declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery", rclcpp::ParameterValue(recovery.oscillation_recovery));
- declare_parameter_if_not_declared(nh, name + "." + "oscillation_v_eps", rclcpp::ParameterValue(recovery.oscillation_v_eps));
- declare_parameter_if_not_declared(nh, name + "." + "oscillation_omega_eps", rclcpp::ParameterValue(recovery.oscillation_omega_eps));
- declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery_min_duration", rclcpp::ParameterValue(recovery.oscillation_recovery_min_duration));
- declare_parameter_if_not_declared(nh, name + "." + "oscillation_filter_duration", rclcpp::ParameterValue(recovery.oscillation_filter_duration));
- declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_enable", rclcpp::ParameterValue(recovery.divergence_detection_enable));
- declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_max_chi_squared", rclcpp::ParameterValue(recovery.divergence_detection_max_chi_squared));
-
- // footprint model
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.type", rclcpp::ParameterType::PARAMETER_STRING);
-}
-
-void TebConfig::loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name)
-{
- nh->get_parameter_or(name + "." + "odom_topic", odom_topic, odom_topic);
-
- // Trajectory
- nh->get_parameter_or(name + "." + "teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize);
- nh->get_parameter_or(name + "." + "dt_ref", trajectory.dt_ref, trajectory.dt_ref);
- nh->get_parameter_or(name + "." + "dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis);
- nh->get_parameter_or(name + "." + "min_samples", trajectory.min_samples, trajectory.min_samples);
- nh->get_parameter_or(name + "." + "max_samples", trajectory.max_samples, trajectory.max_samples);
- nh->get_parameter_or(name + "." + "global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation);
- nh->get_parameter_or(name + "." + "allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion);
- nh->get_parameter_or(name + "." + "global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep);
- nh->get_parameter_or(name + "." + "via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered);
- nh->get_parameter_or(name + "." + "max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist);
- nh->get_parameter_or(name + "." + "global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance);
- nh->get_parameter_or(name + "." + "exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length);
- nh->get_parameter_or(name + "." + "force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist);
- nh->get_parameter_or(name + "." + "force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular);
- nh->get_parameter_or(name + "." + "feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses);
- nh->get_parameter_or(name + "." + "publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
- nh->get_parameter_or(name + "." + "min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular);
- nh->get_parameter_or(name + "." + "control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses);
- nh->get_parameter_or(name + "." + "feasibility_check_lookahead_distance", trajectory.feasibility_check_lookahead_distance, trajectory.feasibility_check_lookahead_distance);
-
- // Robot
- nh->get_parameter_or(name + "." + "max_vel_x", robot.max_vel_x, robot.max_vel_x);
- nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
- nh->get_parameter_or(name + "." + "max_vel_y", robot.max_vel_y, robot.max_vel_y);
- nh->get_parameter_or(name + "." + "max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
- nh->get_parameter_or(name + "." + "acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
- nh->get_parameter_or(name + "." + "acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
- nh->get_parameter_or(name + "." + "acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
- nh->get_parameter_or(name + "." + "min_turning_radius", robot.min_turning_radius, robot.min_turning_radius);
- nh->get_parameter_or(name + "." + "wheelbase", robot.wheelbase, robot.wheelbase);
- nh->get_parameter_or(name + "." + "cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel);
- nh->get_parameter_or(name + "." + "is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic);
-
- // GoalTolerance
- nh->get_parameter_or(name + "." + "free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel);
-
- // Obstacles
- nh->get_parameter_or(name + "." + "min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist);
- nh->get_parameter_or(name + "." + "inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist);
- nh->get_parameter_or(name + "." + "dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist);
- nh->get_parameter_or(name + "." + "include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles);
- nh->get_parameter_or(name + "." + "include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles);
- nh->get_parameter_or(name + "." + "costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist);
- nh->get_parameter_or(name + "." + "obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected);
- nh->get_parameter_or(name + "." + "legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association);
- nh->get_parameter_or(name + "." + "obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, obstacles.obstacle_association_force_inclusion_factor);
- nh->get_parameter_or(name + "." + "obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor);
- nh->get_parameter_or(name + "." + "costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin);
- nh->get_parameter_or(name + "." + "costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread);
- nh->get_parameter_or(name + "." + "obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel);
- nh->get_parameter_or(name + "." + "obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound);
- nh->get_parameter_or(name + "." + "obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound);
-
- // Optimization
- nh->get_parameter_or(name + "." + "no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations);
- nh->get_parameter_or(name + "." + "no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations);
- nh->get_parameter_or(name + "." + "optimization_activate", optim.optimization_activate, optim.optimization_activate);
- nh->get_parameter_or(name + "." + "optimization_verbose", optim.optimization_verbose, optim.optimization_verbose);
- nh->get_parameter_or(name + "." + "penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon);
- nh->get_parameter_or(name + "." + "weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x);
- nh->get_parameter_or(name + "." + "weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y);
- nh->get_parameter_or(name + "." + "weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta);
- nh->get_parameter_or(name + "." + "weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x);
- nh->get_parameter_or(name + "." + "weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y);
- nh->get_parameter_or(name + "." + "weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta);
- nh->get_parameter_or(name + "." + "weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh);
- nh->get_parameter_or(name + "." + "weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive);
- nh->get_parameter_or(name + "." + "weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius);
- nh->get_parameter_or(name + "." + "weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime);
- nh->get_parameter_or(name + "." + "weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path);
- nh->get_parameter_or(name + "." + "weight_obstacle", optim.weight_obstacle, optim.weight_obstacle);
- nh->get_parameter_or(name + "." + "weight_inflation", optim.weight_inflation, optim.weight_inflation);
- nh->get_parameter_or(name + "." + "weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);
- nh->get_parameter_or(name + "." + "weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation);
- nh->get_parameter_or(name + "." + "weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
- nh->get_parameter_or(name + "." + "weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
- nh->get_parameter_or(name + "." + "weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
- nh->get_parameter_or(name + "." + "obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent);
- nh->get_parameter_or(name + "." + "weight_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio);
-
- // Homotopy Class Planner
- nh->get_parameter_or(name + "." + "enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning);
- nh->get_parameter_or(name + "." + "enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading);
- nh->get_parameter_or(name + "." + "simple_exploration", hcp.simple_exploration, hcp.simple_exploration);
- nh->get_parameter_or(name + "." + "max_number_classes", hcp.max_number_classes, hcp.max_number_classes);
- nh->get_parameter_or(name + "." + "selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale);
- nh->get_parameter_or(name + "." + "selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan);
- nh->get_parameter_or(name + "." + "selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale);
- nh->get_parameter_or(name + "." + "selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis);
- nh->get_parameter_or(name + "." + "selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost);
- nh->get_parameter_or(name + "." + "switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period);
- nh->get_parameter_or(name + "." + "roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples);
- nh->get_parameter_or(name + "." + "roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width);
- nh->get_parameter_or(name + "." + "roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale);
- nh->get_parameter_or(name + "." + "h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler);
- nh->get_parameter_or(name + "." + "h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold);
- nh->get_parameter_or(name + "." + "obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset);
- nh->get_parameter_or(name + "." + "obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold);
- nh->get_parameter_or(name + "." + "viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates);
- nh->get_parameter_or(name + "." + "visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph);
- nh->get_parameter_or(name + "." + "visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale);
- nh->get_parameter_or(name + "." + "delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards);
- nh->get_parameter_or(name + "." + "detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance);
- nh->get_parameter_or(name + "." + "length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector);
- nh->get_parameter_or(name + "." + "max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration);
- nh->get_parameter_or(name + "." + "selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability);
-
- // Recovery
- nh->get_parameter_or(name + "." + "shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup);
- nh->get_parameter_or(name + "." + "shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration);
- nh->get_parameter_or(name + "." + "oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery);
- nh->get_parameter_or(name + "." + "oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps);
- nh->get_parameter_or(name + "." + "oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps);
- nh->get_parameter_or(name + "." + "oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration);
- nh->get_parameter_or(name + "." + "oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration);
- nh->get_parameter_or(name + "." + "divergence_detection_enable", recovery.divergence_detection_enable, recovery.divergence_detection_enable);
- nh->get_parameter_or(name + "." + "divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared);
-
- // footprint model
- if (!nh->get_parameter(name + "." + "footprint_model.type", model_name))
- {
- RCLCPP_INFO(logger_, "No robot footprint model specified for trajectory optimization. Using point-shaped model.");
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
- // point
- else if (model_name.compare("point") == 0)
- {
- RCLCPP_INFO(logger_, "Footprint model 'point' loaded for trajectory optimization.");
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
- // circular
- else if (model_name.compare("circular") == 0)
- {
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
- // get radius
- double radius;
- if (!nh->get_parameter(name + "." + "footprint_model.radius", radius))
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'circular' cannot be loaded for trajectory optimization, since param '%s.footprint_model.radius' does not exist. Using point-model instead.",
- nh->get_namespace());
-
- robot_model = std::make_shared<PointRobotFootprint>();
- }
- RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) loaded for trajectory optimization.", radius);
- robot_model = std::make_shared<CircularRobotFootprint>(radius);
- }
-
-
- // line
- else if (model_name.compare("line") == 0)
- {
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_start", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY);
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_end", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY);
- std::vector<double> line_start, line_end;
- // check parameters
- if (!nh->get_parameter(name + "." + "footprint_model.line_start", line_start) || !nh->get_parameter(name + "." + "footprint_model.line_end", line_end))
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not exist. Using point-model instead.",
- nh->get_namespace());
- robot_model = std::make_shared<PointRobotFootprint>();
- }
- if (line_start.size() != 2 || line_end.size() != 2)
- {
- RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead.",
- nh->get_namespace());
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
- RCLCPP_INFO(logger_,
- "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) loaded for trajectory optimization.",
- line_start[0], line_start[1], line_end[0], line_end[1]);
-
- robot_model = std::make_shared<LineRobotFootprint>(Eigen::Map<const Eigen::Vector2d>(line_start.data()), Eigen::Map<const Eigen::Vector2d>(line_end.data()));
- }
-
- // two circles
- else if (model_name.compare("two_circles") == 0)
- {
- rclcpp::Parameter dummy;
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_offset", rclcpp::ParameterType::PARAMETER_DOUBLE);
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_offset", rclcpp::ParameterType::PARAMETER_DOUBLE);
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_radius", rclcpp::ParameterType::PARAMETER_DOUBLE);
- // check parameters
- if (!nh->get_parameter(name + "." + "footprint_model.front_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.front_radius", dummy)
- || !nh->get_parameter(name + "." + "footprint_model.rear_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.rear_radius", dummy))
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '%s.footprint_model.front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using point-model instead.",
- nh->get_namespace());
- robot_model = std::make_shared<PointRobotFootprint>();
- }
- double front_offset, front_radius, rear_offset, rear_radius;
- nh->get_parameter(name + "." + "footprint_model.front_offset", front_offset);
- nh->get_parameter(name + "." + "footprint_model.front_radius", front_radius);
- nh->get_parameter(name + "." + "footprint_model.rear_offset", rear_offset);
- nh->get_parameter(name + "." + "footprint_model.rear_radius", rear_radius);
- RCLCPP_INFO(logger_,
- "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.",
- front_offset, front_radius, rear_offset, rear_radius);
-
- robot_model = std::make_shared<TwoCirclesRobotFootprint>(front_offset, front_radius, rear_offset, rear_radius);
- }
-
- // polygon
- else if (model_name.compare("polygon") == 0)
- {
- declare_parameter_if_not_declared(nh, name + "." + "footprint_model.vertices", rclcpp::ParameterType::PARAMETER_STRING);
- // check parameters
- std::string footprint_string;
- if (!nh->get_parameter(name + "." + "footprint_model.vertices", footprint_string) )
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not exist. Using point-model instead.",
- nh->get_namespace());
-
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
- std::vector<geometry_msgs::msg::Point> footprint;
- // get vertices
- if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint))
- {
- Point2dContainer polygon;
- for(const auto &pt : footprint) {
- polygon.push_back(Eigen::Vector2d(pt.x, pt.y));
- }
- RCLCPP_INFO(logger_, "Footprint model 'polygon' loaded for trajectory optimization.");
- robot_model = std::make_shared<PolygonRobotFootprint>(polygon);
- }
- else
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not define an array of coordinates. Using point-model instead.",
- nh->get_namespace());
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
- }
- // otherwise
- else
- {
- RCLCPP_WARN(logger_, "Unknown robot footprint model specified with parameter '%s.footprint_model.type'. Using point model instead.",
- nh->get_namespace());
- robot_model = std::make_shared<PointRobotFootprint>();
- }
-
-
- checkParameters();
- checkDeprecated(nh, name);
-}
-
-rcl_interfaces::msg::SetParametersResult
- TebConfig::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
-{
- auto result = rcl_interfaces::msg::SetParametersResult();
- std::lock_guard<std::mutex> l(config_mutex_);
-
- bool reload_footprint = false;
-
- for (auto parameter : parameters) {
- const auto & type = parameter.get_type();
- const auto & name = parameter.get_name();
-
- if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
- // Trajectory
- if (name == node_name + ".teb_autosize") {
- trajectory.teb_autosize = parameter.as_double();
- } else if (name == node_name + ".dt_ref") {
- trajectory.dt_ref = parameter.as_double();
- } else if (name == node_name + ".dt_hysteresis") {
- trajectory.dt_hysteresis = parameter.as_double();
- } else if (name == node_name + ".global_plan_viapoint_sep") {
- trajectory.global_plan_viapoint_sep = parameter.as_double();
- } else if (name == node_name + ".max_global_plan_lookahead_dist") {
- trajectory.max_global_plan_lookahead_dist = parameter.as_double();
- } else if (name == node_name + ".global_plan_prune_distance") {
- trajectory.global_plan_prune_distance = parameter.as_double();
- } else if (name == node_name + ".force_reinit_new_goal_dist") {
- trajectory.force_reinit_new_goal_dist = parameter.as_double();
- } else if (name == node_name + ".force_reinit_new_goal_angular") {
- trajectory.force_reinit_new_goal_angular = parameter.as_double();
- } else if (name == node_name + ".min_resolution_collision_check_angular") {
- trajectory.min_resolution_collision_check_angular = parameter.as_double();
- } else if (name == node_name + ".feasibility_check_lookahead_distance") {
- trajectory.feasibility_check_lookahead_distance = parameter.as_double();
- }
- // Robot
- else if (name == node_name + ".max_vel_x") {
- robot.max_vel_x = parameter.as_double();
- robot.base_max_vel_x = parameter.as_double();
- } else if (name == node_name + ".max_vel_x_backwards") {
- robot.max_vel_x_backwards = parameter.as_double();
- robot.base_max_vel_x_backwards = parameter.as_double();
- } else if (name == node_name + ".max_vel_y") {
- robot.max_vel_y = parameter.as_double();
- robot.base_max_vel_y = parameter.as_double();
- } else if (name == node_name + ".max_vel_theta") {
- robot.max_vel_theta = parameter.as_double();
- robot.base_max_vel_theta = parameter.as_double();
- } else if (name == node_name + ".acc_lim_x") {
- robot.acc_lim_x = parameter.as_double();
- } else if (name == node_name + ".acc_lim_y") {
- robot.acc_lim_y = parameter.as_double();
- } else if (name == node_name + ".acc_lim_theta") {
- robot.acc_lim_theta = parameter.as_double();
- } else if (name == node_name + ".min_turning_radius") {
- robot.min_turning_radius = parameter.as_double();
- } else if (name == node_name + ".wheelbase") {
- robot.wheelbase = parameter.as_double();
- }
- // GoalTolerance
- // Obstacles
- else if (name == node_name + ".min_obstacle_dist") {
- obstacles.min_obstacle_dist = parameter.as_double();
- } else if (name == node_name + ".inflation_dist") {
- obstacles.inflation_dist = parameter.as_double();
- } else if (name == node_name + ".dynamic_obstacle_inflation_dist") {
- obstacles.dynamic_obstacle_inflation_dist = parameter.as_double();
- } else if (name == node_name + ".costmap_obstacles_behind_robot_dist") {
- obstacles.costmap_obstacles_behind_robot_dist = parameter.as_double();
- } else if (name == node_name + ".obstacle_association_force_inclusion_factor") {
- obstacles.obstacle_association_force_inclusion_factor = parameter.as_double();
- } else if (name == node_name + ".obstacle_association_cutoff_factor") {
- obstacles.obstacle_association_cutoff_factor = parameter.as_double();
- } else if (name == node_name + ".obstacle_proximity_ratio_max_vel") {
- obstacles.obstacle_proximity_ratio_max_vel = parameter.as_double();
- } else if (name == node_name + ".obstacle_proximity_lower_bound") {
- obstacles.obstacle_proximity_lower_bound = parameter.as_double();
- } else if (name == node_name + ".obstacle_proximity_upper_bound") {
- obstacles.obstacle_proximity_upper_bound = parameter.as_double();
- }
- // Optimization
- else if (name == node_name + ".penalty_epsilon") {
- optim.penalty_epsilon = parameter.as_double();
- } else if (name == node_name + ".weight_max_vel_x") {
- optim.weight_max_vel_x = parameter.as_double();
- } else if (name == node_name + ".weight_max_vel_y") {
- optim.weight_max_vel_y = parameter.as_double();
- } else if (name == node_name + ".weight_max_vel_theta") {
- optim.weight_max_vel_theta = parameter.as_double();
- } else if (name == node_name + ".weight_acc_lim_x") {
- optim.weight_acc_lim_x = parameter.as_double();
- } else if (name == node_name + ".weight_acc_lim_y") {
- optim.weight_acc_lim_y = parameter.as_double();
- } else if (name == node_name + ".weight_acc_lim_theta") {
- optim.weight_acc_lim_theta = parameter.as_double();
- } else if (name == node_name + ".weight_kinematics_nh") {
- optim.weight_kinematics_nh = parameter.as_double();
- } else if (name == node_name + ".weight_kinematics_forward_drive") {
- optim.weight_kinematics_forward_drive = parameter.as_double();
- } else if (name == node_name + ".weight_kinematics_turning_radius") {
- optim.weight_kinematics_turning_radius = parameter.as_double();
- } else if (name == node_name + ".weight_optimaltime") {
- optim.weight_optimaltime = parameter.as_double();
- } else if (name == node_name + ".weight_shortest_path") {
- optim.weight_shortest_path = parameter.as_double();
- } else if (name == node_name + ".weight_obstacle") {
- optim.weight_obstacle = parameter.as_double();
- } else if (name == node_name + ".weight_inflation") {
- optim.weight_inflation = parameter.as_double();
- } else if (name == node_name + ".weight_dynamic_obstacle") {
- optim.weight_dynamic_obstacle = parameter.as_double();
- } else if (name == node_name + ".weight_dynamic_obstacle_inflation") {
- optim.weight_dynamic_obstacle_inflation = parameter.as_double();
- } else if (name == node_name + ".weight_viapoint") {
- optim.weight_viapoint = parameter.as_double();
- } else if (name == node_name + ".weight_prefer_rotdir") {
- optim.weight_prefer_rotdir = parameter.as_double();
- } else if (name == node_name + ".weight_adapt_factor") {
- optim.weight_adapt_factor = parameter.as_double();
- } else if (name == node_name + ".obstacle_cost_exponent") {
- optim.obstacle_cost_exponent = parameter.as_double();
- }
- // Homotopy Class Planner
- else if (name == node_name + ".selection_cost_hysteresis") {
- hcp.selection_cost_hysteresis = parameter.as_double();
- } else if (name == node_name + ".selection_prefer_initial_plan") {
- hcp.selection_prefer_initial_plan = parameter.as_double();
- } else if (name == node_name + ".selection_obst_cost_scale") {
- hcp.selection_obst_cost_scale = parameter.as_double();
- } else if (name == node_name + ".selection_viapoint_cost_scale") {
- hcp.selection_viapoint_cost_scale = parameter.as_double();
- } else if (name == node_name + ".switching_blocking_period") {
- hcp.switching_blocking_period = parameter.as_double();
- } else if (name == node_name + ".roadmap_graph_area_width") {
- hcp.roadmap_graph_area_width = parameter.as_double();
- } else if (name == node_name + ".roadmap_graph_area_length_scale") {
- hcp.roadmap_graph_area_length_scale = parameter.as_double();
- } else if (name == node_name + ".h_signature_prescaler") {
- hcp.h_signature_prescaler = parameter.as_double();
- } else if (name == node_name + ".h_signature_threshold") {
- hcp.h_signature_threshold = parameter.as_double();
- } else if (name == node_name + ".obstacle_keypoint_offset") {
- hcp.obstacle_keypoint_offset = parameter.as_double();
- } else if (name == node_name + ".obstacle_heading_threshold") {
- hcp.obstacle_heading_threshold = parameter.as_double();
- } else if (name == node_name + ".visualize_with_time_as_z_axis_scale") {
- hcp.visualize_with_time_as_z_axis_scale = parameter.as_double();
- } else if (name == node_name + ".detours_orientation_tolerance") {
- hcp.detours_orientation_tolerance = parameter.as_double();
- } else if (name == node_name + ".length_start_orientation_vector") {
- hcp.length_start_orientation_vector = parameter.as_double();
- } else if (name == node_name + ".max_ratio_detours_duration_best_duration") {
- hcp.max_ratio_detours_duration_best_duration = parameter.as_double();
- } else if (name == node_name + ".selection_dropping_probability") {
- hcp.selection_dropping_probability = parameter.as_double();
- }
- // Recovery
- else if (name == node_name + ".shrink_horizon_min_duration") {
- recovery.shrink_horizon_min_duration = parameter.as_double();
- } else if (name == node_name + ".oscillation_v_eps") {
- recovery.oscillation_v_eps = parameter.as_double();
- } else if (name == node_name + ".oscillation_omega_eps") {
- recovery.oscillation_omega_eps = parameter.as_double();
- } else if (name == node_name + ".oscillation_recovery_min_duration") {
- recovery.oscillation_recovery_min_duration = parameter.as_double();
- } else if (name == node_name + ".oscillation_filter_duration") {
- recovery.oscillation_filter_duration = parameter.as_double();
- } else if (name == node_name + ".divergence_detection_max_chi_squared") {
- recovery.divergence_detection_max_chi_squared = parameter.as_double();
- }
- // Footprint model
- else if (name == node_name + ".footprint_model.radius") {
- reload_footprint = true;
- radius = parameter.as_double();
- } else if (name == node_name + ".footprint_model.front_offset") {
- reload_footprint = true;
- front_offset = parameter.as_double();
- } else if (name == node_name + ".footprint_model.front_radius") {
- reload_footprint = true;
- front_radius = parameter.as_double();
- } else if (name == node_name + ".footprint_model.rear_offset") {
- reload_footprint = true;
- rear_offset = parameter.as_double();
- } else if (name == node_name + ".footprint_model.rear_radius") {
- reload_footprint = true;
- rear_radius = parameter.as_double();
- }
- }
-
- else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) {
- if (name == node_name + ".footprint_model.line_start") {
- reload_footprint = true;
- line_start = parameter.as_double_array();
- } else if (name == node_name + ".footprint_model.line_end") {
- reload_footprint = true;
- line_end = parameter.as_double_array();
- }
- }
-
- else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
- // Trajectory
- if (name == node_name + ".min_samples") {
- trajectory.min_samples = parameter.as_int();
- } else if (name == node_name + ".max_samples") {
- trajectory.max_samples = parameter.as_int();
- } else if (name == node_name + ".feasibility_check_no_poses") {
- trajectory.feasibility_check_no_poses = parameter.as_int();
- } else if (name == node_name + ".control_look_ahead_poses") {
- trajectory.control_look_ahead_poses = parameter.as_int();
- }
- // Robot
- // GoalTolerance
- // Obstacles
- else if (name == node_name + ".obstacle_poses_affected") {
- obstacles.obstacle_poses_affected = parameter.as_int();
- } else if (name == node_name + ".costmap_converter_rate") {
- obstacles.costmap_converter_rate = parameter.as_int();
- }
- // Optimization
- else if (name == node_name + ".no_inner_iterations") {
- optim.no_inner_iterations = parameter.as_int();
- } else if (name == node_name + ".no_outer_iterations") {
- optim.no_outer_iterations = parameter.as_int();
- }
- // Homotopy Class Planner
- else if (name == node_name + ".max_number_classes") {
- hcp.max_number_classes = parameter.as_int();
- } else if (name == node_name + ".roadmap_graph_no_samples") {
- hcp.roadmap_graph_no_samples = parameter.as_int();
- }
- // Recovery
- }
-
- else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
- // Trajectory
- if (name == node_name + ".global_plan_overwrite_orientation") {
- trajectory.global_plan_overwrite_orientation = parameter.as_bool();
- } else if (name == node_name + ".allow_init_with_backwards_motion") {
- trajectory.allow_init_with_backwards_motion = parameter.as_bool();
- } else if (name == node_name + ".via_points_ordered") {
- trajectory.via_points_ordered = parameter.as_bool();
- } else if (name == node_name + ".exact_arc_length") {
- trajectory.exact_arc_length = parameter.as_bool();
- } else if (name == node_name + ".publish_feedback") {
- trajectory.publish_feedback = parameter.as_bool();
- }
- // Robot
- else if (name == node_name + ".cmd_angle_instead_rotvel") {
- robot.cmd_angle_instead_rotvel = parameter.as_bool();
- } else if (name == node_name + ".is_footprint_dynamic") {
- robot.is_footprint_dynamic = parameter.as_bool();
- }
- // GoalTolerance
- else if (name == node_name + ".free_goal_vel") {
- goal_tolerance.free_goal_vel = parameter.as_bool();
- }
- // Obstacles
- else if (name == node_name + ".include_dynamic_obstacles") {
- obstacles.include_dynamic_obstacles = parameter.as_bool();
- } else if (name == node_name + ".include_costmap_obstacles") {
- obstacles.include_costmap_obstacles = parameter.as_bool();
- } else if (name == node_name + ".legacy_obstacle_association") {
- obstacles.legacy_obstacle_association = parameter.as_bool();
- } else if (name == node_name + ".costmap_converter_spin_thread") {
- obstacles.costmap_converter_spin_thread = parameter.as_bool();
- }
- // Optimization
- else if (name == node_name + ".optimization_activate") {
- optim.optimization_activate = parameter.as_bool();
- } else if (name == node_name + ".optimization_verbose") {
- optim.optimization_verbose = parameter.as_bool();
- }
- // Homotopy Class Planner
- else if (name == node_name + ".enable_homotopy_class_planning") {
- hcp.enable_homotopy_class_planning = parameter.as_bool();
- } else if (name == node_name + ".enable_multithreading") {
- hcp.enable_multithreading = parameter.as_bool();
- } else if (name == node_name + ".simple_exploration") {
- hcp.simple_exploration = parameter.as_bool();
- } else if (name == node_name + ".selection_alternative_time_cost") {
- hcp.selection_alternative_time_cost = parameter.as_bool();
- } else if (name == node_name + ".viapoints_all_candidates") {
- hcp.viapoints_all_candidates = parameter.as_bool();
- } else if (name == node_name + ".visualize_hc_graph") {
- hcp.visualize_hc_graph = parameter.as_bool();
- } else if (name == node_name + ".delete_detours_backwards") {
- hcp.delete_detours_backwards = parameter.as_bool();
- }
- // Recovery
- else if (name == node_name + ".shrink_horizon_backup") {
- recovery.shrink_horizon_backup = parameter.as_bool();
- } else if (name == node_name + ".oscillation_recovery") {
- recovery.oscillation_recovery = parameter.as_bool();
- } else if (name == node_name + ".divergence_detection_enable") {
- recovery.divergence_detection_enable = parameter.as_bool();
- }
- }
-
- else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
- // Trajectory
- // Robot
- // GoalTolerance
- // Obstacles
- if (name == node_name + ".costmap_converter_plugin") {
- obstacles.costmap_converter_plugin = parameter.as_string();
- }
- // Optimization
- // Homotopy Class Planner
- // Recovery
- // Footprint model
- else if (name == node_name + ".footprint_model.type") {
- RCLCPP_WARN(logger_, "Changing footprint model type is not allowed at runtime");
- } else if (name == node_name + ".footprint_model.vertices") {
- reload_footprint = true;
- footprint_string = parameter.as_string();
- }
- }
- }
- checkParameters();
-
- if (reload_footprint) {
- if (model_name.compare("circular") == 0)
- {
- RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) reloaded for trajectory optimization.", radius);
- robot_model = std::make_shared<CircularRobotFootprint>(radius);
- }
- else if (model_name.compare("line") == 0) {
- if (line_start.size() != 2 || line_end.size() != 2)
- {
- RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be reloaded for trajectory optimization");
- } else {
- RCLCPP_INFO(logger_,
- "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) reloaded for trajectory optimization.",
- line_start[0], line_start[1], line_end[0], line_end[1]);
-
- robot_model = std::make_shared<LineRobotFootprint>(Eigen::Map<const Eigen::Vector2d>(line_start.data()), Eigen::Map<const Eigen::Vector2d>(line_end.data()));
- }
- }
- else if (model_name.compare("two_circles") == 0) {
- RCLCPP_INFO(logger_,
- "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.",
- front_offset, front_radius, rear_offset, rear_radius);
-
- robot_model = std::make_shared<TwoCirclesRobotFootprint>(front_offset, front_radius, rear_offset, rear_radius);
- }
- else if (model_name.compare("polygon") == 0)
- {
- std::vector<geometry_msgs::msg::Point> footprint;
- // get vertices
- if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint))
- {
- Point2dContainer polygon;
- for(const auto &pt : footprint) {
- polygon.push_back(Eigen::Vector2d(pt.x, pt.y));
- }
- RCLCPP_INFO(logger_, "Footprint model 'polygon' reloaded for trajectory optimization.");
- robot_model = std::make_shared<PolygonRobotFootprint>(polygon);
- }
- else
- {
- RCLCPP_ERROR(logger_,
- "Footprint model 'polygon' cannot be reloaded for trajectory optimization, since param 'footprint_model.vertices' does not define an array of coordinates.");
- }
- }
- }
- result.successful = true;
- return result;
-}
-
-
-void TebConfig::checkParameters() const
-{
- //rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
- // positive backward velocity?
- if (robot.max_vel_x_backwards <= 0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
-
- // bounds smaller than penalty epsilon
- if (robot.max_vel_x <= optim.penalty_epsilon)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
-
- if (robot.max_vel_x_backwards <= optim.penalty_epsilon)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
-
- if (robot.max_vel_theta <= optim.penalty_epsilon)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
-
- if (robot.acc_lim_x <= optim.penalty_epsilon)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
-
- if (robot.acc_lim_theta <= optim.penalty_epsilon)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
-
- // dt_ref and dt_hyst
- if (trajectory.dt_ref <= trajectory.dt_hysteresis)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!");
-
- // min number of samples
- if (trajectory.min_samples <3)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ...");
-
- // costmap obstacle behind robot
- if (obstacles.costmap_obstacles_behind_robot_dist < 0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero.");
-
- // hcp: obstacle heading threshold
- if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle.");
-
- // carlike
- if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior.");
-
- if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot");
-
- // positive weight_adapt_factor
- if (optim.weight_adapt_factor < 1.0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0");
-
- if (recovery.oscillation_filter_duration < 0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0");
-
- if (optim.weight_optimaltime <= 0)
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)");
-}
-
-void TebConfig::checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const
-{
- rclcpp::Parameter dummy;
-
- if (nh->get_parameter(name + "." + "line_obstacle_poses_affected", dummy) || nh->get_parameter(name + "." + "polygon_obstacle_poses_affected", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'.");
-
- if (nh->get_parameter(name + "." + "weight_point_obstacle", dummy) || nh->get_parameter(name + "." + "weight_line_obstacle", dummy) || nh->get_parameter(name + "." + "weight_poly_obstacle", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'.");
-
- if (nh->get_parameter(name + "." + "costmap_obstacles_front_only", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account.");
-
- if (nh->get_parameter(name + "." + "costmap_emergency_stop_dist", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config.");
-
- if (nh->get_parameter(name + "." + "alternative_time_cost", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'.");
-
- if (nh->get_parameter(name + "." + "global_plan_via_point_sep", dummy))
- RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons.");
-}
-
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/teb_local_planner_ros.cpp b/src/teb_local_planner/src/teb_local_planner_ros.cpp
deleted file mode 100644
index 5d90794..0000000
--- a/src/teb_local_planner/src/teb_local_planner_ros.cpp
+++ /dev/null
@@ -1,1122 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/teb_local_planner_ros.h"
-
-//#include <tf_conversions/tf_eigen.h>
-#include <boost/algorithm/string.hpp>
-
-#include <string>
-
-// pluginlib macros
-#include <pluginlib/class_list_macros.hpp>
-
-#include "g2o/core/sparse_optimizer.h"
-#include "g2o/core/block_solver.h"
-#include "g2o/core/factory.h"
-#include "g2o/core/optimization_algorithm_gauss_newton.h"
-#include "g2o/core/optimization_algorithm_levenberg.h"
-#include "g2o/solvers/csparse/linear_solver_csparse.h"
-#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
-
-#include <nav2_core/planner_exceptions.hpp>
-#include <nav2_costmap_2d/footprint.hpp>
-#include <nav_2d_utils/tf_help.hpp>
-
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_eigen/tf2_eigen.hpp>
-
-using nav2_util::declare_parameter_if_not_declared;
-
-namespace teb_local_planner
-{
-
-
-TebLocalPlannerROS::TebLocalPlannerROS()
- : costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
- costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
- custom_via_points_active_(false), no_infeasible_plans_(0),
- last_preferred_rotdir_(RotType::none), initialized_(false)
-{
-}
-
-
-TebLocalPlannerROS::~TebLocalPlannerROS()
-{
-}
-
-void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node)
-{
- // check if the plugin is already initialized
- if(!initialized_)
- {
- // declare parameters (ros2-dashing)
- intra_proc_node_.reset(
- new rclcpp::Node("costmap_converter", node->get_namespace(),
- rclcpp::NodeOptions()));
- cfg_->declareParameters(node, name_);
-
- // get parameters of TebConfig via the nodehandle and override the default config
- cfg_->loadRosParamFromNodeHandle(node, name_);
-
- // reserve some memory for obstacles
- obstacles_.reserve(500);
-
- // create the planner instance
- if (cfg_->hcp.enable_homotopy_class_planning)
- {
- planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_));
- RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies enabled.");
- }
- else
- {
- planner_ = PlannerInterfacePtr(new TebOptimalPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_));
- RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies disabled.");
- }
-
- // init other variables
- costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase.
-
- costmap_model_ = std::make_shared<dwb_critics::ObstacleFootprintCritic>();
- std::string costmap_model_name("costmap_model");
- costmap_model_->initialize(node, costmap_model_name, name_, costmap_ros_);
-
- cfg_->map_frame = costmap_ros_->getGlobalFrameID(); // TODO
-
- //Initialize a costmap to polygon converter
- if (!cfg_->obstacles.costmap_converter_plugin.empty())
- {
- try
- {
- costmap_converter_ = costmap_converter_loader_.createSharedInstance(cfg_->obstacles.costmap_converter_plugin);
- std::string converter_name = costmap_converter_loader_.getName(cfg_->obstacles.costmap_converter_plugin);
- RCLCPP_INFO(logger_, "library path : %s", costmap_converter_loader_.getClassLibraryPath(cfg_->obstacles.costmap_converter_plugin).c_str());
- // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
- boost::replace_all(converter_name, "::", "/");
-
- costmap_converter_->setOdomTopic(cfg_->odom_topic);
- costmap_converter_->initialize(intra_proc_node_);
- costmap_converter_->setCostmap2D(costmap_);
- const auto rate = std::make_shared<rclcpp::Rate>((double)cfg_->obstacles.costmap_converter_rate);
- costmap_converter_->startWorker(rate, costmap_, cfg_->obstacles.costmap_converter_spin_thread);
- RCLCPP_INFO(logger_, "Costmap conversion plugin %s loaded.", cfg_->obstacles.costmap_converter_plugin.c_str());
- }
- catch(pluginlib::PluginlibException& ex)
- {
- RCLCPP_INFO(logger_,
- "The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what());
- costmap_converter_.reset();
- }
- }
- else {
- RCLCPP_INFO(logger_, "No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
- }
-
-
- // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
- footprint_spec_ = costmap_ros_->getRobotFootprint();
- nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
-
- // Add callback for dynamic parameters
- dyn_params_handler = node->add_on_set_parameters_callback(
- std::bind(&TebConfig::dynamicParametersCallback, std::ref(cfg_), std::placeholders::_1));
-
- // validate optimization footprint and costmap footprint
- validateFootprints(cfg_->robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_->obstacles.min_obstacle_dist);
-
- // setup callback for custom obstacles
- custom_obst_sub_ = node->create_subscription<costmap_converter_msgs::msg::ObstacleArrayMsg>(
- "obstacles",
- rclcpp::SystemDefaultsQoS(),
- std::bind(&TebLocalPlannerROS::customObstacleCB, this, std::placeholders::_1));
-
- // setup callback for custom via-points
- via_points_sub_ = node->create_subscription<nav_msgs::msg::Path>(
- "via_points",
- rclcpp::SystemDefaultsQoS(),
- std::bind(&TebLocalPlannerROS::customViaPointsCB, this, std::placeholders::_1));
-
- // initialize failure detector
- //rclcpp::Node::SharedPtr nh_move_base("~");
- double controller_frequency = 5;
- node->get_parameter("controller_frequency", controller_frequency);
- failure_detector_.setBufferLength(std::round(cfg_->recovery.oscillation_filter_duration*controller_frequency));
-
- // set initialized flag
- initialized_ = true;
-
- // This should be called since to prevent different time sources exception
- time_last_infeasible_plan_ = clock_->now();
- time_last_oscillation_ = clock_->now();
- RCLCPP_DEBUG(logger_, "teb_local_planner plugin initialized.");
- }
- else
- {
- RCLCPP_INFO(logger_, "teb_local_planner has already been initialized, doing nothing.");
- }
-}
-
-void TebLocalPlannerROS::configure(
- const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
- std::string name,
- std::shared_ptr<tf2_ros::Buffer> tf,
- std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
- nh_ = parent;
-
- auto node = nh_.lock();
- logger_ = node->get_logger();
- clock_ = node->get_clock();
-
- costmap_ros_ = costmap_ros;
- tf_ = tf;
- name_ = name;
-
- initialize(node);
- visualization_ = std::make_shared<TebVisualization>(node, *cfg_);
- visualization_->on_configure();
- planner_->setVisualization(visualization_);
-
- return;
-}
-
-void TebLocalPlannerROS::setPlan(const nav_msgs::msg::Path & orig_global_plan)
-{
- // check if plugin is initialized
- if(!initialized_)
- {
- RCLCPP_ERROR(logger_, "teb_local_planner has not been initialized, please call initialize() before using this planner");
- return;
- }
-
- // store the global plan
- global_plan_.clear();
- global_plan_.reserve(orig_global_plan.poses.size());
- for(const auto &in_pose :orig_global_plan.poses) {
- geometry_msgs::msg::PoseStamped out_pose;
- out_pose.pose = in_pose.pose;
- out_pose.header = orig_global_plan.header;
- global_plan_.push_back(out_pose);
- }
-
- // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan.
- // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step.
-
- return;
-}
-
-
-geometry_msgs::msg::TwistStamped TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
- const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker)
-{
- // check if plugin initialized
- if(!initialized_)
- {
- throw nav2_core::PlannerException(
- std::string("teb_local_planner has not been initialized, please call initialize() before using this planner")
- );
- }
- geometry_msgs::msg::TwistStamped cmd_vel;
-
- cmd_vel.header.stamp = clock_->now();
- cmd_vel.header.frame_id = costmap_ros_->getBaseFrameID();
- cmd_vel.twist.linear.x = 0;
- cmd_vel.twist.linear.y = 0;
- cmd_vel.twist.angular.z = 0;
-
- // Update for the current goal checker's state
- geometry_msgs::msg::Pose pose_tolerance;
- geometry_msgs::msg::Twist vel_tolerance;
- if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) {
- RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!");
- } else {
- cfg_->goal_tolerance.xy_goal_tolerance = pose_tolerance.position.x;
- }
-
- // Get robot pose
- robot_pose_ = PoseSE2(pose.pose);
- geometry_msgs::msg::PoseStamped robot_pose;
- robot_pose.header = pose.header;
- robot_pose_.toPoseMsg(robot_pose.pose);
-
- // Get robot velocity
- robot_vel_ = velocity;
-
- // prune global plan to cut off parts of the past (spatially before the robot)
- pruneGlobalPlan(robot_pose, global_plan_, cfg_->trajectory.global_plan_prune_distance);
-
- // Transform global plan to the frame of interest (w.r.t. the local costmap)
- std::vector<geometry_msgs::msg::PoseStamped> transformed_plan;
- int goal_idx;
- geometry_msgs::msg::TransformStamped tf_plan_to_global;
- if (!transformGlobalPlan(global_plan_, robot_pose, *costmap_, cfg_->map_frame, cfg_->trajectory.max_global_plan_lookahead_dist,
- transformed_plan, &goal_idx, &tf_plan_to_global))
- {
- throw nav2_core::PlannerException(
- std::string("Could not transform the global plan to the frame of the controller")
- );
- }
-
- // update via-points container
- if (!custom_via_points_active_)
- updateViaPointsContainer(transformed_plan, cfg_->trajectory.global_plan_viapoint_sep);
-
- // check if we should enter any backup mode and apply settings
- configureBackupModes(transformed_plan, goal_idx);
-
- // Return false if the transformed global plan is empty
- if (transformed_plan.empty())
- {
- throw nav2_core::PlannerException(
- std::string("Transformed plan is empty. Cannot determine a local plan.")
- );
- }
-
- // Get current goal point (last point of the transformed plan)
- const geometry_msgs::msg::PoseStamped &goal_point = transformed_plan.back();
- robot_goal_.x() = goal_point.pose.position.x;
- robot_goal_.y() = goal_point.pose.position.y;
- if (cfg_->trajectory.global_plan_overwrite_orientation)
- {
- robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);
- // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
- //transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
- tf2::Quaternion q;
- q.setRPY(0, 0, robot_goal_.theta());
- transformed_plan.back().pose.orientation = tf2::toMsg(q);
- }
- else
- {
- robot_goal_.theta() = tf2::getYaw(goal_point.pose.orientation);
- }
-
- // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)
- if (transformed_plan.size()==1) // plan only contains the goal
- {
- transformed_plan.insert(transformed_plan.begin(), geometry_msgs::msg::PoseStamped()); // insert start (not yet initialized)
- }
- //tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start;
- transformed_plan.front().pose = robot_pose.pose;
-
- // clear currently existing obstacles
- obstacles_.clear();
-
- // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin
- if (costmap_converter_)
- updateObstacleContainerWithCostmapConverter();
- else
- updateObstacleContainerWithCostmap();
-
- // also consider custom obstacles (must be called after other updates, since the container is not cleared)
- updateObstacleContainerWithCustomObstacles();
-
-
- // Do not allow config changes during the following optimization step
- std::lock_guard<std::mutex> cfg_lock(cfg_->configMutex());
-
- // Now perform the actual planning
-// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_->goal_tolerance.free_goal_vel); // straight line init
- bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_->goal_tolerance.free_goal_vel);
- if (!success)
- {
- planner_->clearPlanner(); // force reinitialization for next time
-
- ++no_infeasible_plans_; // increase number of infeasible solutions in a row
- time_last_infeasible_plan_ = clock_->now();
- last_cmd_ = cmd_vel.twist;
-
- throw nav2_core::PlannerException(
- std::string("teb_local_planner was not able to obtain a local plan for the current setting.")
- );
- }
-
- // Check for divergence
- if (planner_->hasDiverged())
- {
- cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
-
- // Reset everything to start again with the initialization of new trajectories.
- planner_->clearPlanner();
- RCLCPP_WARN_THROTTLE(logger_, *(clock_), 1, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner...");
-
- ++no_infeasible_plans_; // increase number of infeasible solutions in a row
- time_last_infeasible_plan_ = clock_->now();
- last_cmd_ = cmd_vel.twist;
- throw nav2_core::PlannerException(
- std::string("TebLocalPlannerROS: velocity command invalid (hasDiverged). Resetting planner...")
- );
- }
-
- // Check feasibility (but within the first few states only)
- if(cfg_->robot.is_footprint_dynamic)
- {
- // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
- std::vector<geometry_msgs::msg::Point> updated_footprint_spec_ = costmap_ros_->getRobotFootprint();
- if (updated_footprint_spec_ != footprint_spec_) {
- updated_footprint_spec_ = footprint_spec_;
- nav2_costmap_2d::calculateMinAndMaxDistances(updated_footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
- }
- }
-
- bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_->trajectory.feasibility_check_no_poses, cfg_->trajectory.feasibility_check_lookahead_distance);
- if (!feasible)
- {
- cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
-
- // now we reset everything to start again with the initialization of new trajectories.
- planner_->clearPlanner();
-
- ++no_infeasible_plans_; // increase number of infeasible solutions in a row
- time_last_infeasible_plan_ = clock_->now();
- last_cmd_ = cmd_vel.twist;
-
- throw nav2_core::PlannerException(
- std::string("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...")
- );
- }
-
- // Get the velocity command for this sampling interval
- if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->trajectory.control_look_ahead_poses))
- {
- planner_->clearPlanner();
- ++no_infeasible_plans_; // increase number of infeasible solutions in a row
- time_last_infeasible_plan_ = clock_->now();
- last_cmd_ = cmd_vel.twist;
-
- throw nav2_core::PlannerException(
- std::string("TebLocalPlannerROS: velocity command invalid. Resetting planner...")
- );
- }
-
- // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
- saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->robot.max_vel_x, cfg_->robot.max_vel_y,
- cfg_->robot.max_vel_theta, cfg_->robot.max_vel_x_backwards);
-
- // convert rot-vel to steering angle if desired (carlike robot).
- // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
- // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
- if (cfg_->robot.cmd_angle_instead_rotvel)
- {
- cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z, cfg_->robot.wheelbase, 0.95*cfg_->robot.min_turning_radius);
- if (!std::isfinite(cmd_vel.twist.angular.z))
- {
- cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;
- last_cmd_ = cmd_vel.twist;
- planner_->clearPlanner();
-
- ++no_infeasible_plans_; // increase number of infeasible solutions in a row
- time_last_infeasible_plan_ = clock_->now();
-
- throw nav2_core::PlannerException(
- std::string("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...")
- );
- }
- }
-
- // a feasible solution should be found, reset counter
- no_infeasible_plans_ = 0;
-
- // store last command (for recovery analysis etc.)
- last_cmd_ = cmd_vel.twist;
-
- // Now visualize everything
- planner_->visualize();
- visualization_->publishObstacles(obstacles_);
- visualization_->publishViaPoints(via_points_);
- visualization_->publishGlobalPlan(global_plan_);
-
- return cmd_vel;
-}
-
-void TebLocalPlannerROS::updateObstacleContainerWithCostmap()
-{
- // Add costmap obstacles if desired
- if (cfg_->obstacles.include_costmap_obstacles)
- {
- std::lock_guard<std::recursive_mutex> lock(*costmap_->getMutex());
-
- Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();
-
- for (unsigned int i=0; i<costmap_->getSizeInCellsX()-1; ++i)
- {
- for (unsigned int j=0; j<costmap_->getSizeInCellsY()-1; ++j)
- {
- if (costmap_->getCost(i,j) == nav2_costmap_2d::LETHAL_OBSTACLE)
- {
- Eigen::Vector2d obs;
- costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1));
-
- // check if obstacle is interesting (e.g. not far behind the robot)
- Eigen::Vector2d obs_dir = obs-robot_pose_.position();
- if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_->obstacles.costmap_obstacles_behind_robot_dist )
- continue;
-
- obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
- }
- }
- }
- }
-}
-
-void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
-{
- if (!costmap_converter_)
- return;
-
- //Get obstacles from costmap converter
- costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
- if (!obstacles)
- return;
-
- for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
- {
- const costmap_converter_msgs::msg::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
- const geometry_msgs::msg::Polygon* polygon = &obstacle->polygon;
-
- if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
- {
- obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
- }
- else if (polygon->points.size()==1) // Point
- {
- obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
- }
- else if (polygon->points.size()==2) // Line
- {
- obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
- polygon->points[1].x, polygon->points[1].y )));
- }
- else if (polygon->points.size()>2) // Real polygon
- {
- PolygonObstacle* polyobst = new PolygonObstacle;
- for (std::size_t j=0; j<polygon->points.size(); ++j)
- {
- polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
- }
- polyobst->finalizePolygon();
- obstacles_.push_back(ObstaclePtr(polyobst));
- }
-
- // Set velocity, if obstacle is moving
- if(!obstacles_.empty())
- obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
- }
-}
-
-
-void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
-{
- // Add custom obstacles obtained via message
- std::lock_guard<std::mutex> l(custom_obst_mutex_);
-
- if (!custom_obstacle_msg_.obstacles.empty())
- {
- // We only use the global header to specify the obstacle coordinate system instead of individual ones
- Eigen::Affine3d obstacle_to_map_eig;
- try
- {
- geometry_msgs::msg::TransformStamped obstacle_to_map = tf_->lookupTransform(
- cfg_->map_frame, tf2::timeFromSec(0),
- custom_obstacle_msg_.header.frame_id, tf2::timeFromSec(0),
- custom_obstacle_msg_.header.frame_id, tf2::durationFromSec(0.5));
- obstacle_to_map_eig = tf2::transformToEigen(obstacle_to_map);
- //tf2::fromMsg(obstacle_to_map.transform, obstacle_to_map_eig);
- }
- catch (tf2::TransformException ex)
- {
- RCLCPP_ERROR(logger_, "%s",ex.what());
- obstacle_to_map_eig.setIdentity();
- }
-
- for (size_t i=0; i<custom_obstacle_msg_.obstacles.size(); ++i)
- {
- if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 && custom_obstacle_msg_.obstacles.at(i).radius > 0 ) // circle
- {
- Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
- obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius)));
- }
- else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point
- {
- Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
- obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) )));
- }
- else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line
- {
- Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
- Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y,
- custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z );
- obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2),
- (obstacle_to_map_eig * line_end).head(2) )));
- }
- else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty())
- {
- RCLCPP_INFO(logger_, "Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
- continue;
- }
- else // polygon
- {
- PolygonObstacle* polyobst = new PolygonObstacle;
- for (size_t j=0; j<custom_obstacle_msg_.obstacles.at(i).polygon.points.size(); ++j)
- {
- Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x,
- custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y,
- custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z );
- polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) );
- }
- polyobst->finalizePolygon();
- obstacles_.push_back(ObstaclePtr(polyobst));
- }
-
- // Set velocity, if obstacle is moving
- if(!obstacles_.empty())
- obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation);
- }
- }
-}
-
-void TebLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, double min_separation)
-{
- via_points_.clear();
-
- if (min_separation<=0)
- return;
-
- std::size_t prev_idx = 0;
- for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m]
- {
- // check separation to the previous via-point inserted
- if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation)
- continue;
-
- // add via-point
- via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) );
- prev_idx = i;
- }
-
-}
-
-//Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel)
-//{
-// Eigen::Vector2d vel;
-// vel.coeffRef(0) = std::sqrt( tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY() );
-// vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation());
-// return vel;
-//}
-
-
-bool TebLocalPlannerROS::pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose, std::vector<geometry_msgs::msg::PoseStamped>& global_plan, double dist_behind_robot)
-{
- if (global_plan.empty())
- return true;
-
- try
- {
- // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times)
- //geometry_msgs::msg::TransformStamped global_to_plan_transform = tf_->lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, tf2::timeFromSec(0));
- geometry_msgs::msg::PoseStamped robot = tf_->transform(
- global_pose,
- global_plan.front().header.frame_id);
-
- //robot.setData( global_to_plan_transform * global_pose );
-
- double dist_thresh_sq = dist_behind_robot*dist_behind_robot;
-
- // iterate plan until a pose close the robot is found
- std::vector<geometry_msgs::msg::PoseStamped>::iterator it = global_plan.begin();
- std::vector<geometry_msgs::msg::PoseStamped>::iterator erase_end = it;
- while (it != global_plan.end())
- {
- double dx = robot.pose.position.x - it->pose.position.x;
- double dy = robot.pose.position.y - it->pose.position.y;
- double dist_sq = dx * dx + dy * dy;
- if (dist_sq < dist_thresh_sq)
- {
- erase_end = it;
- break;
- }
- ++it;
- }
- if (erase_end == global_plan.end())
- return false;
-
- if (erase_end != global_plan.begin())
- global_plan.erase(global_plan.begin(), erase_end);
- }
- catch (const tf2::TransformException& ex)
- {
- RCLCPP_DEBUG(logger_, "Cannot prune path since no transform is available: %s\n", ex.what());
- return false;
- }
- return true;
-}
-
-
-bool TebLocalPlannerROS::transformGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan,
- const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length,
- std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int* current_goal_idx, geometry_msgs::msg::TransformStamped* tf_plan_to_global) const
-{
- // this method is a slightly modified version of base_local_planner/goal_functions.h
-
- const geometry_msgs::msg::PoseStamped& plan_pose = global_plan[0];
-
- transformed_plan.clear();
-
- try
- {
- if (global_plan.empty())
- {
- RCLCPP_ERROR(logger_, "Received plan with zero length");
- *current_goal_idx = 0;
- return false;
- }
-
- // get plan_to_global_transform from plan frame to global_frame
- geometry_msgs::msg::TransformStamped plan_to_global_transform = tf_->lookupTransform(
- global_frame, tf2_ros::fromMsg(plan_pose.header.stamp),
- plan_pose.header.frame_id, tf2::timeFromSec(0),
- plan_pose.header.frame_id, tf2::durationFromSec(0.5));
-
-// tf_->waitForTransform(global_frame, ros::Time::now(),
-// plan_pose.header.frame_id, plan_pose.header.stamp,
-// plan_pose.header.frame_id, ros::Duration(0.5));
-// tf_->lookupTransform(global_frame, ros::Time(),
-// plan_pose.header.frame_id, plan_pose.header.stamp,
-// plan_pose.header.frame_id, plan_to_global_transform);
-
- //let's get the pose of the robot in the frame of the plan
- geometry_msgs::msg::PoseStamped robot_pose = tf_->transform(global_pose, plan_pose.header.frame_id);
-
- //we'll discard points on the plan that are outside the local costmap
- double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
- costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
- dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
- // located on the border of the local costmap
-
-
- int i = 0;
- double sq_dist_threshold = dist_threshold * dist_threshold;
- double sq_dist = 1e10;
-
- //we need to loop to a point on the plan that is within a certain distance of the robot
- bool robot_reached = false;
- for(int j=0; j < (int)global_plan.size(); ++j)
- {
- double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
- double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y;
- double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
- if (new_sq_dist > sq_dist_threshold)
- break; // force stop if we have reached the costmap border
-
- if (robot_reached && new_sq_dist > sq_dist)
- break;
-
- if (new_sq_dist < sq_dist) // find closest distance
- {
- sq_dist = new_sq_dist;
- i = j;
- if (sq_dist < 0.05) // 2.5 cm to the robot; take the immediate local minima; if it's not the global
- robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this
- }
- }
-
- geometry_msgs::msg::PoseStamped newer_pose;
-
- double plan_length = 0; // check cumulative Euclidean distance along the plan
-
- //now we'll transform until points are outside of our distance threshold
- while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length))
- {
- //const geometry_msgs::msg::PoseStamped& pose = global_plan[i];
- //tf::poseStampedMsgToTF(pose, tf_pose);
- //tf_pose.setData(plan_to_global_transform * tf_pose);
- tf2::doTransform(global_plan[i], newer_pose, plan_to_global_transform);
-
-// tf_pose.stamp_ = plan_to_global_transform.stamp_;
-// tf_pose.frame_id_ = global_frame;
-// tf::poseStampedTFToMsg(tf_pose, newer_pose);
-
- transformed_plan.push_back(newer_pose);
-
- double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
- double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
- sq_dist = x_diff * x_diff + y_diff * y_diff;
-
- // caclulate distance to previous pose
- if (i>0 && max_plan_length>0)
- plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);
-
- ++i;
- }
-
- // if we are really close to the goal (<sq_dist_threshold) and the goal is not yet reached (e.g. orientation error >>0)
- // the resulting transformed plan can be empty. In that case we explicitly inject the global goal.
- if (transformed_plan.empty())
- {
-// tf::poseStampedMsgToTF(global_plan.back(), tf_pose);
-// tf_pose.setData(plan_to_global_transform * tf_pose);
-// tf_pose.stamp_ = plan_to_global_transform.stamp_;
-// tf_pose.frame_id_ = global_frame;
-// tf::poseStampedTFToMsg(tf_pose, newer_pose);
- tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform);
-
- transformed_plan.push_back(newer_pose);
-
- // Return the index of the current goal point (inside the distance threshold)
- if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
- }
- else
- {
- // Return the index of the current goal point (inside the distance threshold)
- if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop
- }
-
- // Return the transformation from the global plan to the global planning frame if desired
- if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform;
- }
- catch(tf2::LookupException& ex)
- {
- RCLCPP_ERROR(logger_, "No Transform available Error: %s\n", ex.what());
- return false;
- }
- catch(tf2::ConnectivityException& ex)
- {
- RCLCPP_ERROR(logger_, "Connectivity Error: %s\n", ex.what());
- return false;
- }
- catch(tf2::ExtrapolationException& ex)
- {
- RCLCPP_ERROR(logger_, "Extrapolation Error: %s\n", ex.what());
- if (global_plan.size() > 0)
- RCLCPP_ERROR(logger_, "Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
-
- return false;
- }
-
- return true;
-}
-
-
-
-
-double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan, const geometry_msgs::msg::PoseStamped& local_goal,
- int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length) const
-{
- int n = (int)global_plan.size();
-
- // check if we are near the global goal already
- if (current_goal_idx > n-moving_average_length-2)
- {
- if (current_goal_idx >= n-1) // we've exactly reached the goal
- {
- return tf2::getYaw(local_goal.pose.orientation);
- }
- else
- {
-// tf::Quaternion global_orientation;
-// tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation);
-// return tf2::getYaw(tf_plan_to_global.getRotation() * global_orientation );
-
- tf2::Quaternion global_orientation, tf_plan_to_global_orientation;
- tf2::fromMsg(global_plan.back().pose.orientation, global_orientation);
- tf2::fromMsg(tf_plan_to_global.transform.rotation, tf_plan_to_global_orientation);
-
- return tf2::getYaw(tf_plan_to_global_orientation * global_orientation);
- }
- }
-
- // reduce number of poses taken into account if the desired number of poses is not available
- moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); // maybe redundant, since we have checked the vicinity of the goal before
-
- std::vector<double> candidates;
- geometry_msgs::msg::PoseStamped tf_pose_k = local_goal;
- geometry_msgs::msg::PoseStamped tf_pose_kp1;
-
- int range_end = current_goal_idx + moving_average_length;
- for (int i = current_goal_idx; i < range_end; ++i)
- {
- // Transform pose of the global plan to the planning frame
- const geometry_msgs::msg::PoseStamped& pose = global_plan.at(i+1);
- tf2::doTransform(global_plan.at(i+1), tf_pose_kp1, tf_plan_to_global);
-
- // calculate yaw angle
- candidates.push_back( std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y,
- tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x ) );
-
- if (i<range_end-1)
- tf_pose_k = tf_pose_kp1;
- }
- return average_angles(candidates);
-}
-
-
-void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
-{
- double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
- // Limit translational velocity for forward driving
- if (vx > max_vel_x)
- ratio_x = max_vel_x / vx;
-
- // limit strafing velocity
- if (vy > max_vel_y || vy < -max_vel_y)
- ratio_y = std::abs(max_vel_y / vy);
-
- // Limit angular velocity
- if (omega > max_vel_theta || omega < -max_vel_theta)
- ratio_omega = std::abs(max_vel_theta / omega);
-
- // Limit backwards velocity
- if (max_vel_x_backwards<=0)
- {
- RCLCPP_WARN_ONCE(
- logger_,
- "TebLocalPlannerROS(): Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
- }
- else if (vx < -max_vel_x_backwards)
- ratio_x = - max_vel_x_backwards / vx;
-
- if (cfg_->robot.use_proportional_saturation)
- {
- double ratio = std::min(std::min(ratio_x, ratio_y), ratio_omega);
- vx *= ratio;
- vy *= ratio;
- omega *= ratio;
- }
- else
- {
- vx *= ratio_x;
- vy *= ratio_y;
- omega *= ratio_omega;
- }
-}
-
-
-double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const
-{
- if (omega==0 || v==0)
- return 0;
-
- double radius = v/omega;
-
- if (fabs(radius) < min_turning_radius)
- radius = double(g2o::sign(radius)) * min_turning_radius;
-
- return std::atan(wheelbase / radius);
-}
-
-
-void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)
-{
- RCLCPP_WARN_EXPRESSION(
- logger_, opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius,
- "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller "
- "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). "
- "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius);
-}
-
-
-
-void TebLocalPlannerROS::configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int& goal_idx)
-{
- rclcpp::Time current_time = clock_->now();
-
- // reduced horizon backup mode
- if (cfg_->recovery.shrink_horizon_backup &&
- goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations)
- (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).seconds() < cfg_->recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds
- {
- RCLCPP_INFO_EXPRESSION(
- logger_,
- no_infeasible_plans_==1,
- "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_->recovery.shrink_horizon_min_duration);
-
-
- // Shorten horizon if requested
- // reduce to 50 percent:
- int horizon_reduction = goal_idx/2;
-
- if (no_infeasible_plans_ > 9)
- {
- RCLCPP_INFO_EXPRESSION(
- logger_,
- no_infeasible_plans_==10,
- "Infeasible trajectory detected 10 times in a row: further reducing horizon...");
- horizon_reduction /= 2;
- }
-
- // we have a small overhead here, since we already transformed 50% more of the trajectory.
- // But that's ok for now, since we do not need to make transformGlobalPlan more complex
- // and a reduced horizon should occur just rarely.
- int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1;
- goal_idx -= horizon_reduction;
- if (new_goal_idx_transformed_plan>0 && goal_idx >= 0)
- transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end());
- else
- goal_idx += horizon_reduction; // this should not happen, but safety first ;-)
- }
-
-
- // detect and resolve oscillations
- if (cfg_->recovery.oscillation_recovery)
- {
- double max_vel_theta;
- double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_->robot.max_vel_x : cfg_->robot.max_vel_x_backwards;
- if (cfg_->robot.min_turning_radius!=0 && max_vel_current>0)
- max_vel_theta = std::max( max_vel_current/std::abs(cfg_->robot.min_turning_radius), cfg_->robot.max_vel_theta );
- else
- max_vel_theta = cfg_->robot.max_vel_theta;
-
- failure_detector_.update(last_cmd_, cfg_->robot.max_vel_x, cfg_->robot.max_vel_x_backwards, max_vel_theta,
- cfg_->recovery.oscillation_v_eps, cfg_->recovery.oscillation_omega_eps);
-
- bool oscillating = failure_detector_.isOscillating();
- bool recently_oscillated = (clock_->now()-time_last_oscillation_).seconds() < cfg_->recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recently
-
- if (oscillating)
- {
- if (!recently_oscillated)
- {
- // save current turning direction
- if (robot_vel_.angular.z > 0)
- last_preferred_rotdir_ = RotType::left;
- else
- last_preferred_rotdir_ = RotType::right;
- RCLCPP_INFO(logger_, "TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).");
- }
- time_last_oscillation_ = clock_->now();
- planner_->setPreferredTurningDir(last_preferred_rotdir_);
- }
- else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior
- {
- last_preferred_rotdir_ = RotType::none;
- planner_->setPreferredTurningDir(last_preferred_rotdir_);
- RCLCPP_INFO(logger_, "TebLocalPlannerROS: oscillation recovery disabled/expired.");
- }
- }
-
-}
-
-
-void TebLocalPlannerROS::setSpeedLimit(
- const double & speed_limit, const bool & percentage)
-{
- if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
- // Restore default value
- cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x;
- cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
- cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y;
- cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta;
- } else {
- if (percentage) {
- // Speed limit is expressed in % from maximum speed of robot
- cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0;
- cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
- cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
- cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
- } else {
- // Speed limit is expressed in absolute value
- double max_speed_xy = std::max(
- std::max(cfg_->robot.base_max_vel_x,cfg_->robot.base_max_vel_x_backwards),cfg_->robot.base_max_vel_y);
- if (speed_limit < max_speed_xy) {
- // Handling components and angular velocity changes:
- // Max velocities are being changed in the same proportion
- // as absolute linear speed changed in order to preserve
- // robot moving trajectories to be the same after speed change.
- // G. Doisy: not sure if that's applicable to base_max_vel_x_backwards.
- const double ratio = speed_limit / max_speed_xy;
- cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio;
- cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
- cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio;
- cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
- }
- }
- }
-}
-
-void TebLocalPlannerROS::customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg)
-{
- std::lock_guard<std::mutex> l(custom_obst_mutex_);
- custom_obstacle_msg_ = *obst_msg;
-}
-
-void TebLocalPlannerROS::customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg)
-{
- RCLCPP_INFO_ONCE(logger_, "Via-points received. This message is printed once.");
- if (cfg_->trajectory.global_plan_viapoint_sep > 0)
- {
- RCLCPP_INFO(logger_, "Via-points are already obtained from the global plan (global_plan_viapoint_sep>0)."
- "Ignoring custom via-points.");
- custom_via_points_active_ = false;
- return;
- }
-
- std::lock_guard<std::mutex> l(via_point_mutex_);
- via_points_.clear();
- for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses)
- {
- via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y);
- }
- custom_via_points_active_ = !via_points_.empty();
-}
-
-void TebLocalPlannerROS::activate() {
- visualization_->on_activate();
-
- return;
-}
-void TebLocalPlannerROS::deactivate() {
- visualization_->on_deactivate();
-
- return;
-}
-void TebLocalPlannerROS::cleanup() {
- visualization_->on_cleanup();
- costmap_converter_->stopWorker();
-
- return;
-}
-
-} // end namespace teb_local_planner
-
-// register this planner as a nav2_core::Controller plugin
-PLUGINLIB_EXPORT_CLASS(teb_local_planner::TebLocalPlannerROS, nav2_core::Controller)
diff --git a/src/teb_local_planner/src/test_optim_node.cpp b/src/teb_local_planner/src/test_optim_node.cpp
deleted file mode 100644
index 94d9c02..0000000
--- a/src/teb_local_planner/src/test_optim_node.cpp
+++ /dev/null
@@ -1,322 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2017.
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/teb_local_planner_ros.h"
-
-//#include <interactive_markers/interactive_marker_server.h>
-#include <visualization_msgs/msg/marker.hpp>
-
-
-using namespace teb_local_planner; // it is ok here to import everything for testing purposes
-
-// ============= Global Variables ================
-// Ok global variables are bad, but here we only have a simple testing node.
-PlannerInterfacePtr planner;
-TebVisualizationPtr visual;
-std::vector<ObstaclePtr> obst_vector;
-ViaPointContainer via_points;
-TebConfig config;
-//std::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg;
-ros::Subscriber custom_obst_sub;
-ros::Subscriber via_points_sub;
-ros::Subscriber clicked_points_sub;
-std::vector<ros::Subscriber> obst_vel_subs;
-unsigned int no_fixed_obstacles;
-
-// =========== Function declarations =============
-void CB_mainCycle(const ros::TimerEvent& e);
-void CB_publishCycle(const ros::TimerEvent& e);
-void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
-void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
-void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb);
-void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
-void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
-void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg);
-void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id);
-
-
-// =============== Main function =================
-int main( int argc, char** argv )
-{
- ros::init(argc, argv, "test_optim_node");
- rclcpp::Node::SharedPtr n("~");
-
-
- // load ros parameters from node handle
- config.loadRosParamFromNodeHandle(n);
-
- ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
- ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
-
- // setup dynamic reconfigure
- dynamic_recfg = std::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
- dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
- dynamic_recfg->setCallback(cb);
-
- // setup callback for custom obstacles
- custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle);
-
- // setup callback for clicked points (in rviz) that are considered as via-points
- clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points);
-
- // setup callback for via-points (callback overwrites previously set via-points)
- via_points_sub = n.subscribe("via_points", 1, CB_via_points);
-
- // interactive marker server for simulated dynamic obstacles
- interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
-
- obst_vector.push_back( std::make_shared<PointObstacle>(-3,1) );
- obst_vector.push_back( std::make_shared<PointObstacle>(6,2) );
- obst_vector.push_back( std::make_shared<PointObstacle>(0,0.1) );
-// obst_vector.push_back( std::make_shared<LineObstacle>(1,1.5,1,-1.5) ); //90 deg
-// obst_vector.push_back( std::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg
-// obst_vector.push_back( std::make_shared<PointObstacle>(-1.5,-0.5) );
-
- // Dynamic obstacles
- Eigen::Vector2d vel (0.1, -0.3);
- obst_vector.at(0)->setCentroidVelocity(vel);
- vel = Eigen::Vector2d(-0.3, -0.2);
- obst_vector.at(1)->setCentroidVelocity(vel);
-
- /*
- PolygonObstacle* polyobst = new PolygonObstacle;
- polyobst->pushBackVertex(1, -1);
- polyobst->pushBackVertex(0, 1);
- polyobst->pushBackVertex(1, 1);
- polyobst->pushBackVertex(2, 1);
-
- polyobst->finalizePolygon();
- obst_vector.emplace_back(polyobst);
- */
-
- for (unsigned int i=0; i<obst_vector.size(); ++i)
- {
- // setup callbacks for setting obstacle velocities
- std::string topic = "/test_optim_node/obstacle_" + std::to_string(i) + "/cmd_vel";
- obst_vel_subs.push_back(n.subscribe<geometry_msgs::msg::Twist>(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i)));
-
- //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker);
- // Add interactive markers for all point obstacles
- std::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst_vector.at(i));
- if (pobst)
- {
- CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker);
- }
- }
- marker_server.applyChanges();
-
- // Setup visualization
- visual = TebVisualizationPtr(new TebVisualization(n, config));
-
- // Setup robot shape model
- RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n);
-
- // Setup planner (homotopy class planning or just the local teb planner)
- if (config.hcp.enable_homotopy_class_planning)
- planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
- else
- planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
-
-
- no_fixed_obstacles = obst_vector.size();
- ros::spin();
-
- return 0;
-}
-
-// Planning loop
-void CB_mainCycle(const ros::TimerEvent& e)
-{
- planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes
-}
-
-// Visualization loop
-void CB_publishCycle(const ros::TimerEvent& e)
-{
- planner->visualize();
- visual->publishObstacles(obst_vector);
- visual->publishViaPoints(via_points);
-}
-
-void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
-{
- config.reconfigure(reconfig);
-}
-
-void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb)
-{
- // create an interactive marker for our server
- visualization_msgs::InteractiveMarker i_marker;
- i_marker.header.frame_id = frame;
- i_marker.header.stamp = ros::Time::now();
- std::ostringstream oss;
- //oss << "obstacle" << id;
- oss << id;
- i_marker.name = oss.str();
- i_marker.description = "Obstacle";
- i_marker.pose.position.x = init_x;
- i_marker.pose.position.y = init_y;
- i_marker.pose.orientation.w = 1.0f; // make quaternion normalized
-
- // create a grey box marker
- visualization_msgs::msg::Marker box_marker;
- box_marker.type = visualization_msgs::msg::Marker::CUBE;
- box_marker.id = id;
- box_marker.scale.x = 0.2;
- box_marker.scale.y = 0.2;
- box_marker.scale.z = 0.2;
- box_marker.color.r = 0.5;
- box_marker.color.g = 0.5;
- box_marker.color.b = 0.5;
- box_marker.color.a = 1.0;
- box_marker.pose.orientation.w = 1.0f; // make quaternion normalized
-
- // create a non-interactive control which contains the box
- visualization_msgs::InteractiveMarkerControl box_control;
- box_control.always_visible = true;
- box_control.markers.push_back( box_marker );
-
- // add the control to the interactive marker
- i_marker.controls.push_back( box_control );
-
- // create a control which will move the box, rviz will insert 2 arrows
- visualization_msgs::InteractiveMarkerControl move_control;
- move_control.name = "move_x";
- move_control.orientation.w = 0.707107f;
- move_control.orientation.x = 0;
- move_control.orientation.y = 0.707107f;
- move_control.orientation.z = 0;
- move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
-
-
- // add the control to the interactive marker
- i_marker.controls.push_back(move_control);
-
- // add the interactive marker to our collection
- marker_server->insert(i_marker);
- marker_server->setCallback(i_marker.name,feedback_cb);
-}
-
-void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
-{
- std::stringstream ss(feedback->marker_name);
- unsigned int index;
- ss >> index;
-
- if (index>=no_fixed_obstacles)
- return;
- PointObstacle* pobst = static_cast<PointObstacle*>(obst_vector.at(index).get());
- pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y);
-}
-
-void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
-{
- // resize such that the vector contains only the fixed obstacles specified inside the main function
- obst_vector.resize(no_fixed_obstacles);
-
- // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)
- for (size_t i = 0; i < obst_msg->obstacles.size(); ++i)
- {
- if (obst_msg->obstacles.at(i).polygon.points.size() == 1 )
- {
- if (obst_msg->obstacles.at(i).radius == 0)
- {
- obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
- obst_msg->obstacles.at(i).polygon.points.front().y )));
- }
- else
- {
- obst_vector.push_back(ObstaclePtr(new CircularObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
- obst_msg->obstacles.at(i).polygon.points.front().y,
- obst_msg->obstacles.at(i).radius )));
- }
- }
- else if (obst_msg->obstacles.at(i).polygon.points.empty())
- {
- ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
- continue;
- }
- else
- {
- PolygonObstacle* polyobst = new PolygonObstacle;
- for (size_t j=0; j<obst_msg->obstacles.at(i).polygon.points.size(); ++j)
- {
- polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x,
- obst_msg->obstacles.at(i).polygon.points[j].y );
- }
- polyobst->finalizePolygon();
- obst_vector.push_back(ObstaclePtr(polyobst));
- }
-
- if(!obst_vector.empty())
- obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation);
- }
-}
-
-
-void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
-{
- // we assume for simplicity that the fixed frame is already the map/planning frame
- // consider clicked points as via-points
- via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
- ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
- if (config.optim.weight_viapoint<=0)
- ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
-}
-
-void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg)
-{
- ROS_INFO_ONCE("Via-points received. This message is printed once.");
- via_points.clear();
- for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses)
- {
- via_points.emplace_back(pose.pose.position.x, pose.pose.position.y);
- }
-}
-
-void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id)
-{
- if (id >= obst_vector.size())
- {
- ROS_WARN("Cannot set velocity: unknown obstacle id.");
- return;
- }
-
- Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y);
- obst_vector.at(id)->setCentroidVelocity(vel);
-}
diff --git a/src/teb_local_planner/src/timed_elastic_band.cpp b/src/teb_local_planner/src/timed_elastic_band.cpp
deleted file mode 100644
index 4d7d37c..0000000
--- a/src/teb_local_planner/src/timed_elastic_band.cpp
+++ /dev/null
@@ -1,630 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-#include "teb_local_planner/timed_elastic_band.h"
-
-namespace teb_local_planner
-{
-
-namespace
-{
- /**
- * estimate the time to move from start to end.
- * Assumes constant velocity for the motion.
- */
- double estimateDeltaT(const PoseSE2& start, const PoseSE2& end,
- double max_vel_x, double max_vel_theta)
- {
- double dt_constant_motion = 0.1;
- if (max_vel_x > 0) {
- double trans_dist = (end.position() - start.position()).norm();
- dt_constant_motion = trans_dist / max_vel_x;
- }
- if (max_vel_theta > 0) {
- double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta()));
- dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta);
- }
- return dt_constant_motion;
- }
-} // namespace
-
-
-TimedElasticBand::TimedElasticBand()
-{
-}
-
-TimedElasticBand::~TimedElasticBand()
-{
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Destructor Timed_Elastic_Band...");
- clearTimedElasticBand();
-}
-
-
-void TimedElasticBand::addPose(const PoseSE2& pose, bool fixed)
-{
- VertexPose* pose_vertex = new VertexPose(pose, fixed);
- pose_vec_.push_back( pose_vertex );
- return;
-}
-
-void TimedElasticBand::addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed)
-{
- VertexPose* pose_vertex = new VertexPose(position, theta, fixed);
- pose_vec_.push_back( pose_vertex );
- return;
-}
-
- void TimedElasticBand::addPose(double x, double y, double theta, bool fixed)
-{
- VertexPose* pose_vertex = new VertexPose(x, y, theta, fixed);
- pose_vec_.push_back( pose_vertex );
- return;
-}
-
-void TimedElasticBand::addTimeDiff(double dt, bool fixed)
-{
- assert(dt > 0.0 && "Adding a timediff requires a positive dt");
- VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt, fixed);
- timediff_vec_.push_back( timediff_vertex );
- return;
-}
-
-
-void TimedElasticBand::addPoseAndTimeDiff(double x, double y, double angle, double dt)
-{
- if (sizePoses() != sizeTimeDiffs())
- {
- addPose(x,y,angle,false);
- addTimeDiff(dt,false);
- }
- else {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"),
- "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
- }
- return;
-}
-
-
-
-void TimedElasticBand::addPoseAndTimeDiff(const PoseSE2& pose, double dt)
-{
- if (sizePoses() != sizeTimeDiffs())
- {
- addPose(pose,false);
- addTimeDiff(dt,false);
- } else {
- RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
- }
- return;
-}
-
-void TimedElasticBand::addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt)
-{
- if (sizePoses() != sizeTimeDiffs())
- {
- addPose(position, theta,false);
- addTimeDiff(dt,false);
- } else {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
- }
- return;
-}
-
-
-void TimedElasticBand::deletePose(int index)
-{
- assert(index<pose_vec_.size());
- delete pose_vec_.at(index);
- pose_vec_.erase(pose_vec_.begin()+index);
-}
-
-void TimedElasticBand::deletePoses(int index, int number)
-{
- assert(index+number<=(int)pose_vec_.size());
- for (int i = index; i<index+number; ++i)
- delete pose_vec_.at(i);
- pose_vec_.erase(pose_vec_.begin()+index, pose_vec_.begin()+index+number);
-}
-
-void TimedElasticBand::deleteTimeDiff(int index)
-{
- assert(index<(int)timediff_vec_.size());
- delete timediff_vec_.at(index);
- timediff_vec_.erase(timediff_vec_.begin()+index);
-}
-
-void TimedElasticBand::deleteTimeDiffs(int index, int number)
-{
- assert(index+number<=timediff_vec_.size());
- for (int i = index; i<index+number; ++i)
- delete timediff_vec_.at(i);
- timediff_vec_.erase(timediff_vec_.begin()+index, timediff_vec_.begin()+index+number);
-}
-
-void TimedElasticBand::insertPose(int index, const PoseSE2& pose)
-{
- VertexPose* pose_vertex = new VertexPose(pose);
- pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
-}
-
-void TimedElasticBand::insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
-{
- VertexPose* pose_vertex = new VertexPose(position, theta);
- pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
-}
-
-void TimedElasticBand::insertPose(int index, double x, double y, double theta)
-{
- VertexPose* pose_vertex = new VertexPose(x, y, theta);
- pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
-}
-
-void TimedElasticBand::insertTimeDiff(int index, double dt)
-{
- VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt);
- timediff_vec_.insert(timediff_vec_.begin()+index, timediff_vertex);
-}
-
-
-void TimedElasticBand::clearTimedElasticBand()
-{
- for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it)
- delete *pose_it;
- pose_vec_.clear();
-
- for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
- delete *dt_it;
- timediff_vec_.clear();
-}
-
-
-void TimedElasticBand::setPoseVertexFixed(int index, bool status)
-{
- assert(index<sizePoses());
- pose_vec_.at(index)->setFixed(status);
-}
-
-void TimedElasticBand::setTimeDiffVertexFixed(int index, bool status)
-{
- assert(index<sizeTimeDiffs());
- timediff_vec_.at(index)->setFixed(status);
-}
-
-
-void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples, bool fast_mode)
-{
- assert(sizeTimeDiffs() == 0 || sizeTimeDiffs() + 1 == sizePoses());
- /// iterate through all TEB states and add/remove states!
- bool modified = true;
-
- for (int rep = 0; rep < 100 && modified; ++rep) // actually it should be while(), but we want to make sure to not get stuck in some oscillation, hence max 100 repitions.
- {
- modified = false;
-
- for(int i=0; i < sizeTimeDiffs(); ++i) // TimeDiff connects Point(i) with Point(i+1)
- {
- if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
- {
- //RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "teb_local_planner: autoResize() inserting new bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs());
-
- double newtime = 0.5*TimeDiff(i);
-
- TimeDiff(i) = newtime;
- insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
- insertTimeDiff(i+1,newtime);
-
- modified = true;
- }
- else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples) // only remove samples if size is larger than min_samples.
- {
- //RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs());
-
- if(i < ((int)sizeTimeDiffs()-1))
- {
- TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
- deleteTimeDiff(i);
- deletePose(i+1);
- }
- else
- { // last motion should be adjusted, shift time to the interval before
- TimeDiff(i-1) += TimeDiff(i);
- deleteTimeDiff(i);
- deletePose(i);
- }
-
- modified = true;
- }
- }
- if (fast_mode) break;
- }
-}
-
-
-double TimedElasticBand::getSumOfAllTimeDiffs() const
-{
- double time = 0;
-
- for(TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
- {
- time += (*dt_it)->dt();
- }
- return time;
-}
-
-double TimedElasticBand::getSumOfTimeDiffsUpToIdx(int index) const
-{
- assert(index<=timediff_vec_.size());
-
- double time = 0;
-
- for(int i = 0; i < index; ++i)
- {
- time += timediff_vec_.at(i)->dt();
- }
-
- return time;
-}
-
-double TimedElasticBand::getAccumulatedDistance() const
-{
- double dist = 0;
-
- for(int i=1; i<sizePoses(); ++i)
- {
- dist += (Pose(i).position() - Pose(i-1).position()).norm();
- }
- return dist;
-}
-
-bool TimedElasticBand::initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep, double max_vel_x, int min_samples, bool guess_backwards_motion)
-{
- if (!isInit())
- {
- addPose(start); // add starting point
- setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
-
- double timestep = 0.1;
-
- if (diststep!=0)
- {
- Eigen::Vector2d point_to_goal = goal.position()-start.position();
- double dir_to_goal = std::atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
- double dx = diststep*std::cos(dir_to_goal);
- double dy = diststep*std::sin(dir_to_goal);
- double orient_init = dir_to_goal;
- // check if the goal is behind the start pose (w.r.t. start orientation)
- if (guess_backwards_motion && point_to_goal.dot(start.orientationUnitVec()) < 0)
- orient_init = g2o::normalize_theta(orient_init+M_PI);
- // TODO: timestep ~ max_vel_x_backwards for backwards motions
-
- double dist_to_goal = point_to_goal.norm();
- double no_steps_d = dist_to_goal/std::abs(diststep); // ignore negative values
- unsigned int no_steps = (unsigned int) std::floor(no_steps_d);
-
- if (max_vel_x > 0) timestep = diststep / max_vel_x;
-
- for (unsigned int i=1; i<=no_steps; i++) // start with 1! starting point had index 0
- {
- if (i==no_steps && no_steps_d==(float) no_steps)
- break; // if last conf (depending on stepsize) is equal to goal conf -> leave loop
- addPoseAndTimeDiff(start.x()+i*dx,start.y()+i*dy,orient_init,timestep);
- }
-
- }
-
- // if number of samples is not larger than min_samples, insert manually
- if ( sizePoses() < min_samples-1 )
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
- while (sizePoses() < min_samples-1) // subtract goal point that will be added later
- {
- // simple strategy: interpolate between the current pose and the goal
- PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
- if (max_vel_x > 0) timestep = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x;
- addPoseAndTimeDiff( intermediate_pose, timestep ); // let the optimier correct the timestep (TODO: better initialization
- }
- }
-
- // add goal
- if (max_vel_x > 0) timestep = (goal.position()-BackPose().position()).norm()/max_vel_x;
- addPoseAndTimeDiff(goal,timestep); // add goal point
- setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
- }
- else // size!=0
- {
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
- return false;
- }
- return true;
-}
-
-
-bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::msg::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)
-{
-
- if (!isInit())
- {
- PoseSE2 start(plan.front().pose);
- PoseSE2 goal(plan.back().pose);
-
- addPose(start); // add starting point with given orientation
- setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
-
- bool backwards = false;
- if (guess_backwards_motion && (goal.position()-start.position()).dot(start.orientationUnitVec()) < 0) // check if the goal is behind the start pose (w.r.t. start orientation)
- backwards = true;
- // TODO: dt ~ max_vel_x_backwards for backwards motions
-
- for (int i=1; i<(int)plan.size()-1; ++i)
- {
- double yaw;
- if (estimate_orient)
- {
- // get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
- double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
- double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
- yaw = std::atan2(dy,dx);
- if (backwards)
- yaw = g2o::normalize_theta(yaw+M_PI);
- }
- else
- {
- yaw = tf2::getYaw(plan[i].pose.orientation);
- }
- PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);
- double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
- addPoseAndTimeDiff(intermediate_pose, dt);
- }
-
- // if number of samples is not larger than min_samples, insert manually
- if ( sizePoses() < min_samples-1 )
- {
- RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"),
- "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
- while (sizePoses() < min_samples-1) // subtract goal point that will be added later
- {
- // simple strategy: interpolate between the current pose and the goal
- PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
- double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
- addPoseAndTimeDiff( intermediate_pose, dt ); // let the optimier correct the timestep (TODO: better initialization
- }
- }
-
- // Now add final state with given orientation
- double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
- addPoseAndTimeDiff(goal, dt);
- setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
- }
- else // size!=0
- {
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
- RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"),
- "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
- return false;
- }
-
- return true;
-}
-
-
-int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance, int begin_idx) const
-{
- int n = sizePoses();
- if (begin_idx < 0 || begin_idx >= n)
- return -1;
-
- double min_dist_sq = std::numeric_limits<double>::max();
- int min_idx = -1;
-
- for (int i = begin_idx; i < n; i++)
- {
- double dist_sq = (ref_point - Pose(i).position()).squaredNorm();
- if (dist_sq < min_dist_sq)
- {
- min_dist_sq = dist_sq;
- min_idx = i;
- }
- }
-
- if (distance)
- *distance = std::sqrt(min_dist_sq);
-
- return min_idx;
-}
-
-
-int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance) const
-{
- double min_dist = std::numeric_limits<double>::max();
- int min_idx = -1;
-
- for (int i = 0; i < sizePoses(); i++)
- {
- Eigen::Vector2d point = Pose(i).position();
- double dist = distance_point_to_segment_2d(point, ref_line_start, ref_line_end);
- if (dist < min_dist)
- {
- min_dist = dist;
- min_idx = i;
- }
- }
-
- if (distance)
- *distance = min_dist;
- return min_idx;
-}
-
-int TimedElasticBand::findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance) const
-{
- if (vertices.empty())
- return 0;
- else if (vertices.size() == 1)
- return findClosestTrajectoryPose(vertices.front());
- else if (vertices.size() == 2)
- return findClosestTrajectoryPose(vertices.front(), vertices.back());
-
- double min_dist = std::numeric_limits<double>::max();
- int min_idx = -1;
-
- for (int i = 0; i < sizePoses(); i++)
- {
- Eigen::Vector2d point = Pose(i).position();
- double dist_to_polygon = std::numeric_limits<double>::max();
- for (int j = 0; j < (int) vertices.size()-1; ++j)
- {
- dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices[j], vertices[j+1]));
- }
- dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices.back(), vertices.front()));
- if (dist_to_polygon < min_dist)
- {
- min_dist = dist_to_polygon;
- min_idx = i;
- }
- }
-
- if (distance)
- *distance = min_dist;
-
- return min_idx;
-}
-
-
-int TimedElasticBand::findClosestTrajectoryPose(const Obstacle& obstacle, double* distance) const
-{
- const PointObstacle* pobst = dynamic_cast<const PointObstacle*>(&obstacle);
- if (pobst)
- return findClosestTrajectoryPose(pobst->position(), distance);
-
- const LineObstacle* lobst = dynamic_cast<const LineObstacle*>(&obstacle);
- if (lobst)
- return findClosestTrajectoryPose(lobst->start(), lobst->end(), distance);
-
- const PolygonObstacle* polyobst = dynamic_cast<const PolygonObstacle*>(&obstacle);
- if (polyobst)
- return findClosestTrajectoryPose(polyobst->vertices(), distance);
-
- return findClosestTrajectoryPose(obstacle.getCentroid(), distance);
-}
-
-
-void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples)
-{
- // first and simple approach: change only start confs (and virtual start conf for inital velocity)
- // TEST if optimizer can handle this "hard" placement
-
- if (new_start && sizePoses()>0)
- {
- // find nearest state (using l2-norm) in order to prune the trajectory
- // (remove already passed states)
- double dist_cache = (new_start->position()- Pose(0).position()).norm();
- double dist;
- int lookahead = std::min<int>( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples
-
- int nearest_idx = 0;
- for (int i = 1; i<=lookahead; ++i)
- {
- dist = (new_start->position()- Pose(i).position()).norm();
- if (dist<dist_cache)
- {
- dist_cache = dist;
- nearest_idx = i;
- }
- else break;
- }
-
- // prune trajectory at the beginning (and extrapolate sequences at the end if the horizon is fixed)
- if (nearest_idx>0)
- {
- // nearest_idx is equal to the number of samples to be removed (since it counts from 0 ;-) )
- // WARNING delete starting at pose 1, and overwrite the original pose(0) with new_start, since Pose(0) is fixed during optimization!
- deletePoses(1, nearest_idx); // delete first states such that the closest state is the new first one
- deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
- }
-
- // update start
- Pose(0) = *new_start;
- }
-
- if (new_goal && sizePoses()>0)
- {
- BackPose() = *new_goal;
- }
-};
-
-
-bool TimedElasticBand::isTrajectoryInsideRegion(double radius, double max_dist_behind_robot, int skip_poses)
-{
- if (sizePoses()<=0)
- return true;
-
- double radius_sq = radius*radius;
- double max_dist_behind_robot_sq = max_dist_behind_robot*max_dist_behind_robot;
- Eigen::Vector2d robot_orient = Pose(0).orientationUnitVec();
-
- for (int i=1; i<sizePoses(); i=i+skip_poses+1)
- {
- Eigen::Vector2d dist_vec = Pose(i).position()-Pose(0).position();
- double dist_sq = dist_vec.squaredNorm();
-
- if (dist_sq > radius_sq)
- {
- RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot");
- return false;
- }
-
- // check behind the robot with a different distance, if specified (or >=0)
- if (max_dist_behind_robot >= 0 && dist_vec.dot(robot_orient) < 0 && dist_sq > max_dist_behind_robot_sq)
- {
- RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot behind");
- return false;
- }
-
- }
- return true;
-}
-
-
-
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/src/visualization.cpp b/src/teb_local_planner/src/visualization.cpp
deleted file mode 100644
index 025960d..0000000
--- a/src/teb_local_planner/src/visualization.cpp
+++ /dev/null
@@ -1,563 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2016,
- * TU Dortmund - Institute of Control Theory and Systems Engineering.
- * 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 institute nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Christoph Rösmann
- *********************************************************************/
-
-// ros stuff
-#include "teb_local_planner/visualization.h"
-#include "teb_local_planner/optimal_planner.h"
-
-namespace teb_local_planner
-{
-
-void publishPlan(const std::vector<geometry_msgs::msg::PoseStamped>& path,
- rclcpp::Publisher<nav_msgs::msg::Path> *pub) {
- if(path.empty())
- return;
-
- nav_msgs::msg::Path gui_path;
- gui_path.poses.resize(path.size());
- gui_path.header.frame_id = path[0].header.frame_id;
- gui_path.header.stamp = path[0].header.stamp;
-
- // Extract the plan in world co-ordinates, we assume the path is all in the same frame
- for(unsigned int i=0; i < path.size(); i++){
- gui_path.poses[i] = path[i];
- }
-
- pub->publish(gui_path);
-}
-
-TebVisualization::TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg) : nh_(nh), cfg_(&cfg), initialized_(false)
-{
-}
-
-void TebVisualization::publishGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan) const
-{
- if ( printErrorWhenNotInitialized() )
- return;
- publishPlan(global_plan, global_plan_pub_.get());
-}
-
-void TebVisualization::publishLocalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& local_plan) const
-{
- if ( printErrorWhenNotInitialized() )
- return;
- publishPlan(local_plan, local_plan_pub_.get());
-}
-
-void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const
-{
- if ( printErrorWhenNotInitialized() )
- return;
-
- // create path msg
- nav_msgs::msg::Path teb_path;
- teb_path.header.frame_id = cfg_->map_frame;
- teb_path.header.stamp = nh_->now();
-
- // create pose_array (along trajectory)
- geometry_msgs::msg::PoseArray teb_poses;
- teb_poses.header.frame_id = teb_path.header.frame_id;
- teb_poses.header.stamp = teb_path.header.stamp;
-
- // fill path msgs with teb configurations
- for (int i=0; i < teb.sizePoses(); i++)
- {
- geometry_msgs::msg::PoseStamped pose;
-
- pose.header.frame_id = teb_path.header.frame_id;
- pose.header.stamp = teb_path.header.stamp;
- teb.Pose(i).toPoseMsg(pose.pose);
- pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*teb.getSumOfTimeDiffsUpToIdx(i);
- teb_path.poses.push_back(pose);
- teb_poses.poses.push_back(pose.pose);
- }
- local_plan_pub_->publish(teb_path);
- teb_poses_pub_->publish(teb_poses);
-}
-
-
-
-void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns,
- const std_msgs::msg::ColorRGBA &color)
-{
- if ( printErrorWhenNotInitialized() )
- return;
-
- std::vector<visualization_msgs::msg::Marker> markers;
- robot_model.visualizeRobot(current_pose, markers, color);
- if (markers.empty())
- return;
-
- int idx = 1000000; // avoid overshadowing by obstacles
- for (std::vector<visualization_msgs::msg::Marker>::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx)
- {
- marker_it->header.frame_id = cfg_->map_frame;
- marker_it->header.stamp = nh_->now();
- marker_it->action = visualization_msgs::msg::Marker::ADD;
- marker_it->ns = ns;
- marker_it->id = idx;
- marker_it->lifetime = rclcpp::Duration(2, 0);
- teb_marker_pub_->publish(*marker_it);
- }
-
-}
-
-void TebVisualization::publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model)
-{
- publishRobotFootprintModel(current_pose, robot_model, "InfeasibleRobotPoses", toColorMsg(0.5, 0.8, 0.0, 0.0));
-}
-
-
-void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
-{
- if ( obstacles.empty() || printErrorWhenNotInitialized() )
- return;
-
- // Visualize point obstacles
- {
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = "PointObstacles";
- marker.id = 0;
- marker.type = visualization_msgs::msg::Marker::POINTS;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.lifetime = rclcpp::Duration(2, 0);
- marker.pose.orientation.w = 1.0;
-
- for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
- {
- std::shared_ptr<PointObstacle> pobst = std::dynamic_pointer_cast<PointObstacle>(*obst);
- if (!pobst)
- continue;
-
- if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
- {
- geometry_msgs::msg::Point point;
- point.x = pobst->x();
- point.y = pobst->y();
- point.z = 0;
- marker.points.push_back(point);
- }
- else // Spatiotemporally point obstacles become a line
- {
- marker.type = visualization_msgs::msg::Marker::LINE_LIST;
- geometry_msgs::msg::Point start;
- start.x = pobst->x();
- start.y = pobst->y();
- start.z = 0;
- marker.points.push_back(start);
-
- geometry_msgs::msg::Point end;
- double t = 20;
- Eigen::Vector2d pred;
- pobst->predictCentroidConstantVelocity(t, pred);
- end.x = pred[0];
- end.y = pred[1];
- end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*t;
- marker.points.push_back(end);
- }
- }
-
- marker.scale.x = 0.1;
- marker.scale.y = 0.1;
- marker.color.a = 1.0;
- marker.color.r = 1.0;
- marker.color.g = 0.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
- }
-
- // Visualize circular obstacles
- {
- std::size_t idx = 0;
- for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
- {
- std::shared_ptr<CircularObstacle> pobst = std::dynamic_pointer_cast<CircularObstacle>(*obst);
- if (!pobst)
- continue;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = "CircularObstacles";
- marker.id = idx++;
- marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.lifetime = rclcpp::Duration(2, 0);
- geometry_msgs::msg::Point point;
- point.x = pobst->x();
- point.y = pobst->y();
- point.z = 0;
- marker.points.push_back(point);
-
- marker.scale.x = pobst->radius();
- marker.scale.y = pobst->radius();
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
- }
- }
-
- // Visualize line obstacles
- {
- std::size_t idx = 0;
- for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
- {
- std::shared_ptr<LineObstacle> pobst = std::dynamic_pointer_cast<LineObstacle>(*obst);
- if (!pobst)
- continue;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = "LineObstacles";
- marker.id = idx++;
- marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.lifetime = rclcpp::Duration(2, 0);
- marker.pose.orientation.w = 1.0;
-
- geometry_msgs::msg::Point start;
- start.x = pobst->start().x();
- start.y = pobst->start().y();
- start.z = 0;
- marker.points.push_back(start);
- geometry_msgs::msg::Point end;
- end.x = pobst->end().x();
- end.y = pobst->end().y();
- end.z = 0;
- marker.points.push_back(end);
-
- marker.scale.x = 0.1;
- marker.scale.y = 0.1;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
- }
- }
-
-
- // Visualize polygon obstacles
- {
- std::size_t idx = 0;
- for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
- {
- std::shared_ptr<PolygonObstacle> pobst = std::dynamic_pointer_cast<PolygonObstacle>(*obst);
- if (!pobst)
- continue;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = "PolyObstacles";
- marker.id = idx++;
- marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.lifetime = rclcpp::Duration(2, 0);
- marker.pose.orientation.w = 1.0;
-
- for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
- {
- geometry_msgs::msg::Point point;
- point.x = vertex->x();
- point.y = vertex->y();
- point.z = 0;
- marker.points.push_back(point);
- }
-
- // Also add last point to close the polygon
- // but only if polygon has more than 2 points (it is not a line)
- if (pobst->vertices().size() > 2)
- {
- geometry_msgs::msg::Point point;
- point.x = pobst->vertices().front().x();
- point.y = pobst->vertices().front().y();
- point.z = 0;
- marker.points.push_back(point);
- }
- marker.scale.x = 0.1;
- marker.scale.y = 0.1;
- marker.color.a = 1.0;
- marker.color.r = 1.0;
- marker.color.g = 0.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
- }
- }
-}
-
-
-void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
-{
- if ( via_points.empty() || printErrorWhenNotInitialized() )
- return;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = ns;
- marker.id = 0;
- marker.type = visualization_msgs::msg::Marker::POINTS;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.lifetime = rclcpp::Duration(2, 0);
- marker.pose.orientation.w = 1.0;
-
- for (std::size_t i=0; i < via_points.size(); ++i)
- {
- geometry_msgs::msg::Point point;
- point.x = via_points[i].x();
- point.y = via_points[i].y();
- point.z = 0;
- marker.points.push_back(point);
- }
-
- marker.scale.x = 0.1;
- marker.scale.y = 0.1;
- marker.color.a = 1.0;
- marker.color.r = 0.0;
- marker.color.g = 0.0;
- marker.color.b = 1.0;
-
- teb_marker_pub_->publish( marker );
-}
-
-void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns)
-{
-if ( printErrorWhenNotInitialized() )
- return;
-
- visualization_msgs::msg::Marker marker;
- marker.header.frame_id = cfg_->map_frame;
- marker.header.stamp = nh_->now();
- marker.ns = ns;
- marker.id = 0;
- marker.type = visualization_msgs::msg::Marker::LINE_LIST;
- marker.action = visualization_msgs::msg::Marker::ADD;
- marker.pose.orientation.w = 1.0;
-
- // Iterate through teb pose sequence
- for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb )
- {
- // iterate single poses
- PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin();
- TimeDiffSequence::const_iterator it_timediff = it_teb->get()->teb().timediffs().begin();
- PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end();
- std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one.
- double time = 0;
-
- while (it_pose != it_pose_end)
- {
- geometry_msgs::msg::Point point_start;
- point_start.x = (*it_pose)->x();
- point_start.y = (*it_pose)->y();
- point_start.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
- marker.points.push_back(point_start);
-
- time += (*it_timediff)->dt();
-
- geometry_msgs::msg::Point point_end;
- point_end.x = (*boost::next(it_pose))->x();
- point_end.y = (*boost::next(it_pose))->y();
- point_end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
- marker.points.push_back(point_end);
- ++it_pose;
- ++it_timediff;
- }
- }
- marker.scale.x = 0.01;
- marker.color.a = 1.0;
- marker.color.r = 0.5;
- marker.color.g = 1.0;
- marker.color.b = 0.0;
-
- teb_marker_pub_->publish( marker );
-}
-
-void TebVisualization::publishFeedbackMessage(const std::vector< std::shared_ptr<TebOptimalPlanner> >& teb_planners,
- unsigned int selected_trajectory_idx, const ObstContainer& obstacles)
-{
- teb_msgs::msg::FeedbackMsg msg;
- msg.header.stamp = nh_->now();
- msg.header.frame_id = cfg_->map_frame;
- msg.selected_trajectory_idx = selected_trajectory_idx;
-
-
- msg.trajectories.resize(teb_planners.size());
-
- // Iterate through teb pose sequence
- std::size_t idx_traj = 0;
- for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj )
- {
- msg.trajectories[idx_traj].header = msg.header;
- it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory);
- }
-
- // add obstacles
- msg.obstacles_msg.obstacles.resize(obstacles.size());
- for (std::size_t i=0; i<obstacles.size(); ++i)
- {
- msg.obstacles_msg.header = msg.header;
-
- // copy polygon
- msg.obstacles_msg.obstacles[i].header = msg.header;
- obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
-
- // copy id
- msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
-
- // orientation
- //msg.obstacles_msg.obstacles[i].orientation =; // TODO
-
- // copy velocities
- obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
- }
-
- feedback_pub_->publish(msg);
-}
-
-void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
-{
- teb_msgs::msg::FeedbackMsg msg;
- msg.header.stamp = nh_->now();
- msg.header.frame_id = cfg_->map_frame;
- msg.selected_trajectory_idx = 0;
-
- msg.trajectories.resize(1);
- msg.trajectories.front().header = msg.header;
- teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
-
- // add obstacles
- msg.obstacles_msg.obstacles.resize(obstacles.size());
- for (std::size_t i=0; i<obstacles.size(); ++i)
- {
- msg.obstacles_msg.header = msg.header;
-
- // copy polygon
- msg.obstacles_msg.obstacles[i].header = msg.header;
- obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
-
- // copy id
- msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
-
- // orientation
- //msg.obstacles_msg.obstacles[i].orientation =; // TODO
-
- // copy velocities
- obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
- }
-
- feedback_pub_->publish(msg);
-}
-
-std_msgs::msg::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b)
-{
- std_msgs::msg::ColorRGBA color;
- color.a = a;
- color.r = r;
- color.g = g;
- color.b = b;
- return color;
-}
-
-bool TebVisualization::printErrorWhenNotInitialized() const
-{
- if (!initialized_)
- {
- RCLCPP_ERROR(nh_->get_logger(), "TebVisualization class not initialized. You must call initialize or an appropriate constructor");
- return true;
- }
- return false;
-}
-
-nav2_util::CallbackReturn TebVisualization::on_configure()
-{
- // register topics
- global_plan_pub_ = nh_->create_publisher<nav_msgs::msg::Path>("global_plan", 1);;
- local_plan_pub_ = nh_->create_publisher<nav_msgs::msg::Path>("local_plan",1);
- teb_poses_pub_ = nh_->create_publisher<geometry_msgs::msg::PoseArray>("teb_poses", 1);
- teb_marker_pub_ = nh_->create_publisher<visualization_msgs::msg::Marker>("teb_markers", 1);
- feedback_pub_ = nh_->create_publisher<teb_msgs::msg::FeedbackMsg>("teb_feedback", 1);
-
- initialized_ = true;
- return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-TebVisualization::on_activate()
-{
- global_plan_pub_->on_activate();
- local_plan_pub_->on_activate();
- teb_poses_pub_->on_activate();
- teb_marker_pub_->on_activate();
- feedback_pub_->on_activate();
- return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-TebVisualization::on_deactivate()
-{
- global_plan_pub_->on_deactivate();
- local_plan_pub_->on_deactivate();
- teb_poses_pub_->on_deactivate();
- teb_marker_pub_->on_deactivate();
- feedback_pub_->on_deactivate();
- return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-TebVisualization::on_cleanup()
-{
- global_plan_pub_.reset();
- local_plan_pub_.reset();
- teb_poses_pub_.reset();
- teb_marker_pub_.reset();
- feedback_pub_.reset();
-
- return nav2_util::CallbackReturn::SUCCESS;
-}
-
-} // namespace teb_local_planner
diff --git a/src/teb_local_planner/teb_local_planner_plugin.xml b/src/teb_local_planner/teb_local_planner_plugin.xml
deleted file mode 100644
index 45d0bdf..0000000
--- a/src/teb_local_planner/teb_local_planner_plugin.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<library path="teb_local_planner">
- <class type="teb_local_planner::TebLocalPlannerROS" base_class_type="nav2_core::Controller">
- <description>
- The teb_local_planner package implements a plugin
- to the base_local_planner of the 2D navigation stack.
- </description>
- </class>
-</library>
-
diff --git a/src/teb_local_planner/test/homotopy_class_planner_test.cpp b/src/teb_local_planner/test/homotopy_class_planner_test.cpp
deleted file mode 100644
index 8f9251b..0000000
--- a/src/teb_local_planner/test/homotopy_class_planner_test.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "teb_local_planner/homotopy_class_planner.h"
-#include <rclcpp/rclcpp.hpp>
-
-#include <gtest/gtest.h>
-
-class HomotopyClassPlannerTest : public teb_local_planner::HomotopyClassPlanner {
- public:
- void SetUp(rclcpp::Node::SharedPtr node) {
- teb_local_planner::RobotFootprintModelPtr robot_model;
- teb_local_planner::TebVisualizationPtr visualization;
- teb_local_planner::HomotopyClassPlannerPtr homotopy_class_planner;
-
- robot_model.reset(new teb_local_planner::CircularRobotFootprint(0.25));
-
- obstacles.push_back(
- teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 2))
- );
-
- obstacles.push_back(
- teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 3))
- );
-
- obstacles.push_back(
- teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 4))
- );
-
- visualization.reset(
- new teb_local_planner::TebVisualization(node, cfg)
- );
-
- cfg.hcp.visualize_hc_graph = true;
-
- initialize(node, cfg, &obstacles, robot_model, visualization);
- }
- teb_local_planner::ObstContainer obstacles;
- teb_local_planner::TebConfig cfg;
-};
-
-TEST(test, test) {
- HomotopyClassPlannerTest test;
- rclcpp::Node::SharedPtr node( new rclcpp::Node("test") );
- test.SetUp(node);
-
- using namespace teb_local_planner;
- PoseSE2 start(0, 0, 0);
- PoseSE2 goal(5, 5, 0);
- geometry_msgs::msg::Twist twist;
- twist.linear.x = 0;
- twist.linear.y = 0;
- twist.angular.z = 0;
-
- std::vector<geometry_msgs::msg::PoseStamped> initial_plan;
-
- geometry_msgs::msg::PoseStamped start_pose;
- geometry_msgs::msg::PoseStamped goal_pose;
-
- start.toPoseMsg(start_pose.pose);
- goal.toPoseMsg(goal_pose.pose);
-
- initial_plan.push_back(start_pose);
- initial_plan.push_back(goal_pose);
-
- test.plan(initial_plan, &twist, false);
- //homotopy_class_planner_->exploreEquivalenceClassesAndInitTebs(start, goal, 0.3, &twist);
-
- rclcpp::Rate rate(10);
- while(rclcpp::ok()) {
- test.visualize();
- rclcpp::spin_some(node);
- rate.sleep();
- }
-
- ASSERT_TRUE(true);
-}
-
-int main(int argc, char** argv) {
- rclcpp::init(argc, argv);
-
- testing::InitGoogleTest(&argc, argv);
-
- return RUN_ALL_TESTS();
-}
\ No newline at end of file
diff --git a/src/teb_local_planner/test/teb_basics.cpp b/src/teb_local_planner/test/teb_basics.cpp
deleted file mode 100644
index b490974..0000000
--- a/src/teb_local_planner/test/teb_basics.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-#include <gtest/gtest.h>
-
-#include "teb_local_planner/timed_elastic_band.h"
-
-TEST(TEBBasic, autoResizeLargeValueAtEnd)
-{
- double dt = 0.1;
- double dt_hysteresis = dt/3.;
- teb_local_planner::TimedElasticBand teb;
-
- teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
- for (int i = 1; i < 10; ++i) {
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
- }
- // add a pose with a large timediff as the last one
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2*dt_hysteresis);
-
- // auto resize + test of the result
- teb.autoResize(dt, dt_hysteresis, 3, 100, false);
- for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
- ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
- ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
- }
-}
-
-TEST(TEBBasic, autoResizeSmallValueAtEnd)
-{
- double dt = 0.1;
- double dt_hysteresis = dt/3.;
- teb_local_planner::TimedElasticBand teb;
-
- teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
- for (int i = 1; i < 10; ++i) {
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
- }
- // add a pose with a small timediff as the last one
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
-
- // auto resize + test of the result
- teb.autoResize(dt, dt_hysteresis, 3, 100, false);
- for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
- ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
- ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
- }
-}
-
-TEST(TEBBasic, autoResize)
-{
- double dt = 0.1;
- double dt_hysteresis = dt/3.;
- teb_local_planner::TimedElasticBand teb;
-
- teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
- for (int i = 1; i < 10; ++i) {
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
- }
- // modify the timediff in the middle and add a pose with a smaller timediff as the last one
- teb.TimeDiff(5) = dt + 2*dt_hysteresis;
- teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
-
- // auto resize
- teb.autoResize(dt, dt_hysteresis, 3, 100, false);
- for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
- ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
- ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
- }
-}
-
-int main(int argc, char** argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/src/teb_msgs/CMakeLists.txt b/src/teb_msgs/CMakeLists.txt
deleted file mode 100644
index b3ad44e..0000000
--- a/src/teb_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,28 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-
-project(teb_msgs)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(costmap_converter_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-rosidl_generate_interfaces(teb_msgs
- "msg/FeedbackMsg.msg"
- "msg/TrajectoryMsg.msg"
- "msg/TrajectoryPointMsg.msg"
- DEPENDENCIES builtin_interfaces costmap_converter_msgs geometry_msgs std_msgs
-)
-
-ament_package()
diff --git a/src/teb_msgs/msg/FeedbackMsg.msg b/src/teb_msgs/msg/FeedbackMsg.msg
deleted file mode 100644
index aa97c96..0000000
--- a/src/teb_msgs/msg/FeedbackMsg.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-# Message that contains intermediate results
-# and diagnostics of the (predictive) planner.
-
-std_msgs/Header header
-
-# The planned trajectory (or if multiple plans exist, all of them)
-teb_msgs/TrajectoryMsg[] trajectories
-
-# Index of the trajectory in 'trajectories' that is selected currently
-uint16 selected_trajectory_idx
-
-# List of active obstacles
-costmap_converter_msgs/ObstacleArrayMsg obstacles_msg
-
-
diff --git a/src/teb_msgs/msg/TrajectoryMsg.msg b/src/teb_msgs/msg/TrajectoryMsg.msg
deleted file mode 100644
index 14bf608..0000000
--- a/src/teb_msgs/msg/TrajectoryMsg.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# Message that contains a trajectory for mobile robot navigation
-
-std_msgs/Header header
-teb_msgs/TrajectoryPointMsg[] trajectory
-
-
diff --git a/src/teb_msgs/msg/TrajectoryPointMsg.msg b/src/teb_msgs/msg/TrajectoryPointMsg.msg
deleted file mode 100644
index 47b186a..0000000
--- a/src/teb_msgs/msg/TrajectoryPointMsg.msg
+++ /dev/null
@@ -1,21 +0,0 @@
-# Message that contains single point on a trajectory suited for mobile navigation.
-# The trajectory is described by a sequence of poses, velocities,
-# accelerations and temporal information.
-
-# Why this message type?
-# nav_msgs/Path describes only a path without temporal information.
-# trajectory_msgs package contains only messages for joint trajectories.
-
-# The pose of the robot
-geometry_msgs/Pose pose
-
-# Corresponding velocity
-geometry_msgs/Twist velocity
-
-# Corresponding acceleration
-geometry_msgs/Twist acceleration
-
-builtin_interfaces/Duration time_from_start
-
-
-
diff --git a/src/teb_msgs/package.xml b/src/teb_msgs/package.xml
deleted file mode 100644
index 9cfb7d0..0000000
--- a/src/teb_msgs/package.xml
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
- <name>teb_msgs</name>
- <version>0.0.1</version>
- <description>A package containing message definitions for teb_local_planner.</description>
- <maintainer email="vinnam.kim@gmail.com">Vinnam Kim</maintainer>
- <license>Apache License 2.0</license>
-
- <buildtool_depend>ament_cmake</buildtool_depend>
-
- <buildtool_depend>rosidl_default_generators</buildtool_depend>
-
- <build_depend>builtin_interfaces</build_depend>
- <build_depend>costmap_converter_msgs</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>std_msgs</build_depend>
-
- <exec_depend>builtin_interfaces</exec_depend>
- <exec_depend>costmap_converter_msgs</exec_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <exec_depend>rosidl_default_runtime</exec_depend>
- <exec_depend>std_msgs</exec_depend>
-
- <test_depend>ament_lint_common</test_depend>
-
- <member_of_group>rosidl_interface_packages</member_of_group>
-
- <export>
- <build_type>ament_cmake</build_type>
- </export>
-</package>
\ No newline at end of file
--
GitLab
From 51c315005a376ea9a624164ce23707fafe6fc6bd Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Tue, 16 Apr 2024 21:10:16 +0300
Subject: [PATCH 40/64] corrected name of func convert_to_3d
---
src/localisation/localisation/localisation_node.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index e68e647..c077277 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -152,7 +152,7 @@ class LocalisationNode(Node):
filt_msg = self.get_x_y_yaw_pose_filtered()
self.filtered_pose_pub.publish(filt_msg)
- msg_3d = self.convert(filt_msg)
+ msg_3d = self.convert_to_3d(filt_msg)
self.filtered_pose_3d_pub.publish(msg_3d)
def convert_to_3d(self, pose2d):
--
GitLab
From 572f14690eb691fb162917f22d46d4896dbe824a Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Wed, 17 Apr 2024 00:00:06 +0300
Subject: [PATCH 41/64] ros2_aruco added to gitsubmodules
---
.gitmodules | 3 +
src/command/CMakeLists.txt | 34 ------
src/command/LICENSE | 202 ----------------------------------
src/command/msg/Command.msg | 3 -
src/command/msg/Trajector.msg | 0
src/command/package.xml | 25 -----
6 files changed, 3 insertions(+), 264 deletions(-)
delete mode 100644 src/command/CMakeLists.txt
delete mode 100644 src/command/LICENSE
delete mode 100644 src/command/msg/Command.msg
delete mode 100644 src/command/msg/Trajector.msg
delete mode 100644 src/command/package.xml
diff --git a/.gitmodules b/.gitmodules
index 31d07d2..a9c077c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -12,3 +12,6 @@
path = src/teb_local_planner
url = https://github.com/rst-tu-dortmund/teb_local_planner.git
branch = ros2-master
+[submodule "src/ros2_aruco"]
+ path = src/ros2_aruco
+ url = https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco.git
diff --git a/src/command/CMakeLists.txt b/src/command/CMakeLists.txt
deleted file mode 100644
index 0bbd0f2..0000000
--- a/src/command/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(command)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- # the following line skips the linter which checks for copyrights
- # comment the line when a copyright and license is added to all source files
- set(ament_cmake_copyright_FOUND TRUE)
- # the following line skips cpplint (only works in a git repo)
- # comment the line when this package is in a git repo and when
- # a copyright and license is added to all source files
- set(ament_cmake_cpplint_FOUND TRUE)
- ament_lint_auto_find_test_dependencies()
-endif()
-
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- "msg/Command.msg"
- DEPENDENCIES geometry_msgs
-)
-
-ament_package()
diff --git a/src/command/LICENSE b/src/command/LICENSE
deleted file mode 100644
index d645695..0000000
--- a/src/command/LICENSE
+++ /dev/null
@@ -1,202 +0,0 @@
-
- Apache License
- Version 2.0, January 2004
- http://www.apache.org/licenses/
-
- TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
- 1. Definitions.
-
- "License" shall mean the terms and conditions for use, reproduction,
- and distribution as defined by Sections 1 through 9 of this document.
-
- "Licensor" shall mean the copyright owner or entity authorized by
- the copyright owner that is granting the License.
-
- "Legal Entity" shall mean the union of the acting entity and all
- other entities that control, are controlled by, or are under common
- control with that entity. For the purposes of this definition,
- "control" means (i) the power, direct or indirect, to cause the
- direction or management of such entity, whether by contract or
- otherwise, or (ii) ownership of fifty percent (50%) or more of the
- outstanding shares, or (iii) beneficial ownership of such entity.
-
- "You" (or "Your") shall mean an individual or Legal Entity
- exercising permissions granted by this License.
-
- "Source" form shall mean the preferred form for making modifications,
- including but not limited to software source code, documentation
- source, and configuration files.
-
- "Object" form shall mean any form resulting from mechanical
- transformation or translation of a Source form, including but
- not limited to compiled object code, generated documentation,
- and conversions to other media types.
-
- "Work" shall mean the work of authorship, whether in Source or
- Object form, made available under the License, as indicated by a
- copyright notice that is included in or attached to the work
- (an example is provided in the Appendix below).
-
- "Derivative Works" shall mean any work, whether in Source or Object
- form, that is based on (or derived from) the Work and for which the
- editorial revisions, annotations, elaborations, or other modifications
- represent, as a whole, an original work of authorship. For the purposes
- of this License, Derivative Works shall not include works that remain
- separable from, or merely link (or bind by name) to the interfaces of,
- the Work and Derivative Works thereof.
-
- "Contribution" shall mean any work of authorship, including
- the original version of the Work and any modifications or additions
- to that Work or Derivative Works thereof, that is intentionally
- submitted to Licensor for inclusion in the Work by the copyright owner
- or by an individual or Legal Entity authorized to submit on behalf of
- the copyright owner. For the purposes of this definition, "submitted"
- means any form of electronic, verbal, or written communication sent
- to the Licensor or its representatives, including but not limited to
- communication on electronic mailing lists, source code control systems,
- and issue tracking systems that are managed by, or on behalf of, the
- Licensor for the purpose of discussing and improving the Work, but
- excluding communication that is conspicuously marked or otherwise
- designated in writing by the copyright owner as "Not a Contribution."
-
- "Contributor" shall mean Licensor and any individual or Legal Entity
- on behalf of whom a Contribution has been received by Licensor and
- subsequently incorporated within the Work.
-
- 2. Grant of Copyright License. Subject to the terms and conditions of
- this License, each Contributor hereby grants to You a perpetual,
- worldwide, non-exclusive, no-charge, royalty-free, irrevocable
- copyright license to reproduce, prepare Derivative Works of,
- publicly display, publicly perform, sublicense, and distribute the
- Work and such Derivative Works in Source or Object form.
-
- 3. Grant of Patent License. Subject to the terms and conditions of
- this License, each Contributor hereby grants to You a perpetual,
- worldwide, non-exclusive, no-charge, royalty-free, irrevocable
- (except as stated in this section) patent license to make, have made,
- use, offer to sell, sell, import, and otherwise transfer the Work,
- where such license applies only to those patent claims licensable
- by such Contributor that are necessarily infringed by their
- Contribution(s) alone or by combination of their Contribution(s)
- with the Work to which such Contribution(s) was submitted. If You
- institute patent litigation against any entity (including a
- cross-claim or counterclaim in a lawsuit) alleging that the Work
- or a Contribution incorporated within the Work constitutes direct
- or contributory patent infringement, then any patent licenses
- granted to You under this License for that Work shall terminate
- as of the date such litigation is filed.
-
- 4. Redistribution. You may reproduce and distribute copies of the
- Work or Derivative Works thereof in any medium, with or without
- modifications, and in Source or Object form, provided that You
- meet the following conditions:
-
- (a) You must give any other recipients of the Work or
- Derivative Works a copy of this License; and
-
- (b) You must cause any modified files to carry prominent notices
- stating that You changed the files; and
-
- (c) You must retain, in the Source form of any Derivative Works
- that You distribute, all copyright, patent, trademark, and
- attribution notices from the Source form of the Work,
- excluding those notices that do not pertain to any part of
- the Derivative Works; and
-
- (d) If the Work includes a "NOTICE" text file as part of its
- distribution, then any Derivative Works that You distribute must
- include a readable copy of the attribution notices contained
- within such NOTICE file, excluding those notices that do not
- pertain to any part of the Derivative Works, in at least one
- of the following places: within a NOTICE text file distributed
- as part of the Derivative Works; within the Source form or
- documentation, if provided along with the Derivative Works; or,
- within a display generated by the Derivative Works, if and
- wherever such third-party notices normally appear. The contents
- of the NOTICE file are for informational purposes only and
- do not modify the License. You may add Your own attribution
- notices within Derivative Works that You distribute, alongside
- or as an addendum to the NOTICE text from the Work, provided
- that such additional attribution notices cannot be construed
- as modifying the License.
-
- You may add Your own copyright statement to Your modifications and
- may provide additional or different license terms and conditions
- for use, reproduction, or distribution of Your modifications, or
- for any such Derivative Works as a whole, provided Your use,
- reproduction, and distribution of the Work otherwise complies with
- the conditions stated in this License.
-
- 5. Submission of Contributions. Unless You explicitly state otherwise,
- any Contribution intentionally submitted for inclusion in the Work
- by You to the Licensor shall be under the terms and conditions of
- this License, without any additional terms or conditions.
- Notwithstanding the above, nothing herein shall supersede or modify
- the terms of any separate license agreement you may have executed
- with Licensor regarding such Contributions.
-
- 6. Trademarks. This License does not grant permission to use the trade
- names, trademarks, service marks, or product names of the Licensor,
- except as required for reasonable and customary use in describing the
- origin of the Work and reproducing the content of the NOTICE file.
-
- 7. Disclaimer of Warranty. Unless required by applicable law or
- agreed to in writing, Licensor provides the Work (and each
- Contributor provides its Contributions) on an "AS IS" BASIS,
- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
- implied, including, without limitation, any warranties or conditions
- of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
- PARTICULAR PURPOSE. You are solely responsible for determining the
- appropriateness of using or redistributing the Work and assume any
- risks associated with Your exercise of permissions under this License.
-
- 8. Limitation of Liability. In no event and under no legal theory,
- whether in tort (including negligence), contract, or otherwise,
- unless required by applicable law (such as deliberate and grossly
- negligent acts) or agreed to in writing, shall any Contributor be
- liable to You for damages, including any direct, indirect, special,
- incidental, or consequential damages of any character arising as a
- result of this License or out of the use or inability to use the
- Work (including but not limited to damages for loss of goodwill,
- work stoppage, computer failure or malfunction, or any and all
- other commercial damages or losses), even if such Contributor
- has been advised of the possibility of such damages.
-
- 9. Accepting Warranty or Additional Liability. While redistributing
- the Work or Derivative Works thereof, You may choose to offer,
- and charge a fee for, acceptance of support, warranty, indemnity,
- or other liability obligations and/or rights consistent with this
- License. However, in accepting such obligations, You may act only
- on Your own behalf and on Your sole responsibility, not on behalf
- of any other Contributor, and only if You agree to indemnify,
- defend, and hold each Contributor harmless for any liability
- incurred by, or claims asserted against, such Contributor by reason
- of your accepting any such warranty or additional liability.
-
- END OF TERMS AND CONDITIONS
-
- APPENDIX: How to apply the Apache License to your work.
-
- To apply the Apache License to your work, attach the following
- boilerplate notice, with the fields enclosed by brackets "[]"
- replaced with your own identifying information. (Don't include
- the brackets!) The text should be enclosed in the appropriate
- comment syntax for the file format. We also recommend that a
- file or class name and description of purpose be included on the
- same "printed page" as the copyright notice for easier
- identification within third-party archives.
-
- Copyright [yyyy] [name of copyright owner]
-
- Licensed under the Apache License, Version 2.0 (the "License");
- you may not use this file except in compliance with the License.
- You may obtain a copy of the License at
-
- http://www.apache.org/licenses/LICENSE-2.0
-
- Unless required by applicable law or agreed to in writing, software
- distributed under the License is distributed on an "AS IS" BASIS,
- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- See the License for the specific language governing permissions and
- limitations under the License.
diff --git a/src/command/msg/Command.msg b/src/command/msg/Command.msg
deleted file mode 100644
index 16541a5..0000000
--- a/src/command/msg/Command.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-string robot_name
-
-geometry_msgs/Twist velocity
\ No newline at end of file
diff --git a/src/command/msg/Trajector.msg b/src/command/msg/Trajector.msg
deleted file mode 100644
index e69de29..0000000
diff --git a/src/command/package.xml b/src/command/package.xml
deleted file mode 100644
index 1410cd5..0000000
--- a/src/command/package.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
- <name>command</name>
- <version>0.0.0</version>
- <description>TODO: Package description</description>
- <maintainer email="iashin.pa@phystech.edu">funny-ded</maintainer>
- <license>Apache-2.0</license>
-
- <buildtool_depend>ament_cmake</buildtool_depend>
-
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend>
-
- <buildtool_depend>rosidl_default_generators</buildtool_depend>
- <build_depend>geometry_msgs</build_depend>
-
- <exec_depend>rosidl_default_runtime</exec_depend>
-
- <member_of_group>rosidl_interface_packages</member_of_group>
-
- <export>
- <build_type>ament_cmake</build_type>
- </export>
-</package>
--
GitLab
From 0409c64760ff517fb33074221d73fe6254d7ed10 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Wed, 17 Apr 2024 00:01:01 +0300
Subject: [PATCH 42/64] proxy protocol fixes
---
src/androsot_approach_teb/CMakeLists.txt | 3 +--
.../src/androsot_approach_teb.cpp | 27 +++++++++----------
src/proxy/package.xml | 2 --
src/proxy/proxy/CommandSender.py | 21 +++++----------
src/proxy/proxy/Proxy.py | 19 ++++++++-----
5 files changed, 33 insertions(+), 39 deletions(-)
diff --git a/src/androsot_approach_teb/CMakeLists.txt b/src/androsot_approach_teb/CMakeLists.txt
index 01d06ea..eb916fb 100644
--- a/src/androsot_approach_teb/CMakeLists.txt
+++ b/src/androsot_approach_teb/CMakeLists.txt
@@ -9,7 +9,6 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(teb_local_planner REQUIRED)
-find_package(command REQUIRED)
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
find_package(SUITESPARSE REQUIRED)
@@ -35,7 +34,7 @@ if(BUILD_TESTING)
endif()
add_executable(androsot_approach_teb src/androsot_approach_teb.cpp)
-ament_target_dependencies(androsot_approach_teb rclcpp teb_local_planner command)
+ament_target_dependencies(androsot_approach_teb rclcpp teb_local_planner)
install(TARGETS
androsot_approach_teb
diff --git a/src/androsot_approach_teb/src/androsot_approach_teb.cpp b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
index 9c857d5..f01aa54 100644
--- a/src/androsot_approach_teb/src/androsot_approach_teb.cpp
+++ b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
@@ -10,8 +10,6 @@
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/teb_local_planner_ros.h>
-#include "command/msg/command.hpp"
-
using std::placeholders::_1;
// Global vars
@@ -290,8 +288,8 @@ int main(int argc, char **argv)
rclcpp::Subscription < geometry_msgs::msg::Pose2D >::SharedPtr plan_goal_subscriber;
plan_goal_subscriber = node->create_subscription < geometry_msgs::msg::Pose2D > ("plan_goal", 1, std::bind(planGoalCallback, _1));
- rclcpp::Publisher < command::msg::Command >::SharedPtr target_vel_publisher;
- target_vel_publisher = node->create_publisher < command::msg::Command > ("plan_cmd_vel", 1);
+ rclcpp::Publisher < geometry_msgs::msg::Twist >::SharedPtr target_vel_publisher;
+ target_vel_publisher = node->create_publisher < geometry_msgs::msg::Twist > ("plan_cmd_vel", 1);
// Fill config with my parameters
fillTebConfig(test.cfg);
@@ -362,12 +360,11 @@ int main(int argc, char **argv)
test.getVelocityCommand(vx, vy, omega, lookAheadPoses);
// TODO: translation coefficients m/s -> parrots
- command::msg::Command target_vel_msg;
- target_vel_msg.robot_name = robot_name;
+ geometry_msgs::msg::Twist target_vel_msg;
- target_vel_msg.velocity.linear.x = vx;
- target_vel_msg.velocity.linear.y = vy;
- target_vel_msg.velocity.angular.z = omega;
+ target_vel_msg.linear.x = vx;
+ target_vel_msg.linear.y = vy;
+ target_vel_msg.angular.z = omega;
target_vel_publisher->publish(target_vel_msg);
@@ -379,13 +376,13 @@ int main(int argc, char **argv)
{
// Plan start and stop points are equal, publishing zero velocity
//RCLCPP_INFO(rclcpp::get_logger("stellazh_approach_teb_node"), "Planning by TEB skipped - start and goal are equal");
- command::msg::Command target_vel_msg;
-
- target_vel_msg.velocity.linear.x = 0.0;
- target_vel_msg.velocity.linear.y = 0.0;
- target_vel_msg.velocity.angular.z = 0.0;
+ geometry_msgs::msg::Twist target_vel_msg;
+
+ target_vel_msg.linear.x = 0.0;
+ target_vel_msg.linear.y = 0.0;
+ target_vel_msg.angular.z = 0.0;
- target_vel_publisher->publish(target_vel_msg);
+ target_vel_publisher->publish(target_vel_msg);
}
//executor.spin();
diff --git a/src/proxy/package.xml b/src/proxy/package.xml
index 877e19c..18edad0 100644
--- a/src/proxy/package.xml
+++ b/src/proxy/package.xml
@@ -13,8 +13,6 @@
<test_depend>python3-pytest</test_depend>
<exec_depend>rclpy</exec_depend>
-
- <exec_depend>command</exec_depend>
<export>
<build_type>ament_python</build_type>
diff --git a/src/proxy/proxy/CommandSender.py b/src/proxy/proxy/CommandSender.py
index bff3845..f1e2618 100644
--- a/src/proxy/proxy/CommandSender.py
+++ b/src/proxy/proxy/CommandSender.py
@@ -1,37 +1,30 @@
import rclpy
from rclpy.node import Node
-from command.msg import Command
-
+from geometry_msgs.msg import Twist
class CommandSender(Node):
def __init__(self):
super().__init__('androsot_command_sender')
- self.publisher_ = self.create_publisher(Command, 'androsot', 10)
+ self.publisher_ = self.create_publisher(Twist, 'plan_cmd_vel', 10)
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
- msg = Command()
+ msg = Twist()
# Hardcoded params.
# Actually this can be done by any other node with countings
- msg.robot_name = 'ROKI-0060'
- msg.command = 0
- msg.step_count = 10
- msg.step_gain = 0 #int(input())
- msg.turn_gain = 0 #int(input())
- msg.lateral_gain = int(input())
-
- if (msg.lateral_gain == 0):
- msg.step_count = 0
+ msg.linear.x = float(input()) #int(input())
+ msg.linear.y = 0.0 #int(input())
+ msg.angular.z = 0.0
self.publisher_.publish(msg)
- self.get_logger().info(f'Publishing: {msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}')
+ self.get_logger().info(f'Publishing: {msg.angular.z}|{msg.linear.y}|{msg.linear.x}')
def main(args=None):
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index a3e02c1..1973f14 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -6,16 +6,17 @@ import time
import rclpy
from rclpy.node import Node
-from command.msg import Command
+from geometry_msgs.msg import Twist
class Proxy(Node):
def __init__(self, host: str="localhost", port: int=1969,
- connection_atempts: int=10, connection_delay: float=2.5):
+ connection_atempts: int=10, connection_delay: float=2.5,
+ robot_name="ROKI-0060"):
super().__init__('androsot_proxy')
self.subscription = self.create_subscription(
- msg_type=Command,
+ msg_type=Twist,
topic='plan_cmd_vel',
callback=self.listener_callback,
qos_profile=10,
@@ -27,6 +28,8 @@ class Proxy(Node):
self.declare_parameter('n_conn_attempts', connection_atempts)
self.declare_parameter('conn_delay', connection_delay)
+ self.declare_parameter('robot_name', robot_name)
+
self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.writer = None
@@ -45,6 +48,10 @@ class Proxy(Node):
@property
def conn_delay(self):
return self.get_parameter('conn_delay').get_parameter_value().double_value
+
+ @property
+ def robot_name(self):
+ return self.get_parameter('robot_name').get_parameter_value().string_value
async def connect(self) -> None:
for _ in range(self.n_conn_attempts):
@@ -60,17 +67,17 @@ class Proxy(Node):
if self.writer is None or self.writer.is_closing():
raise ConnectionError(f"Connection refused after {self.conn_delay * self.n_conn_attempts} [sec]")
- async def listener_callback(self, msg: Command) -> None:
+ async def listener_callback(self, msg: Twist) -> None:
error_code = self.connection.getsockopt(socket.SOL_SOCKET, socket.SO_ERROR)
if error_code != 0:
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{msg.robot_name}|0|10|{msg.velocity.linear.x}|{msg.velocity.rotational.z}|{msg.velocity.linear.y}\n".encode())
+ self.writer.write(f"{self.robot_name}|0|10|{int(msg.angular.z)}|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
- self.get_logger().info(f'I heard: {msg.robot_name}|{msg.command}|{msg.step_count}|{msg.step_gain}|{msg.turn_gain}|{msg.lateral_gain}')
+ self.get_logger().info(f'I heard: {self.robot_name}|0|10|{msg.angular.z}|{msg.linear.y}|{msg.linear.x}')
async def disconnect(self) -> None:
if self.writer is not None:
--
GitLab
From c680713142811d3eb23acc229b04f8386bf988d2 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Thu, 18 Apr 2024 18:39:06 +0300
Subject: [PATCH 43/64] Last working version of container builder and command
sender
---
.gitignore | 2 ++
run_container.sh | 4 ++--
src/proxy/proxy/CommandSender.py | 4 ++--
3 files changed, 6 insertions(+), 4 deletions(-)
diff --git a/.gitignore b/.gitignore
index 0d5d416..91ce4b3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,5 @@ install
log
.vscode
*.jpg
+
+*.ipynb
diff --git a/run_container.sh b/run_container.sh
index 2a027d0..5cf2207 100755
--- a/run_container.sh
+++ b/run_container.sh
@@ -32,7 +32,7 @@ read launch_from_wavegit
if [ "$launch_from_wavegit" = "y" ]
then
-image_tag=waveregistry.mipt.ru/starkit/sahr-docker:ros2-dev
+image_tag=waveregistry.mipt.ru/starkit/sahr-docker:ros2-androsot
echo "use image tag: $image_tag"
elif [ "$launch_from_wavegit" = "n" ]
then
@@ -63,4 +63,4 @@ sudo docker run --rm -it --name $container_name --network $network_name --ipc="h
else
echo "khm: y or n. goodbye."
exit
-fi
\ No newline at end of file
+fi
diff --git a/src/proxy/proxy/CommandSender.py b/src/proxy/proxy/CommandSender.py
index f1e2618..44c2ded 100644
--- a/src/proxy/proxy/CommandSender.py
+++ b/src/proxy/proxy/CommandSender.py
@@ -18,9 +18,9 @@ class CommandSender(Node):
# Hardcoded params.
# Actually this can be done by any other node with countings
- msg.linear.x = float(input()) #int(input())
+ msg.linear.x = 0.0 #int(input())
msg.linear.y = 0.0 #int(input())
- msg.angular.z = 0.0
+ msg.angular.z = float(input())
self.publisher_.publish(msg)
--
GitLab
From c651323e39a20d1552d52b8071664c88a684b119 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Fri, 19 Apr 2024 18:58:15 +0300
Subject: [PATCH 44/64] Approach launch and config
---
.gitignore | 2 +
src/androsot_approach_teb/CMakeLists.txt | 6 +-
.../config/connections_config.json | 23 +++++++
.../launch/approach_teb.launch.py | 68 +++++++++++++++++++
src/androsot_approach_teb/package.xml | 4 +-
src/proxy/proxy/Proxy.py | 18 ++---
6 files changed, 108 insertions(+), 13 deletions(-)
create mode 100644 src/androsot_approach_teb/config/connections_config.json
create mode 100644 src/androsot_approach_teb/launch/approach_teb.launch.py
diff --git a/.gitignore b/.gitignore
index 91ce4b3..2323435 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,5 @@ log
*.jpg
*.ipynb
+
+roboLinuxPult/
\ No newline at end of file
diff --git a/src/androsot_approach_teb/CMakeLists.txt b/src/androsot_approach_teb/CMakeLists.txt
index eb916fb..9d710e6 100644
--- a/src/androsot_approach_teb/CMakeLists.txt
+++ b/src/androsot_approach_teb/CMakeLists.txt
@@ -36,9 +36,9 @@ endif()
add_executable(androsot_approach_teb src/androsot_approach_teb.cpp)
ament_target_dependencies(androsot_approach_teb rclcpp teb_local_planner)
-install(TARGETS
-androsot_approach_teb
- DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(TARGETS androsot_approach_teb DESTINATION lib/${PROJECT_NAME})
ament_package()
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
new file mode 100644
index 0000000..fb5f0e5
--- /dev/null
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -0,0 +1,23 @@
+{
+ "connections": {
+
+ "0": {
+ "device": "ttyUSB0",
+ "robot_number": "0058",
+ "port": "1969"
+ },
+
+ "1": {
+ "device": "ttyUSB1",
+ "robot_number": "0059",
+ "port": "1970"
+ },
+
+ "2": {
+ "device": "ttyUSB2",
+ "robot_number": "0060",
+ "port": "1971"
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/src/androsot_approach_teb/launch/approach_teb.launch.py b/src/androsot_approach_teb/launch/approach_teb.launch.py
new file mode 100644
index 0000000..4eb76a8
--- /dev/null
+++ b/src/androsot_approach_teb/launch/approach_teb.launch.py
@@ -0,0 +1,68 @@
+import json
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+from launch_ros.actions import Node
+
+
+class LaunchUnitsCreator:
+
+ def __init__(self, connections_config_path: str) -> None:
+ self._config = self._read_connections_config(connections_config_path)
+
+ def create_launch(self) -> LaunchDescription:
+ return LaunchDescription([
+ *self._create_robo_pult_executors(),
+ *self._create_proxy_nodes(),
+ *self._create_approach_nodes()
+ ])
+
+ @staticmethod
+ def _read_connections_config(connections_config_path: str) -> dict:
+ with open(connections_config_path) as connections_config_json:
+ conf = json.load(connections_config_json)
+
+ return conf
+
+ def _create_robo_pult_executors(self) -> list[ExecuteProcess]:
+ return [ExecuteProcess(cmd=[
+ 'sudo',
+ './roboLinuxPult/roboLinuxPult.sh',
+ connection["port"],
+ connection["device"],
+ connection["robot_number"],
+ ]) for connection in self._config["connections"].values()]
+
+ def _create_proxy_nodes(self) -> list[Node]:
+ return [
+ Node(
+ package ="proxy",
+ executable ="proxy",
+ namespace =f"robot_{id}",
+ parameters = [
+ {"port": connection["port"] },
+ {"robot_name": connection["robot_number"]},
+ ],
+ ) for id, connection in self._config["connections"].items()
+ ]
+
+ def _create_approach_nodes(self) -> list[Node]:
+ return [
+ Node(
+ package ="androsot_approach_teb",
+ executable ="androsot_approach_teb",
+ namespace =f"robot_{id}",
+ ) for id in self._config["connections"].keys()
+ ]
+
+def generate_launch_description():
+
+ connections_config = os.path.join(get_package_share_directory('androsot_approach_teb'),
+ 'config',
+ "connections_config.json")
+
+ return LaunchUnitsCreator(connections_config).create_launch()
\ No newline at end of file
diff --git a/src/androsot_approach_teb/package.xml b/src/androsot_approach_teb/package.xml
index 6e47983..a9b39af 100644
--- a/src/androsot_approach_teb/package.xml
+++ b/src/androsot_approach_teb/package.xml
@@ -9,9 +9,11 @@
<buildtool_depend>ament_cmake</buildtool_depend>
+ <build_depend>teb_local_planner</build_depend>
+
<depend>rclcpp</depend>
+ <depend>ros2launch</depend>
- <build_depend>teb_local_planner</build_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index 1973f14..b8ba197 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -10,9 +10,9 @@ from geometry_msgs.msg import Twist
class Proxy(Node):
- def __init__(self, host: str="localhost", port: int=1969,
+ def __init__(self, host: str="localhost", port: str="1969",
connection_atempts: int=10, connection_delay: float=2.5,
- robot_name="ROKI-0060"):
+ robot_number="0060"):
super().__init__('androsot_proxy')
self.subscription = self.create_subscription(
@@ -28,7 +28,7 @@ class Proxy(Node):
self.declare_parameter('n_conn_attempts', connection_atempts)
self.declare_parameter('conn_delay', connection_delay)
- self.declare_parameter('robot_name', robot_name)
+ self.declare_parameter('robot_number', robot_number)
self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.writer = None
@@ -39,7 +39,7 @@ class Proxy(Node):
@property
def port(self):
- return self.get_parameter('port').get_parameter_value().integer_value
+ return self.get_parameter('port').get_parameter_value().string_value
@property
def n_conn_attempts(self):
@@ -50,13 +50,13 @@ class Proxy(Node):
return self.get_parameter('conn_delay').get_parameter_value().double_value
@property
- def robot_name(self):
- return self.get_parameter('robot_name').get_parameter_value().string_value
+ def robot_number(self):
+ return self.get_parameter('robot_number').get_parameter_value().string_value
async def connect(self) -> None:
for _ in range(self.n_conn_attempts):
try:
- await asyncio.to_thread(self.connection.connect, (self.host, self.port))
+ await asyncio.to_thread(self.connection.connect, (self.host, int(self.port)))
_, self.writer = await asyncio.open_connection(sock=self.connection)
break
@@ -74,10 +74,10 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{self.robot_name}|0|10|{int(msg.angular.z)}|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
+ self.writer.write(f"ROKI-{self.robot_number}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
- self.get_logger().info(f'I heard: {self.robot_name}|0|10|{msg.angular.z}|{msg.linear.y}|{msg.linear.x}')
+ self.get_logger().info(f'I heard: ROKI-{self.robot_number}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
async def disconnect(self) -> None:
if self.writer is not None:
--
GitLab
From b70205f9e783bcfad284c26cad05e96d02fa5c8b Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 19 Apr 2024 19:17:38 +0300
Subject: [PATCH 45/64] added launch of multiple localisations nodes
---
.../launch/dummy_loc_multilaunch.launch.py | 58 +++++++++++++++++++
.../launch/loc_multilaunch.launch.py | 51 ++++++++++++++++
.../localisation/dump_aruco_publisher.py | 2 +-
.../localisation/localisation_node.py | 48 ++++++++-------
4 files changed, 136 insertions(+), 23 deletions(-)
create mode 100644 src/localisation/launch/dummy_loc_multilaunch.launch.py
create mode 100644 src/localisation/launch/loc_multilaunch.launch.py
diff --git a/src/localisation/launch/dummy_loc_multilaunch.launch.py b/src/localisation/launch/dummy_loc_multilaunch.launch.py
new file mode 100644
index 0000000..e7c5bbd
--- /dev/null
+++ b/src/localisation/launch/dummy_loc_multilaunch.launch.py
@@ -0,0 +1,58 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ localisation_node_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose')],
+ )
+
+ localisation_node_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose')],
+ )
+
+ localisation_node_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose')],
+ )
+
+ localisation_node_op_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose')],
+ )
+
+ localisation_node_op_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose')],
+ )
+
+ localisation_node_op_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose')],
+ )
+
+ dump_aruco_pub_node_1 = Node(
+ package="localisation",
+ executable="dump_aruco_pub_node",
+ remappings= [("/pose", "robot_1/pose")]
+ )
+
+ ld.add_action(localisation_node_0)
+ ld.add_action(localisation_node_1)
+ ld.add_action(localisation_node_2)
+ ld.add_action(localisation_node_op_0)
+ ld.add_action(localisation_node_op_1)
+ ld.add_action(localisation_node_op_2)
+ ld.add_action(dump_aruco_pub_node_1)
+
+ return ld
+
diff --git a/src/localisation/launch/loc_multilaunch.launch.py b/src/localisation/launch/loc_multilaunch.launch.py
new file mode 100644
index 0000000..c76b8b9
--- /dev/null
+++ b/src/localisation/launch/loc_multilaunch.launch.py
@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ localisation_node_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose')],
+ )
+
+ localisation_node_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose')],
+ )
+
+ localisation_node_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose')],
+ )
+
+ localisation_node_op_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose')],
+ )
+
+ localisation_node_op_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose')],
+ )
+
+ localisation_node_op_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose')],
+ )
+
+ ld.add_action(localisation_node_0)
+ ld.add_action(localisation_node_1)
+ ld.add_action(localisation_node_2)
+ ld.add_action(localisation_node_op_0)
+ ld.add_action(localisation_node_op_1)
+ ld.add_action(localisation_node_op_2)
+
+ return ld
+
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
index a40f0bc..9b6fa3c 100644
--- a/src/localisation/localisation/dump_aruco_publisher.py
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -21,7 +21,7 @@ class DumpArucoPubNode(Node):
def __init__(self):
super().__init__("dump_aruco_pub_node")
self.robot_id = 0
- self.pose_pub= self.create_publisher(PoseWithCovarianceStamped, 'aruco_pose_robot_' + str(self.robot_id), 10)
+ self.pose_pub= self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.counter = 0
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index c077277..8811ea9 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -87,15 +87,17 @@ class AlphaFilter():
return mid
class LocalisationNode(Node):
- def __init__(self):
+ def __init__(self, debug=False):
super().__init__("LocalisationNode")
+ self.debug = debug
self.robot_id = 0
self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
- self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose_robot_" + str(self.robot_id), 10)
- self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose_robot_" + str(self.robot_id), 10)
- self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
- self.rpy_vector3_pub = self.create_publisher(Vector3, 'vector3_rpy' + str(self.robot_id), 10)
- self.filtered_pose_3d_pub= self.create_publisher(PoseStamped, "/filtered_pose_3d_robot_" + str(self.robot_id), 10)
+ self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose", 10)
+ if self.debug:
+ self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
+ self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
+ self.rpy_vector3_pub = self.create_publisher(Vector3, 'vector3_rpy' + str(self.robot_id), 10)
+ self.filtered_pose_3d_pub= self.create_publisher(PoseStamped, "/filtered_pose_3d_robot_" + str(self.robot_id), 10)
self.loged_data = []
self.current_aruco_poses = {}
self.current_robot_poses = {}
@@ -114,20 +116,20 @@ class LocalisationNode(Node):
self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, sin, cos)
self.robot_pose.update(pose_sin_cos)
-
- # Extract quaternion from PoseStamped message
- quaternion = msg.pose.pose.orientation
- # Convert quaternion to RPY
- rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
- roll, pitch, yaw = rpy
+ if self.debug:
+ # Extract quaternion from PoseStamped message
+ quaternion = msg.pose.pose.orientation
+ # Convert quaternion to RPY
+ rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
+ roll, pitch, yaw = rpy
- rpy_msg = RPY()
- rpy_msg.roll, rpy_msg.pitch, rpy_msg.yaw = roll, pitch, yaw
- self.rpy_pub.publish(rpy_msg)
+ rpy_msg = RPY()
+ rpy_msg.roll, rpy_msg.pitch, rpy_msg.yaw = roll, pitch, yaw
+ self.rpy_pub.publish(rpy_msg)
- rpy_vector3_msg = Vector3()
- rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
- self.rpy_vector3_pub.publish(rpy_vector3_msg)
+ rpy_vector3_msg = Vector3()
+ rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
+ self.rpy_vector3_pub.publish(rpy_vector3_msg)
def get_x_y_yaw_pose_filtered(self):
@@ -147,13 +149,15 @@ class LocalisationNode(Node):
return pose
def timer_callback(self):
- unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
- self.unfiltered_pose_pub.publish(unfilt_msg)
+ if self.debug:
+ unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
+ self.unfiltered_pose_pub.publish(unfilt_msg)
filt_msg = self.get_x_y_yaw_pose_filtered()
self.filtered_pose_pub.publish(filt_msg)
- msg_3d = self.convert_to_3d(filt_msg)
- self.filtered_pose_3d_pub.publish(msg_3d)
+ if self.debug:
+ msg_3d = self.convert_to_3d(filt_msg)
+ self.filtered_pose_3d_pub.publish(msg_3d)
def convert_to_3d(self, pose2d):
# Create a Pose message
--
GitLab
From 92611c4ae1ef8d57286a8b717fe52ad1f4f5ddb8 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 19 Apr 2024 20:14:34 +0300
Subject: [PATCH 46/64] changed filtered pose to PoseWithCovariance
---
.../localisation/dump_aruco_publisher.py | 4 +-
.../localisation/localisation_node.py | 62 +++++++++++++++----
2 files changed, 53 insertions(+), 13 deletions(-)
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
index 9b6fa3c..7d65998 100644
--- a/src/localisation/localisation/dump_aruco_publisher.py
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -35,7 +35,7 @@ class DumpArucoPubNode(Node):
# yaw = -110 degrees
x = 3.0
y = 4.0
- z = 0.2
+ z = 0.05
q_x = 0.0
q_y = -0.819
q_z = 0.0
@@ -45,7 +45,7 @@ class DumpArucoPubNode(Node):
# yaw = 45 degrees
x = 1.0
y = -2.0
- z = 0.2
+ z = 0.25
q_x = -0.0
q_y = 0.0
q_z = 0.383
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 8811ea9..039ee3b 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,6 +1,7 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
+
from rpy_msg.msg import RPY
from geometry_msgs.msg import Vector3
import tf2_ros
@@ -47,9 +48,10 @@ from collections import deque
# return pose
class PoseSinCos():
- def __init__(self, x, y, sin, cos):
+ def __init__(self, x, y, z, sin, cos):
self.x = x
self.y = y
+ self.z = z
self.sin = sin
self.cos = cos
@@ -70,10 +72,11 @@ class AlphaFilter():
def get(self):
weight_sum = 0.0
## find yaw mid
- mid = PoseSinCos(0.0, 0.0, 0.0, 0.0)
+ mid = PoseSinCos(0.0, 0.0, 0.0, 0.0, 0.0)
for i, elem in enumerate(self.buffer):
mid.x += elem.x * self.alpha ** (len(self.buffer) - i)
mid.y += elem.y * self.alpha ** (len(self.buffer) - i)
+ mid.z += elem.z * self.alpha ** (len(self.buffer) - i)
mid.sin += elem.sin * self.alpha ** (len(self.buffer) - i)
mid.cos += elem.cos * self.alpha ** (len(self.buffer) - i)
weight_sum += self.alpha ** (len(self.buffer) - i ) #TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
@@ -82,6 +85,7 @@ class AlphaFilter():
weight_sum = 1.0
mid.x /= weight_sum
mid.y /= weight_sum
+ mid.z /= weight_sum
mid.sin /= weight_sum
mid.cos /= weight_sum
return mid
@@ -92,7 +96,7 @@ class LocalisationNode(Node):
self.debug = debug
self.robot_id = 0
self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
- self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose", 10)
+ self.filtered_pose_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose", 10)
if self.debug:
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
@@ -103,17 +107,28 @@ class LocalisationNode(Node):
self.current_robot_poses = {}
self.aruco_observed_poses = {}
self.robot_pose = AlphaFilter()
- self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.0, 0.0)
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.25, 0.0, 0.0)
+ self.robot_pose.update(self.unfiltered_last_observed_robot_pos)
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
+ xyz_cov = 1e-4
+ rpy_cov = 1e-6
+ self.covariance = [xyz_cov, 0.0, 0.0, 0.0, 0.0, 0.0,# 0 0
+ 0.0,xyz_cov, 0.0, 0.0, 0.0, 0.0,# 7
+ 0.0, 0.0, xyz_cov, 0.0, 0.0, 0.0,# 14
+ 0.0, 0.0, 0.0, rpy_cov, 0.0, 0.0,# 21
+ 0.0, 0.0, 0.0, 0.0, rpy_cov, 0.0,# 28
+ 0.0, 0.0, 0.0, 0.0, 0.0, rpy_cov]# 35
+
def aruco_observation_callback(self, msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
+ z = msg.pose.pose.position.z
sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
# print(sin, cos)
- pose_sin_cos = PoseSinCos(x, y, sin, cos)
- self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, sin, cos)
+ pose_sin_cos = PoseSinCos(x, y, z, sin, cos)
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, z, sin, cos)
self.robot_pose.update(pose_sin_cos)
if self.debug:
@@ -134,11 +149,13 @@ class LocalisationNode(Node):
def get_x_y_yaw_pose_filtered(self):
pose_sin_cos = self.robot_pose.get()
- pose = Pose2D()
+ pose = Pose()
pose.x = pose_sin_cos.x
pose.y = pose_sin_cos.y
+ pose.z = pose_sin_cos.z
pose.theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
return pose
+
def get_x_y_yaw_pose_unfiltered(self):
pose_sin_cos = self.unfiltered_last_observed_robot_pos
@@ -152,8 +169,15 @@ class LocalisationNode(Node):
if self.debug:
unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
self.unfiltered_pose_pub.publish(unfilt_msg)
- filt_msg = self.get_x_y_yaw_pose_filtered()
- self.filtered_pose_pub.publish(filt_msg)
+
+ pose_sin_cos = self.robot_pose.get()
+ pose = Pose()
+ x = pose_sin_cos.x
+ y = pose_sin_cos.y
+ z = pose_sin_cos.z
+ theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
+ filtered_msg = self.convert_to_3d_with_covariance(x, y, z, theta)
+ self.filtered_pose_pub.publish(filtered_msg)
if self.debug:
msg_3d = self.convert_to_3d(filt_msg)
@@ -175,7 +199,23 @@ class LocalisationNode(Node):
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
return pose
-
+
+ def convert_to_3d_with_covariance(self, x, y, z, theta):
+ # Create a Pose message
+ p = PoseWithCovariance()
+
+ p.pose.position.x = x
+ p.pose.position.y = y
+ p.pose.position.z = z
+ # Convert theta to quaternion for orientation
+ quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
+ p.pose.orientation.x = quaternion[0]
+ p.pose.orientation.y = quaternion[1]
+ p.pose.orientation.z = quaternion[2]
+ p.pose.orientation.w = quaternion[3]
+
+ p.covariance = self.covariance
+ return p
def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
--
GitLab
From 329cf82bf7b6bbb4a3d34028cb6dc7dc7dbdd3b8 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 19 Apr 2024 22:13:21 +0300
Subject: [PATCH 47/64] added visualisation for debug in rviz
---
.../localisation/localisation_node.py | 32 +++++++++++++++++++
1 file changed, 32 insertions(+)
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 039ee3b..a70cb21 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,6 +1,7 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
+from std_msgs.msg import Bool
from rpy_msg.msg import RPY
from geometry_msgs.msg import Vector3
@@ -97,6 +98,7 @@ class LocalisationNode(Node):
self.robot_id = 0
self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
self.filtered_pose_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose", 10)
+ self.filtered_stamped_pose_pub= self.create_publisher(PoseWithCovarianceStamped, "/stamped_filtered_pose", 10)
if self.debug:
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
@@ -119,6 +121,7 @@ class LocalisationNode(Node):
0.0, 0.0, 0.0, rpy_cov, 0.0, 0.0,# 21
0.0, 0.0, 0.0, 0.0, rpy_cov, 0.0,# 28
0.0, 0.0, 0.0, 0.0, 0.0, rpy_cov]# 35
+ self.lowest_standing_z = 0.2
def aruco_observation_callback(self, msg):
@@ -179,9 +182,18 @@ class LocalisationNode(Node):
filtered_msg = self.convert_to_3d_with_covariance(x, y, z, theta)
self.filtered_pose_pub.publish(filtered_msg)
+ filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(x, y, z, theta)
+ self.filtered_stamped_pose_pub.publish(filtered_stamped_msg)
+
if self.debug:
msg_3d = self.convert_to_3d(filt_msg)
self.filtered_pose_3d_pub.publish(msg_3d)
+
+ def is_on_ground(self, z):
+ if z < self.lowest_standing_z:
+ return True
+ else:
+ return False
def convert_to_3d(self, pose2d):
# Create a Pose message
@@ -216,6 +228,26 @@ class LocalisationNode(Node):
p.covariance = self.covariance
return p
+
+ def convert_to_3d_with_covariance_stamped(self, x, y, z, theta):
+ # Create a Pose message
+
+ p = PoseWithCovarianceStamped()
+ p.header.stamp = self.get_clock().now().to_msg()
+ p.header.frame_id = "world"
+
+ p.pose.pose.position.x = x
+ p.pose.pose.position.y = y
+ p.pose.pose.position.z = z
+ # Convert theta to quaternion for orientation
+ quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
+ p.pose.pose.orientation.x = quaternion[0]
+ p.pose.pose.orientation.y = quaternion[1]
+ p.pose.pose.orientation.z = quaternion[2]
+ p.pose.pose.orientation.w = quaternion[3]
+
+ p.pose.covariance = self.covariance
+ return p
def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
--
GitLab
From 96fff1f54321162e903798ea720cf384d7c38573 Mon Sep 17 00:00:00 2001
From: ivan-sh <shargin.ia@phystech.edu>
Date: Fri, 19 Apr 2024 22:23:24 +0300
Subject: [PATCH 48/64] added fallen topic and publisher
---
.../launch/loc_multilaunch.launch.py | 18 ++++++++++++------
.../localisation/localisation_node.py | 5 +++++
2 files changed, 17 insertions(+), 6 deletions(-)
diff --git a/src/localisation/launch/loc_multilaunch.launch.py b/src/localisation/launch/loc_multilaunch.launch.py
index c76b8b9..5e2ab2b 100644
--- a/src/localisation/launch/loc_multilaunch.launch.py
+++ b/src/localisation/launch/loc_multilaunch.launch.py
@@ -7,37 +7,43 @@ def generate_launch_description():
localisation_node_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose')],
+ remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose'),
+ ("/fallen","/robot_0/fallen")],
)
localisation_node_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose')],
+ remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose'),
+ ("/fallen","/robot_1/fallen")],
)
localisation_node_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose')],
+ remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose'),
+ ("/fallen","/robot_2/fallen")],
)
localisation_node_op_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose')],
+ remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose'),
+ ("/fallen","/opponent_robot_0/fallen")],
)
localisation_node_op_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose')],
+ remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose'),
+ ("/fallen","/opponent_robot_1/fallen")],
)
localisation_node_op_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose')],
+ remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose'),
+ ("/fallen","/opponent_robot_2/fallen")],
)
ld.add_action(localisation_node_0)
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index a70cb21..8dd8b80 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -99,6 +99,7 @@ class LocalisationNode(Node):
self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
self.filtered_pose_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose", 10)
self.filtered_stamped_pose_pub= self.create_publisher(PoseWithCovarianceStamped, "/stamped_filtered_pose", 10)
+ self.on_ground_publisher = self.create_publisher(Bool, "fallen", 10)
if self.debug:
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
@@ -185,6 +186,10 @@ class LocalisationNode(Node):
filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(x, y, z, theta)
self.filtered_stamped_pose_pub.publish(filtered_stamped_msg)
+ fallen_msg = Bool()
+ fallen_msg.data = self.is_on_ground(z)
+ self.on_ground_publisher.publish(fallen_msg)
+
if self.debug:
msg_3d = self.convert_to_3d(filt_msg)
self.filtered_pose_3d_pub.publish(msg_3d)
--
GitLab
From 31c39449d51efe8d7f56647f2dc769ca591dc78e Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 02:01:54 +0300
Subject: [PATCH 49/64] Proxy launch and config fixes
---
run_container.sh | 6 ++++--
src/androsot_approach_teb/config/connections_config.json | 6 +++---
src/androsot_approach_teb/launch/approach_teb.launch.py | 9 +++++----
src/proxy/proxy/Proxy.py | 8 ++++----
4 files changed, 16 insertions(+), 13 deletions(-)
diff --git a/run_container.sh b/run_container.sh
index 5cf2207..fb772fd 100755
--- a/run_container.sh
+++ b/run_container.sh
@@ -54,12 +54,14 @@ else
camera_devs="--device=/dev/video2"
fi
+robots_connected="--device=/dev/ttyUSB0 --device=/dev/ttyUSB1 --device=/dev/ttyUSB2"
+
if [ "$launch_on_kondo" = "y" ]
then
-sudo docker run --rm -it --name $container_name $camera_devs --ipc="host" --network $network_name -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
+sudo docker run --rm -it --name $container_name $camera_devs $robots_connected --ipc="host" --network $network_name -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
elif [ "$launch_on_kondo" = "n" ]
then
-sudo docker run --rm -it --name $container_name --network $network_name --ipc="host" -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
+sudo docker run --rm -it --name $container_name $robots_connected --network $network_name --ipc="host" -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix:rw --mount type=bind,source=$workspace_path,target=/home/root/workspace --mount type=bind,source=$HOME/.ssh,target=/home/root/.ssh $image_tag
else
echo "khm: y or n. goodbye."
exit
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
index fb5f0e5..8a1c97a 100644
--- a/src/androsot_approach_teb/config/connections_config.json
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -3,19 +3,19 @@
"0": {
"device": "ttyUSB0",
- "robot_number": "0058",
+ "robot_name": "ROKI-0058",
"port": "1969"
},
"1": {
"device": "ttyUSB1",
- "robot_number": "0059",
+ "robot_name": "ROKI-0067",
"port": "1970"
},
"2": {
"device": "ttyUSB2",
- "robot_number": "0060",
+ "robot_name": "ROKI-0060",
"port": "1971"
}
diff --git a/src/androsot_approach_teb/launch/approach_teb.launch.py b/src/androsot_approach_teb/launch/approach_teb.launch.py
index 4eb76a8..c16958a 100644
--- a/src/androsot_approach_teb/launch/approach_teb.launch.py
+++ b/src/androsot_approach_teb/launch/approach_teb.launch.py
@@ -24,7 +24,7 @@ class LaunchUnitsCreator:
@staticmethod
def _read_connections_config(connections_config_path: str) -> dict:
with open(connections_config_path) as connections_config_json:
- conf = json.load(connections_config_json)
+ conf = json.load(connections_config_json, parse_float=lambda x: str(x), parse_int=lambda x:str(x))
return conf
@@ -34,18 +34,19 @@ class LaunchUnitsCreator:
'./roboLinuxPult/roboLinuxPult.sh',
connection["port"],
connection["device"],
- connection["robot_number"],
+ connection["robot_name"][5:],
]) for connection in self._config["connections"].values()]
def _create_proxy_nodes(self) -> list[Node]:
+ print(self._config["connections"])
return [
Node(
package ="proxy",
executable ="proxy",
namespace =f"robot_{id}",
parameters = [
- {"port": connection["port"] },
- {"robot_name": connection["robot_number"]},
+ {"port": connection["port"] },
+ {"robot_name": connection["robot_name"] },
],
) for id, connection in self._config["connections"].items()
]
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index b8ba197..6218999 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -12,7 +12,7 @@ class Proxy(Node):
def __init__(self, host: str="localhost", port: str="1969",
connection_atempts: int=10, connection_delay: float=2.5,
- robot_number="0060"):
+ robot_name="ROKI-0058"):
super().__init__('androsot_proxy')
self.subscription = self.create_subscription(
@@ -28,7 +28,7 @@ class Proxy(Node):
self.declare_parameter('n_conn_attempts', connection_atempts)
self.declare_parameter('conn_delay', connection_delay)
- self.declare_parameter('robot_number', robot_number)
+ self.declare_parameter('robot_name', robot_name)
self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.writer = None
@@ -74,10 +74,10 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"ROKI-{self.robot_number}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
+ self.writer.write(f"{self.robot_number}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
- self.get_logger().info(f'I heard: ROKI-{self.robot_number}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
+ self.get_logger().info(f'I heard: {self.robot_number}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
async def disconnect(self) -> None:
if self.writer is not None:
--
GitLab
From 7229d3358960019e631f8fdb4625d3a8ac442e2a Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 02:24:02 +0300
Subject: [PATCH 50/64] Comment about ROS2 issue
---
src/androsot_approach_teb/launch/approach_teb.launch.py | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/androsot_approach_teb/launch/approach_teb.launch.py b/src/androsot_approach_teb/launch/approach_teb.launch.py
index c16958a..3701678 100644
--- a/src/androsot_approach_teb/launch/approach_teb.launch.py
+++ b/src/androsot_approach_teb/launch/approach_teb.launch.py
@@ -34,7 +34,8 @@ class LaunchUnitsCreator:
'./roboLinuxPult/roboLinuxPult.sh',
connection["port"],
connection["device"],
- connection["robot_name"][5:],
+ connection["robot_name"][5:], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
+ # found in Taiwan April 2024
]) for connection in self._config["connections"].values()]
def _create_proxy_nodes(self) -> list[Node]:
--
GitLab
From e200939a4937b6396c841921a306d6509db77705 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 20 Apr 2024 01:50:48 +0300
Subject: [PATCH 51/64] Add strategy node
Add Strategy core with simple test.
Node not tried to launch
---
src/strategy/launch/game.launch.py | 168 +++++++++++
src/strategy/package.xml | 18 ++
src/strategy/resource/strategy | 0
src/strategy/setup.cfg | 4 +
src/strategy/setup.py | 26 ++
src/strategy/strategy/__init__.py | 0
src/strategy/strategy/solo_robot_strategy.py | 288 +++++++++++++++++++
src/strategy/strategy/strategy_node.py | 107 +++++++
src/strategy/strategy/strategy_test.py | 34 +++
src/strategy/strategy/utils.py | 0
src/strategy/test/test_copyright.py | 25 ++
src/strategy/test/test_flake8.py | 25 ++
src/strategy/test/test_pep257.py | 23 ++
13 files changed, 718 insertions(+)
create mode 100644 src/strategy/launch/game.launch.py
create mode 100644 src/strategy/package.xml
create mode 100644 src/strategy/resource/strategy
create mode 100644 src/strategy/setup.cfg
create mode 100644 src/strategy/setup.py
create mode 100644 src/strategy/strategy/__init__.py
create mode 100644 src/strategy/strategy/solo_robot_strategy.py
create mode 100644 src/strategy/strategy/strategy_node.py
create mode 100644 src/strategy/strategy/strategy_test.py
create mode 100644 src/strategy/strategy/utils.py
create mode 100644 src/strategy/test/test_copyright.py
create mode 100644 src/strategy/test/test_flake8.py
create mode 100644 src/strategy/test/test_pep257.py
diff --git a/src/strategy/launch/game.launch.py b/src/strategy/launch/game.launch.py
new file mode 100644
index 0000000..d5dbd4f
--- /dev/null
+++ b/src/strategy/launch/game.launch.py
@@ -0,0 +1,168 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+import yaml
+import json
+
+
+def load_json_file(file_path):
+ with open(file_path, "r") as file:
+ return json.load(file)
+
+
+def load_yaml_file(file_path):
+ with open(file_path, "r") as file:
+ return yaml.safe_load(file)
+
+
+def create_tf_nodes(static_transforms: dict):
+ tf_nodes = []
+ for transform in static_transforms["static_transforms"]:
+ node = Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name=transform["child_frame_id"],
+ arguments=[
+ str(transform["translation"][0]),
+ str(transform["translation"][1]),
+ str(transform["translation"][2]),
+ str(transform["rotation"][0]),
+ str(transform["rotation"][1]),
+ str(transform["rotation"][2]),
+ str(transform["rotation"][3]),
+ transform["parent_frame_id"],
+ transform["child_frame_id"],
+ ],
+ )
+ tf_nodes.append(node)
+ return tf_nodes
+
+
+def create_localization_nodes(robots_config) -> list[Node]:
+ robots = load_json_file(robots_config)["robots"]
+ for robot_id in robots:
+ # preprocessor node
+ # localization node
+ pass
+
+
+def create_approach_nodes(robots_config) -> list[Node]:
+ pass
+
+
+def generate_launch_description():
+ camera_config1 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ yolo_config_dir = yolo_dir + "/config/"
+
+ tf_config = os.path.join(
+ get_package_share_directory("calibration_node"),
+ "config",
+ "static_transforms.yaml",
+ )
+
+ static_transforms = load_yaml_file(tf_config)
+ tf_nodes = create_tf_nodes(static_transforms)
+ localization_nodes = create_localization_nodes(robot_config)
+ approach_nodes = create_approach_nodes(robot_config)
+
+ return LaunchDescription(
+ [
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config1],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera1",
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera1/image_raw"),
+ ("/camera/camera_info", "/camera1/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera1/ball_on_image"),
+ ("/image_raw", "/camera1/image_raw"),
+ ],
+ parameters=[os.path.join(yolo_config_dir, "yolo_roma.yaml")],
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera2",
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ parameters=[
+ os.path.join(yolo_config_dir, "yolo_roma.yaml"),
+ ],
+ remappings=[
+ ("/ball_on_image", "/camera2/ball_on_image"),
+ ("/image_raw", "/camera2/image_raw"),
+ ],
+
+ output="screen",
+ ),
+ *localization_nodes,
+ *tf_nodes,
+ # launch localization for every robot
+ # launch approach_teb for every robot
+ # launch strategy
+ # launch pic2real camera1
+ # launch pic2real camera2
+ Node(
+ package="strategy",
+ executable="strategy_node",
+ parameters=[
+ {"robots_params": robot_config},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/strategy/package.xml b/src/strategy/package.xml
new file mode 100644
index 0000000..d23693e
--- /dev/null
+++ b/src/strategy/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>strategy</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>TODO: License declaration</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/strategy/resource/strategy b/src/strategy/resource/strategy
new file mode 100644
index 0000000..e69de29
diff --git a/src/strategy/setup.cfg b/src/strategy/setup.cfg
new file mode 100644
index 0000000..6731b2b
--- /dev/null
+++ b/src/strategy/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/strategy
+[install]
+install_scripts=$base/lib/strategy
diff --git a/src/strategy/setup.py b/src/strategy/setup.py
new file mode 100644
index 0000000..e7a11a7
--- /dev/null
+++ b/src/strategy/setup.py
@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'strategy'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='root',
+ maintainer_email='root@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'strategy_node = strategy.strategy_node:main'
+ ],
+ },
+)
diff --git a/src/strategy/strategy/__init__.py b/src/strategy/strategy/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/strategy/strategy/solo_robot_strategy.py b/src/strategy/strategy/solo_robot_strategy.py
new file mode 100644
index 0000000..d9d79c2
--- /dev/null
+++ b/src/strategy/strategy/solo_robot_strategy.py
@@ -0,0 +1,288 @@
+from dataclasses import dataclass
+from enum import Enum
+import typing as tp
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, Pose2D, TransformStamped
+import math
+
+# need struct with motions mapping and json
+KICK_RIGHT = 128
+KICK_LEFT = 64
+STAND_UP = None
+FIELD_LENGTH = 4.0
+FIELD_WIDE = 3.0
+
+
+class RobotState(Enum):
+ WAIT = 1
+ GO_TO_BALL = 2 # Approach local and global
+ APPROACH = 3
+ STAND_UP = 4
+ KICK = 5
+ GO_HOME = 6
+
+
+
+
+class CmdType(Enum):
+ GO = 1
+ MOTION = 2
+
+
+class Role(Enum):
+ ATTACKER = "attacker"
+ DEFENDER = "defender"
+ GOALKEEPER = "goalkeeper"
+
+
+class GameState(Enum):
+ START = 1
+ RUN = 2
+ PAUSED = 3
+
+
+@dataclass
+class RobotInfo:
+ robot_pose: Pose2D
+ ball_pose: Pose2D
+ game_state: GameState
+ role: Role
+ goal_pose: tp.Optional[Pose2D]
+ # Add home position
+
+@dataclass
+class Cmd:
+ type_: CmdType
+ data: tp.Any
+
+
+@dataclass
+class Transition:
+ condition: tp.Callable[..., bool]
+ action: tp.Callable[..., None]
+
+
+def angle_betwen(from_: Pose2D, to: Pose2D) -> float:
+ # calculate theta
+ # from 0 to pi okay so use acos
+ # Only forward. Russuian go! Russian go!
+ x = to.x - from_.x
+ y = to.y - from_.y
+ return math.atan2(y, x)
+
+
+def approach_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
+ go_pose = robot_info.ball_pose # copy ?
+ goal_centre = Pose2D()
+ goal_centre.x = 0 #FIELD_WIDE / 2
+ goal_centre.y = FIELD_LENGTH / 2
+ goal_centre.theta = 0
+ # FIELD_WIDE / 2, FIELD_LENGTH / 2, 0)
+ go_pose.theta = angle_betwen(
+ robot_info.robot_pose, goal_centre
+ )
+ robot_info.goal_pose = go_pose
+ return Cmd(1, go_pose)
+
+
+def kick_action(robot_info) -> tp.Optional[Cmd]:
+ # def calculate leg for kick
+
+ return Cmd(2, KICK_LEFT)
+
+
+def stand_up_action():
+ return Cmd(2, STAND_UP)
+
+
+def wait_action(robot_info) -> tp.Optional[Cmd]:
+ return None
+
+
+def go_home_action(robot_info) -> tp.Optional[Cmd]:
+ return None
+
+
+attacker_state_actions = {
+ RobotState.WAIT: wait_action,
+ RobotState.APPROACH: approach_action,
+ RobotState.KICK: kick_action,
+ RobotState.STAND_UP: stand_up_action,
+ RobotState.GO_HOME: go_home_action,
+}
+
+distance_epsilon = 0.05
+angle_epsilon_rad = 0.35 # 20 degree
+
+# Other metric ?
+def is_ball_near_to_robot(robot_info:RobotInfo):
+ print (f"Goal is {robot_info.goal_pose}")
+ print(f"Robot pose is {robot_info.robot_pose}")
+ return abs(robot_info.goal_pose.x - robot_info.robot_pose.x) < distance_epsilon and \
+ abs(robot_info.goal_pose.y - robot_info.robot_pose.y) < distance_epsilon and \
+ abs(robot_info.goal_pose.theta - robot_info.robot_pose.theta) < angle_epsilon_rad
+
+def is_robot_at_home():
+ return True
+
+@dataclass
+class StateDict():
+ states: tuple[RobotState]
+ state_transitions: dict[RobotState, Transition]
+
+attacker_state_transitions = {
+ RobotState.WAIT: StateDict((RobotState.GO_HOME, RobotState.APPROACH),{
+ RobotState.GO_HOME: Transition(
+ lambda robot_info: robot_info.game_state == GameState.START, lambda: None
+ ),
+ RobotState.APPROACH: Transition(
+ lambda robot_info: robot_info.game_state == GameState.RUN,
+ lambda: print("SET PARAM Approach On"),
+ ),
+ }),
+ RobotState.APPROACH: StateDict((RobotState.KICK, RobotState.WAIT),{
+ RobotState.KICK: Transition(
+ lambda robot_info: is_ball_near_to_robot(robot_info), lambda: print("SET param off")
+ ),
+ RobotState.WAIT: Transition(
+ lambda robot_info: robot_info.game_state == GameState.PAUSED, lambda: None
+ ),
+ }),
+ RobotState.KICK:StateDict( (RobotState.APPROACH, RobotState.WAIT),{RobotState.APPROACH: Transition(lambda robot_info: not is_ball_near_to_robot(robot_info), lambda: print("Set param on")),
+ RobotState.WAIT: Transition(lambda robot_info: True, lambda: None)}),
+ RobotState.GO_HOME:StateDict((RobotState.WAIT), {RobotState.WAIT: Transition(lambda robot_info: is_robot_at_home(robot_info),lambda: None)}),
+}
+
+class Robot(object):
+ def __init__(
+ self,
+ role: Role,
+ behaviour_dict: dict[RobotState, dict[StateDict]],
+ action_dict: dict[RobotState, tp.Callable],
+ start_robot_info
+ ):
+ self.pose:Pose2D
+ self.role = role
+ self.goal_pose:Pose2D
+ self.state = RobotState.WAIT
+ self.action_dict = action_dict
+ self.action = self.action_dict[self.state]
+ self.behaviour_dict = behaviour_dict
+ self.robot_info = start_robot_info
+ def set_robot_info(self, robot_info):
+ self.robot_info = robot_info
+ def update_pose(self, pose: Pose2D) -> None:
+ self.robot_info.robot_pose = pose
+####ALRAM update goal pose
+ def get_pose(self) -> Pose2D:
+ return self.robot_info.robot_pose
+
+ def set_ball_pose(self, ball_pose: Pose2D):
+ self.robot_info.ball_pose = ball_pose
+ def set_game_state(self, game_state: GameState):
+ self.robot_info.game_state = game_state
+ def step(self) -> None:
+ for state in self.behaviour_dict[self.state].states:
+ if (self.behaviour_dict[self.state].state_transitions[state].condition(self.robot_info)):
+ print(state)
+ self.behaviour_dict[self.state].state_transitions[state].action()#self.robot_info)
+ self.action = self.action_dict[state]
+ self.state = state
+ break
+ self.action(self.robot_info)
+
+ def change_state(self, state: RobotState) -> None:
+ if self.state not in self.behaviour_dict:
+ print(f"Current state is a terminal state: {self.state}")
+ return
+ if state not in self.behaviour_dict[state]:
+ print(f"Has no transition from {self.state} to {state}")
+ return
+ self.behaviour_dict[self.state][state]
+ # HUETA!
+ self.action = self.behaviour_dict[self.state][state].action
+ return
+
+ # def do_action(self):
+ # self.action()
+
+ def reset(self) -> None:
+ self.state = RobotState.WAIT
+ self.action = self.action_dict[self.state]
+
+action_dict = {}
+behaviour_dict = {}
+@dataclass
+class RobotTopicConfig:
+ robot_name: str
+ localization_topic: str # "/robot_" + robot_id + "/... "
+ low_level_topic: str # "/robot_" + robot_id + "/..."
+ approach_topic: str # "/robot_" + robot_id + "/..."
+
+class RobotFabrica:
+ def __init__(self, config):
+ self.config = config
+ self.robot_configs = []
+ self.robots = dict()
+ self.init_robots()
+ def init_robots(self):
+ for robot_id in self.config["robots"]:
+ self.robot_configs.append(
+ RobotTopicConfig(
+ "robot_" + robot_id,
+ "idk", "idk", "idk"
+ )
+ )
+ role_set = {role.value for role in Role}
+ if ("role" in self.config["robots"] and self.config["robots"]["role"] in role_set):
+ self.robots["robot_" + robot_id] = Robot(
+ Role(self.config["robots"]["role"]), behaviour_dict=behaviour_dict, action_dict=action_dict,
+ )
+ else:
+ default_robot_info = RobotInfo(Pose2D(), Pose2D(), GameState.START, Role.ATTACKER, Pose2D())
+ self.robots["robot_" + robot_id] = Robot(
+ Role.ATTACKER, attacker_state_transitions, attacker_state_actions,
+ default_robot_info
+ )
+
+
+ def get_robots(self) -> dict[str, Robot]:
+ return self.robots
+ def get_robots_configs(self) -> list[RobotTopicConfig]:
+ return self.robot_configs
+
+
+class Strategy(object):
+ def __init__(self, fabrica: RobotFabrica) -> None:
+ self.fabrica = fabrica
+ self.robots = self.fabrica.get_robots()
+ self.ball_pose:Pose2D # Pose2D x, y
+ self.game_state = GameState.START
+
+ def set_robot_pose(self, name: str, pose: Pose2D):
+ # try
+ self.robots[name].update_pose(pose)
+
+ def set_ball_pose(self, pose: Pose2D):
+ for robot in self.robots.values():
+ robot.set_ball_pose(pose)
+
+ def change_game_state(self, state: GameState) -> None:
+ self.game_state = state
+ for robot in self.robots.values():
+ robot.set_game_state(state)
+
+ def step(self):
+
+ for robot in self.robots.values():
+ robot.step()
+
+ def is_have_cmd(self, name: str) -> bool:
+ # try
+ self.robots[name]
+
+ def get_cmd(self, name) -> tp.Optional[Cmd]:
+ if (self.robots[name].is_have_cmd()):
+ return self.robots[name]
+ return None
+
+
diff --git a/src/strategy/strategy/strategy_node.py b/src/strategy/strategy/strategy_node.py
new file mode 100644
index 0000000..f7b6dfd
--- /dev/null
+++ b/src/strategy/strategy/strategy_node.py
@@ -0,0 +1,107 @@
+from ros2_aruco_interfaces.msg import ArucoMarkers
+
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, Pose2D, TransformStamped
+from sensor_msgs.msg import CameraInfo
+from std_msgs.msg import Int32
+from soccer_vision_2d_msgs.msg import BallArray, Ball
+import tf2_ros
+
+from image_geometry import PinholeCameraModel
+
+import rclpy
+from rclpy.node import Node
+import json
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_geometry_msgs import do_transform_point
+import tf_transformations as tf_t
+import math
+from skspatial.objects import Line, Plane
+from pyquaternion import Quaternion
+import paho.mqtt.client as mqtt
+
+import numpy as np
+from dataclasses import dataclass
+from solo_robot_strategy import Strategy, Cmd, CmdType, RobotFabrica, FieldPose
+
+
+class StrategyNode(Node):
+ def __init__(self):
+ self.declare_parameter("robots_config", rclpy.Parameter.Type.STRING_ARRAY)
+ self.robot_config = self.get_parameter("robots_config")
+ #
+ # [TODO] Param for get events from cli
+ #
+ self.fabrica = RobotFabrica(self.robot_config)
+ self.strategy = Strategy(self.fabrica)
+ self._init_topics()
+ self.timer = self.create_timer(0.01, self.step)
+ self.queue_size = 10
+
+ def _init_topics(self):
+ robot_configs = self.fabrica.get_robot_configs()
+ self.approach_pubs = {}
+ self.low_lvl_pubs = {}
+ for config in robot_configs:
+ # in perfect world tf but pose will be okay
+ self.create_subscription(
+ PoseStamped,
+ config.localization_topic,
+ lambda msg: self.strategy.set_robot_pose(
+ config.robot_name, self._msg_preprocess(msg)
+ ),
+ self.queue_size,
+ )
+
+ self.approach_pubs[config.robot_name] = self.create_publisher(
+ """Approach message type""", config.approach_topic, self.queue_size
+ )
+ self.low_lvl_pubs[config.robot_name] = self.create_publisher(
+ """low_level_msg""", config.low_level_topic, self.queue_size
+ )
+
+ def step(self):
+ self.strategy.step()
+ for name in self.robot_publishers:
+ if self.strategy.is_have_cmd(name):
+ cmd = self.strategy.get_cmd()
+ self.send_cmd(cmd)
+
+ def send_cmd(self, robot_name, cmd: Cmd):
+ if cmd.type_ == CmdType.GO:
+ msg = self._create_approach_msg("""What inside""")
+ self.approach_pubs[robot_name].publish(msg)
+ elif cmd.type_ == CmdType.KICK:
+ # Need stop approach
+ # then send kick command
+ msg = self._create_low_lvl_msg("""What inside""")
+ self.low_lvl_pubs[robot_name].publish(msg)
+
+ def _create_low_lvl_msg(robot_name):
+ # We hasnot kick api right now
+ pass
+
+ def _create_approach_msg(cmd: Cmd) -> PoseStamped:
+ pass
+
+ def _msg_preprocess(self, msg: Pose2D) -> Pose2D:
+ return Pose2D
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ strategy_node = StrategyNode()
+
+ rclpy.spin(strategy_node)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ strategy_node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/strategy/strategy/strategy_test.py b/src/strategy/strategy/strategy_test.py
new file mode 100644
index 0000000..5b5004c
--- /dev/null
+++ b/src/strategy/strategy/strategy_test.py
@@ -0,0 +1,34 @@
+import pytest
+from solo_robot_strategy import Robot, RobotState, GameState, RobotFabrica, RobotInfo, Strategy
+from dataclasses import dataclass
+
+@dataclass
+class Pose2D:
+ x: float
+ y: float
+ theta: float
+
+solo_config = {
+ "robots":
+ {
+ "0":{None}
+ }
+}
+
+def test_strategy():
+ fabrica = RobotFabrica(solo_config)
+ strategy = Strategy(fabrica)
+ strategy.change_game_state(GameState.RUN)
+ strategy.set_robot_pose("robot_0", Pose2D(0., 0.0, 0.))
+ strategy.step()
+ strategy.step()
+ strategy.set_ball_pose( Pose2D(0.,1., 0.))
+ strategy.step()
+ print(strategy.robots["robot_0"].state)
+ assert (strategy.robots["robot_0"].state == RobotState.APPROACH)
+ strategy.set_robot_pose("robot_0", Pose2D(0.0, 1.00001, 1.57))
+ strategy.step()
+ print(strategy.robots["robot_0"].state)
+ assert (strategy.robots["robot_0"].state == RobotState.KICK)
+ strategy.step()
+ assert (strategy.robots["robot_0"].state == RobotState.WAIT)
diff --git a/src/strategy/strategy/utils.py b/src/strategy/strategy/utils.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/strategy/test/test_copyright.py b/src/strategy/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/strategy/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/strategy/test/test_flake8.py b/src/strategy/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/strategy/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/strategy/test/test_pep257.py b/src/strategy/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/strategy/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From 148f7d877e6cac40d5826f6bbc2ae1da871e7d88 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 05:30:10 +0300
Subject: [PATCH 52/64] Odometry estimation node initial commit
---
.../config/connections_config.json | 2 +-
.../launch/approach_teb.launch.py | 12 +-
src/odometry_estimator/LICENSE | 202 ++++++++++++++++++
.../odometry_estimator/OdometryEstimator.py | 86 ++++++++
.../odometry_estimator/__init__.py | 0
src/odometry_estimator/package.xml | 23 ++
.../resource/odometry_estimator | 0
src/odometry_estimator/setup.cfg | 4 +
src/odometry_estimator/setup.py | 26 +++
src/odometry_estimator/test/test_copyright.py | 25 +++
src/odometry_estimator/test/test_flake8.py | 25 +++
src/odometry_estimator/test/test_pep257.py | 23 ++
src/proxy/proxy/Proxy.py | 8 +-
13 files changed, 430 insertions(+), 6 deletions(-)
create mode 100644 src/odometry_estimator/LICENSE
create mode 100644 src/odometry_estimator/odometry_estimator/OdometryEstimator.py
create mode 100644 src/odometry_estimator/odometry_estimator/__init__.py
create mode 100644 src/odometry_estimator/package.xml
create mode 100644 src/odometry_estimator/resource/odometry_estimator
create mode 100644 src/odometry_estimator/setup.cfg
create mode 100644 src/odometry_estimator/setup.py
create mode 100644 src/odometry_estimator/test/test_copyright.py
create mode 100644 src/odometry_estimator/test/test_flake8.py
create mode 100644 src/odometry_estimator/test/test_pep257.py
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
index 8a1c97a..5a858f3 100644
--- a/src/androsot_approach_teb/config/connections_config.json
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -15,7 +15,7 @@
"2": {
"device": "ttyUSB2",
- "robot_name": "ROKI-0060",
+ "robot_name": "ROKI-0057",
"port": "1971"
}
diff --git a/src/androsot_approach_teb/launch/approach_teb.launch.py b/src/androsot_approach_teb/launch/approach_teb.launch.py
index 3701678..7143b5b 100644
--- a/src/androsot_approach_teb/launch/approach_teb.launch.py
+++ b/src/androsot_approach_teb/launch/approach_teb.launch.py
@@ -18,7 +18,8 @@ class LaunchUnitsCreator:
return LaunchDescription([
*self._create_robo_pult_executors(),
*self._create_proxy_nodes(),
- *self._create_approach_nodes()
+ *self._create_approach_nodes(),
+ *self._create_odometry_estimator_node(),
])
@staticmethod
@@ -61,6 +62,15 @@ class LaunchUnitsCreator:
) for id in self._config["connections"].keys()
]
+ def _create_odometry_estimator_node(self) -> list[Node]:
+ return [
+ Node(
+ package="odometry_estimator",
+ executable="odometry_estimator",
+ namespace=f"robot_{id}",
+ ) for id in self._config["connections"].keys()
+ ]
+
def generate_launch_description():
connections_config = os.path.join(get_package_share_directory('androsot_approach_teb'),
diff --git a/src/odometry_estimator/LICENSE b/src/odometry_estimator/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/src/odometry_estimator/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/odometry_estimator/odometry_estimator/OdometryEstimator.py b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
new file mode 100644
index 0000000..630338f
--- /dev/null
+++ b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
@@ -0,0 +1,86 @@
+import rclpy
+
+from rclpy.node import Node
+
+from geometry_msgs.msg import PoseWithCovariance
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistWithCovariance
+from nav_msgs.msg import Odometry
+
+class OdometryEstimator(Node):
+
+ def __init__(self):
+ super().__init__('minimal_publisher')
+
+ self._odometry_publisher = self.create_publisher(
+ Odometry,
+ 'true_odom',
+ 10
+ )
+
+ self._localization_subscriber = self.create_subscription(
+ PoseWithCovariance,
+ 'filtered_pose',
+ self.localization_callback,
+ 10
+ )
+
+ self._twist_subscriber = self.create_subscription(
+ Twist,
+ 'plan_cmd_vel',
+ self.twist_callback,
+ 10
+ )
+
+ self.last_pose = PoseWithCovariance()
+ self.last_twist = TwistWithCovariance()
+
+ def localization_callback(self, msg: PoseWithCovariance):
+ current_odometry = self._generate_frames_info()
+
+ current_odometry.twist = self.last_twist
+
+ current_odometry.pose = msg
+ self.last_pose = msg
+
+ self._odometry_publisher.publish(current_odometry)
+
+ def twist_callback(self, msg: Twist):
+ current_odometry = self._generate_frames_info()
+
+ current_odometry.pose = self.last_pose
+
+ current_odometry.twist.twist = msg
+ current_odometry.twist.covariance = [float(0.01) for _ in range(36)]
+
+ self.last_twist = msg
+
+ self._odometry_publisher.publish(current_odometry)
+
+ def _generate_frames_info(self) -> Odometry:
+ current_odometry = Odometry()
+
+ current_odometry.header.stamp = rclpy.time.Time().to_msg()
+ current_odometry.header.frame_id = "world"
+
+ current_odometry.child_frame_id = "baselink"
+
+ return current_odometry
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ odometry_estimator = OdometryEstimator()
+
+ rclpy.spin(odometry_estimator)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ odometry_estimator.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/odometry_estimator/odometry_estimator/__init__.py b/src/odometry_estimator/odometry_estimator/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/odometry_estimator/package.xml b/src/odometry_estimator/package.xml
new file mode 100644
index 0000000..d83c5b1
--- /dev/null
+++ b/src/odometry_estimator/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>odometry_estimator</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="iashin.pa@phystech.edu">funny-ded</maintainer>
+ <license>Apache-2.0</license>
+
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <exec_depend>rclpy</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>nav_msgs</exec_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/odometry_estimator/resource/odometry_estimator b/src/odometry_estimator/resource/odometry_estimator
new file mode 100644
index 0000000..e69de29
diff --git a/src/odometry_estimator/setup.cfg b/src/odometry_estimator/setup.cfg
new file mode 100644
index 0000000..c099ec4
--- /dev/null
+++ b/src/odometry_estimator/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/odometry_estimator
+[install]
+install_scripts=$base/lib/odometry_estimator
diff --git a/src/odometry_estimator/setup.py b/src/odometry_estimator/setup.py
new file mode 100644
index 0000000..f54416c
--- /dev/null
+++ b/src/odometry_estimator/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'odometry_estimator'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='funny-ded',
+ maintainer_email='iashin.pa@phystech.edu',
+ description='TODO: Package description',
+ license='Apache-2.0',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'odometry_estimator = odometry_estimator.OdometryEstimator:main',
+ ],
+ }
+)
diff --git a/src/odometry_estimator/test/test_copyright.py b/src/odometry_estimator/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/odometry_estimator/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/odometry_estimator/test/test_flake8.py b/src/odometry_estimator/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/odometry_estimator/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/odometry_estimator/test/test_pep257.py b/src/odometry_estimator/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/odometry_estimator/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index 6218999..b9a9336 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -50,8 +50,8 @@ class Proxy(Node):
return self.get_parameter('conn_delay').get_parameter_value().double_value
@property
- def robot_number(self):
- return self.get_parameter('robot_number').get_parameter_value().string_value
+ def robot_name(self):
+ return self.get_parameter('robot_name').get_parameter_value().string_value
async def connect(self) -> None:
for _ in range(self.n_conn_attempts):
@@ -74,10 +74,10 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{self.robot_number}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
+ self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
- self.get_logger().info(f'I heard: {self.robot_number}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
+ # self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
async def disconnect(self) -> None:
if self.writer is not None:
--
GitLab
From 503d953f8cb0c981e65e2bec43a328b804d60c3b Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 11:01:22 +0300
Subject: [PATCH 53/64] Fix multicamera with usb cam builded from source
---
is_symlink | 5 +
run_container.sh | 2 +-
.../config/static_transforms.yaml | 30 +-
.../launch/multi_camera_calibration.launch.py | 8 +-
.../single_camera_calibration.launch.py | 4 +-
.../solo_camera_aruco_preprocessor.launch.py | 9 +-
src/server_main/config/camera_info.yaml | 21 -
src/server_main/config/camera_info2.yaml | 20 -
src/server_main/config/camera_info_1.yaml | 22 +
src/server_main/config/camera_info_2.yaml | 21 +
src/server_main/config/camera_info_3.yaml | 21 +
src/server_main/config/camera_info_4.yaml | 21 +
src/server_main/config/camera_info_5.yaml | 21 +
src/server_main/config/camera_info_6.yaml | 21 +
src/server_main/config/camera_params.yaml | 4 +-
src/server_main/config/camera_params2.yaml | 4 +-
src/server_main/config/robots_config.json | 14 +-
src/server_main/config/robots_config2.json | 27 +-
.../actions/generate-doxybook/action.yml | 59 +
.../actions/generate-doxygen/action.yml | 32 +
.../.github/workflows/build_test.yml | 118 ++
src/usb_cam-ros2/.github/workflows/docs.yml | 130 ++
src/usb_cam-ros2/.gitignore | 9 +
src/usb_cam-ros2/AUTHORS.md | 10 +
src/usb_cam-ros2/CHANGELOG.rst | 626 +++++++++
src/usb_cam-ros2/CMakeLists.txt | 165 +++
src/usb_cam-ros2/CONTRIBUTING.md | 3 +
src/usb_cam-ros2/LICENSE | 30 +
src/usb_cam-ros2/README.md | 189 +++
src/usb_cam-ros2/config/camera_info.yaml | 20 +
src/usb_cam-ros2/config/params_1.yaml | 23 +
src/usb_cam-ros2/config/params_2.yaml | 24 +
src/usb_cam-ros2/docs/.pages | 4 +
src/usb_cam-ros2/docs/doxybook2_config.json | 44 +
src/usb_cam-ros2/docs/doxygen.config | 389 ++++++
src/usb_cam-ros2/docs/index.md | 1 +
src/usb_cam-ros2/docs/requirements.txt | 4 +
.../include/usb_cam/constants.hpp | 157 +++
.../include/usb_cam/conversions.hpp | 65 +
.../formats/av_pixel_format_helper.hpp | 1145 +++++++++++++++++
.../include/usb_cam/formats/m420.hpp | 79 ++
.../include/usb_cam/formats/mjpeg.hpp | 256 ++++
.../include/usb_cam/formats/mono.hpp | 114 ++
.../usb_cam/formats/pixel_format_base.hpp | 218 ++++
.../include/usb_cam/formats/rgb.hpp | 63 +
.../include/usb_cam/formats/utils.hpp | 108 ++
.../include/usb_cam/formats/uyvy.hpp | 113 ++
.../include/usb_cam/formats/yuyv.hpp | 115 ++
src/usb_cam-ros2/include/usb_cam/usb_cam.hpp | 419 ++++++
.../include/usb_cam/usb_cam_node.hpp | 99 ++
src/usb_cam-ros2/include/usb_cam/utils.hpp | 201 +++
src/usb_cam-ros2/launch/__init__.py | 0
src/usb_cam-ros2/launch/camera.launch.py | 80 ++
src/usb_cam-ros2/launch/camera_config.py | 65 +
src/usb_cam-ros2/mainpage.dox | 5 +
src/usb_cam-ros2/mkdocs.yml | 15 +
src/usb_cam-ros2/msg/Format.msg | 5 +
src/usb_cam-ros2/package.xml | 54 +
src/usb_cam-ros2/scripts/show_image.py | 97 ++
src/usb_cam-ros2/src/LICENSE | 14 +
src/usb_cam-ros2/src/ros1/usb_cam_node.cpp | 271 ++++
src/usb_cam-ros2/src/ros2/usb_cam_node.cpp | 438 +++++++
src/usb_cam-ros2/src/usb_cam.cpp | 734 +++++++++++
src/usb_cam-ros2/test/gtest_main.cpp | 38 +
src/usb_cam-ros2/test/test_pixel_formats.cpp | 49 +
src/usb_cam-ros2/test/test_usb_cam_lib.cpp | 59 +
src/usb_cam-ros2/test/test_usb_cam_utils.cpp | 83 ++
static_transforms.yaml | 25 +
68 files changed, 7260 insertions(+), 79 deletions(-)
create mode 100644 is_symlink
delete mode 100644 src/server_main/config/camera_info.yaml
delete mode 100644 src/server_main/config/camera_info2.yaml
create mode 100644 src/server_main/config/camera_info_1.yaml
create mode 100644 src/server_main/config/camera_info_2.yaml
create mode 100644 src/server_main/config/camera_info_3.yaml
create mode 100644 src/server_main/config/camera_info_4.yaml
create mode 100644 src/server_main/config/camera_info_5.yaml
create mode 100644 src/server_main/config/camera_info_6.yaml
create mode 100644 src/usb_cam-ros2/.github/actions/generate-doxybook/action.yml
create mode 100644 src/usb_cam-ros2/.github/actions/generate-doxygen/action.yml
create mode 100644 src/usb_cam-ros2/.github/workflows/build_test.yml
create mode 100644 src/usb_cam-ros2/.github/workflows/docs.yml
create mode 100644 src/usb_cam-ros2/.gitignore
create mode 100644 src/usb_cam-ros2/AUTHORS.md
create mode 100644 src/usb_cam-ros2/CHANGELOG.rst
create mode 100644 src/usb_cam-ros2/CMakeLists.txt
create mode 100644 src/usb_cam-ros2/CONTRIBUTING.md
create mode 100644 src/usb_cam-ros2/LICENSE
create mode 100644 src/usb_cam-ros2/README.md
create mode 100644 src/usb_cam-ros2/config/camera_info.yaml
create mode 100644 src/usb_cam-ros2/config/params_1.yaml
create mode 100644 src/usb_cam-ros2/config/params_2.yaml
create mode 100644 src/usb_cam-ros2/docs/.pages
create mode 100644 src/usb_cam-ros2/docs/doxybook2_config.json
create mode 100644 src/usb_cam-ros2/docs/doxygen.config
create mode 120000 src/usb_cam-ros2/docs/index.md
create mode 100644 src/usb_cam-ros2/docs/requirements.txt
create mode 100644 src/usb_cam-ros2/include/usb_cam/constants.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/conversions.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/av_pixel_format_helper.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/m420.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/mjpeg.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/mono.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/pixel_format_base.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/rgb.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/utils.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/uyvy.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/formats/yuyv.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/usb_cam.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/usb_cam_node.hpp
create mode 100644 src/usb_cam-ros2/include/usb_cam/utils.hpp
create mode 100644 src/usb_cam-ros2/launch/__init__.py
create mode 100644 src/usb_cam-ros2/launch/camera.launch.py
create mode 100644 src/usb_cam-ros2/launch/camera_config.py
create mode 100644 src/usb_cam-ros2/mainpage.dox
create mode 100644 src/usb_cam-ros2/mkdocs.yml
create mode 100644 src/usb_cam-ros2/msg/Format.msg
create mode 100644 src/usb_cam-ros2/package.xml
create mode 100644 src/usb_cam-ros2/scripts/show_image.py
create mode 100644 src/usb_cam-ros2/src/LICENSE
create mode 100644 src/usb_cam-ros2/src/ros1/usb_cam_node.cpp
create mode 100644 src/usb_cam-ros2/src/ros2/usb_cam_node.cpp
create mode 100644 src/usb_cam-ros2/src/usb_cam.cpp
create mode 100644 src/usb_cam-ros2/test/gtest_main.cpp
create mode 100644 src/usb_cam-ros2/test/test_pixel_formats.cpp
create mode 100644 src/usb_cam-ros2/test/test_usb_cam_lib.cpp
create mode 100644 src/usb_cam-ros2/test/test_usb_cam_utils.cpp
create mode 100644 static_transforms.yaml
diff --git a/is_symlink b/is_symlink
new file mode 100644
index 0000000..5a5de0c
--- /dev/null
+++ b/is_symlink
@@ -0,0 +1,5 @@
+#! bin/bash
+
+if [[ -L "/sys/class/video4linux/video2" ]]; then
+ echo "It's a link!"
+fi
diff --git a/run_container.sh b/run_container.sh
index fb772fd..3f5c396 100755
--- a/run_container.sh
+++ b/run_container.sh
@@ -54,7 +54,7 @@ else
camera_devs="--device=/dev/video2"
fi
-robots_connected="--device=/dev/ttyUSB0 --device=/dev/ttyUSB1 --device=/dev/ttyUSB2"
+robots_connected="--device=/dev/ttyUSB0" # --device=/dev/ttyUSB1 --device=/dev/ttyUSB2"
if [ "$launch_on_kondo" = "y" ]
then
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index 5a976da..df35b8c 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -2,24 +2,24 @@ static_transforms:
- child_frame_id: camera1
name: camera1
parent_frame_id: world
- rotation:
- - 0.6285916431197951
- - 0.7009228787488695
- - -0.23376289532559236
- - -0.24276443935500497
+ rotation:
+ - 0.9040756200965469
+ - -0.04513508982503454
+ - 0.03903623145495232
+ - -0.4231858568611735
translation:
- - 0.896828403155739
- - 0.29078709084040627
- - 1.6071356253071611
+ - 0.2377450563929882
+ - -3.954531077024356
+ - 2.896648104324571
- child_frame_id: camera2
name: camera2
parent_frame_id: world
rotation:
- - 0.7003310187541241
- - -0.6274366713598328
- - 0.20942611904603334
- - -0.26832888078814915
+ - 0.023345895194435935
+ - 0.9217664604335917
+ - -0.3839850764247843
+ - -0.0485491779575187
translation:
- - -1.1996148871091314
- - 0.28907139176512053
- - 1.7855842385290335
+ - 0.035852423609577216
+ - 4.437697599274499
+ - 2.9021133125723435
diff --git a/src/calibration_node/launch/multi_camera_calibration.launch.py b/src/calibration_node/launch/multi_camera_calibration.launch.py
index 63a3381..b236865 100644
--- a/src/calibration_node/launch/multi_camera_calibration.launch.py
+++ b/src/calibration_node/launch/multi_camera_calibration.launch.py
@@ -15,8 +15,8 @@ def generate_launch_description():
return LaunchDescription([
Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
+ package='usb_cam_ros2',
+ executable='usb_cam_ros2_node_exe',
parameters=[camera_config],
output='screen',
namespace='camera1',
@@ -39,8 +39,8 @@ def generate_launch_description():
emulate_tty=True
),
Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
+ package='usb_cam_ros2',
+ executable='usb_cam_ros2_node_exe',
parameters=[camera_config2],
output='screen',
namespace='camera2',
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
index 7389688..0ac50ad 100644
--- a/src/calibration_node/launch/single_camera_calibration.launch.py
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -27,8 +27,8 @@ def generate_launch_description():
emulate_tty=True
),
Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
+ package='usb_cam_ros2',
+ executable='usb_cam_ros2_node_exe',
parameters=[camera_config],
output='screen',
namespace='camera',
diff --git a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
index 046fcc3..7b3e4db 100644
--- a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
@@ -53,8 +53,15 @@ def generate_launch_description():
executable='aruco_preprocessor_node',
parameters=[
{'robots_params': robot_config},
- {'camera_frames': ["camera"]}
+ {'camera_frames': ["camera1"]}
],
+
+ remappings=[
+ ('/camera/image_raw', '/camera1/image_raw'),
+ ('/pose', '/robot_0/pose'),
+ ('/camera/camera_info', '/camera1/camera_info')
+
+ ],
output='screen',
emulate_tty=True
)
diff --git a/src/server_main/config/camera_info.yaml b/src/server_main/config/camera_info.yaml
deleted file mode 100644
index 4f26561..0000000
--- a/src/server_main/config/camera_info.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-image_width: 1920
-image_height: 1080
-camera_name: camera1
-camera_matrix:
- rows: 3
- cols: 3
- data: [1454.6684146024668, 0.0, 999.049079061686, 0.0, 1439.4606636404403, 513.6532631258082, 0.0, 0.0, 1.0]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0.043097070537003644, -0.10599336336465601, 0.0012429129258369842, 0.004094904502998846, 0.0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1,0,0, 0,1,0, 0,0,1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1450.0821533203125, 0.0, 1008.4757212882178, 0.0, 0.0, 1448.0181884765625, 514.0421690974472, 0.0, 0.0, 0.0, 1.0, 0.0]
-
diff --git a/src/server_main/config/camera_info2.yaml b/src/server_main/config/camera_info2.yaml
deleted file mode 100644
index 3c89cd5..0000000
--- a/src/server_main/config/camera_info2.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-image_width: 1920
-image_height: 1080
-camera_name: camera2
-camera_matrix:
- rows: 3
- cols: 3
- data: [1419.4187950618846, 0.0, 974.9896663237244, 0.0, 1414.0655832766458, 495.6548379114915, 0.0, 0.0, 1.0]
-distortion_model: plumb_bob
-distortion_coefficients:
- rows: 1
- cols: 5
- data: [0.00920295872972792, -0.0695572098352804, 0.0018571515990045589, 0.0001661032848335393, 0.0]
-rectification_matrix:
- rows: 3
- cols: 3
- data: [1,0,0, 0,1,0, 0,0,1]
-projection_matrix:
- rows: 3
- cols: 4
- data: [1403.2264404296875, 0.0, 975.6681712616119, 0.0, 0.0, 1412.9404296875, 496.1955598228087, 0.0, 0.0, 0.0, 1.0, 0.0]
diff --git a/src/server_main/config/camera_info_1.yaml b/src/server_main/config/camera_info_1.yaml
new file mode 100644
index 0000000..2474879
--- /dev/null
+++ b/src/server_main/config/camera_info_1.yaml
@@ -0,0 +1,22 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera_1
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1705.392612, 0.0, 1128.405272, 0.0, 1704.735765, 745.835355, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.037945, -0.088068, 0.000319, -0.003593, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1701.895874, 0.0, 1118.205854, 0.0, 0.0, 1713.358154, 745.596647, 0.0, 0.0, 0.0, 1.0, 0.0]
+
+
diff --git a/src/server_main/config/camera_info_2.yaml b/src/server_main/config/camera_info_2.yaml
new file mode 100644
index 0000000..e600a84
--- /dev/null
+++ b/src/server_main/config/camera_info_2.yaml
@@ -0,0 +1,21 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera_2
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1698.266418, 0.0, 1159.147169, 0.0, 1700.911909, 707.915321, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.027982, -0.091183, -0.000038, -0.000691, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1685.891479, 0.0, 1157.314982, 0.0, 0.0, 1702.888428, 707.170405, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/server_main/config/camera_info_3.yaml b/src/server_main/config/camera_info_3.yaml
new file mode 100644
index 0000000..ec602b9
--- /dev/null
+++ b/src/server_main/config/camera_info_3.yaml
@@ -0,0 +1,21 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera1
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1667.826143, 0.0, 1138.575668, 0.0, 1670.183659, 780.932475, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.094722, -0.160338, -0.000437, 0.000212, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1682.388306, 0.0, 1137.875104, 0.0, 0.0, 1692.441040, 779.936789, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/server_main/config/camera_info_4.yaml b/src/server_main/config/camera_info_4.yaml
new file mode 100644
index 0000000..48fd40c
--- /dev/null
+++ b/src/server_main/config/camera_info_4.yaml
@@ -0,0 +1,21 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera2
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1745.606619, 0.0, 1200.659341, 0.0, 1747.554903, 754.179311, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.010119, -0.076276, 0.000320, 0.003884, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1724.136841, 0.0, 1212.366957, 0.0, 0.0, 1746.046509, 753.905950, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/server_main/config/camera_info_5.yaml b/src/server_main/config/camera_info_5.yaml
new file mode 100644
index 0000000..3a5de84
--- /dev/null
+++ b/src/server_main/config/camera_info_5.yaml
@@ -0,0 +1,21 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera_5
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1682.731043, 0.0, 1140.320594, 0.0, 1685.483053, 796.744407, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.115351, -0.200016, -0.000923, -0.000568, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1700.116211, 0.0, 1137.653527, 0.0, 0.0, 1712.949707, 795.213773, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/server_main/config/camera_info_6.yaml b/src/server_main/config/camera_info_6.yaml
new file mode 100644
index 0000000..44ef6fd
--- /dev/null
+++ b/src/server_main/config/camera_info_6.yaml
@@ -0,0 +1,21 @@
+image_width: 2304
+image_height: 1536
+camera_name: camera_6
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1671.934541, 0.0, 1174.049429, 0.0, 1673.408229, 777.959151, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.105683, -0.176839, 0.003390, 0.003059, 0.0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1688.695312, 0.0, 1182.482974, 0.0, 0.0, 1701.707764, 781.526336, 0.0, 0.0, 0.0, 1.0, 0.0]
+
diff --git a/src/server_main/config/camera_params.yaml b/src/server_main/config/camera_params.yaml
index 814104e..8704dcc 100644
--- a/src/server_main/config/camera_params.yaml
+++ b/src/server_main/config/camera_params.yaml
@@ -1,6 +1,6 @@
/**:
ros__parameters:
- video_device: "/dev/video4"
+ video_device: "/dev/video2"
framerate: 30.0
io_method: "mmap"
frame_id: "camera1"
@@ -8,7 +8,7 @@
image_width: 1920
image_height: 1080
camera_name: "camera1"
- camera_info_url: "package://server_main/config/camera_info.yaml"
+ camera_info_url: "package://server_main/config/camera_info_2.yaml"
brightness: -1
contrast: -1
saturation: -1
diff --git a/src/server_main/config/camera_params2.yaml b/src/server_main/config/camera_params2.yaml
index 837502b..91b5dc6 100644
--- a/src/server_main/config/camera_params2.yaml
+++ b/src/server_main/config/camera_params2.yaml
@@ -1,6 +1,6 @@
/**:
ros__parameters:
- video_device: "/dev/video6"
+ video_device: "/dev/video4"
framerate: 30.0
io_method: "mmap"
frame_id: "camera2"
@@ -8,7 +8,7 @@
image_width: 1920
image_height: 1080
camera_name: "camera2"
- camera_info_url: "package://server_main/config/camera_info2.yaml"
+ camera_info_url: "package://server_main/config/camera_info_6.yaml"
brightness: -1
contrast: -1
saturation: -1
diff --git a/src/server_main/config/robots_config.json b/src/server_main/config/robots_config.json
index f3dcf5f..9cc60ef 100644
--- a/src/server_main/config/robots_config.json
+++ b/src/server_main/config/robots_config.json
@@ -1,35 +1,33 @@
{
"robots": {
"0": {
- "aruco_ids": [0, 1, 2, 3, 4],
+ "aruco_ids": [0, 2, 4, 6, 8],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [-1.57, 0, -1.57]
+ [-1.57, 0, 0]
]
},
-
"1": {
- "aruco_ids": [5,6,7,8,9],
+ "aruco_ids": [10,12,14,16,18],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [0, 1.57, 1.57]
+ [-1.57, 0, 0]
]
},
-
"2": {
- "aruco_ids": [10,11,12,13,14],
+ "aruco_ids": [20,22,24,26,28],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [0, 1.57, 1.57]
+ [-1.57, 0, 0]
]
}
}
diff --git a/src/server_main/config/robots_config2.json b/src/server_main/config/robots_config2.json
index 818a916..ac75b44 100644
--- a/src/server_main/config/robots_config2.json
+++ b/src/server_main/config/robots_config2.json
@@ -1,14 +1,35 @@
{
"robots": {
+ "0": {
+ "aruco_ids": [1, 3, 5, 7, 9],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [-1.57, 0, 0]
+ ]
+ },
"1": {
- "aruco_ids": [15, 16, 17, 18, 19],
+ "aruco_ids": [11, 13, 15, 17, 19],
+ "aruco_transforms": [
+ [0, 0, 0],
+ [0, -1.57, 0],
+ [0, 3.14, 0],
+ [0, 1.57, 0],
+ [-1.57, 0, 0]
+ ]
+ },
+ "2": {
+ "aruco_ids": [21, 23, 25, 27, 29],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [0, 1.57, 1.57]
+ [-1.57, 0, 0]
]
- }
+ },
+
}
}
\ No newline at end of file
diff --git a/src/usb_cam-ros2/.github/actions/generate-doxybook/action.yml b/src/usb_cam-ros2/.github/actions/generate-doxybook/action.yml
new file mode 100644
index 0000000..03c0c59
--- /dev/null
+++ b/src/usb_cam-ros2/.github/actions/generate-doxybook/action.yml
@@ -0,0 +1,59 @@
+name: Generate doxybook2
+description: Generate markdown using doxybook2 with given configuration
+inputs:
+ input-doxygen-artifact:
+ required: true
+ description: Name of input doxygen artifact to download
+ doxygen-artifact-extraction-path:
+ required: true
+ description: Path to extract input doxygen artifact to
+ doxybook2-version:
+ required: true # TODO: make this optional
+ description: Version of doxybook2 to download and use
+ output-path:
+ required: true
+ description: The path to generate the doxybook2 markdown to
+ doxybook2-config-path:
+ required: true # TODO: make this optional with smart default
+ description: Path to the doxybook2 configuration file (including .json extension)
+ base-url:
+ required: true # TODO: make this optional by adding logic below
+ description: Base URL to overwrite the default with
+ artifact-path:
+ required: true
+ description: Path to directory to upload as artifact
+ artifact-name:
+ required: true
+ description: Name of the artifact to upload
+ artifact-retention-days:
+ required: true
+ description: Number of days to keep the artifact for
+runs:
+ using: composite
+ steps:
+ - name: Download Doxygen XML
+ uses: actions/download-artifact@v3
+ with:
+ name: ${{ inputs.input-doxygen-artifact }}
+ path: ${{ inputs.doxygen-artifact-extraction-path }}
+ - name: Build API Reference
+ shell: bash
+ run: |
+ # ensure output directory exists
+ mkdir -p ${{ inputs.output-path }}
+ export DOXYBOOK_URL=https://github.com/matusnovak/doxybook2/releases/download
+ export DOXYBOOK_URL=${DOXYBOOK_URL}/${{ inputs.doxybook2-version }}
+ export DOXYBOOK_ZIP=doxybook2-linux-amd64-${{ inputs.doxybook2-version }}.zip
+ wget ${DOXYBOOK_URL}/${DOXYBOOK_ZIP}
+ sudo apt-get install unzip
+ unzip ${DOXYBOOK_ZIP}
+ ./bin/doxybook2 --input ${{ inputs.doxygen-artifact-extraction-path }} \
+ --output ${{ inputs.output-path }} \
+ --config ${{ inputs.doxybook2-config-path }} \
+ --config-data '{"baseUrl": "${{ inputs.base-url }}"}'
+ - name: Upload API reference
+ uses: actions/upload-artifact@v3
+ with:
+ name: ${{ inputs.artifact-name }}
+ path: ${{ inputs.artifact-path }}
+ retention-days: ${{ inputs.artifact-retention-days }}
\ No newline at end of file
diff --git a/src/usb_cam-ros2/.github/actions/generate-doxygen/action.yml b/src/usb_cam-ros2/.github/actions/generate-doxygen/action.yml
new file mode 100644
index 0000000..2c769e8
--- /dev/null
+++ b/src/usb_cam-ros2/.github/actions/generate-doxygen/action.yml
@@ -0,0 +1,32 @@
+name: Generate doxygen documentation and upload as artifact
+description: Build doxygen documentation with given configuration
+inputs:
+ working-directory:
+ required: true
+ description: Working directory to run doxygen from
+ doxyfile-path:
+ required: true
+ description: The path to checkout the given repository too
+ artifact-path:
+ required: true
+ description: Path to directory to upload as artifact
+ artifact-name:
+ required: true
+ description: Name of the artifact to upload
+ artifact-retention-days:
+ required: true
+ description: Number of days to keep the artifact for
+runs:
+ using: composite
+ steps:
+ - name: Run doxygen on source
+ uses: mattnotmitt/doxygen-action@v1
+ with:
+ working-directory: ${{ inputs.working-directory }}
+ doxyfile-path: ${{ inputs.doxyfile-path }}
+ - name: Upload Doxygen XML
+ uses: actions/upload-artifact@v3
+ with:
+ name: ${{ inputs.artifact-name }}
+ path: ${{ inputs.artifact-path }}
+ retention-days: ${{ inputs.artifact-retention-days }}
\ No newline at end of file
diff --git a/src/usb_cam-ros2/.github/workflows/build_test.yml b/src/usb_cam-ros2/.github/workflows/build_test.yml
new file mode 100644
index 0000000..ae44d33
--- /dev/null
+++ b/src/usb_cam-ros2/.github/workflows/build_test.yml
@@ -0,0 +1,118 @@
+name: Build and Test
+
+on:
+ push:
+ branches:
+ - 'ros2'
+ pull_request:
+ branches:
+ - 'ros2/*'
+ - 'ros2'
+ workflow_dispatch:
+
+jobs:
+ get_ros_distros:
+ runs-on: ubuntu-latest
+ steps:
+ - name: Install dependencies
+ run: |
+ sudo apt-get update
+ sudo apt-get -y install curl jq
+ - name: Get active distributions
+ run: |
+ wget https://raw.githubusercontent.com/flynneva/active_ros_distros/0.1.0/main.py -O active_ros_distros.py
+ python3 -m pip install rosdistro
+ python3 active_ros_distros.py --distribution-type ros2
+ - name: Generate actions matrix
+ id: set-matrix
+ run: |
+ ACTIVE_ROS_DISTROS=(noetic)
+ DOCKER_DISTRO_MATRIX=()
+ RAW_DOCKER_JSON=$(curl -s "https://hub.docker.com/v2/repositories/rostooling/setup-ros-docker/tags?page_size=1000")
+ while read distro; do
+ ACTIVE_ROS_DISTROS+=( $distro )
+ done < "/tmp/active_ros_distros.txt"
+ DISTRO_STR="["
+ MATRIX_STR="["
+ for distro in ${ACTIVE_ROS_DISTROS[@]}; do
+ docker_image=$(echo $RAW_DOCKER_JSON |
+ jq -r --arg DISTRO "$distro" '.results[] | select(.tag_status=="active") | select(.name | contains("ros-base-latest")) | select(.name | contains($DISTRO)) | .name' |
+ sort -u)
+
+ # Handle the case if two docker images were declared for one distro
+ # e.g. rolling moving from one Ubuntu Focal to Ubuntu Jammy
+ docker_image_arr=($docker_image)
+
+ DISTRO_STR+="\"${distro}\", "
+ MATRIX_STR+="{docker_image:\"${docker_image_arr[-1]}\",ros_distro:\"${distro}\"}, "
+ done
+
+ # Remove trailing , at end
+ DISTRO_STR=${DISTRO_STR%??}
+ MATRIX_STR=${MATRIX_STR%??}
+ # Close up brackets
+ DISTRO_STR+="]"
+ MATRIX_STR+="]"
+ echo "DISTRO_STR: ${DISTRO_STR}"
+ echo "MATRIX_STR: ${MATRIX_STR}"
+ echo "series=$DISTRO_STR" >> $GITHUB_OUTPUT
+ echo "matrix=$MATRIX_STR" >> $GITHUB_OUTPUT
+ outputs:
+ series: ${{ steps.set-matrix.outputs.series }}
+ matrix: ${{ steps.set-matrix.outputs.matrix }}
+
+ build_and_test:
+ runs-on: [ubuntu-latest]
+ needs: get_ros_distros
+ strategy:
+ fail-fast: false
+ matrix:
+ ros_distro: ${{ fromJson(needs.get_ros_distros.outputs.series) }}
+ include:
+ ${{ fromJson(needs.get_ros_distros.outputs.matrix) }}
+ container:
+ image: rostooling/setup-ros-docker:${{ matrix.docker_image }}
+ steps:
+ - name: Setup Directories
+ run: mkdir -p ros_ws/src
+ - name: checkout
+ uses: actions/checkout@v4
+ with:
+ path: ros_ws/src
+ - name: Build and Test ROS 1
+ uses: ros-tooling/action-ros-ci@master
+ if: ${{ contains('["melodic", "noetic"]', matrix.ros_distro) }}
+ continue-on-error: true
+ with:
+ package-name: usb_cam
+ target-ros1-distro: ${{ matrix.ros_distro }}
+ vcs-repo-file-url: ""
+ - name: Build and Test ROS 2
+ id: build_and_test_step
+ uses: ros-tooling/action-ros-ci@master
+ if: ${{ contains('["melodic", "noetic"]', matrix.ros_distro) == false }}
+ with:
+ package-name: usb_cam
+ target-ros2-distro: ${{ matrix.ros_distro }}
+ vcs-repo-file-url: ""
+ colcon-defaults: |
+ {
+ "build": {
+ "mixin": ["coverage-gcc"]
+ }
+ }
+ # If possible, pin the repository in the workflow to a specific commit to avoid
+ # changes in colcon-mixin-repository from breaking your tests.
+ colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/1ddb69bedfd1f04c2f000e95452f7c24a4d6176b/index.yaml
+ - uses: actions/upload-artifact@v1
+ with:
+ name: colcon-logs-${{ matrix.ros_distro }}
+ path: ${{ steps.build_and_test_step.outputs.ros-workspace-directory-name }}/log
+ if: always()
+ continue-on-error: true
+ - uses: actions/upload-artifact@v1
+ with:
+ name: lcov-logs-${{ matrix.ros_distro }}
+ path: ${{ steps.build_and_test_step.outputs.ros-workspace-directory-name }}/lcov
+ if: always()
+ continue-on-error: true
\ No newline at end of file
diff --git a/src/usb_cam-ros2/.github/workflows/docs.yml b/src/usb_cam-ros2/.github/workflows/docs.yml
new file mode 100644
index 0000000..64183b0
--- /dev/null
+++ b/src/usb_cam-ros2/.github/workflows/docs.yml
@@ -0,0 +1,130 @@
+name: Build Documentation
+
+on:
+ push:
+ branches: [ros2]
+ pull_request:
+ branches: [ros2]
+ workflow_dispatch:
+
+env:
+ WORKSPACE_PATH: ros_ws/src/usb_cam
+ DOXYGEN_ARTIFACT: doxygen_xml
+ DOXYBOOK_ARTIFACT: api_reference
+ DOXYBOOK_VERSION: v1.4.0
+
+# only run one build doc workflow at a time, cancel any running ones
+concurrency:
+ group: ${{ github.workflow }}-${{ github.ref }}
+ cancel-in-progress: true
+
+jobs:
+ generate-doxygen:
+ runs-on: ubuntu-latest
+ steps:
+ - name: Make sure output directory exists
+ run: mkdir -p ${{ env.WORKSPACE_PATH }}
+ - name: Checkout repository
+ uses: actions/checkout@v2
+ with:
+ path: ${{ env.WORKSPACE_PATH }}
+ - name: Generate doxygen and upload as artifact
+ # TODO: figure out way to use WORKSPACE_PATH var here
+ uses: ./ros_ws/src/usb_cam/.github/actions/generate-doxygen
+ with:
+ working-directory: ${{ env.WORKSPACE_PATH }}
+ doxyfile-path: 'docs/doxygen.config'
+ artifact-path: ${{ env.WORKSPACE_PATH }}/docs/xml
+ artifact-name: ${{ env.DOXYGEN_ARTIFACT }}
+ artifact-retention-days: 30
+ generate-doxybook2:
+ runs-on: ubuntu-latest
+ needs: generate-doxygen
+ steps:
+ - name: Make sure output directory exists
+ run: mkdir -p ${{ env.WORKSPACE_PATH }}
+ - name: Checkout repository
+ uses: actions/checkout@v2
+ with:
+ path: ${{ env.WORKSPACE_PATH }}
+ - name: Generate API reference and upload as artifact
+ # TODO: figure out way to use WORKSPACE_PATH var here
+ uses: ./ros_ws/src/usb_cam/.github/actions/generate-doxybook
+ with:
+ input-doxygen-artifact: ${{ env.DOXYGEN_ARTIFACT }}
+ doxygen-artifact-extraction-path: ${{ env.WORKSPACE_PATH }}/docs/xml
+ doxybook2-version: ${{ env.DOXYBOOK_VERSION }}
+ doxybook2-config-path: ${{ env.WORKSPACE_PATH }}/docs/doxybook2_config.json
+ output-path: ${{ env.WORKSPACE_PATH }}/docs/api-reference
+ base-url: /usb_cam/latest/api-reference/
+ artifact-path: ${{ env.WORKSPACE_PATH }}/docs/api-reference
+ artifact-name: ${{ env.DOXYBOOK_ARTIFACT }}
+ artifact-retention-days: 30
+ build-mkdocs:
+ runs-on: ubuntu-latest
+ if: github.ref != 'refs/heads/ros2'
+ needs: generate-doxybook2
+ steps:
+ - name: Make sure output directory exists
+ run: mkdir -p ${{ env.WORKSPACE_PATH }}
+ - name: Checkout repository
+ uses: actions/checkout@v2
+ with:
+ path: ${{ env.WORKSPACE_PATH }}
+ - name: Download API Reference
+ uses: actions/download-artifact@v3
+ with:
+ name: ${{ env.DOXYBOOK_ARTIFACT }}
+ path: ${{ env.WORKSPACE_PATH }}/docs/api-reference
+ - name: Build mkdocs site
+ run: |
+ cd ${{ env.WORKSPACE_PATH }}
+ # ensure gh-pages git history is fetched
+ git fetch origin gh-pages --depth=1
+ sudo apt-get update -y
+ # install mkdocs dependencies
+ python3 -m pip install -r docs/requirements.txt
+ # build site
+ mkdocs build
+ - name: Upload docs site
+ uses: actions/upload-artifact@v3
+ with:
+ name: usb_cam_site
+ path: ${{ env.WORKSPACE_PATH }}/site
+ deploy_docs:
+ runs-on: ubuntu-latest
+ # only run on main ros2 branch after jobs listed in `needs` have finished (successful or not)
+ if: github.ref == 'refs/heads/ros2' && always()
+ needs: [build-mkdocs]
+ steps:
+ - name: Make sure output directory exists
+ run: mkdir -p ${{ env.WORKSPACE_PATH }}
+ - name: Checkout repository
+ uses: actions/checkout@v2
+ with:
+ path: ${{ env.WORKSPACE_PATH }}
+ - name: Download API reference
+ uses: actions/download-artifact@v3
+ with:
+ name: ${{ env.DOXYBOOK_ARTIFACT }}
+ path: ${{ env.WORKSPACE_PATH }}/docs/api-reference
+ - name: Deploy mkdocs site
+ shell: bash
+ run: |
+ cd ${{ env.WORKSPACE_PATH }}
+ # ensure gh-pages git history is fetched
+ git fetch origin gh-pages --depth=1
+ sudo apt-get update -y
+ # install docs dependencies
+ python3 -m pip install -r docs/requirements.txt
+ # TODO: mike rebuilds entire site, instead we should
+ # skip the build and download site artifact from previous workflow
+ if [ -z ${{ github.event.release.tag_name }}]; then
+ export NEW_VERSION=main
+ else
+ export NEW_VERSION=${{ github.event.release.tag_name }}
+ fi
+ git config user.name doc-bot
+ git config user.email doc-bot@usb_cam.com
+ mike deploy --push --update-aliases $NEW_VERSION latest
+
diff --git a/src/usb_cam-ros2/.gitignore b/src/usb_cam-ros2/.gitignore
new file mode 100644
index 0000000..719d27c
--- /dev/null
+++ b/src/usb_cam-ros2/.gitignore
@@ -0,0 +1,9 @@
+.cproject
+.project
+.settings
+**__pycache__
+
+
+# documentation
+docs/xml
+site/
\ No newline at end of file
diff --git a/src/usb_cam-ros2/AUTHORS.md b/src/usb_cam-ros2/AUTHORS.md
new file mode 100644
index 0000000..6d04803
--- /dev/null
+++ b/src/usb_cam-ros2/AUTHORS.md
@@ -0,0 +1,10 @@
+Original Authors
+----------------
+
+ * [Benjamin Pitzer] (benjamin.pitzer@bosch.com)
+
+Contributors
+------------
+
+ * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu)
+ * [Evan Flynn](https://www.flynnlabs.dev) (evanflynn.msu@gmail.com)
diff --git a/src/usb_cam-ros2/CHANGELOG.rst b/src/usb_cam-ros2/CHANGELOG.rst
new file mode 100644
index 0000000..94f3ff9
--- /dev/null
+++ b/src/usb_cam-ros2/CHANGELOG.rst
@@ -0,0 +1,626 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package usb_cam
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.0 (2023-12-04)
+------------------
+* Bump for release 0.8.0, update docs
+* Merge pull request `#300 <https://github.com/ros-drivers/usb_cam/issues/300>`_ from ros-drivers/263-check-if-specified-pixel-format-is-supported
+ 263 check if specified pixel format is supported
+* Check if specified pixel format is supported by driver and by device
+ - Also fix typo in params_2 yaml file
+ - fix linter errors too
+* Add v4l2_str method to pixel_format_base class
+* Add common arguments strut to pixel format classes
+ - Use common arguments struct in all format classes
+* Merge pull request `#292 <https://github.com/ros-drivers/usb_cam/issues/292>`_ from ros-drivers/288-fix-uyvy2rgb-size
+ 288 fix uyvy2rgb size
+* Merge pull request `#270 <https://github.com/ros-drivers/usb_cam/issues/270>`_ from boitumeloruf/raw-mjpeg-stream
+ Publish raw mjpeg stream directly via compressed image topic
+* Introduced funtions get\_..._from_av_format, fixed code style errors
+* Merge branch 'ros2' into raw-mjpeg-stream
+* fixed minor build issues
+* Merge branch 'ros2' into raw-mjpeg-stream
+* Merge branch 'ros2' of https://github.com/boitumeloruf/usb_cam into ros2
+* Merge pull request `#295 <https://github.com/ros-drivers/usb_cam/issues/295>`_ from ros-drivers/run-ci-on-ros2-branch-too
+ Run CI on ros2 branch pushes too
+* Bump checkout action to latest
+* Ensure ROS_VERSION is set for ROS build farms
+* Run CI on ros2 branch pushes too
+* Merge pull request `#294 <https://github.com/ros-drivers/usb_cam/issues/294>`_ from ros-drivers/291-auto-generate-ci-matrix
+ Auto generate ci matrix
+* Continue on error for ROS 1 build and test job
+* Hard-code noetic to actions matrix step until 2025
+* Handle rolling case where two docker images are listed
+* Switch to tagged version of the active_ros_distros script
+* Add basic ROS 1 node, update CMakelists and package.xml
+* Update CI to automatically generate distro matrix
+* Merge pull request `#293 <https://github.com/ros-drivers/usb_cam/issues/293>`_ from ros-drivers/add-mjpeg-device-format-param
+ Add mjpeg device format param
+* fixed cppcheck and uncrustify errors
+* Refactored use of av_device_format
+* Updated example YAML files to av_device_format
+* Added av_device_format parameter
+* Fix bytes per line logic for base image class
+* Fix number of channels for uyvy2rgb format
+* Merge pull request `#286 <https://github.com/ros-drivers/usb_cam/issues/286>`_ from ros-drivers/285-handle-unavailable-device
+ 285 handle unavailable device, list available v4l2 devices
+* Fix manual triggering of CI pipelines
+* Check if given v4l2 device exists before opening it
+* fixed cppcheck and uncrustify errors
+* Merge pull request `#282 <https://github.com/ros-drivers/usb_cam/issues/282>`_ from flynneva/ros2-prepare-release
+ Bump to 0.7.0, generate CHANGELOG
+* Merge branch 'ros2' into raw-mjpeg-stream
+* Fix seg fault when unref av_packet using av_codec < 58.133.100
+* Merge branch 'ros2' into raw-mjpeg-stream
+* Refactored use of av_device_format
+* Merge branch 'ros-drivers:ros2' into ros2
+* Added feature to access raw mjpeg stream and publish directly on compressed image topic
+* Updated example YAML files to av_device_format
+* Added av_device_format parameter
+* Contributors: Boitumelo Ruf, Evan Flynn
+
+0.7.0 (2023-08-30)
+------------------
+* Fix mjpeg invalid ptr and mjpeg memory leak
+* Allocate unique avpacket for each frame
+* Fix some minor memorly leaks for mjpeg
+ Relates to `#262 <https://github.com/ros-drivers/usb_cam/issues/262>`_
+* Update docs to new launch file name
+ Closes `#277 <https://github.com/ros-drivers/usb_cam/issues/277>`_
+* Only unref packet in destructor
+ Closes `#274 <https://github.com/ros-drivers/usb_cam/issues/274>`_ `#275 <https://github.com/ros-drivers/usb_cam/issues/275>`_
+* Enable manaul trigger of ROS 2 CI, add Iron, deprecate Foxy
+* Add Iron to CI, remove Foxy
+* Enable manaul trigger of ROS 2 CI
+* Fix memory leaks in mjpeg2rgb conversion
+* Add SANITIZE option to package to help with debugging, document it
+* Fix memory leaks caused by buffer allocation by using smart pointers
+* Fix linter errors
+* Update params2 file for second camera
+* Fixed wrong image timestamp due to missing handling of microseconds in epoch time shift
+* Removed debug output of timestamp
+* Fixed wring image timestamp due to missing handling of microseconds in epoch time shift.
+* Address multiple memory leak issues after ros2 rewrite
+* Remove EOL Galactic distro from CI
+* Address multiple memory leak issues after ros2 rewrite
+* Create CameraConfig class, use it in launch file
+* imports no longer needed.
+* Multiple cameras + compression
+* Remove debug print accidentally added
+* Clean up ROS 2 node, update parameter logic
+* Contributors: Boitumelo Ruf, Brendon Cintas, Evan Flynn
+
+0.6.0 (2023-04-02)
+------------------
+* If auto exposure is true, set it
+* Migrate previous pixel formats to new approach
+ - Add M4202RGB pixel format (aka YUV420 to RGB8)
+ - Add Y102MONO8 pixel format (aka MONO10 to MONO8)
+* Update documentation related to supported formats
+ - update doc strings in new pixel format base class
+* Fix linter errors, clean up tests
+ - fix humble and rolling build
+* Implement new pixel_format class structure
+ - implement virtual convert method for new pixel format class
+ - fix MJPEG2RGB conversion logic using new pixel format class
+* Fix typo in workspace path in README
+* fix whitespace around comments
+* fix unused variable error
+* possible fix for timestamp jumping
+* fix code style
+* dont change brightness with default config
+* use v4l2 for "brightness", "contrast", "saturation", "sharpness", "gain", "auto_white_balance",
+ "white_balance", "autoexposure", "exposure", "autofocus", "focus"
+* Contributors: Evan Flynn, john
+
+0.5.0 (2023-01-14)
+------------------
+* Merge pull request `#212 <https://github.com/flynneva/usb_cam/issues/212>`_ from flynneva/203-refactor-usb-cam-library-with-no-ros-deps
+ Improve ros2 rewrite some more
+* Ensure usb_cam lib and node are installed
+* Add missing include to test_usb_cam_lib
+* Rename format enums to make code easier to read
+* Make supported formats a member variable of the UsbCam class
+* Adjust get_image method to return image to reduce required args
+* Use unused arguments in MJPEG2RGB conversion function
+* Treat all compiler warnings as errors to be more strict
+* Improve logging errors for usb_cam lib
+* Fix opencv include path
+* Merge pull request `#210 <https://github.com/flynneva/usb_cam/issues/210>`_ from revanthsenthil/ros2
+ ROS 2 installation instruction typo fix
+* Merge branch 'ros-drivers:ros2' into ros2
+* Merge pull request `#209 <https://github.com/flynneva/usb_cam/issues/209>`_ from flynneva/203-refactor-usb-cam-library-with-no-ros-deps
+ Refactor usb cam library with no ros deps
+* Update README.md
+ resolved error with `apt-get` from `apt get`
+* Add back in missing copyrights
+* Improve supported formats method for UsbCam object
+* Fix MJPEG2RGB conversion function
+* Enable code coverage using lcov
+* Add integration test for usb_cam lib
+* Clean up usb_cam lib, remove rclcpp dep
+* Bump default framerate to 30hz
+* Improve CLIPVALUE method, add unit test for it
+* Fix humble CI name
+* Add some basic unit tests to usb_cam
+* Remove ROS dep from usb_cam by rewriting timestamping of frames
+* Restructure usb_cam code into more digestible pieces
+* Merge pull request `#207 <https://github.com/flynneva/usb_cam/issues/207>`_ from flynneva/fix-compiler-warnings
+ Fix compiler warnings, replace deprecated code
+* Add basic linters to CMake, fix linter errors found
+* Fix compiler warnings, replace deprecated code
+* Merge pull request `#206 <https://github.com/flynneva/usb_cam/issues/206>`_ from flynneva/ros2
+ Add humble to CI
+* Add humble to CI
+* Merge pull request `#177 <https://github.com/flynneva/usb_cam/issues/177>`_ from benmaidel/feature/YUV420_ros2
+ [ros2] add support for YUV420 (yu12) pixel format
+* Merge pull request `#193 <https://github.com/flynneva/usb_cam/issues/193>`_ from mad0x60/patch-1
+* update the deprecated uncompressed command
+ The current ros2 image decompression command produces the following warning because it is deprecated:
+ [WARN] [1662133933.155713605] [rcl]: Found remap rule 'in/compressed:=image_raw/compressed'. This syntax is deprecated. Use '--ros-args --remap in/compressed:=image_raw/compressed' instead.
+ [WARN] [1662133933.155877454] [rcl]: Found remap rule 'out:=image_raw/uncompressed'. This syntax is deprecated. Use '--ros-args --remap out:=image_raw/uncompressed' instead.
+ This change update to match the most recent ROS2 format
+* Merge pull request `#189 <https://github.com/flynneva/usb_cam/issues/189>`_ from flynneva/ros2
+ Suppress libav deprecated pixel format used warnings (backport from #…
+* Suppress libav deprecated pixel format used warnings (backport from `#115 <https://github.com/flynneva/usb_cam/issues/115>`_)
+* Merge branch 'ros2' into feature/YUV420_ros2
+* Merge pull request `#188 <https://github.com/flynneva/usb_cam/issues/188>`_ from krsche/feat/ros2-add-h264-support
+* feat: add color_format param to support yuv422p
+ required for using cams like the Logitech C920 with the h264 pixel_format
+* feat: add h264 support
+* Merge pull request `#185 <https://github.com/flynneva/usb_cam/issues/185>`_ from progtologist/ros2
+ Enabled dynamic reconfiguration of usb_cam_node
+* Added debug print in callback
+ Co-authored-by: Evan Flynn <6157095+flynneva@users.noreply.github.com>
+* Merge pull request `#186 <https://github.com/flynneva/usb_cam/issues/186>`_ from ros-drivers/prep-for-release
+ Prep for release
+* Enabled dynamic reconfiguration of usb_cam_node
+* Merge branch 'ros2' into feature/YUV420_ros2
+* add support for YUV420 (yu12) pixel format
+* Contributors: Andrei Vukolov, Aris Synodinos, Benjamin Maidel, Evan Flynn, Fabian Kirschner, Mohamed Moustafa, Revanth S
+
+0.4.2 (2022-04-25)
+------------------
+* Minor bump for release
+* Merge pull request `#184 <https://github.com/ros-drivers/usb_cam/issues/184>`_ from clalancette/clalancette/cleanups
+* Switch the rolling docker image to use jammy.
+* Add default cases to switches.
+ This just quiets the compiler warnings.
+* Switch xioctl to take an unsigned long request.
+ This matches what ioctl actually takes, and gets rid of a sign
+ comparison warning.
+* Use uint32_t to store image sizes.
+ This matches the v4l2 structures, and ensures we don't get
+ sign warnings when compiling.
+* Merge pull request `#178 <https://github.com/ros-drivers/usb_cam/issues/178>`_ from benmaidel/feature/unsupported_set_format_options_ros2
+ [ros2] allow cameras that do not support setting format options via VIDIOC_S_FMT
+* allow cameras that do not support setting format options via VIDIOC_S_FMT
+* Merge pull request `#170 <https://github.com/ros-drivers/usb_cam/issues/170>`_ from kenji-miyake/fix-small-warnings
+ Fix small warnings
+* Fix -Wreturn-type
+* Fix -Wparentheses
+* Change static functions to inline to fix -Wunused-function
+* Fix -Wunused-parameter
+* Fix -Worder
+* Fix -Wcomment
+* Fix -Wformat
+* add instructions for multiple launches
+* Contributors: Benjamin Maidel, Chris Lalancette, Evan Flynn, Kenji Miyake
+
+0.4.1 (2021-08-08)
+------------------
+* update README, add compression section
+* update package.xml to include image_transport_plugins
+* clean up README instructions
+* update README ros2 run executable name
+* Contributors: Evan Flynn
+
+0.4.0 (2021-07-21)
+------------------
+* bump version for ros2 first release
+* add galactic to ci, closes `#157 <https://github.com/ros-drivers/usb_cam/issues/157>`_
+ update camera name
+* Merge pull request `#158 <https://github.com/ros-drivers/usb_cam/issues/158>`_ from wep21/feature/add_camera_info
+ Feature/add camera info
+* Add sample camera info yaml
+* Add camera info
+* Merge pull request `#156 <https://github.com/ros-drivers/usb_cam/issues/156>`_ from wep21/feature/composable_node
+* Make usb_cam_node composable
+* Merge pull request `#153 <https://github.com/ros-drivers/usb_cam/issues/153>`_ from flynneva/lint/make-utils-file
+* add utils file, fix lint errors
+* Merge pull request `#151 <https://github.com/ros-drivers/usb_cam/issues/151>`_ from flynneva/fix/remove-boost
+* replace boost lexical_cast with snprintf
+* Merge pull request `#149 <https://github.com/ros-drivers/usb_cam/issues/149>`_ from flynneva/fix/update-readme
+ fix readme headers
+* fix readme headers
+* Merge pull request `#148 <https://github.com/ros-drivers/usb_cam/issues/148>`_ from flynneva/update-ros2-readme-and-lint
+ Update ros2 readme and lint
+* fix most lint errors
+* update readme, fix linter errors
+* Merge pull request `#146 <https://github.com/ros-drivers/usb_cam/issues/146>`_ from flynneva/ros2-clean-up
+ Ros2 clean up
+* fix show_image script lag
+* run, launch and params file working
+* add service, install launch, separate header
+* Merge pull request `#139 <https://github.com/ros-drivers/usb_cam/issues/139>`_ from flynneva/cmake-cleanup
+ consolidate srcs, use ament_auto macros, closes `#138 <https://github.com/ros-drivers/usb_cam/issues/138>`_
+* consolidate srcs, use ament_auto macros, closes `#137 <https://github.com/ros-drivers/usb_cam/issues/137>`_ `#138 <https://github.com/ros-drivers/usb_cam/issues/138>`_
+* Merge pull request `#132 <https://github.com/ros-drivers/usb_cam/issues/132>`_ from flynneva/foxy
+ updates for foxy
+* add myself to authors
+* fixing lint errors
+* add ros2 github actions
+* minor updates to foxy
+* adding launch file
+ try to fix low framerate `#103 <https://github.com/ros-drivers/usb_cam/issues/103>`_
+ add ros parameters
+ loading more parameters from parameter server `#103 <https://github.com/ros-drivers/usb_cam/issues/103>`_
+ use argparse to get arguments from command line
+ untested correction to args
+ ignore unknown args
+ set proper default device and look for more bad return values
+ trying to find why framerate is limited to about 8 fps
+ framerate ok for low-exposure settings
+ print list of valid formats `#105 <https://github.com/ros-drivers/usb_cam/issues/105>`_
+* use the v4l2_buffer timestamp if available. `#75 <https://github.com/ros-drivers/usb_cam/issues/75>`_
+ usb_cam.cpp is building but untested `#103 <https://github.com/ros-drivers/usb_cam/issues/103>`_
+ Builds but crashes immediately after running
+ data is getting published, no image shown
+ image shown, frame rate is very slow `#103 <https://github.com/ros-drivers/usb_cam/issues/103>`_
+* move the timestamping closer to when the image was acquired. `#75 <https://github.com/ros-drivers/usb_cam/issues/75>`_
+* Merge pull request `#136 <https://github.com/ros-drivers/usb_cam/issues/136>`_ from flynneva/patch-1
+ add myself as a maintainer for ros2
+* add myself as a maintainer for ros2
+* Merge pull request `#124 <https://github.com/ros-drivers/usb_cam/issues/124>`_ from k-okada/add_noetic
+ add noetic .travis.yml
+* add noetic .travis.yml
+* Contributors: Evan Flynn, Kei Okada, Lucas Walter, flynneva, wep21
+
+0.3.7 (2018-10-29)
+------------------
+* ROS2 version
+
+0.3.6 (2017-06-15)
+------------------
+* .travis.yml: udpate to trusty
+* add AV\_ prefix to PIX_FMT\_* for X,Y (`#71 <https://github.com/ros-drivers/usb_cam/issues/71>`_)
+* Contributors: Kei Okada
+
+0.3.5 (2017-06-14)
+------------------
+* add ROS Orphaned Package Maintainers to maintainer tag (`#69 <https://github.com/ros-drivers/usb_cam/issues/69>`_)
+* support for Kinetic / Ubuntu 16.04 (`#58 <https://github.com/ros-drivers/usb_cam/issues/58>`_)
+ * replace use of deprecated functions in newer ffmpeg/libav versions
+ ffmpeg/libav 55.x (used in ROS Kinetic) deprecated the avcodec_alloc_frame.
+* Add grey scale pixel format. (`#45 <https://github.com/ros-drivers/usb_cam/issues/45>`_)
+* add start/stop capture services (`#44 <https://github.com/ros-drivers/usb_cam/issues/44>`_ )
+ * better management of start/stop
+ * up package.xml
+ * add capture service
+
+* fix bug for byte count in a pixel (3 bytes not 24 bytes) (`#40 <https://github.com/ros-drivers/usb_cam/issues/40>`_ )
+* Contributors: Daniel Seifert, Eric Zavesky, Kei Okada, Ludovico Russo, Russell Toris, honeytrap15
+
+0.3.4 (2015-08-18)
+------------------
+* Installs launch files
+* Merge pull request #37 from tzutalin/develop
+ Add a launch file for easy test
+* Add a launch file for easy test
+* Contributors: Russell Toris, tzu.ta.lin
+
+0.3.3 (2015-05-14)
+------------------
+* Merge pull request #36 from jsarrett/develop
+ add gain parameter
+* add gain parameter
+* Contributors: James Sarrett, Russell Toris
+
+0.3.2 (2015-03-24)
+------------------
+* Merge pull request #34 from eliasm/develop
+ fixed check whether calibration file exists
+* fixed check whether calibration file exists
+* Contributors: Elias Mueggler, Russell Toris
+
+0.3.1 (2015-02-20)
+------------------
+* Merge pull request #32 from kmhallen/mono8
+ Publish YUVMONO10 images as mono8 instead of rgb8
+* Publish YUVMONO10 images as mono8 instead of rgb8
+* Contributors: Kevin Hallenbeck, Russell Toris
+
+0.3.0 (2015-01-26)
+------------------
+* Merge pull request #30 from mitchellwills/develop
+ Removed global state from usb_cam by encapsulating it inside an object
+* Made device name a std::string instead of const char*
+* Added usb_cam namespace
+* Added underscore sufix to class fields
+* Removed camera_ prefix from methods
+* Moved methods to parse pixel_format and io_method from string to UsbCam
+* Moved camera_image_t struct to be private in UsbCam
+* Cleaned up parameter assignment
+* Made set_v4l_parameters a non-static function
+* Moved set_v4l_parameters to UsbCam object
+* Removed global state from usb_cam by encapsulating it inside an object
+ function and structions in usb_cam.h became public and everything else is private
+* Merge pull request #28 from mitchellwills/develop
+ Fix installation of header files
+* Fix installation of header files
+* Contributors: Mitchell Wills, Russell Toris
+
+0.2.0 (2015-01-16)
+------------------
+* Bug fix in camera info settings.
+* Update .travis.yml
+* Merge pull request #27 from bosch-ros-pkg/default_camera_info
+ sets default camera info
+* sets default camera info
+* Contributors: Russell Toris
+
+0.1.13 (2014-12-02)
+-------------------
+* Merge pull request #25 from blutack/patch-1
+ Warn rather than error if framerate can't be set
+* Warn rather than error if framerate can't be set
+ The driver doesn't currently work with em28xx based devices as they don't allow the framerate to be set directly and the node exits with an error. Changing to a warning allows these devices to be used.
+* Update README.md
+* Merge pull request #24 from rjw57/do-not-touch-parameters-unless-asked
+ do not modify parameters unless explicitly set
+* do not modify parameters unless explicitly set
+ The contrast, saturation, brightness, sharpness and focus parameters
+ were recently added to usb_cam. This caused a regression
+ (sigproc/robotic_surgery#17) whereby the default settings for a webcam
+ are overridden in all cases by the hard-coded defaults in usb_cam.
+ In the absence of a know good set of "default" values, leave the
+ parameters unset unless the user has explicitly set them in the launch
+ file.
+* Contributors: Rich Wareham, Russell Toris, blutack
+
+0.1.12 (2014-11-05)
+-------------------
+* Merge pull request #22 from dekent/develop
+ White balance parameters
+* Parameter to enable/disable auto white balance
+* Added parameters for white balance
+* uses version major to check for av_codec
+* uses version header to check for AV_CODEC_ID_MJPEG
+* Contributors: David Kent, Russell Toris
+
+0.1.11 (2014-10-30)
+-------------------
+* Merge pull request #20 from dekent/develop
+ More Parameters
+* bug fix
+* Setting focus when autofocus is disabled
+* Parameter adjusting
+* Added parameter setting for absolute focus, brightness, contrast, saturation, and sharpness
+* Contributors: David Kent, Russell Toris
+
+0.1.10 (2014-10-24)
+-------------------
+* Merge pull request #19 from bosch-ros-pkg/av_codec_id
+ Removed deprecated CODEC_ID
+* added legacy macro constants for libav 10
+* Renamed deprecated CODEC_ID constants to AV_CODEC_ID to fix compilation for libav 10
+* Contributors: Andrzej Pronobis, Russell Toris
+
+0.1.9 (2014-08-26)
+------------------
+* Uses ros::Rate to enforce software framerate instead of custom time check
+* Merge pull request #16 from liangfok/feature/app_level_framerate_control
+ Modified to enforce framerate control at the application level in additi...
+* Modified to enforce framerate control at the application level in addition to at the driver level. This is necessary since the drivers for my webcam did not obey the requested framerate.
+* Contributors: Russell Toris, liang
+
+0.1.8 (2014-08-21)
+------------------
+* autoexposure and exposure settings now exposed via ROS parameters
+* added ability to call v4l-utils as well as correctly set autofocus
+* cleanup of output
+* Merge pull request #15 from mistoll/develop
+ added support for RGB24 pixel format
+* Added RGB24 as pixel format
+* Contributors: Michael Stoll, Russell Toris
+
+0.1.7 (2014-08-20)
+------------------
+* changelog fixed
+* minor cleanup and ability to change camera name and info
+* Contributors: Russell Toris
+
+0.1.6 (2014-08-15)
+------------------
+* Merge pull request #14 from KaijenHsiao/master
+ added support for 10-bit mono cameras advertising as YUV
+* added support for 10-bit mono cameras advertising as YUV (such as Leopard Imaging's LI-USB30-V034)
+* Update CHANGELOG.rst
+* changelog updated
+* Merge pull request #13 from vrabaud/develop
+ add a a ros::spinOnce to get set_camera_info working
+* add a a ros::spinOnce to get set_camera_info working
+ This is explained in the docs of CameraInfoManager
+ https://github.com/ros-perception/image_common/blob/hydro-devel/camera_info_manager/include/camera_info_manager/camera_info_manager.h#L71
+ Also, this fixes https://github.com/ros-perception/image_pipeline/issues/78
+* Contributors: Kaijen Hsiao, Russell Toris, Vincent Rabaud, sosentos
+
+0.1.5 (2014-07-28)
+------------------
+* auto format
+* cleanup of readme and such
+* Merge branch 'hydro-devel' of github.com:bosch-ros-pkg/usb_cam
+* Merge pull request #11 from pronobis/hydro-devel
+ Fixed a bug with av_free missing by adding a proper include.
+* Fixed a bug with av_free missing by adding a proper include on Ubuntu 14.04.
+* Merge pull request #7 from cottsay/groovy-devel
+ Use pkg-config to find avcodec and swscale
+* Merge pull request #5 from FriedCircuits/hydro-devel
+ Remove requirments for self_test
+* Use pkg-config to find avcodec and swscale
+* Update package.xml
+* Remove selftest
+* Remove selftest
+* Update usb_cam_node.cpp
+* Merge pull request #2 from jonbinney/7_17
+ swap out deprecated libavcodec functions
+* swap out deprecated libavcodec functions
+* Contributors: Andrzej Pronobis, Jon Binney, Russell Toris, Scott K Logan, William
+
+0.1.3 (2013-07-11)
+------------------
+* Merge pull request #1 from jonbinney/rosify
+ Bag of improvements
+* add framerate parameter
+* use ROS_* for output
+* use camera_info_manager
+* Contributors: Jon Binney, Russell Toris
+
+0.1.2 (2013-05-06)
+------------------
+* installs usb_cam_node
+* Contributors: Russell Toris
+
+0.1.1 (2013-05-02)
+------------------
+* cmake fixed
+* ffmpeg added
+* Contributors: Russell Toris
+
+0.1.0 (2013-05-01)
+------------------
+* Update package.xml
+* minor cleanup
+* inital merge
+* Update README.md
+* Update README.md
+* Update README.md
+* Update README.md
+* Update README.md
+* Update CLONE_SETUP.sh
+* Update README.md
+* Updated the README.md.
+* Updated the installation instructions.
+* Fixed syntax in the README.
+* Updated README for ARDUINO support.
+* Fixed update script.
+* Updated the readme and updating scripts.
+* Updating for installation on Robot.
+* Updated installs and README for ROS.
+* Make sure the User knows to source the devel/setup.sh.
+* Getting rid of subtrees and Catkinized USB CAM.
+* Updating home to use ROSWS.
+* Fixing the launch file for video1.
+* Merge commit '0bc3322966e4c0ed259320827dd1f5cc8460efce'
+ Conflicts:
+ src/sofie_ros/package.xml
+* Removed unnecessary file.
+* Compiles.
+* Adding the Catkin build scripts.
+* Merge commit 'b2c739cb476e1e01425947e46dc2431464f241b3' as 'src/ar_track_alvar'
+* Squashed 'src/ar_track_alvar/' content from commit 9ecca95
+ git-subtree-dir: src/ar_track_alvar
+ git-subtree-split: 9ecca9558edc7d3a9e692eacc93e082bf1e9a3e6
+* Merge commit '9feb470d0ebdaa51e426be4d58f419b45928a671' as 'src/sofie_ros'
+* Squashed 'src/sofie_ros/' content from commit 3ca5edf
+ git-subtree-dir: src/sofie_ros
+ git-subtree-split: 3ca5edfba496840b41bfe01dfdff883cacff1a97
+* Removing stackts.
+* Removing submodules.
+* Fixed submodules.
+* Removing old package.
+* Merge branch 'catkin'
+ Conflicts:
+ README.md
+ cmake_install.cmake
+* Brancing package down to stack base.
+* Catkininizing.
+* (catkin)Catkininizing.
+* Modifying the setup of roshome.
+* Starting to Catkininize the project.
+* (catkin)Starting to Catkininize the project.
+* Going to catinize it.
+* (catkin)Going to catinize it.
+* Modified to new version of sofie_ros.
+* Renamed import_csv_data.py to fileUtils.py, because it does more now.
+* (catkin)Renamed import_csv_data.py to fileUtils.py, because it does more now.
+* Updating to use a csv file specified by the user. Separating PyTables path manipulation into SOFIEHDFFORMAT.
+* (catkin)Updating to use a csv file specified by the user. Separating PyTables path manipulation into SOFIEHDFFORMAT.
+* Merge branch 'release/0.0.2'
+* Created the install script.
+* Removed the Python Packages as submodules.
+* Merge branch 'release/0.0.1'
+* Update the Git submodules.
+* Modified the README and CLONE_SETUP.sh
+* Added SOFIEHDFFORMAT as a submodule.
+* Added the ExperimentControl Repo as a submodule.
+* Working the CLONE install.
+* Modifiying install script.
+* Added a script to update the gitmodules for read-only clones.
+* Merge branch 'master' of github.com:agcooke/roshome
+* Initial commit
+* Added the modules.
+* Added usb_cam,
+* Updating to Groovy.
+* (catkin)Updating to Groovy.
+* Added another potential launch file for exporting video from rosbag.
+* (catkin)Added another potential launch file for exporting video from rosbag.
+* Added a launcher to ros bag the usb_cam, for later playback.
+* (catkin)Added a launcher to ros bag the usb_cam, for later playback.
+* Added some files that were possibly not correct
+* (catkin)Added some files that were possibly not correct
+* Fixed bugs with the importing.
+* (catkin)Fixed bugs with the importing.
+* Added forgotten __init__.py file and changed to importdata sofiehdfformat funciton.
+* (catkin)Added forgotten __init__.py file and changed to importdata sofiehdfformat funciton.
+* Refractoring to make it possible to log to CSV.
+ There were problems handling concurrent writing to
+ pytables files. The package now logs to CSV and then
+ provides a function to post import the data into
+ SOFIEHDFFORMAT.
+* (catkin)Refractoring to make it possible to log to CSV.
+ There were problems handling concurrent writing to
+ pytables files. The package now logs to CSV and then
+ provides a function to post import the data into
+ SOFIEHDFFORMAT.
+* Exporting to a CSV. Does not work yet.
+* (catkin)Exporting to a CSV. Does not work yet.
+* Added a close on terminate signal handler.
+* (catkin)Added a close on terminate signal handler.
+* Made the marker size be set via a parameter to the launch file.
+* (catkin)Made the marker size be set via a parameter to the launch file.
+* Changed the Callibration data.
+* (catkin)Changed the Callibration data.
+* The ar_pose listener.
+* (catkin)The ar_pose listener.
+* Changed the sofie driver to directly safe the ar_pose data.
+ We are going to perform experiments and this means that the extra
+ data might be useful at a later stage.
+* (catkin)Changed the sofie driver to directly safe the ar_pose data.
+ We are going to perform experiments and this means that the extra
+ data might be useful at a later stage.
+* Changed the size of the marker.
+* Updated the usb_cam config to work for home camera.
+* Added callibration files and launch files.
+* Turned off history.
+* (catkin)Added some comments and renamed.
+* Added some comments and renamed.
+* (catkin)The Quaternions were mixed around. Fixed the launch file to log to file instead of screen.
+* The Quaternions were mixed around. Fixed the launch file to log to file instead of screen.
+* (catkin)Updating the README's.
+* Updating the README's.
+* Updated the launch file to launch ar_pose and rviz for debugging.
+* (catkin)Added arguments to the launch script.
+* Added arguments to the launch script.
+* Added the Stack formating files.
+* (catkin)Organising into a stack instead of separate packages.
+* Organising into a stack instead of separate packages.
+* Trying to figure out how to start and stop the node.
+* Adding simple parameters.
+* Added the ROS files.
+* Basic driver now works for listening on a channel that broadcasts geometry_msgs.msg.QuaternionStamped messages.
+* Working on the listerner that will write to HDFFormat.
+* Creating a listerner that can write to sofiehdfformat files.
+* Initial commit
+* Contributors: Adrian Cooke, Russell Toris, Adrian
diff --git a/src/usb_cam-ros2/CMakeLists.txt b/src/usb_cam-ros2/CMakeLists.txt
new file mode 100644
index 0000000..82f34b8
--- /dev/null
+++ b/src/usb_cam-ros2/CMakeLists.txt
@@ -0,0 +1,165 @@
+cmake_minimum_required(VERSION 3.5)
+project(usb_cam_ros2)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ if($ENV{ROS_VERSION} EQUAL 2)
+ add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+ endif()
+endif()
+
+# Remove once ROS 1 Noetic is deprecated
+if($ENV{ROS_VERSION} EQUAL 1)
+ find_package(catkin REQUIRED COMPONENTS
+ cv_bridge
+ roscpp
+ std_msgs
+ std_srvs
+ sensor_msgs
+ camera_info_manager
+ image_transport)
+else()
+ find_package(ament_cmake_auto REQUIRED)
+ ament_auto_find_build_dependencies()
+endif()
+
+find_package(OpenCV REQUIRED)
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(avcodec REQUIRED libavcodec)
+pkg_check_modules(avutil REQUIRED libavutil)
+pkg_check_modules(swscale REQUIRED libswscale)
+
+if(EXISTS ${avcodec})
+ message(STATUS "Found libavcodec: ${avcodec}")
+endif()
+
+if(EXISTS ${avutil})
+ message(STATUS "Found libavutil: ${avutil}")
+endif()
+
+if(EXISTS ${swscale})
+ message(STATUS "Found libswscale: ${swscale}")
+endif()
+
+## Build the USB camera library
+## Do not use ament_auto here so as to not link to rclcpp
+add_library(${PROJECT_NAME} SHARED
+ src/usb_cam.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+ "include"
+ ${OpenCV_INCLUDE_DIRS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+ ${avcodec_LIBRARIES}
+ ${avutil_LIBRARIES}
+ ${swscale_LIBRARIES}
+ ${OpenCV_LIBRARIES})
+
+# Remove once ROS 1 Noetic is deprecated
+if($ENV{ROS_VERSION} EQUAL 1)
+ catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+ add_executable(${PROJECT_NAME}_node src/ros1/usb_cam_node.cpp)
+ target_link_libraries(${PROJECT_NAME}_node
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ )
+ target_include_directories(${PROJECT_NAME}_node PUBLIC
+ ${catkin_INCLUDE_DIRS})
+else()
+ ament_export_libraries(${PROJECT_NAME})
+
+ ## Declare a ROS 2 composible node as a library
+ ament_auto_add_library(${PROJECT_NAME}_node SHARED
+ src/ros2/usb_cam_node.cpp
+ )
+
+ target_link_libraries(${PROJECT_NAME}_node
+ ${PROJECT_NAME})
+
+ ## Use node to generate an executable
+ rclcpp_components_register_node(${PROJECT_NAME}_node
+ PLUGIN "usb_cam::UsbCamNode"
+ EXECUTABLE ${PROJECT_NAME}_node_exe
+ )
+ if(SANITIZE)
+ target_compile_options(${PROJECT_NAME} PUBLIC -fsanitize=address -fsanitize=leak)
+ target_link_libraries(${PROJECT_NAME} -fsanitize=address -fsanitize=leak)
+ target_compile_options(${PROJECT_NAME}_node PUBLIC -fsanitize=address -fsanitize=leak)
+ target_link_libraries(${PROJECT_NAME}_node -fsanitize=address -fsanitize=leak)
+ target_link_libraries(${PROJECT_NAME}_node_exe -fsanitize=address -fsanitize=leak)
+ endif()
+endif()
+
+if(BUILD_TESTING)
+ if($ENV{ROS_VERSION} EQUAL 2)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+
+ find_package(ament_cmake_gtest)
+
+ # Unit tests
+ ament_add_gtest(test_usb_cam_utils
+ test/test_usb_cam_utils.cpp)
+ target_link_libraries(test_usb_cam_utils
+ ${PROJECT_NAME})
+ ament_add_gtest(test_pixel_formats
+ test/test_pixel_formats.cpp)
+ target_link_libraries(test_pixel_formats
+ ${PROJECT_NAME})
+ # TODO(flynneva): rewrite this test in another PR
+ # Integration tests
+ # ament_add_gtest(test_usb_cam_lib
+ # test/test_usb_cam_lib.cpp)
+ # target_link_libraries(test_usb_cam_lib
+ # ${PROJECT_NAME})
+ endif()
+endif()
+
+
+if($ENV{ROS_VERSION} EQUAL 1)
+ install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ )
+
+ ## Copy launch files
+ install(DIRECTORY launch/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+ FILES_MATCHING PATTERN "*.launch"
+ )
+
+ install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+ )
+else()
+ install(
+ PROGRAMS scripts/show_image.py
+ DESTINATION lib/${PROJECT_NAME})
+
+ install(TARGETS
+ ${PROJECT_NAME}
+ ${PROJECT_NAME}_node
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION lib
+ )
+
+ ament_auto_package(
+ INSTALL_TO_SHARE
+ launch
+ config
+ )
+endif()
diff --git a/src/usb_cam-ros2/CONTRIBUTING.md b/src/usb_cam-ros2/CONTRIBUTING.md
new file mode 100644
index 0000000..309be1e
--- /dev/null
+++ b/src/usb_cam-ros2/CONTRIBUTING.md
@@ -0,0 +1,3 @@
+Any contribution that you make to this repository will
+be under the 3-Clause BSD License, as dictated by that
+[license](https://opensource.org/licenses/BSD-3-Clause).
diff --git a/src/usb_cam-ros2/LICENSE b/src/usb_cam-ros2/LICENSE
new file mode 100644
index 0000000..24d43da
--- /dev/null
+++ b/src/usb_cam-ros2/LICENSE
@@ -0,0 +1,30 @@
+All rights reserved.
+
+Software License Agreement (BSD License 2.0)
+
+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 holder nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/usb_cam-ros2/README.md b/src/usb_cam-ros2/README.md
new file mode 100644
index 0000000..8878984
--- /dev/null
+++ b/src/usb_cam-ros2/README.md
@@ -0,0 +1,189 @@
+# usb_cam [](https://github.com/ros-drivers/usb_cam/actions/workflows/build_test.yml)
+
+## A ROS 2 Driver for V4L USB Cameras
+This package is based off of V4L devices specifically instead of just UVC.
+
+For ros1 documentation, see [the ROS wiki](http://ros.org/wiki/usb_cam).
+
+## Supported ROS 2 Distros and Platforms
+
+All Officially supported Linux Distros and corresponding ROS 2 releases are supported. Please create an issue if you experience any problems on these platforms.
+
+Windows: TBD/Untested/Unproven
+MacOS: TBD/Untested/Unproven
+
+For either MacOS or Windows - if you would like to try and get it working please create an issue to document your effort. If it works we can add it to the instructions here!
+
+## Quickstart
+
+Assuming you have a supported ROS 2 distro installed, run the following command to install the binary release:
+
+```shell
+sudo apt-get install ros-<ros2-distro>-usb-cam
+```
+
+As of today this package should be available for binary installation on all active ROS 2 distros.
+
+If for some reason you cannot install the binaries, follow the directions below to compile from source.
+
+## Building from Source
+
+Clone/Download the source code into your workspace:
+
+```shell
+cd /path/to/colcon_ws/src
+git clone https://github.com/ros-drivers/usb_cam.git
+```
+
+Or click on the green "Download zip" button on the repo's github webpage.
+
+Once downloaded and ensuring you have sourced your ROS 2 underlay, go ahead and install the dependencies:
+
+```shell
+cd /path/to/colcon_ws
+rosdep install --from-paths src --ignore-src -y
+```
+
+From there you should have all the necessary dependencies installed to compile the `usb_cam` package:
+
+```shell
+cd /path/to/colcon_ws
+colcon build
+source /path/to/colcon_ws/install/setup.bash
+```
+
+Be sure to source the newly built packages after a successful build.
+
+Once sourced, you should be able to run the package in one of three ways, shown in the next section.
+
+## Running
+
+The `usb_cam_node` can be ran with default settings, by setting specific parameters either via the command line or by loading in a parameters file.
+
+We provide a "default" params file in the `usb_cam/config/params.yaml` directory to get you started. Feel free to modify this file as you wish.
+
+Also provided is a launch file that should launch the `usb_cam_node_exe` executable along with an additional node that displays an image topic.
+
+The commands to run each of these different ways of starting the node are shown below:
+
+**NOTE: you only need to run ONE of the commands below to run the node**
+
+```shell
+# run the executable with default settings (without params file)
+ros2 run usb_cam usb_cam_node_exe
+
+# run the executable while passing in parameters via a yaml file
+ros2 run usb_cam usb_cam_node_exe --ros-args --params-file /path/to/colcon_ws/src/usb_cam/config/params.yaml
+
+# launch the usb_cam executable that loads parameters from the same `usb_cam/config/params.yaml` file as above
+# along with an additional image viewer node
+ros2 launch usb_cam camera.launch.py
+```
+## Launching Multiple usb_cam's
+
+To launch multiple nodes at once, simply remap the namespace of each one:
+
+```shell
+ros2 run usb_cam usb_cam_node_exe --remap __ns:=/usb_cam_0 --params-file /path/to/usb_cam/config/params_0.yaml
+ros2 run usb_cam usb_cam_node_exe --remap __ns:=/usb_cam_1 --params-file /path/to/usb_cam/config/params_1.yaml
+```
+
+## Supported formats
+
+### Device supported formats
+
+To see a connected devices supported formats, run the `usb_cam_node` and observe the console output.
+
+An example output is:
+
+```log
+This devices supproted formats:
+ Motion-JPEG: 1280 x 720 (30 Hz)
+ Motion-JPEG: 960 x 540 (30 Hz)
+ Motion-JPEG: 848 x 480 (30 Hz)
+ Motion-JPEG: 640 x 480 (30 Hz)
+ Motion-JPEG: 640 x 360 (30 Hz)
+ YUYV 4:2:2: 640 x 480 (30 Hz)
+ YUYV 4:2:2: 1280 x 720 (10 Hz)
+ YUYV 4:2:2: 640 x 360 (30 Hz)
+ YUYV 4:2:2: 424 x 240 (30 Hz)
+ YUYV 4:2:2: 320 x 240 (30 Hz)
+ YUYV 4:2:2: 320 x 180 (30 Hz)
+ YUYV 4:2:2: 160 x 120 (30 Hz)
+```
+
+### Driver supported formats
+
+The driver has its own supported formats. See [the source code](include/usb_cam/formats/)
+for details.
+
+After observing [the devices supported formats](#device-supported-formats), specify which
+format to use via [the parameters file](config/params.yaml) with the `pixel_format` parameter.
+
+To see a list of all currently supported driver formats, run the following command:
+
+```shell
+ros2 run usb_cam usb_cam_node_exe --ros-args -p pixel_format:="test"
+```
+
+Note: "test" here could be replaced with any non-supported pixel format string. The driver
+will detect if the given pixel format is supported or not.
+
+More formats and conversions can be added, contributions welcome!
+
+### Supported IO methods
+
+This driver supports three different IO methods as of today:
+
+1. `read`: copies the video frame between user and kernal space
+1. `mmap`: memory mapped buffers allocated in kernel space
+1. `userptr`: memory buffers allocated in the user space
+
+To read more on the different methods, check out [this article that provides a good overview
+of each](https://lwn.net/Articles/240667/)
+
+## Compression
+
+Big thanks to [the `ros2_v4l2_camera` package](https://gitlab.com/boldhearts/ros2_v4l2_camera#usage-1) and their documentation on this topic.
+
+The `usb_cam` should support compression by default since it uses `image_transport` to publish its images as long as the `image_transport_plugins` package is installed on your system. With the plugins installed the `usb_cam` package should publish a `compressed` topic automatically.
+
+Unfortunately `rviz2` and `show_image.py` do not support visualizing the compressed images just yet so you will need to republish the compressed image downstream to uncompress it:
+
+```shell
+ros2 run image_transport republish compressed raw --ros-args --remap in/compressed:=image_raw/compressed --remap out:=image_raw/uncompressed
+```
+
+## Address and leak sanitizing
+
+Incorporated into the `CMakelists.txt` file to assist with memory leak and address sanitizing
+is a flag to add these compile commands to the targets.
+
+To enable them, pass in the `SANITIZE=1` flag:
+
+```shell
+colcon build --packages-select usb_cam --cmake-args -DSANITIZE=1
+```
+
+Once built, run the nodes executable directly and pass any `ASAN_OPTIONS` that are needed:
+
+```shell
+ASAN_OPTIONS=new_delete_type_mismatch=0 ./install/usb_cam/lib/usb_cam/usb_cam_node_exe
+```
+
+After shutting down the executable with `Ctrl+C`, the sanitizer will report any memory leaks.
+
+By default this is turned off since compiling with the sanatizer turned on causes bloat and slows
+down performance.
+
+## Documentation
+
+[Doxygen](http://docs.ros.org/indigo/api/usb_cam/html/) files can be found on the ROS wiki.
+
+### License
+
+usb_cam is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file.
+
+### Authors
+
+See the [AUTHORS](AUTHORS.md) file for a full list of contributors.
diff --git a/src/usb_cam-ros2/config/camera_info.yaml b/src/usb_cam-ros2/config/camera_info.yaml
new file mode 100644
index 0000000..9cdd6b5
--- /dev/null
+++ b/src/usb_cam-ros2/config/camera_info.yaml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: test_camera
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/src/usb_cam-ros2/config/params_1.yaml b/src/usb_cam-ros2/config/params_1.yaml
new file mode 100644
index 0000000..f3b70ac
--- /dev/null
+++ b/src/usb_cam-ros2/config/params_1.yaml
@@ -0,0 +1,23 @@
+/**:
+ ros__parameters:
+ video_device: "/dev/video0"
+ framerate: 30.0
+ io_method: "mmap"
+ frame_id: "camera"
+ pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
+ av_device_format: "YUV422P"
+ image_width: 640
+ image_height: 480
+ camera_name: "test_camera"
+ camera_info_url: "package://usb_cam/config/camera_info.yaml"
+ brightness: -1
+ contrast: -1
+ saturation: -1
+ sharpness: -1
+ gain: -1
+ auto_white_balance: true
+ white_balance: 4000
+ autoexposure: true
+ exposure: 100
+ autofocus: false
+ focus: -1
\ No newline at end of file
diff --git a/src/usb_cam-ros2/config/params_2.yaml b/src/usb_cam-ros2/config/params_2.yaml
new file mode 100644
index 0000000..b3c6cb8
--- /dev/null
+++ b/src/usb_cam-ros2/config/params_2.yaml
@@ -0,0 +1,24 @@
+/**:
+ ros__parameters:
+ video_device: "/dev/video2"
+ framerate: 15.0
+ io_method: "mmap"
+ frame_id: "camera2"
+ pixel_format: "mjpeg2rgb"
+ av_device_format: "YUV422P"
+ image_width: 640
+ image_height: 480
+ camera_name: "test_camera2"
+ # reusing same camera intrinsics only for demo, should really be unique for camera2"
+ camera_info_url: "package://usb_cam/config/camera_info.yaml"
+ brightness: -1
+ contrast: -1
+ saturation: -1
+ sharpness: -1
+ gain: -1
+ auto_white_balance: true
+ white_balance: 4000
+ autoexposure: true
+ exposure: 100
+ autofocus: false
+ focus: -1
\ No newline at end of file
diff --git a/src/usb_cam-ros2/docs/.pages b/src/usb_cam-ros2/docs/.pages
new file mode 100644
index 0000000..87340d5
--- /dev/null
+++ b/src/usb_cam-ros2/docs/.pages
@@ -0,0 +1,4 @@
+nav:
+ - Home: index.md
+ - API Reference: api-reference
+ - ...
\ No newline at end of file
diff --git a/src/usb_cam-ros2/docs/doxybook2_config.json b/src/usb_cam-ros2/docs/doxybook2_config.json
new file mode 100644
index 0000000..7137791
--- /dev/null
+++ b/src/usb_cam-ros2/docs/doxybook2_config.json
@@ -0,0 +1,44 @@
+{
+ "baseUrl": "/usb_cam/api-reference/",
+ "copyImages": true,
+ "fileExt": "md",
+ "filesFilter": [],
+ "folderClassesName": "Classes",
+ "folderGroupsName": "Modules",
+ "folderNamespacesName": "Namespaces",
+ "foldersToGenerate": [
+ "modules",
+ "classes",
+ "namespaces"
+ ],
+ "formulaBlockEnd": "\\]",
+ "formulaBlockStart": "\\[",
+ "formulaInlineEnd": "\\)",
+ "formulaInlineStart": "\\(",
+ "imagesFolder": "images",
+ "indexClassesName": "index",
+ "indexClassesTitle": "Classes",
+ "indexExamplesName": "index",
+ "indexGroupsName": "index",
+ "indexGroupsTitle": "Modules",
+ "indexInFolders": true,
+ "indexNamespacesName": "index",
+ "indexNamespacesTitle": "Namespaces",
+ "linkAndInlineCodeAsHTML": false,
+ "linkLowercase": false,
+ "linkSuffix": "/",
+ "mainPageInRoot": true,
+ "mainPageName": "index",
+ "sort": true,
+ "templateIndexClasses": "index_classes",
+ "templateIndexGroups": "index_groups",
+ "templateIndexNamespaces": "index_namespaces",
+ "templateKindClass": "kind_class",
+ "templateKindDir": "kind_file",
+ "templateKindGroup": "kind_nonclass",
+ "templateKindInterface": "kind_class",
+ "templateKindNamespace": "kind_nonclass",
+ "templateKindStruct": "kind_class",
+ "templateKindUnion": "kind_class",
+ "useFolders": true
+}
diff --git a/src/usb_cam-ros2/docs/doxygen.config b/src/usb_cam-ros2/docs/doxygen.config
new file mode 100644
index 0000000..42367d4
--- /dev/null
+++ b/src/usb_cam-ros2/docs/doxygen.config
@@ -0,0 +1,389 @@
+# Doxyfile 1.9.1
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING = UTF-8
+PROJECT_NAME = "usb_cam"
+PROJECT_NUMBER =
+PROJECT_BRIEF =
+PROJECT_LOGO =
+OUTPUT_DIRECTORY = docs
+CREATE_SUBDIRS = NO
+ALLOW_UNICODE_NAMES = NO
+OUTPUT_LANGUAGE = English
+OUTPUT_TEXT_DIRECTION = None
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = YES
+ABBREVIATE_BRIEF = "The $name class" \
+ "The $name widget" \
+ "The $name file" \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+ALWAYS_DETAILED_SEC = NO
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = YES
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = NO
+JAVADOC_BANNER = NO
+QT_AUTOBRIEF = NO
+MULTILINE_CPP_IS_BRIEF = NO
+PYTHON_DOCSTRING = YES
+INHERIT_DOCS = YES
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 4
+ALIASES =
+OPTIMIZE_OUTPUT_FOR_C = NO
+OPTIMIZE_OUTPUT_JAVA = NO
+OPTIMIZE_FOR_FORTRAN = NO
+OPTIMIZE_OUTPUT_VHDL = NO
+OPTIMIZE_OUTPUT_SLICE = NO
+EXTENSION_MAPPING =
+MARKDOWN_SUPPORT = YES
+TOC_INCLUDE_HEADINGS = 5
+AUTOLINK_SUPPORT = YES
+BUILTIN_STL_SUPPORT = NO
+CPP_CLI_SUPPORT = NO
+SIP_SUPPORT = NO
+IDL_PROPERTY_SUPPORT = YES
+DISTRIBUTE_GROUP_DOC = NO
+GROUP_NESTED_COMPOUNDS = NO
+SUBGROUPING = YES
+INLINE_GROUPED_CLASSES = NO
+INLINE_SIMPLE_STRUCTS = NO
+TYPEDEF_HIDES_STRUCT = NO
+LOOKUP_CACHE_SIZE = 0
+NUM_PROC_THREADS = 1
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = NO
+EXTRACT_PRIVATE = NO
+EXTRACT_PRIV_VIRTUAL = NO
+EXTRACT_PACKAGE = NO
+EXTRACT_STATIC = NO
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+EXTRACT_ANON_NSPACES = NO
+RESOLVE_UNNAMED_PARAMS = YES
+HIDE_UNDOC_MEMBERS = NO
+HIDE_UNDOC_CLASSES = NO
+HIDE_FRIEND_COMPOUNDS = NO
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+CASE_SENSE_NAMES = YES
+HIDE_SCOPE_NAMES = NO
+HIDE_COMPOUND_REFERENCE= NO
+SHOW_INCLUDE_FILES = YES
+SHOW_GROUPED_MEMB_INC = NO
+FORCE_LOCAL_INCLUDES = NO
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = YES
+SORT_BRIEF_DOCS = NO
+SORT_MEMBERS_CTORS_1ST = NO
+SORT_GROUP_NAMES = NO
+SORT_BY_SCOPE_NAME = NO
+STRICT_PROTO_MATCHING = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = YES
+GENERATE_BUGLIST = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+MAX_INITIALIZER_LINES = 30
+SHOW_USED_FILES = YES
+SHOW_FILES = YES
+SHOW_NAMESPACES = YES
+FILE_VERSION_FILTER =
+LAYOUT_FILE =
+CITE_BIB_FILES =
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = NO
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_AS_ERROR = NO
+WARN_FORMAT = "$file:$line: $text"
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT = include/
+ src/
+INPUT_ENCODING = UTF-8
+FILE_PATTERNS = *.c \
+ *.cc \
+ *.cxx \
+ *.cpp \
+ *.c++ \
+ *.java \
+ *.ii \
+ *.ixx \
+ *.ipp \
+ *.i++ \
+ *.inl \
+ *.idl \
+ *.ddl \
+ *.odl \
+ *.h \
+ *.hh \
+ *.hxx \
+ *.hpp \
+ *.h++ \
+ *.cs \
+ *.d \
+ *.php \
+ *.php4 \
+ *.php5 \
+ *.phtml \
+ *.inc \
+ *.m \
+ *.markdown \
+ *.md \
+ *.mm \
+ *.dox \
+ *.py \
+ *.pyw \
+ *.f90 \
+ *.f95 \
+ *.f03 \
+ *.f08 \
+ *.f18 \
+ *.f \
+ *.for \
+ *.vhd \
+ *.vhdl \
+ *.ucf \
+ *.qsf \
+ *.ice
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXCLUDE_PATTERNS =
+EXCLUDE_SYMBOLS =
+EXAMPLE_PATH =
+EXAMPLE_PATTERNS = *
+EXAMPLE_RECURSIVE = NO
+IMAGE_PATH =
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+FILTER_SOURCE_PATTERNS =
+USE_MDFILE_AS_MAINPAGE =
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = NO
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = NO
+REFERENCES_RELATION = NO
+REFERENCES_LINK_SOURCE = YES
+SOURCE_TOOLTIPS = YES
+USE_HTAGS = NO
+VERBATIM_HEADERS = YES
+CLANG_ASSISTED_PARSING = NO
+CLANG_ADD_INC_PATHS = YES
+CLANG_OPTIONS =
+CLANG_DATABASE_PATH =
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = YES
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = NO
+HTML_OUTPUT = html
+HTML_FILE_EXTENSION = .html
+HTML_HEADER =
+HTML_FOOTER =
+HTML_STYLESHEET =
+HTML_EXTRA_STYLESHEET =
+HTML_EXTRA_FILES =
+HTML_COLORSTYLE_HUE = 220
+HTML_COLORSTYLE_SAT = 100
+HTML_COLORSTYLE_GAMMA = 80
+HTML_TIMESTAMP = NO
+HTML_DYNAMIC_MENUS = YES
+HTML_DYNAMIC_SECTIONS = NO
+HTML_INDEX_NUM_ENTRIES = 100
+GENERATE_DOCSET = NO
+DOCSET_FEEDNAME = "Doxygen generated docs"
+DOCSET_BUNDLE_ID = org.doxygen.Project
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+DOCSET_PUBLISHER_NAME = Publisher
+GENERATE_HTMLHELP = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+CHM_INDEX_ENCODING =
+BINARY_TOC = NO
+TOC_EXPAND = NO
+GENERATE_QHP = NO
+QCH_FILE =
+QHP_NAMESPACE = org.doxygen.Project
+QHP_VIRTUAL_FOLDER = doc
+QHP_CUST_FILTER_NAME =
+QHP_CUST_FILTER_ATTRS =
+QHP_SECT_FILTER_ATTRS =
+QHG_LOCATION =
+GENERATE_ECLIPSEHELP = NO
+ECLIPSE_DOC_ID = org.doxygen.Project
+DISABLE_INDEX = NO
+GENERATE_TREEVIEW = NO
+ENUM_VALUES_PER_LINE = 4
+TREEVIEW_WIDTH = 250
+EXT_LINKS_IN_WINDOW = NO
+HTML_FORMULA_FORMAT = png
+FORMULA_FONTSIZE = 10
+FORMULA_TRANSPARENT = YES
+FORMULA_MACROFILE =
+USE_MATHJAX = NO
+MATHJAX_FORMAT = HTML-CSS
+MATHJAX_RELPATH = https://cdn.jsdelivr.net/npm/mathjax@2
+MATHJAX_EXTENSIONS =
+MATHJAX_CODEFILE =
+SEARCHENGINE = YES
+SERVER_BASED_SEARCH = NO
+EXTERNAL_SEARCH = NO
+SEARCHENGINE_URL =
+SEARCHDATA_FILE = searchdata.xml
+EXTERNAL_SEARCH_ID =
+EXTRA_SEARCH_MAPPINGS =
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME =
+MAKEINDEX_CMD_NAME = makeindex
+LATEX_MAKEINDEX_CMD = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4
+EXTRA_PACKAGES =
+LATEX_HEADER =
+LATEX_FOOTER =
+LATEX_EXTRA_STYLESHEET =
+LATEX_EXTRA_FILES =
+PDF_HYPERLINKS = YES
+USE_PDFLATEX = YES
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+LATEX_SOURCE_CODE = NO
+LATEX_BIB_STYLE = plain
+LATEX_TIMESTAMP = NO
+LATEX_EMOJI_DIRECTORY =
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+RTF_SOURCE_CODE = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_SUBDIR =
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = YES
+XML_OUTPUT = xml
+XML_PROGRAMLISTING = YES
+XML_NS_MEMB_FILE_SCOPE = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+GENERATE_DOCBOOK = NO
+DOCBOOK_OUTPUT = docbook
+DOCBOOK_PROGRAMLISTING = NO
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = NO
+EXPAND_ONLY_PREDEF = NO
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+PREDEFINED =
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+TAGFILES =
+GENERATE_TAGFILE =
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+EXTERNAL_PAGES = YES
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+DIA_PATH =
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = YES
+DOT_NUM_THREADS = 0
+DOT_FONTNAME = Helvetica
+DOT_FONTSIZE = 10
+DOT_FONTPATH =
+CLASS_GRAPH = YES
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+UML_LIMIT_NUM_FIELDS = 10
+DOT_UML_DETAILS = NO
+DOT_WRAP_THRESHOLD = 17
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = YES
+INCLUDED_BY_GRAPH = YES
+CALL_GRAPH = NO
+CALLER_GRAPH = NO
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = YES
+DOT_IMAGE_FORMAT = png
+INTERACTIVE_SVG = NO
+DOT_PATH =
+DOTFILE_DIRS =
+MSCFILE_DIRS =
+DIAFILE_DIRS =
+PLANTUML_JAR_PATH =
+PLANTUML_CFG_FILE =
+PLANTUML_INCLUDE_PATH =
+DOT_GRAPH_MAX_NODES = 50
+MAX_DOT_GRAPH_DEPTH = 0
+DOT_TRANSPARENT = NO
+DOT_MULTI_TARGETS = NO
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
diff --git a/src/usb_cam-ros2/docs/index.md b/src/usb_cam-ros2/docs/index.md
new file mode 120000
index 0000000..32d46ee
--- /dev/null
+++ b/src/usb_cam-ros2/docs/index.md
@@ -0,0 +1 @@
+../README.md
\ No newline at end of file
diff --git a/src/usb_cam-ros2/docs/requirements.txt b/src/usb_cam-ros2/docs/requirements.txt
new file mode 100644
index 0000000..1f95a75
--- /dev/null
+++ b/src/usb_cam-ros2/docs/requirements.txt
@@ -0,0 +1,4 @@
+ mkdocs==1.4.3
+ mkdocs-material==9.1.17
+ mkdocs-awesome-pages-plugin==2.7.0
+ mike==1.1.2
\ No newline at end of file
diff --git a/src/usb_cam-ros2/include/usb_cam/constants.hpp b/src/usb_cam-ros2/include/usb_cam/constants.hpp
new file mode 100644
index 0000000..7aad1a7
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/constants.hpp
@@ -0,0 +1,157 @@
+// Copyright 2023 Evan Flynn
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__CONSTANTS_HPP_
+#define USB_CAM__CONSTANTS_HPP_
+
+#include <string>
+#include <vector>
+
+
+namespace usb_cam
+{
+namespace constants
+{
+
+
+/// @brief image encodings duplicated from
+/// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+const char RGB8[] = "rgb8";
+const char RGBA8[] = "rgba8";
+const char RGB16[] = "rgb16";
+const char RGBA16[] = "rgba16";
+const char BGR8[] = "bgr8";
+const char BGRA8[] = "bgra8";
+const char BGR16[] = "bgr16";
+const char BGRA16[] = "bgra16";
+const char MONO8[] = "mono8";
+const char MONO16[] = "mono16";
+
+// OpenCV CvMat types
+const char TYPE_8UC1[] = "8UC1";
+const char TYPE_8UC2[] = "8UC2";
+const char TYPE_8UC3[] = "8UC3";
+const char TYPE_8UC4[] = "8UC4";
+const char TYPE_8SC1[] = "8SC1";
+const char TYPE_8SC2[] = "8SC2";
+const char TYPE_8SC3[] = "8SC3";
+const char TYPE_8SC4[] = "8SC4";
+const char TYPE_16UC1[] = "16UC1";
+const char TYPE_16UC2[] = "16UC2";
+const char TYPE_16UC3[] = "16UC3";
+const char TYPE_16UC4[] = "16UC4";
+const char TYPE_16SC1[] = "16SC1";
+const char TYPE_16SC2[] = "16SC2";
+const char TYPE_16SC3[] = "16SC3";
+const char TYPE_16SC4[] = "16SC4";
+const char TYPE_32SC1[] = "32SC1";
+const char TYPE_32SC2[] = "32SC2";
+const char TYPE_32SC3[] = "32SC3";
+const char TYPE_32SC4[] = "32SC4";
+const char TYPE_32FC1[] = "32FC1";
+const char TYPE_32FC2[] = "32FC2";
+const char TYPE_32FC3[] = "32FC3";
+const char TYPE_32FC4[] = "32FC4";
+const char TYPE_64FC1[] = "64FC1";
+const char TYPE_64FC2[] = "64FC2";
+const char TYPE_64FC3[] = "64FC3";
+const char TYPE_64FC4[] = "64FC4";
+
+// Bayer encodings
+const char BAYER_RGGB8[] = "bayer_rggb8";
+const char BAYER_BGGR8[] = "bayer_bggr8";
+const char BAYER_GBRG8[] = "bayer_gbrg8";
+const char BAYER_GRBG8[] = "bayer_grbg8";
+const char BAYER_RGGB16[] = "bayer_rggb16";
+const char BAYER_BGGR16[] = "bayer_bggr16";
+const char BAYER_GBRG16[] = "bayer_gbrg16";
+const char BAYER_GRBG16[] = "bayer_grbg16";
+
+// Miscellaneous
+// YUV 4:2:2 encodings with an 8-bit depth
+// UYUV version: http://www.fourcc.org/pixel-format/yuv-uyvy
+const char YUV422[] = "yuv422";
+// YUYV version: http://www.fourcc.org/pixel-format/yuv-yuy2/
+const char YUV422_YUY2[] = "yuv422_yuy2";
+// YUV 4:2:0 encodings with an 8-bit depth
+// NV21: https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/pixfmt-yuv-planar.html
+const char NV21[] = "nv21";
+// YUV 4:4:4 encodings with 8-bit depth
+// NV24: https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/pixfmt-yuv-planar.html
+const char NV24[] = "nv24";
+
+const char UNKNOWN[] = "unknown";
+
+const std::vector<unsigned char> uchar_clipping_table = {
+ 0, 0, 0, 0, 0, 0, 0, 0, // -128 - -121
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -120 - -101
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -100 - -81
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -80 - -61
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -60 - -41
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -40 - -21
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -20 - -1
+ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
+ 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
+ 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60,
+ 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80,
+ 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100,
+ 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115,
+ 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130,
+ 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145,
+ 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160,
+ 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175,
+ 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190,
+ 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205,
+ 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220,
+ 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235,
+ 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250,
+ 251, 252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263
+ 255, 255, 255, 255, 255, 255, 255, 255, // 264-271
+ 255, 255, 255, 255, 255, 255, 255, 255, // 272-279
+ 255, 255, 255, 255, 255, 255, 255, 255, // 280-287
+ 255, 255, 255, 255, 255, 255, 255, 255, // 288-295
+ 255, 255, 255, 255, 255, 255, 255, 255, // 296-303
+ 255, 255, 255, 255, 255, 255, 255, 255, // 304-311
+ 255, 255, 255, 255, 255, 255, 255, 255, // 312-319
+ 255, 255, 255, 255, 255, 255, 255, 255, // 320-327
+ 255, 255, 255, 255, 255, 255, 255, 255, // 328-335
+ 255, 255, 255, 255, 255, 255, 255, 255, // 336-343
+ 255, 255, 255, 255, 255, 255, 255, 255, // 344-351
+ 255, 255, 255, 255, 255, 255, 255, 255, // 352-359
+ 255, 255, 255, 255, 255, 255, 255, 255, // 360-367
+ 255, 255, 255, 255, 255, 255, 255, 255, // 368-375
+ 255, 255, 255, 255, 255, 255, 255, 255, // 376-383
+};
+const int clipping_table_offset = 128;
+
+} // namespace constants
+} // namespace usb_cam
+
+#endif // USB_CAM__CONSTANTS_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/conversions.hpp b/src/usb_cam-ros2/include/usb_cam/conversions.hpp
new file mode 100644
index 0000000..4cf1ba9
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/conversions.hpp
@@ -0,0 +1,65 @@
+// Copyright 2023 Evan Flynn
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__CONVERSIONS_HPP_
+#define USB_CAM__CONVERSIONS_HPP_
+
+#include <string>
+
+#include "opencv2/imgproc.hpp"
+
+#include "usb_cam/constants.hpp"
+#include "usb_cam/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace conversions
+{
+
+
+std::string FCC2S(const unsigned int & val)
+{
+ std::string s;
+
+ s += val & 0x7f;
+ s += (val >> 8) & 0x7f;
+ s += (val >> 16) & 0x7f;
+ s += (val >> 24) & 0x7f;
+ if (val & (1 << 31)) {
+ s += "-BE";
+ }
+ return s;
+}
+
+} // namespace conversions
+} // namespace usb_cam
+
+#endif // USB_CAM__CONVERSIONS_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/av_pixel_format_helper.hpp b/src/usb_cam-ros2/include/usb_cam/formats/av_pixel_format_helper.hpp
new file mode 100644
index 0000000..ad1792e
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/av_pixel_format_helper.hpp
@@ -0,0 +1,1145 @@
+// Copyright 2023 Boitumelo Ruf
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#ifndef USB_CAM__FORMATS__AV_PIXEL_FORMAT_HELPER_HPP_
+#define USB_CAM__FORMATS__AV_PIXEL_FORMAT_HELPER_HPP_
+
+#include <unordered_map>
+#include <algorithm>
+#include <string>
+
+extern "C" {
+#define __STDC_CONSTANT_MACROS // Required for libavutil
+#include "libavutil/pixfmt.h"
+}
+
+#include "usb_cam/constants.hpp"
+
+#define stringify(name) #name
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+/// Map to associate string of pixel format name to actual pixel format enum
+const std::unordered_map<std::string, AVPixelFormat> STR_2_AVPIXFMT = {
+ {stringify(AV_PIX_FMT_NONE), AV_PIX_FMT_NONE},
+
+ {stringify(AV_PIX_FMT_YUV420P), AV_PIX_FMT_YUV420P}, ///< planar YUV 4:2:0, 12bpp,
+ ///< (1 Cr & Cb sample per 2x2 Y
+ ///< samples)
+
+ {stringify(AV_PIX_FMT_YUYV422), AV_PIX_FMT_YUYV422}, ///< packed YUV 4:2:2, 16bpp,
+ ///< Y0 Cb Y1 Cr
+
+ {stringify(AV_PIX_FMT_RGB24), AV_PIX_FMT_RGB24}, ///< packed RGB 8:8:8, 24bpp,
+ ///< RGBRGB...
+
+ {stringify(AV_PIX_FMT_BGR24), AV_PIX_FMT_BGR24}, ///< packed RGB 8:8:8, 24bpp,
+ ///< BGRBGR...
+
+ {stringify(AV_PIX_FMT_YUV422P), AV_PIX_FMT_YUV422P}, ///< planar YUV 4:2:2, 16bpp,
+ ///< (1 Cr & Cb sample per 2x1 Y
+ ///< samples)
+
+ {stringify(AV_PIX_FMT_YUV444P), AV_PIX_FMT_YUV444P}, ///< planar YUV 4:4:4, 24bpp, (1 Cr
+ ///< & Cb sample per 1x1 Y samples)
+
+ {stringify(AV_PIX_FMT_YUV410P), AV_PIX_FMT_YUV410P}, ///< planar YUV 4:1:0, 9bpp, (1 Cr
+ ///< & Cb sample per 4x4 Y samples)
+
+ {stringify(AV_PIX_FMT_YUV411P), AV_PIX_FMT_YUV411P}, ///< planar YUV 4:1:1, 12bpp, (1 Cr
+ ///< & Cb sample per 4x1 Y samples)
+
+ {stringify(AV_PIX_FMT_GRAY8), AV_PIX_FMT_GRAY8}, ///< Y , 8bpp
+
+ {stringify(AV_PIX_FMT_MONOWHITE), AV_PIX_FMT_MONOWHITE}, ///< Y , 1bpp, 0 is
+ ///< white, 1 is black, in each byte
+ ///< pixels are ordered from the msb
+ ///< to the lsb
+
+ {stringify(AV_PIX_FMT_MONOBLACK), AV_PIX_FMT_MONOBLACK}, ///< Y , 1bpp, 0 is
+ ///< black, 1 is white, in each byte
+ ///< pixels are ordered from the msb
+ ///< to the lsb
+
+ {stringify(AV_PIX_FMT_PAL8), AV_PIX_FMT_PAL8}, ///< 8 bits with AV_PIX_FMT_RGB32
+ ///< palette
+
+ {stringify(AV_PIX_FMT_YUVJ420P), AV_PIX_FMT_YUVJ420P}, ///< planar YUV 4:2:0, 12bpp, full
+ ///< scale (JPEG), deprecated in
+ ///< favor of AV_PIX_FMT_YUV420P and
+ ///< setting color_range
+
+ {stringify(AV_PIX_FMT_YUVJ422P), AV_PIX_FMT_YUVJ422P}, ///< planar YUV 4:2:2, 16bpp, full
+ ///< scale (JPEG), deprecated in
+ ///< favor of AV_PIX_FMT_YUV422P and
+ ///< setting color_range
+
+ {stringify(AV_PIX_FMT_YUVJ444P), AV_PIX_FMT_YUVJ444P}, ///< planar YUV 4:4:4, 24bpp,
+ ///< full scale (JPEG), deprecated
+ ///< in favor of AV_PIX_FMT_YUV444P
+ ///< and setting color_range
+
+ {stringify(AV_PIX_FMT_UYVY422), AV_PIX_FMT_UYVY422}, ///< packed YUV 4:2:2, 16bpp, Cb Y0
+ ///< Cr Y1
+
+ {stringify(AV_PIX_FMT_UYYVYY411), AV_PIX_FMT_UYYVYY411}, ///< packed YUV 4:1:1, 12bpp, Cb Y0
+ ///< Y1 Cr Y2 Y3
+
+ {stringify(AV_PIX_FMT_BGR8), AV_PIX_FMT_BGR8}, ///< packed RGB 3:3:2, 8bpp,
+ ///< (msb)2B 3G 3R(lsb)
+
+ {stringify(AV_PIX_FMT_BGR4), AV_PIX_FMT_BGR4}, ///< packed RGB 1:2:1 bitstream,
+ ///< 4bpp, (msb)1B 2G 1R(lsb), a
+ ///< byte contains two pixels, the
+ ///< first pixel in the byte is the
+ ///< one composed by the 4 msb bits
+
+ {stringify(AV_PIX_FMT_BGR4_BYTE), AV_PIX_FMT_BGR4_BYTE}, ///< packed RGB 1:2:1, 8bpp,
+ ///< (msb)1B 2G 1R(lsb)
+
+ {stringify(AV_PIX_FMT_RGB8), AV_PIX_FMT_RGB8}, ///< packed RGB 3:3:2, 8bpp,
+ ///< (msb)2R 3G 3B(lsb)
+
+ {stringify(AV_PIX_FMT_RGB4), AV_PIX_FMT_RGB4}, ///< packed RGB 1:2:1 bitstream,
+ ///< 4bpp, (msb)1R 2G 1B(lsb), a
+ ///< byte contains two pixels, the
+ ///< first pixel in the byte is the
+ ///< one composed by the 4 msb bits
+
+ {stringify(AV_PIX_FMT_RGB4_BYTE), AV_PIX_FMT_RGB4_BYTE}, ///< packed RGB 1:2:1, 8bpp,
+ ///< (msb)1R 2G 1B(lsb)
+
+ {stringify(AV_PIX_FMT_NV12), AV_PIX_FMT_NV12}, ///< planar YUV 4:2:0, 12bpp, 1
+ ///< plane for Y and 1 plane for the
+ ///< UV components, which are
+ ///< interleaved (first byte U and
+ ///< the following byte V)
+
+ {stringify(AV_PIX_FMT_NV21), AV_PIX_FMT_NV21}, ///< as above, but U and V bytes are
+ ///< swapped
+
+
+ {stringify(AV_PIX_FMT_ARGB), AV_PIX_FMT_ARGB}, ///< packed ARGB 8:8:8:8, 32bpp,
+ ///< ARGBARGB...
+
+ {stringify(AV_PIX_FMT_RGBA), AV_PIX_FMT_RGBA}, ///< packed RGBA 8:8:8:8, 32bpp,
+ ///< RGBARGBA...
+
+ {stringify(AV_PIX_FMT_ABGR), AV_PIX_FMT_ABGR}, ///< packed ABGR 8:8:8:8, 32bpp,
+ ///< ABGRABGR...
+
+ {stringify(AV_PIX_FMT_BGRA), AV_PIX_FMT_BGRA}, ///< packed BGRA 8:8:8:8, 32bpp,
+ ///< BGRABGRA...
+
+
+ {stringify(AV_PIX_FMT_GRAY16BE), AV_PIX_FMT_GRAY16BE}, ///< Y , 16bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GRAY16LE), AV_PIX_FMT_GRAY16LE}, ///< Y , 16bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUV440P), AV_PIX_FMT_YUV440P}, ///< planar YUV 4:4:0 (1 Cr & Cb
+ ///< sample per 1x2 Y samples)
+
+ {stringify(AV_PIX_FMT_YUVJ440P), AV_PIX_FMT_YUVJ440P}, ///< planar YUV 4:4:0 full scale
+ ///< (JPEG), deprecated in favor of
+ ///< AV_PIX_FMT_YUV440P and setting
+ ///< color_range
+
+ {stringify(AV_PIX_FMT_YUVA420P), AV_PIX_FMT_YUVA420P}, ///< planar YUV 4:2:0, 20bpp, (1 Cr
+ ///< & Cb sample per 2x2 Y & A
+ ///< samples)
+
+ {stringify(AV_PIX_FMT_RGB48BE), AV_PIX_FMT_RGB48BE}, ///< packed RGB 16:16:16, 48bpp,
+ ///< 16R, 16G, 16B, the 2-byte value
+ ///< for each R/G/B component is
+ ///< stored as big-endian
+
+ {stringify(AV_PIX_FMT_RGB48LE), AV_PIX_FMT_RGB48LE}, ///< packed RGB 16:16:16, 48bpp,
+ ///< 16R, 16G, 16B, the 2-byte value
+ ///< for each R/G/B component is
+ ///< stored as little-endian
+
+
+ {stringify(AV_PIX_FMT_RGB565BE), AV_PIX_FMT_RGB565BE}, ///< packed RGB 5:6:5, 16bpp, (msb)
+ ///< 5R 6G 5B(lsb), big-endian
+
+ {stringify(AV_PIX_FMT_RGB565LE), AV_PIX_FMT_RGB565LE}, ///< packed RGB 5:6:5, 16bpp, (msb)
+ ///< 5R 6G 5B(lsb), little-endian
+
+ {stringify(AV_PIX_FMT_RGB555BE), AV_PIX_FMT_RGB555BE}, ///< packed RGB 5:5:5, 16bpp,
+ ///< (msb)1X 5R 5G 5B(lsb),
+ ///< big-endian , X=unused/undefined
+
+ {stringify(AV_PIX_FMT_RGB555LE), AV_PIX_FMT_RGB555LE}, ///< packed RGB 5:5:5, 16bpp,
+ ///< (msb)1X 5R 5G 5B(lsb),
+ ///< little-endian,
+ ///< X=unused/undefined
+
+
+ {stringify(AV_PIX_FMT_BGR565BE), AV_PIX_FMT_RGB565BE}, ///< packed BGR 5:6:5, 16bpp, (msb)
+ ///< 5B 6G 5R(lsb), big-endian
+
+ {stringify(AV_PIX_FMT_BGR565LE), AV_PIX_FMT_RGB565LE}, ///< packed BGR 5:6:5, 16bpp, (msb)
+ ///< 5B 6G 5R(lsb), little-endian
+
+ {stringify(AV_PIX_FMT_BGR555BE), AV_PIX_FMT_RGB555BE}, ///< packed BGR 5:5:5, 16bpp,
+ ///< (msb)1X 5B 5G 5R(lsb),
+ ///< big-endian, X=unused/undefined
+
+ {stringify(AV_PIX_FMT_BGR555LE), AV_PIX_FMT_RGB555LE}, ///< packed BGR 5:5:5, 16bpp,
+ ///< (msb)1X 5B 5G 5R(lsb),
+ ///< little-endian,
+ ///< X=unused/undefined
+
+#if FF_API_VAAPI
+ /** @name Deprecated pixel formats */
+ /**@{*/
+ {stringify(AV_PIX_FMT_VAAPI_MOCO), AV_PIX_FMT_VAAPI_MOCO}, ///< HW acceleration through VA API
+ ///< at motion compensation
+ ///< entry-point, Picture.data[3]
+ ///< contains a vaapi_render_state
+ ///< struct which contains
+ ///< macroblocks as well as various
+ ///< fields extracted from headers
+
+ {stringify(AV_PIX_FMT_VAAPI_IDCT), AV_PIX_FMT_VAAPI_IDCT}, ///< HW acceleration through VA API
+ ///< at IDCT entry-point,
+ ///< Picture.data[3] contains a
+ ///< vaapi_render_state struct which
+ ///< contains fields extracted from
+ ///< headers
+
+ {stringify(AV_PIX_FMT_VAAPI_VLD), AV_PIX_FMT_VAAPI_VLD}, ///< HW decoding through VA API,
+ ///< Picture.data[3] contains a
+ ///< VASurfaceID
+ /**@}*/
+ {stringify(AV_PIX_FMT_VAAPI), AV_PIX_FMT_VAAPI},
+#else
+ /**
+ * Hardware acceleration through VA-API, data[3] contains a
+ * VASurfaceID.
+ */
+ {stringify(AV_PIX_FMT_VAAPI), AV_PIX_FMT_VAAPI},
+#endif
+
+ {stringify(AV_PIX_FMT_YUV420P16LE), AV_PIX_FMT_YUV420P16LE}, ///< planar YUV 4:2:0, 24bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV420P16BE), AV_PIX_FMT_YUV420P16BE}, ///< planar YUV 4:2:0, 24bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV422P16LE), AV_PIX_FMT_YUV422P16LE}, ///< planar YUV 4:2:2, 32bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV422P16BE), AV_PIX_FMT_YUV422P16BE}, ///< planar YUV 4:2:2, 32bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV444P16LE), AV_PIX_FMT_YUV444P16LE}, ///< planar YUV 4:4:4, 48bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV444P16BE), AV_PIX_FMT_YUV444P16BE}, ///< planar YUV 4:4:4, 48bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_DXVA2_VLD), AV_PIX_FMT_DXVA2_VLD}, ///< HW decoding through DXVA2,
+ ///< Picture.data[3] contains a
+ ///< LPDIRECT3DSURFACE9 pointer
+
+
+ {stringify(AV_PIX_FMT_RGB444LE), AV_PIX_FMT_RGB444LE}, ///< packed RGB 4:4:4, 16bpp,
+ ///< (msb)4X 4R 4G 4B(lsb),
+ ///< little-endian,
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_RGB444BE), AV_PIX_FMT_RGB444BE}, ///< packed RGB 4:4:4, 16bpp,
+ ///< (msb)4X 4R 4G 4B(lsb),
+ ///< big-endian,
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_BGR444LE), AV_PIX_FMT_BGR444LE}, ///< packed BGR 4:4:4, 16bpp,
+ ///< (msb)4X 4B 4G 4R(lsb),
+ ///< little-endian,
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_BGR444BE), AV_PIX_FMT_BGR444BE}, ///< packed BGR 4:4:4, 16bpp,
+ ///< (msb)4X 4B 4G 4R(lsb),
+ ///< big-endian,
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_YA8), AV_PIX_FMT_YA8}, ///< 8 bits gray, 8 bits alpha
+
+
+ {stringify(AV_PIX_FMT_Y400A), AV_PIX_FMT_Y400A}, ///< alias for AV_PIX_FMT_YA8
+
+ {stringify(AV_PIX_FMT_GRAY8A), AV_PIX_FMT_GRAY8A}, ///< alias for AV_PIX_FMT_YA8
+
+
+ {stringify(AV_PIX_FMT_BGR48BE), AV_PIX_FMT_BGR48BE}, ///< packed RGB 16:16:16, 48bpp,
+ ///< 16B, 16G, 16R, the 2-byte
+ ///< value for each R/G/B
+ ///< component is stored as
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_BGR48LE), AV_PIX_FMT_BGR48LE}, ///< packed RGB 16:16:16, 48bpp,
+ ///< 16B, 16G, 16R, the 2-byte
+ ///< value for each R/G/B
+ ///< component is stored as
+ ///< little-endian
+
+ /**
+ * The following 12 formats have the disadvantage of needing 1 format for each bit depth.
+ * Notice that each 9/10 bits sample is stored in 16 bits with extra padding.
+ * If you want to support multiple bit depths, then using AV_PIX_FMT_YUV420P16* with the bpp
+ * stored separately is better.
+ */
+ {stringify(AV_PIX_FMT_YUV420P9BE), AV_PIX_FMT_YUV420P9BE}, ///< planar YUV 4:2:0, 13.5bpp,
+ ///< (1 Cr & Cb sample per 2x2 Y
+ ///< samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV420P9LE), AV_PIX_FMT_YUV420P9LE}, ///< planar YUV 4:2:0, 13.5bpp,
+ ///< (1 Cr & Cb sample per 2x2 Y
+ ///< samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV420P10BE), AV_PIX_FMT_YUV420P10BE}, ///< planar YUV 4:2:0, 15bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV420P10LE), AV_PIX_FMT_YUV420P10LE}, ///< planar YUV 4:2:0, 15bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV422P10BE), AV_PIX_FMT_YUV422P10BE}, ///< planar YUV 4:2:2, 20bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV422P10LE), AV_PIX_FMT_YUV422P10LE}, ///< planar YUV 4:2:2, 20bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV444P9BE), AV_PIX_FMT_YUV444P9BE}, ///< planar YUV 4:4:4, 27bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV444P9LE), AV_PIX_FMT_YUV444P9LE}, ///< planar YUV 4:4:4, 27bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV444P10BE), AV_PIX_FMT_YUV444P10BE}, ///< planar YUV 4:4:4, 30bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV444P10LE), AV_PIX_FMT_YUV444P10LE}, ///< planar YUV 4:4:4, 30bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV422P9BE), AV_PIX_FMT_YUV422P9BE}, ///< planar YUV 4:2:2, 18bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV422P9LE), AV_PIX_FMT_YUV422P9LE}, ///< planar YUV 4:2:2, 18bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_GBRP), AV_PIX_FMT_GBRP}, ///< planar GBR 4:4:4 24bpp
+
+ {stringify(AV_PIX_FMT_GBR24P), AV_PIX_FMT_GBR24P}, // alias for #AV_PIX_FMT_GBRP
+
+ {stringify(AV_PIX_FMT_GBRP9BE), AV_PIX_FMT_GBRP9BE}, ///< planar GBR 4:4:4 27bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRP9LE), AV_PIX_FMT_GBRP9LE}, ///< planar GBR 4:4:4 27bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_GBRP10BE), AV_PIX_FMT_GBRP10BE}, ///< planar GBR 4:4:4 30bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRP10LE), AV_PIX_FMT_GBRP10LE}, ///< planar GBR 4:4:4 30bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_GBRP16BE), AV_PIX_FMT_GBRP16BE}, ///< planar GBR 4:4:4 48bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRP16LE), AV_PIX_FMT_GBRP16LE}, ///< planar GBR 4:4:4 48bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVA422P), AV_PIX_FMT_YUVA422P}, ///< planar YUV 4:2:2 24bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples)
+
+ {stringify(AV_PIX_FMT_YUVA444P), AV_PIX_FMT_YUVA444P}, ///< planar YUV 4:4:4 32bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples)
+
+ {stringify(AV_PIX_FMT_YUVA420P9BE), AV_PIX_FMT_YUVA420P9BE}, ///< planar YUV 4:2:0 22.5bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUVA420P9LE), AV_PIX_FMT_YUVA420P9LE}, ///< planar YUV 4:2:0 22.5bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVA422P9BE), AV_PIX_FMT_YUVA422P9BE}, ///< planar YUV 4:2:2 27bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUVA422P9LE), AV_PIX_FMT_YUVA422P9LE}, ///< planar YUV 4:2:2 27bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVA444P9BE), AV_PIX_FMT_YUVA444P9BE}, ///< planar YUV 4:4:4 36bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUVA444P9LE), AV_PIX_FMT_YUVA444P9LE}, ///< planar YUV 4:4:4 36bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVA420P10BE), AV_PIX_FMT_YUVA420P10BE}, ///< planar YUV 4:2:0 25bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA420P10LE), AV_PIX_FMT_YUVA420P10LE}, ///< planar YUV 4:2:0 25bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples,
+ ///< little-endian)
+
+ {stringify(AV_PIX_FMT_YUVA422P10BE), AV_PIX_FMT_YUVA422P10BE}, ///< planar YUV 4:2:2 30bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA422P10LE), AV_PIX_FMT_YUVA422P10LE}, ///< planar YUV 4:2:2 30bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples,
+ ///< little-endian)
+
+ {stringify(AV_PIX_FMT_YUVA444P10BE), AV_PIX_FMT_YUVA444P10BE}, ///< planar YUV 4:4:4 40bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA444P10LE), AV_PIX_FMT_YUVA444P10LE}, ///< planar YUV 4:4:4 40bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples,
+ ///< little-endian)
+
+ {stringify(AV_PIX_FMT_YUVA420P16BE), AV_PIX_FMT_YUVA420P16BE}, ///< planar YUV 4:2:0 40bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA420P16LE), AV_PIX_FMT_YUVA420P16LE}, ///< planar YUV 4:2:0 40bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y & A samples,
+ ///< little-endian)
+
+ {stringify(AV_PIX_FMT_YUVA422P16BE), AV_PIX_FMT_YUVA422P16BE}, ///< planar YUV 4:2:2 48bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA422P16LE), AV_PIX_FMT_YUVA422P16LE}, ///< planar YUV 4:2:2 48bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y & A samples,
+ ///< little-endian)
+
+ {stringify(AV_PIX_FMT_YUVA444P16BE), AV_PIX_FMT_YUVA444P16BE}, ///< planar YUV 4:4:4 64bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples, big-endian)
+
+ {stringify(AV_PIX_FMT_YUVA444P16LE), AV_PIX_FMT_YUVA444P16LE}, ///< planar YUV 4:4:4 64bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y & A samples,
+ ///< little-endian)
+
+
+ {stringify(AV_PIX_FMT_VDPAU), AV_PIX_FMT_VDPAU}, ///< HW acceleration through
+ ///< VDPAU, Picture.data[3]
+ ///< contains a VdpVideoSurface
+
+
+ {stringify(AV_PIX_FMT_XYZ12LE), AV_PIX_FMT_XYZ12LE}, ///< packed XYZ 4:4:4, 36 bpp,
+ ///< (msb) 12X, 12Y, 12Z (lsb),
+ ///< the 2-byte value for each
+ ///< X/Y/Z is stored as
+ ///< little-endian, the 4 lower
+ ///< bits are set to 0
+
+ {stringify(AV_PIX_FMT_XYZ12BE), AV_PIX_FMT_XYZ12BE}, ///< packed XYZ 4:4:4, 36 bpp,
+ ///< (msb) 12X, 12Y, 12Z (lsb),
+ ///< the 2-byte value for each
+ ///< X/Y/Z is stored as
+ ///< big-endian, the 4 lower
+ ///< bits are set to 0
+
+ {stringify(AV_PIX_FMT_NV16), AV_PIX_FMT_NV16}, ///< interleaved chroma
+ ///< YUV 4:2:2, 16bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples)
+
+ {stringify(AV_PIX_FMT_NV20LE), AV_PIX_FMT_NV16}, ///< interleaved chroma
+ ///< YUV 4:2:2, 20bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_NV20BE), AV_PIX_FMT_NV16}, ///< interleaved chroma
+ ///< YUV 4:2:2, 20bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+
+ {stringify(AV_PIX_FMT_RGBA64BE), AV_PIX_FMT_RGBA64BE}, ///< packed RGBA 16:16:16:16,
+ ///< 64bpp, 16R, 16G, 16B, 16A,
+ ///< the 2-byte value for each
+ ///< R/G/B/A component is stored
+ ///< as big-endian
+
+ {stringify(AV_PIX_FMT_RGBA64LE), AV_PIX_FMT_RGBA64LE}, ///< packed RGBA 16:16:16:16,
+ ///< 64bpp, 16R, 16G, 16B, 16A,
+ ///< the 2-byte value for each
+ ///< R/G/B/A component is stored
+ ///< as little-endian
+
+ {stringify(AV_PIX_FMT_BGRA64BE), AV_PIX_FMT_BGRA64BE}, ///< packed RGBA 16:16:16:16,
+ ///< 64bpp, 16B, 16G, 16R, 16A,
+ ///< the 2-byte value for each
+ ///< R/G/B/A component is stored
+ ///< as big-endian
+
+ {stringify(AV_PIX_FMT_BGRA64LE), AV_PIX_FMT_BGRA64LE}, ///< packed RGBA 16:16:16:16,
+ ///< 64bpp, 16B, 16G, 16R, 16A,
+ ///< the 2-byte value for each
+ ///< R/G/B/A component is stored
+ ///< as little-endian
+
+
+ {stringify(AV_PIX_FMT_YVYU422), AV_PIX_FMT_YVYU422}, ///< packed YUV 4:2:2, 16bpp, Y0
+ ///< Cr Y1 Cb
+
+
+ {stringify(AV_PIX_FMT_YA16BE), AV_PIX_FMT_YA16BE}, ///< 16 bits gray, 16 bits alpha
+ ///< (big-endian)
+
+ {stringify(AV_PIX_FMT_YA16LE), AV_PIX_FMT_YA16LE}, ///< 16 bits gray, 16 bits alpha
+ ///< (little-endian)
+
+
+ {stringify(AV_PIX_FMT_GBRAP), AV_PIX_FMT_GBRAP}, ///< planar GBRA 4:4:4:4 32bpp
+
+ {stringify(AV_PIX_FMT_GBRAP16BE), AV_PIX_FMT_GBRAP16BE}, ///< planar GBRA 4:4:4:4 64bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRAP16LE), AV_PIX_FMT_GBRAP16LE}, ///< planar GBRA 4:4:4:4 64bpp,
+ ///< little-endian
+
+ /**
+ * HW acceleration through QSV, data[3] contains a pointer to the
+ * mfxFrameSurface1 structure.
+ */
+ {stringify(AV_PIX_FMT_QSV), AV_PIX_FMT_QSV},
+ /**
+ * HW acceleration though MMAL, data[3] contains a pointer to the
+ * MMAL_BUFFER_HEADER_T structure.
+ */
+ {stringify(AV_PIX_FMT_MMAL), AV_PIX_FMT_MMAL},
+
+ {stringify(AV_PIX_FMT_D3D11VA_VLD), AV_PIX_FMT_D3D11VA_VLD}, ///< HW decoding through
+ ///< Direct3D11 via old API,
+ ///< Picture.data[3] contains a
+ ///< ID3D11VideoDecoderOutput-
+ ///< View pointer
+
+ /**
+ * HW acceleration through CUDA. data[i] contain CUdeviceptr pointers
+ * exactly as for system memory frames.
+ */
+ {stringify(AV_PIX_FMT_CUDA), AV_PIX_FMT_CUDA},
+
+ {stringify(AV_PIX_FMT_0RGB), AV_PIX_FMT_0RGB}, ///< packed RGB 8:8:8, 32bpp,
+ ///< XRGBXRGB...
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_RGB0), AV_PIX_FMT_RGB0}, ///< packed RGB 8:8:8, 32bpp,
+ ///< RGBXRGBX...
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_0BGR), AV_PIX_FMT_0BGR}, ///< packed BGR 8:8:8, 32bpp,
+ ///< XBGRXBGR...
+ ///< X=unused/undefined
+
+ {stringify(AV_PIX_FMT_BGR0), AV_PIX_FMT_BGR0}, ///< packed BGR 8:8:8, 32bpp,
+ ///< BGRXBGRX...
+ ///< X=unused/undefined
+
+
+ {stringify(AV_PIX_FMT_YUV420P12BE), AV_PIX_FMT_YUV420P12BE}, ///< planar YUV 4:2:0,18bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV420P12LE), AV_PIX_FMT_YUV420P12LE}, ///< planar YUV 4:2:0,18bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV420P14BE), AV_PIX_FMT_YUV420P14BE}, ///< planar YUV 4:2:0,21bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV420P14LE), AV_PIX_FMT_YUV420P14LE}, ///< planar YUV 4:2:0,21bpp,
+ ///< (1 Cr & Cb sample per 2x2
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV422P12BE), AV_PIX_FMT_YUV422P12BE}, ///< planar YUV 4:2:2,24bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV422P12LE), AV_PIX_FMT_YUV422P12LE}, ///< planar YUV 4:2:2,24bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV422P14BE), AV_PIX_FMT_YUV422P14BE}, ///< planar YUV 4:2:2,28bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV422P14LE), AV_PIX_FMT_YUV422P14LE}, ///< planar YUV 4:2:2,28bpp,
+ ///< (1 Cr & Cb sample per 2x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV444P12BE), AV_PIX_FMT_YUV444P12BE}, ///< planar YUV 4:4:4,36bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV444P12LE), AV_PIX_FMT_YUV444P12LE}, ///< planar YUV 4:4:4,36bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), little-endian
+
+ {stringify(AV_PIX_FMT_YUV444P14BE), AV_PIX_FMT_YUV444P14BE}, ///< planar YUV 4:4:4,42bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), big-endian
+
+ {stringify(AV_PIX_FMT_YUV444P14LE), AV_PIX_FMT_YUV444P14LE}, ///< planar YUV 4:4:4,42bpp,
+ ///< (1 Cr & Cb sample per 1x1
+ ///< Y samples), little-endian
+
+
+ {stringify(AV_PIX_FMT_GBRP12BE), AV_PIX_FMT_GBRP12BE}, ///< planar GBR 4:4:4 36bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRP12LE), AV_PIX_FMT_GBRP12LE}, ///< planar GBR 4:4:4 36bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_GBRP14BE), AV_PIX_FMT_GBRP14BE}, ///< planar GBR 4:4:4 42bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRP14LE), AV_PIX_FMT_GBRP14LE}, ///< planar GBR 4:4:4 42bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVJ411P), AV_PIX_FMT_YUVJ411P}, ///< planar YUV 4:1:1, 12bpp,
+ ///< (1 Cr & Cb sample per 4x1
+ ///< Y samples) full scale
+ ///< (JPEG), deprecated in favor
+ ///< of AV_PIX_FMT_YUV411P and
+ ///< setting color_range
+
+
+ {stringify(AV_PIX_FMT_BAYER_BGGR8), AV_PIX_FMT_BAYER_BGGR8}, ///< bayer,
+ ///< BGBG..(odd line),
+ ///< GRGR..(even line),
+ ///< 8-bit samples
+
+ {stringify(AV_PIX_FMT_BAYER_RGGB8), AV_PIX_FMT_BAYER_RGGB8}, ///< bayer,
+ ///< RGRG..(odd line),
+ ///< GBGB..(even line),
+ ///< 8-bit samples
+
+ {stringify(AV_PIX_FMT_BAYER_GBRG8), AV_PIX_FMT_BAYER_GBRG8}, ///< bayer,
+ ///< GBGB..(odd line),
+ ///< RGRG..(even line),
+ ///< 8-bit samples
+
+ {stringify(AV_PIX_FMT_BAYER_GRBG8), AV_PIX_FMT_BAYER_GRBG8}, ///< bayer,
+ ///< GRGR..(odd line),
+ ///< BGBG..(even line),
+ ///< 8-bit samples
+
+ {stringify(AV_PIX_FMT_BAYER_BGGR16LE), AV_PIX_FMT_BAYER_BGGR16LE}, ///< bayer,
+ ///< BGBG..(odd line),
+ ///< GRGR..(even line),
+ ///< 16-bit samples,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_BAYER_BGGR16BE), AV_PIX_FMT_BAYER_BGGR16BE}, ///< bayer,
+ ///< BGBG..(odd line),
+ ///< GRGR..(even line),
+ ///< 16-bit samples,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_BAYER_RGGB16LE), AV_PIX_FMT_BAYER_RGGB16LE}, ///< bayer,
+ ///< RGRG..(odd line),
+ ///< GBGB..(even line),
+ ///< 16-bit samples,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_BAYER_RGGB16BE), AV_PIX_FMT_BAYER_RGGB16BE}, ///< bayer,
+ ///< RGRG..(odd line),
+ ///< GBGB..(even line),
+ ///< 16-bit samples,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_BAYER_GBRG16LE), AV_PIX_FMT_BAYER_GBRG16LE}, ///< bayer,
+ ///< GBGB..(odd line),
+ ///< RGRG..(even line),
+ ///< 16-bit samples,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_BAYER_GBRG16BE), AV_PIX_FMT_BAYER_GBRG16BE}, ///< bayer,
+ ///< GBGB..(odd line),
+ ///< RGRG..(even line),
+ ///< 16-bit samples,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_BAYER_GRBG16LE), AV_PIX_FMT_BAYER_GRBG16LE}, ///< bayer,
+ ///< GRGR..(odd line),
+ ///< BGBG..(even line),
+ ///< 16-bit samples,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_BAYER_GRBG16BE), AV_PIX_FMT_BAYER_GRBG16BE}, ///< bayer,
+ ///< GRGR..(odd line),
+ ///< BGBG..(even line),
+ ///< 16-bit samples,
+ ///< big-endian
+
+
+ {stringify(AV_PIX_FMT_XVMC), AV_PIX_FMT_XVMC}, ///< XVideo Motion
+ ///< Acceleration via common
+ ///< packet passing
+
+
+ {stringify(AV_PIX_FMT_YUV440P10LE), AV_PIX_FMT_YUV440P10LE}, ///< planar YUV 4:4:0,20bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x2 Y samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUV440P10BE), AV_PIX_FMT_YUV440P10BE}, ///< planar YUV 4:4:0,20bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x2 Y samples),
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_YUV440P12LE), AV_PIX_FMT_YUV440P12LE}, ///< planar YUV 4:4:0,24bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x2 Y samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUV440P12BE), AV_PIX_FMT_YUV440P12BE}, ///< planar YUV 4:4:0,24bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x2 Y samples),
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_AYUV64LE), AV_PIX_FMT_AYUV64LE}, ///< packed AYUV 4:4:4,64bpp
+ ///< (1 Cr & Cb sample per
+ ///< 1x1 Y & A samples),
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_AYUV64BE), AV_PIX_FMT_AYUV64BE}, ///< packed AYUV 4:4:4,64bpp
+ ///< (1 Cr & Cb sample per
+ ///< 1x1 Y & A samples),
+ ///< big-endian
+
+
+ {stringify(AV_PIX_FMT_VIDEOTOOLBOX), AV_PIX_FMT_VIDEOTOOLBOX}, ///< hardware decoding
+ ///< through Videotoolbox
+
+
+ {stringify(AV_PIX_FMT_P010LE), AV_PIX_FMT_P010LE}, ///< like NV12, with 10bpp
+ ///< per component, data in
+ ///< the high bits, zeros in
+ ///< the low bits,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_P010BE), AV_PIX_FMT_P010BE}, ///< like NV12, with 10bpp
+ ///< per component, data in
+ ///< the high bits, zeros in
+ ///< the low bits,
+ ///< big-endian
+
+
+ {stringify(AV_PIX_FMT_GBRAP12BE), AV_PIX_FMT_GBRAP12BE}, ///< planar GBR 4:4:4:4
+ ///< 48bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GBRAP12LE), AV_PIX_FMT_GBRAP12LE}, ///< planar GBR 4:4:4:4
+ ///< 48bpp, little-endian
+
+
+ {stringify(AV_PIX_FMT_GBRAP10BE), AV_PIX_FMT_GBRAP10BE}, ///< planar GBR 4:4:4:4
+ ///< 40bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GBRAP10LE), AV_PIX_FMT_GBRAP10LE}, ///< planar GBR 4:4:4:4
+ ///< 40bpp, little-endian
+
+
+ {stringify(AV_PIX_FMT_MEDIACODEC), AV_PIX_FMT_MEDIACODEC}, ///< hardware decoding
+ ///< through MediaCodec
+
+
+ {stringify(AV_PIX_FMT_GRAY12BE), AV_PIX_FMT_GRAY12BE}, ///< Y, 12bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GRAY12LE), AV_PIX_FMT_GRAY12LE}, ///< Y, 12bpp, little-endian
+
+ {stringify(AV_PIX_FMT_GRAY10BE), AV_PIX_FMT_GRAY10BE}, ///< Y, 10bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GRAY10LE), AV_PIX_FMT_GRAY10LE}, ///< Y, 10bpp, little-endian
+
+
+ {stringify(AV_PIX_FMT_P016LE), AV_PIX_FMT_P016LE}, ///< like NV12, with 16bpp
+ ///< per component,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_P016BE), AV_PIX_FMT_P016BE}, ///< like NV12, with 16bpp
+ ///< per component,
+ ///< big-endian
+
+ /**
+ * Hardware surfaces for Direct3D11.
+ *
+ * This is preferred over the legacy AV_PIX_FMT_D3D11VA_VLD. The new D3D11
+ * hwaccel API and filtering support AV_PIX_FMT_D3D11 only.
+ *
+ * data[0] contains a ID3D11Texture2D pointer, and data[1] contains the
+ * texture array index of the frame as intptr_t if the ID3D11Texture2D is
+ * an array texture (or always 0 if it's a normal texture).
+ */
+ {stringify(AV_PIX_FMT_D3D11), AV_PIX_FMT_D3D11},
+
+ {stringify(AV_PIX_FMT_GRAY9BE), AV_PIX_FMT_GRAY9BE}, ///< Y, 9bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GRAY9LE), AV_PIX_FMT_GRAY9LE}, ///< Y, 9bpp, little-endian
+
+
+ {stringify(AV_PIX_FMT_GBRPF32BE), AV_PIX_FMT_GBRPF32BE}, ///< IEEE-754 single
+ ///< precision planar
+ ///< GBR 4:4:4, 96bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRPF32LE), AV_PIX_FMT_GBRPF32LE}, ///< IEEE-754 single
+ ///< precision planar GBR
+ ///< 4:4:4, 96bpp,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_GBRAPF32BE), AV_PIX_FMT_GBRAPF32BE}, ///< IEEE-754 single
+ ///< precision planar GBRA
+ ///< 4:4:4:4, 128bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GBRAPF32LE), AV_PIX_FMT_GBRAPF32LE}, ///< IEEE-754 single
+ ///< precision planar GBRA
+ ///< 4:4:4:4, 128bpp,
+ ///< little-endian
+
+
+ /**
+ * DRM-managed buffers exposed through PRIME buffer sharing.
+ *
+ * data[0] points to an AVDRMFrameDescriptor.
+ */
+ {stringify(AV_PIX_FMT_DRM_PRIME), AV_PIX_FMT_DRM_PRIME},
+ /**
+ * Hardware surfaces for OpenCL.
+ *
+ * data[i] contain 2D image objects (typed in C as cl_mem, used
+ * in OpenCL as image2d_t) for each plane of the surface.
+ */
+ {stringify(AV_PIX_FMT_OPENCL), AV_PIX_FMT_OPENCL},
+
+ {stringify(AV_PIX_FMT_GRAY14BE), AV_PIX_FMT_GRAY14BE}, ///< Y, 14bpp, big-endian
+
+ {stringify(AV_PIX_FMT_GRAY14LE), AV_PIX_FMT_GRAY14LE}, ///< Y, 14bpp, little-endian
+
+
+ {stringify(AV_PIX_FMT_GRAYF32BE), AV_PIX_FMT_GRAYF32BE}, ///< IEEE-754 single
+ ///< precision Y, 32bpp,
+ ///< big-endian
+
+ {stringify(AV_PIX_FMT_GRAYF32LE), AV_PIX_FMT_GRAYF32LE}, ///< IEEE-754 single
+ ///< precision Y, 32bpp,
+ ///< little-endian
+
+
+ {stringify(AV_PIX_FMT_YUVA422P12BE), AV_PIX_FMT_YUVA422P12BE}, ///< planar YUV 4:2:2,24bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 2x1 Y samples),
+ ///< 12b alpha, big-endian
+
+ {stringify(AV_PIX_FMT_YUVA422P12LE), AV_PIX_FMT_YUVA422P12LE}, ///< planar YUV 4:2:2,24bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 2x1 Y samples),
+ ///< 12b alpha,
+ ///< little-endian
+
+ {stringify(AV_PIX_FMT_YUVA444P12BE), AV_PIX_FMT_YUVA444P12BE}, ///< planar YUV 4:4:4,36bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x1 Y samples),
+ ///< 12b alpha, big-endian
+
+ {stringify(AV_PIX_FMT_YUVA444P12LE), AV_PIX_FMT_YUVA444P12LE}, ///< planar YUV 4:4:4,36bpp,
+ ///< (1 Cr & Cb sample per
+ ///< 1x1 Y samples), 12b
+ ///< alpha, little-endian
+
+
+ {stringify(AV_PIX_FMT_NV24), AV_PIX_FMT_NV24}, ///< planar YUV 4:4:4,
+ ///< 24bpp, 1 plane for Y
+ ///< and 1 plane for the UV
+ ///< components, which are
+ ///< interleaved (first byte
+ ///< U and the following
+ ///< byte V)
+
+ {stringify(AV_PIX_FMT_NV42), AV_PIX_FMT_NV42}, ///< as above, but U and V
+ ///< bytes are swapped
+
+ {stringify(AV_PIX_FMT_NB), AV_PIX_FMT_NB}, ///< number of pixel
+ ///< formats, DO NOT USE
+ ///< THIS if you want to
+ ///< link with shared libav*
+ ///< because the number of
+ ///< formats might differ
+ ///< between versions
+};
+
+/// @brief Get AVPixelFormat from string. This string should correspond to the AVPixelFormat name.
+/// The name can either be given with or without the 'AV_PIX_FMT_' prefix.
+/// @param str AVPixelFormat name
+/// @return Pixel format enum corresponding to a given name
+inline AVPixelFormat get_av_pixel_format_from_string(const std::string & str)
+{
+ std::string upperCaseStr = str;
+ std::transform(upperCaseStr.begin(), upperCaseStr.end(), upperCaseStr.begin(), ::toupper);
+
+ std::string fullFmtStr;
+ if (upperCaseStr.rfind("AV_PIX_FMT_", 0) == std::string::npos) {
+ // passed string does not start with 'AV_PIX_FMT_'
+ fullFmtStr = "AV_PIX_FMT_" + upperCaseStr;
+ } else {
+ fullFmtStr = upperCaseStr;
+ }
+
+ return STR_2_AVPIXFMT.find(fullFmtStr)->second;
+}
+
+
+/// @brief Get ROS PixelFormat from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return String specifying the ROS pixel format.
+inline std::string get_ros_pixel_format_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+ std::string ros_format = "";
+
+ switch (avPixelFormat) {
+ default:
+ {
+ ros_format = usb_cam::constants::UNKNOWN;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_RGB24:
+ {
+ ros_format = usb_cam::constants::RGB8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_RGBA:
+ {
+ ros_format = usb_cam::constants::RGBA8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_BGR24:
+ {
+ ros_format = usb_cam::constants::BGR8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_BGRA:
+ {
+ ros_format = usb_cam::constants::BGRA8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_GRAY8:
+ {
+ ros_format = usb_cam::constants::MONO8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+ {
+ ros_format = usb_cam::constants::MONO16;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_YUV422P:
+ {
+ ros_format = usb_cam::constants::YUV422;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_YUV420P:
+ {
+ ros_format = usb_cam::constants::NV21;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_YUV444P:
+ {
+ ros_format = usb_cam::constants::NV24;
+ }
+ break;
+ }
+
+ return ros_format;
+}
+
+
+/// @brief Overload to support av pixel formats being passed as strings
+/// @param avPixelFormat AvPixelFormat as string
+/// @return String specifying the ROS pixel format.
+inline std::string get_ros_pixel_format_from_av_format(const std::string avPixelFormatStr)
+{
+ auto fmt = get_av_pixel_format_from_string(avPixelFormatStr);
+ return get_ros_pixel_format_from_av_format(fmt);
+}
+
+
+/// @brief Get the number of channels from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return Number of channels as uint8
+inline uint8_t get_channels_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+ uint8_t channels = 1;
+
+ switch (avPixelFormat) {
+ default:
+ case AVPixelFormat::AV_PIX_FMT_GRAY8:
+ case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+ {
+ channels = 1;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_YUV422P:
+ case AVPixelFormat::AV_PIX_FMT_YUV420P:
+ case AVPixelFormat::AV_PIX_FMT_YUV444P:
+ {
+ channels = 2;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_RGB24:
+ case AVPixelFormat::AV_PIX_FMT_BGR24:
+ {
+ channels = 3;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_RGBA:
+ case AVPixelFormat::AV_PIX_FMT_BGRA:
+ {
+ channels = 4;
+ }
+ break;
+ }
+
+ return channels;
+}
+
+
+/// @brief Overload of function below to support av pixel formats as strings
+/// @param avPixelFormatStr AvPixelFormat as string
+/// @return Number of channels as uint8_t
+inline uint8_t get_channels_from_av_format(const std::string & avPixelFormatStr)
+{
+ auto fmt = get_av_pixel_format_from_string(avPixelFormatStr);
+ return get_channels_from_av_format(fmt);
+}
+
+
+/// @brief Get the pixel bit depth from AVPixelFormat.
+/// @param avPixelFormat AVPixelFormat
+/// @return Bit depth as uint8
+inline uint8_t get_bit_depth_from_av_format(const AVPixelFormat & avPixelFormat)
+{
+ uint8_t bit_depth = 8;
+
+ switch (avPixelFormat) {
+ default:
+ case AVPixelFormat::AV_PIX_FMT_GRAY8:
+ case AVPixelFormat::AV_PIX_FMT_RGB24:
+ case AVPixelFormat::AV_PIX_FMT_BGR24:
+ case AVPixelFormat::AV_PIX_FMT_RGBA:
+ case AVPixelFormat::AV_PIX_FMT_BGRA:
+ case AVPixelFormat::AV_PIX_FMT_YUV422P:
+ case AVPixelFormat::AV_PIX_FMT_YUV420P:
+ case AVPixelFormat::AV_PIX_FMT_YUV444P:
+ {
+ bit_depth = 8;
+ }
+ break;
+
+ case AVPixelFormat::AV_PIX_FMT_GRAY16BE:
+ {
+ bit_depth = 16;
+ }
+ break;
+ }
+
+ return bit_depth;
+}
+
+
+/// @brief Overload of function below to support passing av pixel formats as strings
+/// @param avPixelFormatStr AVPixelFormat as string
+/// @return Bit depth as uint8
+inline uint8_t get_bit_depth_from_av_format(const std::string & avPixelFormatStr)
+{
+ auto fmt = get_av_pixel_format_from_string(avPixelFormatStr);
+ return get_bit_depth_from_av_format(fmt);
+}
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__AV_PIXEL_FORMAT_HELPER_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/m420.hpp b/src/usb_cam-ros2/include/usb_cam/formats/m420.hpp
new file mode 100644
index 0000000..e490330
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/m420.hpp
@@ -0,0 +1,79 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__M420_HPP_
+#define USB_CAM__FORMATS__M420_HPP_
+
+#include "linux/videodev2.h"
+
+#include "opencv2/imgproc.hpp"
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class M4202RGB : public pixel_format_base
+{
+public:
+ explicit M4202RGB(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "m4202rgb",
+ V4L2_PIX_FMT_M420,
+ usb_cam::constants::RGB8,
+ 3,
+ 8,
+ true),
+ m_width(args.width),
+ m_height(args.height)
+ {}
+
+ /// @brief Convert a YUV420 (aka M420) image to RGB8
+ void convert(const char * & src, char * & dest, const int & bytes_used) override
+ {
+ (void)bytes_used; // not used by this conversion method
+ cv::Size size(m_height, m_width);
+ const cv::Mat cv_img(m_height, m_width, CV_8UC1, const_cast<char *>(src));
+ cv::Mat cv_out(m_height, m_width, CV_8UC3, dest);
+ cv::cvtColor(cv_img, cv_out, cv::COLOR_YUV420p2RGB);
+ }
+
+private:
+ int m_width;
+ int m_height;
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__M420_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/mjpeg.hpp b/src/usb_cam-ros2/include/usb_cam/formats/mjpeg.hpp
new file mode 100644
index 0000000..2b40bb2
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/mjpeg.hpp
@@ -0,0 +1,256 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__MJPEG_HPP_
+#define USB_CAM__FORMATS__MJPEG_HPP_
+///
+/// @file Most of this code follows the example provided by ffmpeg:
+///
+/// https://www.ffmpeg.org/doxygen/5.1/decode__video_8c_source.html
+/// https://www.ffmpeg.org/doxygen/4.0/decode__video_8c_source.html
+///
+
+#include <iostream>
+
+
+extern "C" {
+#define __STDC_CONSTANT_MACROS // Required for libavutil
+#include "libavutil/imgutils.h"
+#include "libavformat/avformat.h"
+#include "libavutil/error.h"
+#include "libavutil/log.h"
+#include "linux/videodev2.h"
+#include "libswscale/swscale.h"
+}
+
+#include "usb_cam/usb_cam.hpp"
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+#include "usb_cam/formats/av_pixel_format_helper.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class RAW_MJPEG : public pixel_format_base
+{
+public:
+ explicit RAW_MJPEG(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "raw_mjpeg",
+ V4L2_PIX_FMT_MJPEG,
+ get_ros_pixel_format_from_av_format(args.av_device_format_str),
+ get_channels_from_av_format(args.av_device_format_str),
+ get_bit_depth_from_av_format(args.av_device_format_str),
+ false)
+ {}
+};
+
+class MJPEG2RGB : public pixel_format_base
+{
+public:
+ explicit MJPEG2RGB(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "mjpeg2rgb",
+ V4L2_PIX_FMT_MJPEG,
+ usb_cam::constants::RGB8,
+ 3,
+ 8,
+ true),
+ m_avcodec(avcodec_find_decoder(AVCodecID::AV_CODEC_ID_MJPEG)),
+ m_avparser(av_parser_init(AVCodecID::AV_CODEC_ID_MJPEG)),
+ m_avframe_device(av_frame_alloc()),
+ m_avframe_rgb(av_frame_alloc()),
+ m_avoptions(NULL),
+ m_averror_str(reinterpret_cast<char *>(malloc(AV_ERROR_MAX_STRING_SIZE)))
+ {
+ if (!m_avcodec) {
+ throw std::runtime_error("Could not find MJPEG decoder");
+ }
+
+ if (!m_avparser) {
+ throw std::runtime_error("Could not find MJPEG parser");
+ }
+
+ m_avcodec_context = avcodec_alloc_context3(m_avcodec);
+
+ m_avframe_device->width = args.width;
+ m_avframe_device->height = args.height;
+ m_avframe_device->format = AV_PIX_FMT_YUV422P;
+ m_avframe_device->format = formats::get_av_pixel_format_from_string(args.av_device_format_str);
+
+ m_avframe_rgb->width = args.width;
+ m_avframe_rgb->height = args.height;
+ m_avframe_rgb->format = AV_PIX_FMT_RGB24;
+
+ m_sws_context = sws_getContext(
+ args.width, args.height, (AVPixelFormat)m_avframe_device->format,
+ args.width, args.height, (AVPixelFormat)m_avframe_rgb->format, SWS_FAST_BILINEAR,
+ NULL, NULL, NULL);
+
+ // Suppress warnings from ffmpeg libraries to avoid spamming the console
+ av_log_set_level(AV_LOG_FATAL);
+ av_log_set_flags(AV_LOG_SKIP_REPEATED);
+ // av_log_set_flags(AV_LOG_PRINT_LEVEL);
+
+ m_avcodec_context->width = args.width;
+ m_avcodec_context->height = args.height;
+ m_avcodec_context->pix_fmt = (AVPixelFormat)m_avframe_device->format;
+ m_avcodec_context->codec_type = AVMEDIA_TYPE_VIDEO;
+
+ m_avframe_device_size = static_cast<size_t>(
+ av_image_get_buffer_size(
+ (AVPixelFormat)m_avframe_device->format,
+ m_avframe_device->width,
+ m_avframe_device->height,
+ m_align));
+ m_avframe_rgb_size = static_cast<size_t>(
+ av_image_get_buffer_size(
+ (AVPixelFormat)m_avframe_rgb->format,
+ m_avframe_rgb->width,
+ m_avframe_rgb->height,
+ m_align));
+
+ // Initialize AVCodecContext
+ if (avcodec_open2(m_avcodec_context, m_avcodec, &m_avoptions) < 0) {
+ throw std::runtime_error("Could not open decoder");
+ return;
+ }
+
+ m_result = av_frame_get_buffer(m_avframe_device, m_align);
+ if (m_result != 0) {
+ print_av_error_string(m_result);
+ }
+ m_result = av_frame_get_buffer(m_avframe_rgb, m_align);
+ if (m_result != 0) {
+ print_av_error_string(m_result);
+ }
+ }
+
+ ~MJPEG2RGB()
+ {
+ if (m_averror_str) {
+ free(m_averror_str);
+ }
+ if (m_avoptions) {
+ free(m_avoptions);
+ }
+ if (m_avcodec_context) {
+ avcodec_close(m_avcodec_context);
+ avcodec_free_context(&m_avcodec_context);
+ }
+ if (m_avframe_device) {
+ av_frame_free(&m_avframe_device);
+ }
+ if (m_avframe_rgb) {
+ av_frame_free(&m_avframe_rgb);
+ }
+ if (m_avparser) {
+ av_parser_close(m_avparser);
+ }
+
+ if (m_sws_context) {
+ sws_freeContext(m_sws_context);
+ }
+ }
+
+ void convert(const char * & src, char * & dest, const int & bytes_used) override
+ {
+ m_result = 0;
+ // clear the picture
+ memset(dest, 0, m_avframe_device_size);
+
+ auto avpacket = av_packet_alloc();
+ av_new_packet(avpacket, bytes_used);
+ memcpy(avpacket->data, src, bytes_used);
+
+ // Pass src MJPEG image to decoder
+ m_result = avcodec_send_packet(m_avcodec_context, avpacket);
+
+ av_packet_free(&avpacket);
+
+ // If result is not 0, report what went wrong
+ if (m_result != 0) {
+ std::cerr << "Failed to send AVPacket to decode: ";
+ print_av_error_string(m_result);
+ return;
+ }
+
+ m_result = avcodec_receive_frame(m_avcodec_context, m_avframe_device);
+
+ if (m_result == AVERROR(EAGAIN) || m_result == AVERROR_EOF) {
+ return;
+ } else if (m_result < 0) {
+ std::cerr << "Failed to recieve decoded frame from codec: ";
+ print_av_error_string(m_result);
+ return;
+ }
+
+ sws_scale(
+ m_sws_context, m_avframe_device->data,
+ m_avframe_device->linesize, 0, m_avframe_device->height,
+ m_avframe_rgb->data, m_avframe_rgb->linesize);
+
+ av_image_copy_to_buffer(
+ const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(dest)),
+ m_avframe_rgb_size, m_avframe_rgb->data,
+ m_avframe_rgb->linesize, (AVPixelFormat)m_avframe_rgb->format,
+ m_avframe_rgb->width, m_avframe_rgb->height, m_align);
+ }
+
+private:
+ void print_av_error_string(int & err_code)
+ {
+ av_make_error_string(m_averror_str, AV_ERROR_MAX_STRING_SIZE, err_code);
+ std::cerr << m_averror_str << std::endl;
+ }
+
+ const AVCodec * m_avcodec;
+ AVCodecContext * m_avcodec_context;
+ AVCodecParserContext * m_avparser;
+ AVFrame * m_avframe_device;
+ AVFrame * m_avframe_rgb;
+ AVDictionary * m_avoptions;
+ SwsContext * m_sws_context;
+ size_t m_avframe_device_size;
+ size_t m_avframe_rgb_size;
+ char * m_averror_str;
+ int m_result = 0;
+ int m_counter = 0;
+ const int * m_linesize;
+
+ const int m_align = 32;
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__MJPEG_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/mono.hpp b/src/usb_cam-ros2/include/usb_cam/formats/mono.hpp
new file mode 100644
index 0000000..507988f
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/mono.hpp
@@ -0,0 +1,114 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__MONO_HPP_
+#define USB_CAM__FORMATS__MONO_HPP_
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class MONO8 : public pixel_format_base
+{
+public:
+ explicit MONO8(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "mono8",
+ V4L2_PIX_FMT_GREY,
+ usb_cam::constants::MONO8,
+ 1,
+ 8,
+ false)
+ {
+ (void)args;
+ }
+};
+
+
+class MONO16 : public pixel_format_base
+{
+public:
+ explicit MONO16(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "mono16",
+ V4L2_PIX_FMT_Y16,
+ usb_cam::constants::MONO16,
+ 1,
+ 16,
+ false)
+ {
+ (void)args;
+ }
+};
+
+
+/// @brief Also known as MONO10 to MONO8
+class Y102MONO8 : public pixel_format_base
+{
+public:
+ explicit Y102MONO8(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "y102mono8",
+ V4L2_PIX_FMT_Y10,
+ usb_cam::constants::MONO8,
+ 1,
+ 8,
+ true),
+ m_number_of_pixels(args.pixels)
+ {}
+
+ /// @brief Convert a Y10 (MONO10) image to MONO8
+ /// @param src pointer to source Y10 (MONO10) image
+ /// @param dest pointer to destination MONO8 image
+ /// @param bytes_used number of bytes used by source image
+ void convert(const char * & src, char * & dest, const int & bytes_used) override
+ {
+ (void)bytes_used; // not used by this conversion method
+ int i, j;
+ for (i = 0, j = 0; i < (m_number_of_pixels << 1); i += 2, j += 1) {
+ // first byte is low byte, second byte is high byte; smash together and convert to 8-bit
+ dest[j] = (unsigned char)(((src[i + 0] >> 2) & 0x3F) | ((src[i + 1] << 6) & 0xC0));
+ }
+ }
+
+private:
+ int m_number_of_pixels;
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__MONO_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/pixel_format_base.hpp b/src/usb_cam-ros2/include/usb_cam/formats/pixel_format_base.hpp
new file mode 100644
index 0000000..81eedc8
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/pixel_format_base.hpp
@@ -0,0 +1,218 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__PIXEL_FORMAT_BASE_HPP_
+#define USB_CAM__FORMATS__PIXEL_FORMAT_BASE_HPP_
+
+#include <string>
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/constants.hpp"
+#include "usb_cam/conversions.hpp"
+
+namespace usb_cam
+{
+namespace formats
+{
+
+
+/// @brief Helper structure to standardize all pixel_format_base
+/// constructors, so that they all have the same argument type.
+///
+/// This should also be easily extendable in the future if we need to add additional
+/// arguments for future pixel format(s) that are added.
+typedef struct
+{
+ std::string name = "";
+ int width = 640;
+ int height = 480;
+ size_t pixels = 640 * 480;
+ std::string av_device_format_str = "AV_PIX_FMT_YUV422P";
+} format_arguments_t;
+
+
+/// @brief Base pixel format class. Provide all necessary information for converting between
+/// V4L2 and ROS formats. Meant to be overridden if conversion function is required.
+class pixel_format_base
+{
+public:
+ pixel_format_base(
+ std::string name, uint32_t v4l2, std::string ros,
+ uint8_t channels, uint8_t bit_depth, bool requires_conversion)
+ : m_name(name),
+ m_v4l2(v4l2),
+ m_ros(ros),
+ m_channels(channels),
+ m_bit_depth(bit_depth),
+ m_requires_conversion(requires_conversion)
+ {}
+
+ /// @brief Name of pixel format. Used in the parameters file to select this format
+ /// @return
+ inline std::string name() {return m_name;}
+
+ /// @brief Integer value of V4L2 capture pixel format
+ /// @return uint32_t V4L2 capture pixel format
+ inline uint32_t v4l2() {return m_v4l2;}
+
+ /// @brief String value of V4L2 capture pixel format
+ /// @return std::string V4L2 capture pixel format
+ inline std::string v4l2_str() {return usb_cam::conversions::FCC2S(m_v4l2);}
+
+ /// @brief Name of output pixel (encoding) format to ROS
+ /// @return
+ inline std::string ros() {return m_ros;}
+
+ /// @brief Number of channels (e.g. bytes) per pixel
+ /// @return
+ inline uint8_t channels() {return m_channels;}
+
+ /// @brief Number for bit depth of image
+ /// @return
+ inline uint8_t bit_depth() {return m_bit_depth;}
+
+ /// @brief Number of bytes per channel
+ inline uint8_t byte_depth() {return m_bit_depth / 8;}
+
+ /// @brief True if the current pixel format requires a call to the `convert` method
+ /// Used in the usb_cam library logic to determine if a plain `memcopy` call can be
+ /// used instead of a call to the `convert` method of this class.
+ /// @return
+ inline bool requires_conversion() {return m_requires_conversion;}
+
+ /// @brief Conversion method. Meant to be overridden if pixel format requires it.
+ virtual void convert(const char * & src, char * & dest, const int & bytes_used)
+ {
+ // provide default implementation so derived classes do not have to implement
+ // this method if not required
+ (void)src;
+ (void)dest;
+ (void)bytes_used;
+ }
+
+ /// @brief Returns if the final output format is color
+ /// Copied from:
+ /// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+ /// @return
+ inline bool is_color()
+ {
+ return
+ m_ros == usb_cam::constants::RGB8 ||
+ m_ros == usb_cam::constants::BGR8 ||
+ m_ros == usb_cam::constants::RGBA8 ||
+ m_ros == usb_cam::constants::BGRA8 ||
+ m_ros == usb_cam::constants::RGB16 ||
+ m_ros == usb_cam::constants::BGR16 ||
+ m_ros == usb_cam::constants::RGBA16 ||
+ m_ros == usb_cam::constants::BGRA16 ||
+ m_ros == usb_cam::constants::NV21 ||
+ m_ros == usb_cam::constants::NV24;
+ }
+
+ /// @brief Returns if the final output format is monocolor (gray)
+ /// Copied from:
+ /// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+ /// @return
+ inline bool is_mono()
+ {
+ return
+ m_ros == usb_cam::constants::MONO8 ||
+ m_ros == usb_cam::constants::MONO16;
+ }
+
+ /// @brief Returns if the final output format is bayer
+ /// Copied from:
+ /// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+ /// @return
+ inline bool is_bayer()
+ {
+ return
+ m_ros == usb_cam::constants::BAYER_RGGB8 ||
+ m_ros == usb_cam::constants::BAYER_BGGR8 ||
+ m_ros == usb_cam::constants::BAYER_GBRG8 ||
+ m_ros == usb_cam::constants::BAYER_GRBG8 ||
+ m_ros == usb_cam::constants::BAYER_RGGB16 ||
+ m_ros == usb_cam::constants::BAYER_BGGR16 ||
+ m_ros == usb_cam::constants::BAYER_GBRG16 ||
+ m_ros == usb_cam::constants::BAYER_GRBG16;
+ }
+
+ /// @brief Returns if the final output format has an alpha value
+ /// Copied from:
+ /// https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+ /// @return
+ inline bool has_alpha()
+ {
+ return
+ m_ros == usb_cam::constants::RGBA8 ||
+ m_ros == usb_cam::constants::BGRA8 ||
+ m_ros == usb_cam::constants::RGBA16 ||
+ m_ros == usb_cam::constants::BGRA16;
+ }
+
+protected:
+ /// @brief Unique name for this pixel format
+ std::string m_name;
+ /// @brief Integer correspoding to a specific V4L2_PIX_FMT_* constant
+ /// See `linux/videodev2.h` for a list of all possible values for here
+ uint32_t m_v4l2;
+ // TODO(flynneva): make this a vector of supported conversions for the specified V4L2 format
+ /// @brief This should match ROS encoding string
+ /// See `sensor_msgs/image_encodings.hpp` for corresponding possible values. Copy of
+ /// those values are stored in `usb_cam/constants.hpp`
+ std::string m_ros;
+ /// @brief Number of channels (aka bytes per pixel) of output (ROS format above)
+ uint8_t m_channels;
+ /// @brief Bitdepth of output (ROS format above)
+ uint8_t m_bit_depth;
+ /// @brief boolean whether or not the current format requires a call to `convert`.
+ /// Setting this to true requires that the virtual `convert` method is implemented.
+ bool m_requires_conversion;
+};
+
+
+class default_pixel_format : public pixel_format_base
+{
+public:
+ default_pixel_format()
+ : pixel_format_base(
+ "yuyv",
+ V4L2_PIX_FMT_YUYV,
+ usb_cam::constants::YUV422_YUY2,
+ 2,
+ 8,
+ false)
+ {}
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__PIXEL_FORMAT_BASE_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/rgb.hpp b/src/usb_cam-ros2/include/usb_cam/formats/rgb.hpp
new file mode 100644
index 0000000..d2c1349
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/rgb.hpp
@@ -0,0 +1,63 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__RGB_HPP_
+#define USB_CAM__FORMATS__RGB_HPP_
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class RGB8 : public pixel_format_base
+{
+public:
+ explicit RGB8(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "rgb8",
+ V4L2_PIX_FMT_RGB332,
+ usb_cam::constants::RGB8,
+ 3,
+ 8,
+ false)
+ {
+ (void)args;
+ }
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__RGB_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/utils.hpp b/src/usb_cam-ros2/include/usb_cam/formats/utils.hpp
new file mode 100644
index 0000000..12c12c9
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/utils.hpp
@@ -0,0 +1,108 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__UTILS_HPP_
+#define USB_CAM__FORMATS__UTILS_HPP_
+
+#include "usb_cam/utils.hpp"
+
+namespace usb_cam
+{
+namespace formats
+{
+
+
+/** Clip a value to the range 0<val<255. For speed this is done using an
+ * array, so can only cope with numbers in the range -128<=val<=383.
+ */
+inline unsigned char CLIPVALUE(const int & val)
+{
+ // Old method (if)
+ /* val = val < 0 ? 0 : val; */
+ /* return val > 255 ? 255 : val; */
+
+ try {
+ // New method array
+ return usb_cam::constants::uchar_clipping_table.at(
+ val + usb_cam::constants::clipping_table_offset);
+ } catch (std::out_of_range const &) {
+ // fall back to old method
+ unsigned char clipped_val = val < 0 ? 0 : static_cast<unsigned char>(val);
+ return val > 255 ? 255 : clipped_val;
+ }
+}
+
+/// @brief Conversion from YUV to RGB.
+///
+/// The normal conversion matrix is due to Julien (surname unknown):
+///
+/// [ R ] [ 1.0 0.0 1.403 ] [ Y ]
+/// [ G ] = [ 1.0 -0.344 -0.714 ] [ U ]
+/// [ B ] [ 1.0 1.770 0.0 ] [ V ]
+///
+/// and the firewire one is similar:
+///
+/// [ R ] [ 1.0 0.0 0.700 ] [ Y ]
+/// [ G ] = [ 1.0 -0.198 -0.291 ] [ U ]
+/// [ B ] [ 1.0 1.015 0.0 ] [ V ]
+///
+/// Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB
+/// do not get you back to the same RGB!)
+/// [ R ] [ 1.0 0.0 1.136 ] [ Y ]
+/// [ G ] = [ 1.0 -0.396 -0.578 ] [ U ]
+/// [ B ] [ 1.0 2.041 0.002 ] [ V ]
+///
+inline void YUV2RGB(
+ const unsigned char & y, const unsigned char & u, const unsigned char & v,
+ unsigned char * r, unsigned char * g, unsigned char * b)
+{
+ const int y2 = static_cast<int>(y);
+ const int u2 = static_cast<int>(u - 128);
+ const int v2 = static_cast<int>(v - 128);
+
+ // This is the normal YUV conversion, but
+ // appears to be incorrect for the firewire cameras
+ // int r2 = y2 + ( (v2*91947) >> 16);
+ // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
+ // int b2 = y2 + ( (u2*115999) >> 16);
+ // This is an adjusted version (UV spread out a bit)
+ int r2 = y2 + ((v2 * 37221) >> 15);
+ int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15);
+ int b2 = y2 + ((u2 * 66883) >> 15);
+
+ // Cap the values.
+ *r = CLIPVALUE(r2);
+ *g = CLIPVALUE(g2);
+ *b = CLIPVALUE(b2);
+}
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__UTILS_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/uyvy.hpp b/src/usb_cam-ros2/include/usb_cam/formats/uyvy.hpp
new file mode 100644
index 0000000..61fcf23
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/uyvy.hpp
@@ -0,0 +1,113 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__UYVY_HPP_
+#define USB_CAM__FORMATS__UYVY_HPP_
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class UYVY : public pixel_format_base
+{
+public:
+ explicit UYVY(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "uyvy",
+ V4L2_PIX_FMT_UYVY,
+ usb_cam::constants::YUV422,
+ 2,
+ 8,
+ false)
+ {
+ (void)args;
+ }
+};
+
+
+class UYVY2RGB : public pixel_format_base
+{
+public:
+ explicit UYVY2RGB(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "uyvy2rgb",
+ V4L2_PIX_FMT_UYVY,
+ usb_cam::constants::RGB8,
+ 3,
+ 8,
+ true),
+ m_number_of_pixels(args.pixels)
+ {}
+
+ /// @brief In this format each four bytes is two pixels.
+ /// Each four bytes is two Y's, a Cb and a Cr.
+ /// Each Y goes to one of the pixels, and the Cb and Cr belong to both pixels.
+ /// As you can see, the Cr and Cb components have half the horizontal resolution
+ /// of the Y component. V4L2_PIX_FMT_UYVY is known in the Windows environment as YUY2.
+ ///
+ /// Source: https://www.linuxtv.org/downloads/v4l-dvb-apis-old/V4L2-PIX-FMT-YUYV.html
+ ///
+ void convert(const char * & src, char * & dest, const int & bytes_used) override
+ {
+ (void)bytes_used; // not used by this conversion method
+ int i, j;
+ unsigned char y0, y1, u, v;
+ unsigned char r, g, b;
+
+ for (i = 0, j = 0; i < (static_cast<int>(m_number_of_pixels) << 1); i += 4, j += 6) {
+ u = (unsigned char)src[i + 0];
+ y0 = (unsigned char)src[i + 1];
+ v = (unsigned char)src[i + 2];
+ y1 = (unsigned char)src[i + 3];
+ YUV2RGB(y0, u, v, &r, &g, &b);
+ dest[j + 0] = r;
+ dest[j + 1] = g;
+ dest[j + 2] = b;
+ YUV2RGB(y1, u, v, &r, &g, &b);
+ dest[j + 3] = r;
+ dest[j + 4] = g;
+ dest[j + 5] = b;
+ }
+ }
+
+private:
+ size_t m_number_of_pixels;
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__UYVY_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/formats/yuyv.hpp b/src/usb_cam-ros2/include/usb_cam/formats/yuyv.hpp
new file mode 100644
index 0000000..bb2505a
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/formats/yuyv.hpp
@@ -0,0 +1,115 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__FORMATS__YUYV_HPP_
+#define USB_CAM__FORMATS__YUYV_HPP_
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+namespace usb_cam
+{
+namespace formats
+{
+
+class YUYV : public pixel_format_base
+{
+public:
+ explicit YUYV(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "yuyv",
+ V4L2_PIX_FMT_YUYV,
+ usb_cam::constants::YUV422_YUY2,
+ 2,
+ 8,
+ false)
+ {
+ (void)args;
+ }
+};
+
+
+class YUYV2RGB : public pixel_format_base
+{
+public:
+ explicit YUYV2RGB(const format_arguments_t & args = format_arguments_t())
+ : pixel_format_base(
+ "yuyv2rgb",
+ V4L2_PIX_FMT_YUYV,
+ usb_cam::constants::RGB8,
+ 3,
+ 8,
+ true),
+ m_number_of_pixels(args.pixels)
+ {}
+
+ /// @brief In this format each four bytes is two pixels.
+ /// Each four bytes is two Y's, a Cb and a Cr.
+ /// Each Y goes to one of the pixels, and the Cb and Cr belong to both pixels.
+ /// As you can see, the Cr and Cb components have half the horizontal resolution
+ /// of the Y component. V4L2_PIX_FMT_YUYV is known in the Windows environment as YUY2.
+ ///
+ /// Source: https://www.linuxtv.org/downloads/v4l-dvb-apis-old/V4L2-PIX-FMT-YUYV.html
+ ///
+ void convert(const char * & src, char * & dest, const int & bytes_used) override
+ {
+ (void)bytes_used; // not used by this conversion method
+ int i, j;
+ unsigned char y0, y1, u, v;
+ unsigned char r, g, b;
+
+ /// Total number of bytes should be 2 * number of pixels. Achieve this by bit-shifting
+ /// (NumPixels << 1). See format description above.
+ for (i = 0, j = 0; i < (m_number_of_pixels << 1); i += 4, j += 6) {
+ y0 = (unsigned char)src[i + 0];
+ u = (unsigned char)src[i + 1];
+ y1 = (unsigned char)src[i + 2];
+ v = (unsigned char)src[i + 3];
+ YUV2RGB(y0, u, v, &r, &g, &b);
+ dest[j + 0] = r;
+ dest[j + 1] = g;
+ dest[j + 2] = b;
+ YUV2RGB(y1, u, v, &r, &g, &b);
+ dest[j + 3] = r;
+ dest[j + 4] = g;
+ dest[j + 5] = b;
+ }
+ }
+
+private:
+ int m_number_of_pixels;
+};
+
+} // namespace formats
+} // namespace usb_cam
+
+#endif // USB_CAM__FORMATS__YUYV_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/usb_cam.hpp b/src/usb_cam-ros2/include/usb_cam/usb_cam.hpp
new file mode 100644
index 0000000..1663e0f
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/usb_cam.hpp
@@ -0,0 +1,419 @@
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Robert Bosch, LLC nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__USB_CAM_HPP_
+#define USB_CAM__USB_CAM_HPP_
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <linux/videodev2.h>
+}
+
+#include <chrono>
+#include <memory>
+#include <algorithm>
+#include <sstream>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "usb_cam/utils.hpp"
+#include "usb_cam/formats/pixel_format_base.hpp"
+#include "usb_cam/formats/av_pixel_format_helper.hpp"
+
+#include "usb_cam/formats/mjpeg.hpp"
+#include "usb_cam/formats/mono.hpp"
+#include "usb_cam/formats/rgb.hpp"
+#include "usb_cam/formats/uyvy.hpp"
+#include "usb_cam/formats/yuyv.hpp"
+#include "usb_cam/formats/m420.hpp"
+
+
+namespace usb_cam
+{
+
+using usb_cam::utils::io_method_t;
+using usb_cam::formats::pixel_format_base;
+
+/// @brief Add more formats here and to driver_supported_formats below as
+/// they are added to this library
+using usb_cam::formats::RGB8;
+using usb_cam::formats::YUYV;
+using usb_cam::formats::YUYV2RGB;
+using usb_cam::formats::UYVY;
+using usb_cam::formats::UYVY2RGB;
+using usb_cam::formats::MONO8;
+using usb_cam::formats::MONO16;
+using usb_cam::formats::Y102MONO8;
+using usb_cam::formats::RAW_MJPEG;
+using usb_cam::formats::MJPEG2RGB;
+using usb_cam::formats::M4202RGB;
+
+
+/// @brief list all supported formats that this driver supports
+std::vector<std::shared_ptr<pixel_format_base>> driver_supported_formats(
+ const formats::format_arguments_t & args = formats::format_arguments_t())
+{
+ std::vector<std::shared_ptr<pixel_format_base>> fmts = {
+ std::make_shared<RGB8>(args),
+ std::make_shared<YUYV>(args),
+ std::make_shared<YUYV2RGB>(args),
+ std::make_shared<UYVY>(args),
+ std::make_shared<UYVY2RGB>(args),
+ std::make_shared<MONO8>(args),
+ std::make_shared<MONO16>(args),
+ std::make_shared<Y102MONO8>(args),
+ std::make_shared<RAW_MJPEG>(args),
+ std::make_shared<MJPEG2RGB>(args),
+ std::make_shared<M4202RGB>(args),
+ };
+ return fmts;
+}
+
+typedef struct
+{
+ struct v4l2_fmtdesc format;
+ struct v4l2_frmivalenum v4l2_fmt;
+} capture_format_t;
+
+typedef struct
+{
+ std::string camera_name; // can be anything
+ std::string device_name; // usually /dev/video0 or something similiar
+ std::string frame_id;
+ std::string io_method_name;
+ std::string camera_info_url;
+ // these parameters all have to be a combination supported by the device
+ // Use
+ // v4l2-ctl --device=0 --list-formats-ext
+ // to discover them,
+ // or guvcview
+ std::string pixel_format_name;
+ std::string av_device_format;
+ int image_width;
+ int image_height;
+ int framerate;
+ int brightness;
+ int contrast;
+ int saturation;
+ int sharpness;
+ int gain;
+ int white_balance;
+ int exposure;
+ int focus;
+ bool auto_white_balance;
+ bool autoexposure;
+ bool autofocus;
+} parameters_t;
+
+typedef struct
+{
+ char * data;
+ size_t width;
+ size_t height;
+ std::shared_ptr<pixel_format_base> pixel_format;
+ size_t number_of_pixels;
+ size_t bytes_per_line;
+ size_t size_in_bytes;
+ v4l2_format v4l2_fmt;
+ struct timespec stamp;
+
+ size_t set_number_of_pixels()
+ {
+ number_of_pixels = width * height;
+ return number_of_pixels;
+ }
+ size_t set_bytes_per_line()
+ {
+ bytes_per_line = width * pixel_format->byte_depth() * pixel_format->channels();
+ return bytes_per_line;
+ }
+ size_t set_size_in_bytes()
+ {
+ size_in_bytes = height * bytes_per_line;
+ return size_in_bytes;
+ }
+
+ /// @brief make it a shorter API call to get the pixel format
+ unsigned int get_format_fourcc()
+ {
+ return pixel_format->v4l2();
+ }
+} image_t;
+
+class UsbCam
+{
+public:
+ UsbCam();
+ ~UsbCam();
+
+ /// @brief Configure device, should be called before start
+ void configure(parameters_t & parameters, const io_method_t & io_method);
+
+ /// @brief Start the configured device
+ void start();
+
+ /// @brief shutdown camera
+ void shutdown(void);
+
+ /// @brief Take a new image with device and return it
+ /// To copy the returned image to another format:
+ /// sensor_msgs::msg::Image image_msg;
+ /// auto new_image = get_image();
+ /// image_msg.data.resize(step * height);
+ /// memcpy(&image_msg.data[0], new_image->frame.base, image_msg.data.size());
+ char * get_image();
+
+ /// @brief Overload of get_image to allow users to pass
+ /// in an image pointer to fill in
+ void get_image(char * destination);
+
+ std::vector<capture_format_t> get_supported_formats();
+
+ // enables/disable auto focus
+ bool set_auto_focus(int value);
+
+ // Set video device parameters
+ bool set_v4l_parameter(const std::string & param, int value);
+ bool set_v4l_parameter(const std::string & param, const std::string & value);
+
+ void stop_capturing();
+ void start_capturing();
+
+ inline size_t get_image_width()
+ {
+ return m_image.width;
+ }
+
+ inline size_t get_image_height()
+ {
+ return m_image.height;
+ }
+
+ inline size_t get_image_size_in_bytes()
+ {
+ return m_image.size_in_bytes;
+ }
+
+ inline size_t get_image_size_in_pixels()
+ {
+ return m_image.number_of_pixels;
+ }
+
+ inline timespec get_image_timestamp()
+ {
+ return m_image.stamp;
+ }
+
+ /// @brief Get number of bytes per line in image
+ /// @return number of bytes per line in image
+ inline unsigned int get_image_step()
+ {
+ return m_image.bytes_per_line;
+ }
+
+ inline std::string get_device_name()
+ {
+ return m_device_name;
+ }
+
+ inline std::shared_ptr<pixel_format_base> get_pixel_format()
+ {
+ return m_image.pixel_format;
+ }
+
+ inline usb_cam::utils::io_method_t get_io_method()
+ {
+ return m_io;
+ }
+
+ inline int get_fd()
+ {
+ return m_fd;
+ }
+
+ inline std::shared_ptr<usb_cam::utils::buffer[]> get_buffers()
+ {
+ return m_buffers;
+ }
+
+ inline unsigned int number_of_buffers()
+ {
+ return m_number_of_buffers;
+ }
+
+ inline AVCodec * get_avcodec()
+ {
+ return m_avcodec;
+ }
+
+ inline AVDictionary * get_avoptions()
+ {
+ return m_avoptions;
+ }
+
+ inline AVCodecContext * get_avcodec_context()
+ {
+ return m_avcodec_context;
+ }
+
+ inline AVFrame * get_avframe()
+ {
+ return m_avframe;
+ }
+
+ inline bool is_capturing()
+ {
+ return m_is_capturing;
+ }
+
+ inline time_t get_epoch_time_shift_us()
+ {
+ return m_epoch_time_shift_us;
+ }
+
+ inline std::vector<capture_format_t> supported_formats()
+ {
+ if (m_supported_formats.size() == 0) {
+ this->get_supported_formats();
+ }
+
+ return m_supported_formats;
+ }
+
+ /// @brief Check if the given format is supported by this device
+ /// If it is supported, set the m_pixel_format variable to it.
+ /// @param format the format to check if it is supported
+ /// @return bool true if the given format is supported, false otherwise
+ inline bool set_pixel_format(const formats::format_arguments_t & args)
+ {
+ bool result = false;
+
+ std::shared_ptr<pixel_format_base> found_driver_format = nullptr;
+
+ // First check if given format is supported by this driver
+ for (auto driver_fmt : driver_supported_formats(args)) {
+ if (driver_fmt->name() == args.name) {
+ found_driver_format = driver_fmt;
+ }
+ }
+
+ if (found_driver_format == nullptr) {
+ // List the supported formats of this driver for the user before throwing
+ std::cerr << "This driver supports the following formats:" << std::endl;
+ for (auto driver_fmt : driver_supported_formats(args)) {
+ std::cerr << "\t" << driver_fmt->name() << std::endl;
+ }
+ throw std::invalid_argument(
+ "Specified format `" + args.name + "` is unsupported by this ROS driver"
+ );
+ }
+
+ std::cout << "This device supports the following formats:" << std::endl;
+ for (auto fmt : this->supported_formats()) {
+ // Always list the devices supported formats for the user
+ std::cout << "\t" << fmt.format.description << " ";
+ std::cout << fmt.v4l2_fmt.width << " x " << fmt.v4l2_fmt.height << " (";
+ std::cout << fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator << " Hz)";
+ std::cout << std::endl;
+
+ if (fmt.v4l2_fmt.pixel_format == found_driver_format->v4l2()) {
+ result = true;
+ m_image.pixel_format = found_driver_format;
+ }
+ }
+
+ return result;
+ }
+
+ /// @brief Set pixel format from parameter list. Required to have logic within UsbCam object
+ /// in case pixel format class requires additional information for conversion function
+ /// (e.g. number of pixels, width, height, etc.)
+ /// @param parameters list of parameters from which the pixel format is to be set
+ /// @return pixel format structure corresponding to a given name
+ inline std::shared_ptr<pixel_format_base> set_pixel_format(const parameters_t & parameters)
+ {
+ // create format arguments structure
+ formats::format_arguments_t format_args({
+ parameters.pixel_format_name,
+ parameters.image_width,
+ parameters.image_height,
+ m_image.number_of_pixels,
+ parameters.av_device_format,
+ });
+
+ // Look for specified pixel format
+ if (!this->set_pixel_format(format_args)) {
+ throw std::invalid_argument(
+ "Specified format `" + parameters.pixel_format_name + "` is unsupported by the " +
+ "selected device `" + parameters.device_name + "`"
+ );
+ }
+
+ return m_image.pixel_format;
+ }
+
+private:
+ void init_read();
+ void init_mmap();
+ void init_userp();
+ void init_device();
+
+ void open_device();
+ void grab_image();
+ void read_frame();
+ void process_image(const char * src, char * & dest, const int & bytes_used);
+
+ void uninit_device();
+ void close_device();
+
+ std::string m_device_name;
+ usb_cam::utils::io_method_t m_io;
+ int m_fd;
+ unsigned int m_number_of_buffers;
+ std::shared_ptr<usb_cam::utils::buffer[]> m_buffers;
+ image_t m_image;
+
+ AVFrame * m_avframe;
+ int m_avframe_size;
+ AVCodec * m_avcodec;
+ AVCodecID m_codec_id;
+ AVDictionary * m_avoptions;
+ AVCodecContext * m_avcodec_context;
+
+ int64_t m_buffer_time_us;
+ bool m_is_capturing;
+ int m_framerate;
+ const time_t m_epoch_time_shift_us;
+ std::vector<capture_format_t> m_supported_formats;
+};
+
+} // namespace usb_cam
+
+#endif // USB_CAM__USB_CAM_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/usb_cam_node.hpp b/src/usb_cam-ros2/include/usb_cam/usb_cam_node.hpp
new file mode 100644
index 0000000..5de9579
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/usb_cam_node.hpp
@@ -0,0 +1,99 @@
+// Copyright 2021 Evan Flynn
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#ifndef USB_CAM__USB_CAM_NODE_HPP_
+#define USB_CAM__USB_CAM_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "camera_info_manager/camera_info_manager.hpp"
+#include "image_transport/image_transport.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+#include "usb_cam/usb_cam.hpp"
+
+
+std::ostream & operator<<(std::ostream & ostr, const rclcpp::Time & tm)
+{
+ ostr << tm.nanoseconds();
+ return ostr;
+}
+
+
+namespace usb_cam
+{
+
+class UsbCamNode : public rclcpp::Node
+{
+public:
+ explicit UsbCamNode(const rclcpp::NodeOptions & node_options);
+ ~UsbCamNode();
+
+ void init();
+ void get_params();
+ void assign_params(const std::vector<rclcpp::Parameter> & parameters);
+ void set_v4l2_params();
+ void update();
+ bool take_and_send_image();
+ bool take_and_send_image_mjpeg();
+
+ rcl_interfaces::msg::SetParametersResult parameters_callback(
+ const std::vector<rclcpp::Parameter> & parameters);
+
+ void service_capture(
+ const std::shared_ptr<rmw_request_id_t> request_header,
+ const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+ std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+
+ UsbCam * m_camera;
+
+ sensor_msgs::msg::Image::UniquePtr m_image_msg;
+ sensor_msgs::msg::CompressedImage::UniquePtr m_compressed_img_msg;
+ std::shared_ptr<image_transport::CameraPublisher> m_image_publisher;
+ rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr m_compressed_image_publisher;
+ rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr m_compressed_cam_info_publisher;
+
+ parameters_t m_parameters;
+
+ sensor_msgs::msg::CameraInfo::SharedPtr m_camera_info_msg;
+ std::shared_ptr<camera_info_manager::CameraInfoManager> m_camera_info;
+
+ rclcpp::TimerBase::SharedPtr m_timer;
+
+ rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr m_service_capture;
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr m_parameters_callback_handle;
+};
+} // namespace usb_cam
+#endif // USB_CAM__USB_CAM_NODE_HPP_
diff --git a/src/usb_cam-ros2/include/usb_cam/utils.hpp b/src/usb_cam-ros2/include/usb_cam/utils.hpp
new file mode 100644
index 0000000..add1ff9
--- /dev/null
+++ b/src/usb_cam-ros2/include/usb_cam/utils.hpp
@@ -0,0 +1,201 @@
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Robert Bosch, LLC nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#ifndef USB_CAM__UTILS_HPP_
+#define USB_CAM__UTILS_HPP_
+
+extern "C" {
+#include <fcntl.h> // for open()
+}
+
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <unistd.h> // for access
+
+#include <cmath>
+#include <ctime>
+#include <cstring>
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+#include <iostream>
+#include <string>
+#include <map>
+
+#include "linux/videodev2.h"
+
+#include "usb_cam/constants.hpp"
+
+namespace fs = std::filesystem;
+
+
+namespace usb_cam
+{
+namespace utils
+{
+
+
+/// @brief Read more on IO methods here: https://lwn.net/Articles/240667/
+typedef enum
+{
+ /// @brief read method copies the video frame between user and kernal space
+ IO_METHOD_READ,
+ /// @brief memory mapped buffers allocated in kernel space
+ IO_METHOD_MMAP,
+ /// @brief memory buffers allocated in the user space
+ IO_METHOD_USERPTR,
+ IO_METHOD_UNKNOWN,
+} io_method_t;
+
+struct buffer
+{
+ char * start;
+ size_t length;
+};
+
+
+/// @brief Get epoch time shift in microseconds
+/// @details Run this at start of process to calculate epoch time shift
+/// @ref https://stackoverflow.com/questions/10266451/where-does-v4l2-buffer-timestamp-value-starts-counting
+inline time_t get_epoch_time_shift_us()
+{
+ struct timeval epoch_time;
+ struct timespec monotonic_time;
+
+ gettimeofday(&epoch_time, NULL);
+ clock_gettime(CLOCK_MONOTONIC, &monotonic_time);
+
+ const int64_t uptime_us =
+ monotonic_time.tv_sec * 1000000 + static_cast<int64_t>(
+ std::round(monotonic_time.tv_nsec / 1000.0));
+ const int64_t epoch_us =
+ epoch_time.tv_sec * 1000000 + epoch_time.tv_usec / 1000.0;
+
+ return static_cast<time_t>(epoch_us - uptime_us);
+}
+
+/// @brief Calculate image timestamp from buffer time and epoch time shift.
+/// In this, the buffer time is first converted into microseconds before the epoch time shift,
+/// which is to be given in microseconds is added to it. Afterwards it is split into seconds
+/// and nanoseconds for the image timestamp.
+inline timespec calc_img_timestamp(const timeval & buffer_time, const time_t & epoch_time_shift_us)
+{
+ timespec img_timestamp;
+
+ int64_t buffer_time_us = (buffer_time.tv_sec * 1000000) + buffer_time.tv_usec;
+ buffer_time_us += epoch_time_shift_us;
+
+ img_timestamp.tv_sec = (buffer_time_us / 1000000);
+ img_timestamp.tv_nsec = (buffer_time_us % 1000000) * 1000;
+
+ return img_timestamp;
+}
+
+inline int xioctl(int fd, uint64_t request, void * arg)
+{
+ int r = 0;
+
+ do {
+ r = ioctl(fd, request, arg);
+ continue;
+ } while (-1 == r && EINTR == errno);
+
+ return r;
+}
+
+
+inline io_method_t io_method_from_string(const std::string & str)
+{
+ if (str == "mmap") {
+ return io_method_t::IO_METHOD_MMAP;
+ } else if (str == "read") {
+ return io_method_t::IO_METHOD_READ;
+ } else if (str == "userptr") {
+ return io_method_t::IO_METHOD_USERPTR;
+ } else {
+ return io_method_t::IO_METHOD_UNKNOWN;
+ }
+}
+
+/// \brief List currently available valid V4L2 devices
+/// Can be used to check if a device string is valid before
+/// starting up.
+///
+/// Inspired by: http://stackoverflow.com/questions/4290834/how-to-get-a-list-of-video-capture-devices-web-cameras-on-linux-ubuntu-c
+inline std::map<std::string, v4l2_capability> available_devices()
+{
+ // Initialize vector of device strings to fill in
+ std::map<std::string, v4l2_capability> v4l2_devices;
+
+ // Get a list of all v4l2 devices from /sys/class/video4linux
+ const std::string v4l2_symlinks_dir = "/sys/class/video4linux/";
+ for (const auto & device_symlink : fs::directory_iterator(v4l2_symlinks_dir)) {
+ if (fs::is_symlink(device_symlink)) {
+ // device_str is the full path to the device in /sys/devices/*
+ std::string device_str = fs::canonical(
+ v4l2_symlinks_dir + fs::read_symlink(
+ device_symlink).generic_string());
+ // get the proper device name (e.g. /dev/*)
+ std::ifstream uevent_file(device_str + "/uevent");
+ std::string line;
+ std::string device_name;
+ while (std::getline(uevent_file, line)) {
+ auto dev_name_index = line.find("DEVNAME=");
+ if (dev_name_index != std::string::npos) {
+ device_name = "/dev/" + line.substr(dev_name_index + 8, line.size());
+ break;
+ }
+ }
+
+ int fd;
+ // Try and open device to test access
+ if ((fd = open(device_name.c_str(), O_RDONLY)) == -1) {
+ std::cerr << "Cannot open device: `" << device_name << "`, ";
+ std::cerr << "double-check read / write permissions for device" << std::endl;
+ } else {
+ struct v4l2_capability device_capabilities = {};
+ if (xioctl(fd, VIDIOC_QUERYCAP, &device_capabilities) == -1) {
+ std::cerr << "Could not retrieve device capabilities: `" << device_name;
+ std::cerr << "`" << std::endl;
+ } else {
+ v4l2_devices[device_name] = device_capabilities;
+ }
+ }
+ } else {
+ // device doesn't exist, continue
+ continue;
+ }
+ }
+
+ return v4l2_devices;
+}
+
+} // namespace utils
+} // namespace usb_cam
+
+#endif // USB_CAM__UTILS_HPP_
diff --git a/src/usb_cam-ros2/launch/__init__.py b/src/usb_cam-ros2/launch/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/usb_cam-ros2/launch/camera.launch.py b/src/usb_cam-ros2/launch/camera.launch.py
new file mode 100644
index 0000000..40f94b8
--- /dev/null
+++ b/src/usb_cam-ros2/launch/camera.launch.py
@@ -0,0 +1,80 @@
+# Copyright 2018 Lucas Walter
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# 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 Lucas Walter nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import argparse
+import os
+from pathlib import Path # noqa: E402
+import sys
+
+# Hack to get relative import of .camera_config file working
+dir_path = os.path.dirname(os.path.realpath(__file__))
+sys.path.append(dir_path)
+
+from camera_config import CameraConfig, USB_CAM_DIR # noqa: E402
+
+from launch import LaunchDescription # noqa: E402
+from launch.actions import GroupAction # noqa: E402
+from launch_ros.actions import Node # noqa: E402
+
+
+CAMERAS = []
+CAMERAS.append(
+ CameraConfig(
+ name='camera1',
+ param_path=Path(USB_CAM_DIR, 'config', 'params_1.yaml')
+ )
+ # Add more Camera's here and they will automatically be launched below
+)
+
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ parser = argparse.ArgumentParser(description='usb_cam demo')
+ parser.add_argument('-n', '--node-name', dest='node_name', type=str,
+ help='name for device', default='usb_cam')
+
+ camera_nodes = [
+ Node(
+ package='usb_cam', executable='usb_cam_node_exe', output='screen',
+ name=camera.name,
+ namespace=camera.namespace,
+ parameters=[camera.param_path],
+ remappings=camera.remappings
+ )
+ for camera in CAMERAS
+ ]
+
+ camera_group = GroupAction(camera_nodes)
+
+ ld.add_action(camera_group)
+ return ld
diff --git a/src/usb_cam-ros2/launch/camera_config.py b/src/usb_cam-ros2/launch/camera_config.py
new file mode 100644
index 0000000..c7dd756
--- /dev/null
+++ b/src/usb_cam-ros2/launch/camera_config.py
@@ -0,0 +1,65 @@
+# Copyright 2023 usb_cam Authors
+#
+# 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 usb_cam Authors nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+from pathlib import Path
+from typing import List, Optional
+
+from ament_index_python.packages import get_package_share_directory
+from pydantic import BaseModel, root_validator, validator
+
+USB_CAM_DIR = get_package_share_directory('usb_cam')
+
+
+class CameraConfig(BaseModel):
+ name: str = 'camera1'
+ param_path: Path = Path(USB_CAM_DIR, 'config', 'params_1.yaml')
+ remappings: Optional[List]
+ namespace: Optional[str]
+
+ @validator('param_path')
+ def validate_param_path(cls, value):
+ if value and not value.exists():
+ raise FileNotFoundError(f'Could not find parameter file: {value}')
+ return value
+
+ @root_validator
+ def validate_root(cls, values):
+ name = values.get('name')
+ remappings = values.get('remappings')
+ if name and not remappings:
+ # Automatically set remappings if name is set
+ remappings = [
+ ('image_raw', f'{name}/image_raw'),
+ ('image_raw/compressed', f'{name}/image_compressed'),
+ ('image_raw/compressedDepth', f'{name}/compressedDepth'),
+ ('image_raw/theora', f'{name}/image_raw/theora'),
+ ('camera_info', f'{name}/camera_info'),
+ ]
+ values['remappings'] = remappings
+ return values
diff --git a/src/usb_cam-ros2/mainpage.dox b/src/usb_cam-ros2/mainpage.dox
new file mode 100644
index 0000000..19670e5
--- /dev/null
+++ b/src/usb_cam-ros2/mainpage.dox
@@ -0,0 +1,5 @@
+/**
+\mainpage
+
+\b usb_cam is a ROS driver for V4L USB cameras.
+*/
diff --git a/src/usb_cam-ros2/mkdocs.yml b/src/usb_cam-ros2/mkdocs.yml
new file mode 100644
index 0000000..f9b22ef
--- /dev/null
+++ b/src/usb_cam-ros2/mkdocs.yml
@@ -0,0 +1,15 @@
+site_name: usb_cam Documentation
+site_url: https://ros-drivers.github.io/usb_cam/
+site_author: Evan Flynn
+repo_url: https://github.com/ros-drivers/usb_cam.git
+theme:
+ name: 'material'
+ features:
+ - navigation.indexes
+ - navigation.tracking
+plugins:
+ - search
+ - awesome-pages
+extra:
+ version:
+ provider: mike
diff --git a/src/usb_cam-ros2/msg/Format.msg b/src/usb_cam-ros2/msg/Format.msg
new file mode 100644
index 0000000..03058a4
--- /dev/null
+++ b/src/usb_cam-ros2/msg/Format.msg
@@ -0,0 +1,5 @@
+# uint32 type
+string pixel_format
+uint32 width
+uint32 height
+float32 fps
diff --git a/src/usb_cam-ros2/package.xml b/src/usb_cam-ros2/package.xml
new file mode 100644
index 0000000..0735127
--- /dev/null
+++ b/src/usb_cam-ros2/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>usb_cam_ros2</name>
+ <version>0.8.0</version>
+ <description>A ROS Driver for V4L USB Cameras</description>
+
+ <maintainer email="evanflynn.msu@gmail.com">Evan Flynn</maintainer>
+
+ <license>BSD</license>
+
+ <url type="website">http://wiki.ros.org/usb_cam</url>
+ <url type="bugtracker">https://github.com/ros-drivers/usb_cam/issues</url>
+ <url type="repository">https://github.com/ros-drivers/usb_cam</url>
+
+ <!-- Required so ROS build farms properly set ROS_VERSION -->
+ <build_depend>ros_environment</build_depend>
+
+ <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+
+ <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_auto</buildtool_depend>
+ <buildtool_depend condition="$ROS_VERSION == 2">rosidl_default_generators</buildtool_depend>
+
+ <build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
+
+ <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+ <depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
+
+ <depend>cv_bridge</depend>
+ <depend>std_msgs</depend>
+ <depend>std_srvs</depend>
+ <depend>sensor_msgs</depend>
+ <depend>camera_info_manager</depend>
+ <depend>builtin_interfaces</depend>
+ <depend>image_transport</depend>
+ <depend>image_transport_plugins</depend>
+ <depend>v4l-utils</depend>
+
+ <!-- Only required for MJPEG to RGB converison -->
+ <depend>ffmpeg</depend>
+
+ <test_depend condition="$ROS_VERSION == 2">ament_cmake_gtest</test_depend>
+ <test_depend condition="$ROS_VERSION == 2">ament_lint_auto</test_depend>
+ <test_depend condition="$ROS_VERSION == 2">ament_lint_common</test_depend>
+
+ <exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
+
+ <member_of_group condition="$ROS_VERSION == 2">rosidl_interface_packages</member_of_group>
+
+ <export>
+ <build_type condition="$ROS_VERSION == 1">ament_cmake</build_type>
+ <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+ </export>
+</package>
diff --git a/src/usb_cam-ros2/scripts/show_image.py b/src/usb_cam-ros2/scripts/show_image.py
new file mode 100644
index 0000000..8e29a7a
--- /dev/null
+++ b/src/usb_cam-ros2/scripts/show_image.py
@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+
+# Copyright 2021 Evan Flynn, Lucas Walter
+#
+# 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 Evan Flynn nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+import cv2
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+
+
+class ExamineImage(Node):
+
+ def __init__(self):
+ super().__init__('examine_image')
+
+ self.mat = None
+ self.sub = self.create_subscription(
+ Image,
+ 'image_raw',
+ self.image_callback,
+ 100)
+
+ def image_callback(self, msg):
+ sz = (msg.height, msg.width)
+ # print(msg.header.stamp)
+ if False:
+ print('{encoding} {width} {height} {step} {data_size}'.format(
+ encoding=msg.encoding, width=msg.width, height=msg.height,
+ step=msg.step, data_size=len(msg.data)))
+ if msg.step * msg.height != len(msg.data):
+ print('bad step/height/data size')
+ return
+
+ if msg.encoding == 'rgb8':
+ dirty = (self.mat is None or msg.width != self.mat.shape[1] or
+ msg.height != self.mat.shape[0] or len(self.mat.shape) < 2 or
+ self.mat.shape[2] != 3)
+ if dirty:
+ self.mat = np.zeros([msg.height, msg.width, 3], dtype=np.uint8)
+ self.mat[:, :, 2] = np.array(msg.data[0::3]).reshape(sz)
+ self.mat[:, :, 1] = np.array(msg.data[1::3]).reshape(sz)
+ self.mat[:, :, 0] = np.array(msg.data[2::3]).reshape(sz)
+ elif msg.encoding == 'mono8':
+ self.mat = np.array(msg.data).reshape(sz)
+ else:
+ print('unsupported encoding {}'.format(msg.encoding))
+ return
+ if self.mat is not None:
+ cv2.imshow('image', self.mat)
+ cv2.waitKey(5)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ examine_image = ExamineImage()
+
+ try:
+ rclpy.spin(examine_image)
+ except KeyboardInterrupt:
+ pass
+
+ examine_image.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/usb_cam-ros2/src/LICENSE b/src/usb_cam-ros2/src/LICENSE
new file mode 100644
index 0000000..3fee832
--- /dev/null
+++ b/src/usb_cam-ros2/src/LICENSE
@@ -0,0 +1,14 @@
+Video for Linux Two API Specification
+Revision 0.24
+Michael H Schimek <mschimek@gmx.at>
+Bill Dirks
+Hans Verkuil
+Martin Rubli
+
+Copyright © 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008 Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin Rubli
+
+This document is copyrighted © 1999-2008 by Bill Dirks, Michael H. Schimek, Hans Verkuil and Martin Rubli.
+
+Permission is granted to copy, distribute and/or modify this document under the terms of the GNU Free Documentation License, Version 1.1 or any later version published by the Free Software Foundation; with no Invariant Sections, with no Front-Cover Texts, and with no Back-Cover Texts. A copy of the license is included in the appendix entitled "GNU Free Documentation License".
+
+Programming examples can be used and distributed without restrictions.
diff --git a/src/usb_cam-ros2/src/ros1/usb_cam_node.cpp b/src/usb_cam-ros2/src/ros1/usb_cam_node.cpp
new file mode 100644
index 0000000..8f9757d
--- /dev/null
+++ b/src/usb_cam-ros2/src/ros1/usb_cam_node.cpp
@@ -0,0 +1,271 @@
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Robert Bosch, LLC nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+#include <sstream>
+#include <string>
+
+#include "ros/ros.h"
+#include "image_transport/image_transport.h"
+#include "camera_info_manager/camera_info_manager.h"
+#include "std_srvs/Empty.h"
+
+#include "usb_cam/usb_cam.hpp"
+#include "usb_cam/utils.hpp"
+
+
+namespace usb_cam
+{
+
+class UsbCamNode
+{
+public:
+ // private ROS node handle
+ ros::NodeHandle m_node;
+
+ // shared image message
+ sensor_msgs::Image m_image;
+ image_transport::CameraPublisher m_image_pub;
+
+ // parameters
+ std::string m_video_device_name, m_io_method_str, m_pixel_format_str, m_camera_name,
+ m_camera_info_url;
+ int m_image_width, m_image_height, m_framerate, m_exposure, m_brightness, m_contrast,
+ m_saturation, m_sharpness, m_focus, m_white_balance, m_gain;
+ bool m_auto_focus, m_auto_exposure, m_auto_white_balance;
+ boost::shared_ptr<camera_info_manager::CameraInfoManager> m_camera_info;
+
+ UsbCam m_camera;
+
+ ros::ServiceServer m_service_start, m_service_stop;
+
+
+ bool m_service_startcap(std_srvs::Empty::Request & req, std_srvs::Empty::Response & res)
+ {
+ (void)req;
+ (void)res;
+ m_camera.start_capturing();
+ return true;
+ }
+
+
+ bool m_service_stopcap(std_srvs::Empty::Request & req, std_srvs::Empty::Response & res)
+ {
+ (void)req;
+ (void)res;
+ m_camera.stop_capturing();
+ return true;
+ }
+
+ UsbCamNode()
+ : m_node("~")
+ {
+ // advertise the main image topic
+ image_transport::ImageTransport it(m_node);
+ m_image_pub = it.advertiseCamera("image_raw", 1);
+
+ // grab the parameters
+ m_node.param("video_device", m_video_device_name, std::string("/dev/video0"));
+ m_node.param("brightness", m_brightness, -1); // 0-255, -1 "leave alone"
+ m_node.param("contrast", m_contrast, -1); // 0-255, -1 "leave alone"
+ m_node.param("saturation", m_saturation, -1); // 0-255, -1 "leave alone"
+ m_node.param("sharpness", m_sharpness, -1); // 0-255, -1 "leave alone"
+ // possible values: mmap, read, userptr
+ m_node.param("io_method", m_io_method_str, std::string("mmap"));
+ m_node.param("image_width", m_image_width, 640);
+ m_node.param("image_height", m_image_height, 480);
+ m_node.param("framerate", m_framerate, 30);
+ // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
+ m_node.param("pixel_format", m_pixel_format_str, std::string("mjpeg"));
+ // enable/disable autofocus
+ m_node.param("autofocus", m_auto_focus, false);
+ m_node.param("focus", m_focus, -1); // 0-255, -1 "leave alone"
+ // enable/disable autoexposure
+ m_node.param("autoexposure", m_auto_exposure, true);
+ m_node.param("exposure", m_exposure, 100);
+ m_node.param("gain", m_gain, -1); // 0-100?, -1 "leave alone"
+ // enable/disable auto white balance temperature
+ m_node.param("auto_white_balance", m_auto_white_balance, true);
+ m_node.param("white_balance", m_white_balance, 4000);
+
+ // load the camera info
+ m_node.param("camera_frame_id", m_image.header.frame_id, std::string("head_camera"));
+ m_node.param("camera_name", m_camera_name, std::string("head_camera"));
+ m_node.param("camera_info_url", m_camera_info_url, std::string(""));
+ m_camera_info.reset(
+ new camera_info_manager::CameraInfoManager(
+ m_node, m_camera_name, m_camera_info_url));
+
+ // create Services
+ m_service_start = \
+ m_node.advertiseService("start_capture", &UsbCamNode::m_service_startcap, this);
+ m_service_stop = \
+ m_node.advertiseService("stop_capture", &UsbCamNode::m_service_stopcap, this);
+
+ // check for default camera info
+ if (!m_camera_info->isCalibrated()) {
+ m_camera_info->setCameraName(m_video_device_name);
+ sensor_msgs::CameraInfo camera_info;
+ camera_info.header.frame_id = m_image.header.frame_id;
+ camera_info.width = m_image_width;
+ camera_info.height = m_image_height;
+ m_camera_info->setCameraInfo(camera_info);
+ }
+
+
+ ROS_INFO(
+ "Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS",
+ m_camera_name.c_str(), m_video_device_name.c_str(),
+ m_image_width, m_image_height, m_io_method_str.c_str(),
+ m_pixel_format_str.c_str(), m_framerate);
+
+ // set the IO method
+ io_method_t io_method = usb_cam::utils::io_method_from_string(m_io_method_str);
+ if (io_method == io_method_t::IO_METHOD_UNKNOWN) {
+ ROS_FATAL("Unknown IO method '%s'", m_io_method_str.c_str());
+ m_node.shutdown();
+ return;
+ }
+
+ // start the camera
+ m_camera.start(
+ m_video_device_name.c_str(), io_method, m_pixel_format_str, m_image_width,
+ m_image_height, m_framerate);
+
+ set_v4l2_params();
+ }
+
+ virtual ~UsbCamNode()
+ {
+ m_camera.shutdown();
+ }
+
+ bool take_and_send_image()
+ {
+ // grab the new image
+ auto new_image = m_camera.get_image();
+
+ // fill in the image message
+ m_image.header.stamp.sec = new_image->stamp.tv_sec;
+ m_image.header.stamp.nsec = new_image->stamp.tv_nsec;
+
+ // Only resize if required
+ if (m_image.data.size() != static_cast<size_t>(new_image->step * new_image->height)) {
+ m_image.width = new_image->width;
+ m_image.height = new_image->height;
+ m_image.encoding = new_image->encoding;
+ m_image.step = new_image->step;
+ m_image.data.resize(new_image->step * new_image->height);
+ }
+
+ // Fill in image data
+ memcpy(&m_image.data[0], new_image->image, m_image.data.size());
+
+ // grab the camera info
+ sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(m_camera_info->getCameraInfo()));
+ ci->header.frame_id = m_image.header.frame_id;
+ ci->header.stamp = m_image.header.stamp;
+
+ // publish the image
+ m_image_pub.publish(m_image, *ci);
+
+ return true;
+ }
+
+ bool spin()
+ {
+ ros::Rate loop_rate(this->m_framerate);
+ while (m_node.ok()) {
+ if (m_camera.is_capturing()) {
+ if (!take_and_send_image()) {ROS_WARN("USB camera did not respond in time.");}
+ }
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+ return true;
+ }
+
+ void set_v4l2_params()
+ {
+ // set camera parameters
+ if (m_brightness >= 0) {
+ m_camera.set_v4l_parameter("brightness", m_brightness);
+ }
+
+ if (m_contrast >= 0) {
+ m_camera.set_v4l_parameter("contrast", m_contrast);
+ }
+
+ if (m_saturation >= 0) {
+ m_camera.set_v4l_parameter("saturation", m_saturation);
+ }
+
+ if (m_sharpness >= 0) {
+ m_camera.set_v4l_parameter("sharpness", m_sharpness);
+ }
+
+ if (m_gain >= 0) {
+ m_camera.set_v4l_parameter("gain", m_gain);
+ }
+
+ // check auto white balance
+ if (m_auto_white_balance) {
+ m_camera.set_v4l_parameter("white_balance_temperature_auto", 1);
+ } else {
+ m_camera.set_v4l_parameter("white_balance_temperature_auto", 0);
+ m_camera.set_v4l_parameter("white_balance_temperature", m_white_balance);
+ }
+
+ // check auto exposure
+ if (!m_auto_exposure) {
+ // turn down exposure control (from max of 3)
+ m_camera.set_v4l_parameter("m_exposureauto", 1);
+ // change the exposure level
+ m_camera.set_v4l_parameter("m_exposureabsolute", m_exposure);
+ }
+
+ // check auto focus
+ if (m_auto_focus) {
+ m_camera.set_auto_focus(1);
+ m_camera.set_v4l_parameter("m_focusauto", 1);
+ } else {
+ m_camera.set_v4l_parameter("m_focusauto", 0);
+ if (m_focus >= 0) {
+ m_camera.set_v4l_parameter("m_focusabsolute", m_focus);
+ }
+ }
+ }
+};
+
+} // namespace usb_cam
+
+int main(int argc, char ** argv)
+{
+ ros::init(argc, argv, "usb_cam");
+ usb_cam::UsbCamNode a;
+ a.spin();
+ return EXIT_SUCCESS;
+}
diff --git a/src/usb_cam-ros2/src/ros2/usb_cam_node.cpp b/src/usb_cam-ros2/src/ros2/usb_cam_node.cpp
new file mode 100644
index 0000000..f8e8ded
--- /dev/null
+++ b/src/usb_cam-ros2/src/ros2/usb_cam_node.cpp
@@ -0,0 +1,438 @@
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Robert Bosch, LLC nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <filesystem>
+#include "usb_cam/usb_cam_node.hpp"
+#include "usb_cam/utils.hpp"
+
+const char BASE_TOPIC_NAME[] = "image_raw";
+
+namespace usb_cam
+{
+
+UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options)
+: Node("usb_cam", node_options),
+ m_camera(new usb_cam::UsbCam()),
+ m_image_msg(new sensor_msgs::msg::Image()),
+ m_compressed_img_msg(nullptr),
+ m_image_publisher(std::make_shared<image_transport::CameraPublisher>(
+ image_transport::create_camera_publisher(this, BASE_TOPIC_NAME,
+ rclcpp::QoS {100}.get_rmw_qos_profile()))),
+ m_compressed_image_publisher(nullptr),
+ m_compressed_cam_info_publisher(nullptr),
+ m_parameters(),
+ m_camera_info_msg(new sensor_msgs::msg::CameraInfo()),
+ m_service_capture(
+ this->create_service<std_srvs::srv::SetBool>(
+ "set_capture",
+ std::bind(
+ &UsbCamNode::service_capture,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2,
+ std::placeholders::_3)))
+{
+ // declare params
+ this->declare_parameter("camera_name", "default_cam");
+ this->declare_parameter("camera_info_url", "");
+ this->declare_parameter("framerate", 30.0);
+ this->declare_parameter("frame_id", "default_cam");
+ this->declare_parameter("image_height", 480);
+ this->declare_parameter("image_width", 640);
+ this->declare_parameter("io_method", "mmap");
+ this->declare_parameter("pixel_format", "yuyv");
+ this->declare_parameter("av_device_format", "YUV422P");
+ this->declare_parameter("video_device", "/dev/video0");
+ this->declare_parameter("brightness", 50); // 0-255, -1 "leave alone"
+ this->declare_parameter("contrast", -1); // 0-255, -1 "leave alone"
+ this->declare_parameter("saturation", -1); // 0-255, -1 "leave alone"
+ this->declare_parameter("sharpness", -1); // 0-255, -1 "leave alone"
+ this->declare_parameter("gain", -1); // 0-100?, -1 "leave alone"
+ this->declare_parameter("auto_white_balance", true);
+ this->declare_parameter("white_balance", 4000);
+ this->declare_parameter("autoexposure", true);
+ this->declare_parameter("exposure", 100);
+ this->declare_parameter("autofocus", false);
+ this->declare_parameter("focus", -1); // 0-255, -1 "leave alone"
+
+ get_params();
+ init();
+ m_parameters_callback_handle = add_on_set_parameters_callback(
+ std::bind(
+ &UsbCamNode::parameters_callback, this,
+ std::placeholders::_1));
+}
+
+UsbCamNode::~UsbCamNode()
+{
+ RCLCPP_WARN(this->get_logger(), "Shutting down");
+ m_image_msg.reset();
+ m_compressed_img_msg.reset();
+ m_camera_info_msg.reset();
+ m_camera_info.reset();
+ m_timer.reset();
+ m_service_capture.reset();
+ m_parameters_callback_handle.reset();
+
+ delete (m_camera);
+}
+
+void UsbCamNode::service_capture(
+ const std::shared_ptr<rmw_request_id_t> request_header,
+ const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+ std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+{
+ (void) request_header;
+ if (request->data) {
+ m_camera->start_capturing();
+ response->message = "Start Capturing";
+ } else {
+ m_camera->stop_capturing();
+ response->message = "Stop Capturing";
+ }
+}
+
+std::string resolve_device_path(const std::string & path)
+{
+ if (std::filesystem::is_symlink(path)) {
+ // For some reason read_symlink only returns videox
+ return "/dev/" + std::string(std::filesystem::read_symlink(path));
+ }
+ return path;
+}
+
+void UsbCamNode::init()
+{
+ while (m_parameters.frame_id == "") {
+ RCLCPP_WARN_ONCE(
+ this->get_logger(), "Required Parameters not set...waiting until they are set");
+ get_params();
+ std::this_thread::sleep_for(std::chrono::milliseconds(500));
+ }
+
+ // load the camera info
+ m_camera_info.reset(
+ new camera_info_manager::CameraInfoManager(
+ this, m_parameters.camera_name, m_parameters.camera_info_url));
+ // check for default camera info
+ if (!m_camera_info->isCalibrated()) {
+ m_camera_info->setCameraName(m_parameters.device_name);
+ m_camera_info_msg->header.frame_id = m_parameters.frame_id;
+ m_camera_info_msg->width = m_parameters.image_width;
+ m_camera_info_msg->height = m_parameters.image_height;
+ m_camera_info->setCameraInfo(*m_camera_info_msg);
+ }
+
+ // Check if given device name is an available v4l2 device
+ auto available_devices = usb_cam::utils::available_devices();
+ if (available_devices.find(m_parameters.device_name) == available_devices.end()) {
+ RCLCPP_ERROR_STREAM(
+ this->get_logger(),
+ "Device specified is not available or is not a vaild V4L2 device: `" <<
+ m_parameters.device_name << "`"
+ );
+ RCLCPP_INFO(this->get_logger(), "Available V4L2 devices are:");
+ for (const auto & device : available_devices) {
+ RCLCPP_INFO_STREAM(this->get_logger(), " " << device.first);
+ RCLCPP_INFO_STREAM(this->get_logger(), " " << device.second.card);
+ }
+ rclcpp::shutdown();
+ return;
+ }
+
+ // if pixel format is equal to 'mjpeg', i.e. raw mjpeg stream, initialize compressed image message
+ // and publisher
+ if (m_parameters.pixel_format_name == "mjpeg") {
+ m_compressed_img_msg.reset(new sensor_msgs::msg::CompressedImage());
+ m_compressed_img_msg->header.frame_id = m_parameters.frame_id;
+ m_compressed_image_publisher =
+ this->create_publisher<sensor_msgs::msg::CompressedImage>(
+ std::string(BASE_TOPIC_NAME) + "/compressed", rclcpp::QoS(100));
+ m_compressed_cam_info_publisher =
+ this->create_publisher<sensor_msgs::msg::CameraInfo>(
+ "camera_info", rclcpp::QoS(100));
+ }
+
+ m_image_msg->header.frame_id = m_parameters.frame_id;
+ RCLCPP_INFO(
+ this->get_logger(), "Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS",
+ m_parameters.camera_name.c_str(), m_parameters.device_name.c_str(),
+ m_parameters.image_width, m_parameters.image_height, m_parameters.io_method_name.c_str(),
+ m_parameters.pixel_format_name.c_str(), m_parameters.framerate);
+ // set the IO method
+ io_method_t io_method =
+ usb_cam::utils::io_method_from_string(m_parameters.io_method_name);
+ if (io_method == usb_cam::utils::IO_METHOD_UNKNOWN) {
+ RCLCPP_ERROR_ONCE(
+ this->get_logger(),
+ "Unknown IO method '%s'", m_parameters.io_method_name.c_str());
+ rclcpp::shutdown();
+ return;
+ }
+
+ // configure the camera
+ m_camera->configure(m_parameters, io_method);
+
+ set_v4l2_params();
+
+ // start the camera
+ m_camera->start();
+
+ // TODO(lucasw) should this check a little faster than expected frame rate?
+ // TODO(lucasw) how to do small than ms, or fractional ms- std::chrono::nanoseconds?
+ const int period_ms = 1000.0 / m_parameters.framerate;
+ m_timer = this->create_wall_timer(
+ std::chrono::milliseconds(static_cast<int64_t>(period_ms)),
+ std::bind(&UsbCamNode::update, this));
+ RCLCPP_INFO_STREAM(this->get_logger(), "Timer triggering every " << period_ms << " ms");
+}
+
+void UsbCamNode::get_params()
+{
+ auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(this);
+ auto parameters = parameters_client->get_parameters(
+ {
+ "camera_name", "camera_info_url", "frame_id", "framerate", "image_height", "image_width",
+ "io_method", "pixel_format", "av_device_format", "video_device", "brightness", "contrast",
+ "saturation", "sharpness", "gain", "auto_white_balance", "white_balance", "autoexposure",
+ "exposure", "autofocus", "focus"
+ }
+ );
+
+ assign_params(parameters);
+}
+
+void UsbCamNode::assign_params(const std::vector<rclcpp::Parameter> & parameters)
+{
+ for (auto & parameter : parameters) {
+ if (parameter.get_name() == "camera_name") {
+ RCLCPP_INFO(this->get_logger(), "camera_name value: %s", parameter.value_to_string().c_str());
+ m_parameters.camera_name = parameter.value_to_string();
+ } else if (parameter.get_name() == "camera_info_url") {
+ m_parameters.camera_info_url = parameter.value_to_string();
+ } else if (parameter.get_name() == "frame_id") {
+ m_parameters.frame_id = parameter.value_to_string();
+ } else if (parameter.get_name() == "framerate") {
+ RCLCPP_WARN(this->get_logger(), "framerate: %f", parameter.as_double());
+ m_parameters.framerate = parameter.as_double();
+ } else if (parameter.get_name() == "image_height") {
+ m_parameters.image_height = parameter.as_int();
+ } else if (parameter.get_name() == "image_width") {
+ m_parameters.image_width = parameter.as_int();
+ } else if (parameter.get_name() == "io_method") {
+ m_parameters.io_method_name = parameter.value_to_string();
+ } else if (parameter.get_name() == "pixel_format") {
+ m_parameters.pixel_format_name = parameter.value_to_string();
+ } else if (parameter.get_name() == "av_device_format") {
+ m_parameters.av_device_format = parameter.value_to_string();
+ } else if (parameter.get_name() == "video_device") {
+ m_parameters.device_name = resolve_device_path(parameter.value_to_string());
+ } else if (parameter.get_name() == "brightness") {
+ m_parameters.brightness = parameter.as_int();
+ } else if (parameter.get_name() == "contrast") {
+ m_parameters.contrast = parameter.as_int();
+ } else if (parameter.get_name() == "saturation") {
+ m_parameters.saturation = parameter.as_int();
+ } else if (parameter.get_name() == "sharpness") {
+ m_parameters.sharpness = parameter.as_int();
+ } else if (parameter.get_name() == "gain") {
+ m_parameters.gain = parameter.as_int();
+ } else if (parameter.get_name() == "auto_white_balance") {
+ m_parameters.auto_white_balance = parameter.as_bool();
+ } else if (parameter.get_name() == "white_balance") {
+ m_parameters.white_balance = parameter.as_int();
+ } else if (parameter.get_name() == "autoexposure") {
+ m_parameters.autoexposure = parameter.as_bool();
+ } else if (parameter.get_name() == "exposure") {
+ m_parameters.exposure = parameter.as_int();
+ } else if (parameter.get_name() == "autofocus") {
+ m_parameters.autofocus = parameter.as_bool();
+ } else if (parameter.get_name() == "focus") {
+ m_parameters.focus = parameter.as_int();
+ } else {
+ RCLCPP_WARN(this->get_logger(), "Invalid parameter name: %s", parameter.get_name().c_str());
+ }
+ }
+}
+
+/// @brief Send current parameters to V4L2 device
+/// TODO(flynneva): should this actuaully be part of UsbCam class?
+void UsbCamNode::set_v4l2_params()
+{
+ // set camera parameters
+ if (m_parameters.brightness >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'brightness' to %d", m_parameters.brightness);
+ m_camera->set_v4l_parameter("brightness", m_parameters.brightness);
+ }
+
+ if (m_parameters.contrast >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'contrast' to %d", m_parameters.contrast);
+ m_camera->set_v4l_parameter("contrast", m_parameters.contrast);
+ }
+
+ if (m_parameters.saturation >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'saturation' to %d", m_parameters.saturation);
+ m_camera->set_v4l_parameter("saturation", m_parameters.saturation);
+ }
+
+ if (m_parameters.sharpness >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'sharpness' to %d", m_parameters.sharpness);
+ m_camera->set_v4l_parameter("sharpness", m_parameters.sharpness);
+ }
+
+ if (m_parameters.gain >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'gain' to %d", m_parameters.gain);
+ m_camera->set_v4l_parameter("gain", m_parameters.gain);
+ }
+
+ // check auto white balance
+ if (m_parameters.auto_white_balance) {
+ m_camera->set_v4l_parameter("white_balance_temperature_auto", 1);
+ RCLCPP_INFO(this->get_logger(), "Setting 'white_balance_temperature_auto' to %d", 1);
+ } else {
+ RCLCPP_INFO(this->get_logger(), "Setting 'white_balance' to %d", m_parameters.white_balance);
+ m_camera->set_v4l_parameter("white_balance_temperature_auto", 0);
+ m_camera->set_v4l_parameter("white_balance_temperature", m_parameters.white_balance);
+ }
+
+ // check auto exposure
+ if (!m_parameters.autoexposure) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'exposure_auto' to %d", 1);
+ RCLCPP_INFO(this->get_logger(), "Setting 'exposure' to %d", m_parameters.exposure);
+ // turn down exposure control (from max of 3)
+ m_camera->set_v4l_parameter("exposure_auto", 1);
+ // change the exposure level
+ m_camera->set_v4l_parameter("exposure_absolute", m_parameters.exposure);
+ } else {
+ RCLCPP_INFO(this->get_logger(), "Setting 'exposure_auto' to %d", 3);
+ m_camera->set_v4l_parameter("exposure_auto", 3);
+ }
+
+ // check auto focus
+ if (m_parameters.autofocus) {
+ m_camera->set_auto_focus(1);
+ RCLCPP_INFO(this->get_logger(), "Setting 'focus_auto' to %d", 1);
+ m_camera->set_v4l_parameter("focus_auto", 1);
+ } else {
+ RCLCPP_INFO(this->get_logger(), "Setting 'focus_auto' to %d", 0);
+ m_camera->set_v4l_parameter("focus_auto", 0);
+ if (m_parameters.focus >= 0) {
+ RCLCPP_INFO(this->get_logger(), "Setting 'focus_absolute' to %d", m_parameters.focus);
+ m_camera->set_v4l_parameter("focus_absolute", m_parameters.focus);
+ }
+ }
+}
+
+bool UsbCamNode::take_and_send_image()
+{
+ // Only resize if required
+ if (sizeof(m_image_msg->data) != m_camera->get_image_size_in_bytes()) {
+ m_image_msg->width = m_camera->get_image_width();
+ m_image_msg->height = m_camera->get_image_height();
+ m_image_msg->encoding = m_camera->get_pixel_format()->ros();
+ m_image_msg->step = m_camera->get_image_step();
+ if (m_image_msg->step == 0) {
+ // Some formats don't have a linesize specified by v4l2
+ // Fall back to manually calculating it step = size / height
+ m_image_msg->step = m_camera->get_image_size_in_bytes() / m_image_msg->height;
+ }
+ m_image_msg->data.resize(m_camera->get_image_size_in_bytes());
+ }
+
+ // grab the image, pass image msg buffer to fill
+ m_camera->get_image(reinterpret_cast<char *>(&m_image_msg->data[0]));
+
+ auto stamp = m_camera->get_image_timestamp();
+ m_image_msg->header.stamp.sec = stamp.tv_sec;
+ m_image_msg->header.stamp.nanosec = stamp.tv_nsec;
+
+ *m_camera_info_msg = m_camera_info->getCameraInfo();
+ m_camera_info_msg->header = m_image_msg->header;
+ m_image_publisher->publish(*m_image_msg, *m_camera_info_msg);
+ return true;
+}
+
+bool UsbCamNode::take_and_send_image_mjpeg()
+{
+ // Only resize if required
+ if (sizeof(m_compressed_img_msg->data) != m_camera->get_image_size_in_bytes()) {
+ m_compressed_img_msg->format = "jpeg";
+ m_compressed_img_msg->data.resize(m_camera->get_image_size_in_bytes());
+ }
+
+ // grab the image, pass image msg buffer to fill
+ m_camera->get_image(reinterpret_cast<char *>(&m_compressed_img_msg->data[0]));
+
+ auto stamp = m_camera->get_image_timestamp();
+ m_compressed_img_msg->header.stamp.sec = stamp.tv_sec;
+ m_compressed_img_msg->header.stamp.nanosec = stamp.tv_nsec;
+
+ *m_camera_info_msg = m_camera_info->getCameraInfo();
+ m_camera_info_msg->header = m_compressed_img_msg->header;
+
+ m_compressed_image_publisher->publish(*m_compressed_img_msg);
+ m_compressed_cam_info_publisher->publish(*m_camera_info_msg);
+ return true;
+}
+
+rcl_interfaces::msg::SetParametersResult UsbCamNode::parameters_callback(
+ const std::vector<rclcpp::Parameter> & parameters)
+{
+ RCLCPP_DEBUG(this->get_logger(), "Setting parameters for %s", m_parameters.camera_name.c_str());
+ m_timer->reset();
+ assign_params(parameters);
+ set_v4l2_params();
+ rcl_interfaces::msg::SetParametersResult result;
+ result.successful = true;
+ result.reason = "success";
+ return result;
+}
+
+void UsbCamNode::update()
+{
+ if (m_camera->is_capturing()) {
+ // If the camera exposure longer higher than the framerate period
+ // then that caps the framerate.
+ // auto t0 = now();
+ bool isSuccessful = (m_parameters.pixel_format_name == "mjpeg") ?
+ take_and_send_image_mjpeg() :
+ take_and_send_image();
+ if (!isSuccessful) {
+ RCLCPP_WARN_ONCE(this->get_logger(), "USB camera did not respond in time.");
+ }
+ }
+}
+} // namespace usb_cam
+
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(usb_cam::UsbCamNode)
diff --git a/src/usb_cam-ros2/src/usb_cam.cpp b/src/usb_cam-ros2/src/usb_cam.cpp
new file mode 100644
index 0000000..b82cdf6
--- /dev/null
+++ b/src/usb_cam-ros2/src/usb_cam.cpp
@@ -0,0 +1,734 @@
+// Copyright 2014 Robert Bosch, LLC
+//
+// 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 Robert Bosch, LLC nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+extern "C" {
+#include <linux/videodev2.h> // Defines V4L2 format constants
+#include <malloc.h> // for memalign
+#include <sys/mman.h> // for mmap
+#include <sys/stat.h> // for stat
+#include <unistd.h> // for getpagesize()
+#include <fcntl.h> // for O_* constants and open()
+}
+
+#include <chrono>
+#include <ctime>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "opencv2/imgproc.hpp"
+
+#include "usb_cam/usb_cam.hpp"
+#include "usb_cam/conversions.hpp"
+#include "usb_cam/utils.hpp"
+
+
+namespace usb_cam
+{
+
+using utils::io_method_t;
+
+
+UsbCam::UsbCam()
+: m_device_name(), m_io(io_method_t::IO_METHOD_MMAP), m_fd(-1),
+ m_number_of_buffers(4), m_buffers(new usb_cam::utils::buffer[m_number_of_buffers]), m_image(),
+ m_avframe(NULL), m_avcodec(NULL), m_avoptions(NULL),
+ m_avcodec_context(NULL), m_is_capturing(false), m_framerate(0),
+ m_epoch_time_shift_us(usb_cam::utils::get_epoch_time_shift_us()), m_supported_formats()
+{}
+
+UsbCam::~UsbCam()
+{
+ shutdown();
+}
+
+
+/// @brief Fill destination image with source image. If required, convert a given
+/// V4L2 Image into another type. Look up possible V4L2 pixe formats in the
+/// `linux/videodev2.h` header file.
+/// @param src a pointer to a V4L2 source image
+/// @param dest a pointer to where the source image should be copied (if required)
+/// @param bytes_used number of bytes used by the src buffer
+void UsbCam::process_image(const char * src, char * & dest, const int & bytes_used)
+{
+ // TODO(flynneva): could we skip the copy here somehow?
+ // If no conversion required, just copy the image from V4L2 buffer
+ if (m_image.pixel_format->requires_conversion() == false) {
+ memcpy(dest, src, m_image.size_in_bytes);
+ } else {
+ m_image.pixel_format->convert(src, dest, bytes_used);
+ }
+}
+
+void UsbCam::read_frame()
+{
+ struct v4l2_buffer buf;
+ unsigned int i;
+ int len;
+
+ switch (m_io) {
+ case io_method_t::IO_METHOD_READ:
+ len = read(m_fd, m_buffers[0].start, m_buffers[0].length);
+ if (len == -1) {
+ switch (errno) {
+ case EAGAIN:
+ return;
+ default:
+ throw std::runtime_error("Unable to read frame");
+ }
+ }
+ return process_image(m_buffers[0].start, m_image.data, len);
+ case io_method_t::IO_METHOD_MMAP:
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ m_image.v4l2_fmt.type = buf.type;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ // Get current v4l2 pixel format
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_G_FMT), &m_image.v4l2_fmt)) {
+ switch (errno) {
+ case EAGAIN:
+ return;
+ default:
+ throw std::runtime_error("Invalid v4l2 format");
+ }
+ }
+ /// Dequeue buffer with the new image
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_DQBUF), &buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return;
+ default:
+ throw std::runtime_error("Unable to retrieve frame with mmap");
+ }
+ }
+
+ // Get timestamp from V4L2 image buffer
+ m_image.stamp = usb_cam::utils::calc_img_timestamp(buf.timestamp, m_epoch_time_shift_us);
+
+ assert(buf.index < m_number_of_buffers);
+ process_image(m_buffers[buf.index].start, m_image.data, buf.bytesused);
+
+ /// Requeue buffer so it can be reused
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
+ throw std::runtime_error("Unable to exchange buffer with the driver");
+ }
+ return;
+ case io_method_t::IO_METHOD_USERPTR:
+ CLEAR(buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_USERPTR;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_DQBUF), &buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return;
+ default:
+ throw std::runtime_error("Unable to exchange buffer with driver");
+ }
+ }
+
+ // Get timestamp from V4L2 image buffer
+ m_image.stamp = usb_cam::utils::calc_img_timestamp(buf.timestamp, m_epoch_time_shift_us);
+
+ for (i = 0; i < m_number_of_buffers; ++i) {
+ if (buf.m.userptr == reinterpret_cast<uint64_t>(m_buffers[i].start) && \
+ buf.length == m_buffers[i].length)
+ {
+ return;
+ }
+ }
+
+ assert(i < m_number_of_buffers);
+ process_image(reinterpret_cast<const char *>(buf.m.userptr), m_image.data, buf.bytesused);
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
+ throw std::runtime_error("Unable to exchange buffer with driver");
+ }
+ return;
+ case io_method_t::IO_METHOD_UNKNOWN:
+ throw std::invalid_argument("IO method unknown");
+ }
+}
+
+void UsbCam::stop_capturing()
+{
+ if (!m_is_capturing) {return;}
+
+ m_is_capturing = false;
+ enum v4l2_buf_type type;
+
+ switch (m_io) {
+ case io_method_t::IO_METHOD_READ:
+ /* Nothing to do. */
+ return;
+ case io_method_t::IO_METHOD_MMAP:
+ case io_method_t::IO_METHOD_USERPTR:
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_STREAMOFF, &type)) {
+ // Set capturing variable to true again, since stream was not stopped successfully
+ m_is_capturing = true;
+ throw std::runtime_error("Unable to stop capturing stream");
+ }
+ return;
+ case io_method_t::IO_METHOD_UNKNOWN:
+ throw std::invalid_argument("IO method unknown");
+ }
+}
+
+void UsbCam::start_capturing()
+{
+ if (m_is_capturing) {return;}
+
+ unsigned int i;
+ enum v4l2_buf_type type;
+
+ switch (m_io) {
+ case io_method_t::IO_METHOD_READ:
+ /* Nothing to do. */
+ break;
+ case io_method_t::IO_METHOD_MMAP:
+ // Queue the buffers
+ for (i = 0; i < m_number_of_buffers; ++i) {
+ struct v4l2_buffer buf;
+ CLEAR(buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
+ throw std::runtime_error("Unable to queue image buffer");
+ }
+ }
+
+ // Start the stream
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_STREAMON, &type)) {
+ throw std::runtime_error("Unable to start stream");
+ }
+ break;
+ case io_method_t::IO_METHOD_USERPTR:
+ for (i = 0; i < m_number_of_buffers; ++i) {
+ struct v4l2_buffer buf;
+
+ CLEAR(buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_USERPTR;
+ buf.index = i;
+ buf.m.userptr = reinterpret_cast<uint64_t>(m_buffers[i].start);
+ buf.length = m_buffers[i].length;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QBUF), &buf)) {
+ throw std::runtime_error("Unable to configure stream");
+ }
+ }
+
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_STREAMON, &type)) {
+ throw std::runtime_error("Unable to start stream");
+ }
+ break;
+ case io_method_t::IO_METHOD_UNKNOWN:
+ throw std::invalid_argument("IO method unknown");
+ }
+ m_is_capturing = true;
+}
+
+void UsbCam::uninit_device()
+{
+ m_buffers.reset();
+}
+
+void UsbCam::init_read()
+{
+ if (!m_buffers) {
+ throw std::overflow_error("Out of memory");
+ }
+
+ m_buffers[0].length = m_image.size_in_bytes;
+
+ if (!m_buffers[0].start) {
+ throw std::overflow_error("Out of memory");
+ }
+}
+
+void UsbCam::init_mmap()
+{
+ struct v4l2_requestbuffers req;
+
+ CLEAR(req);
+
+ req.count = m_number_of_buffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_REQBUFS), &req)) {
+ if (EINVAL == errno) {
+ throw std::runtime_error("Device does not support memory mapping");
+ } else {
+ throw std::runtime_error("Unable to initialize memory mapping");
+ }
+ }
+
+ if (req.count < m_number_of_buffers) {
+ throw std::overflow_error("Insufficient buffer memory on device");
+ }
+
+ if (!m_buffers) {
+ throw std::overflow_error("Out of memory");
+ }
+
+ for (uint32_t current_buffer = 0; current_buffer < req.count; ++current_buffer) {
+ struct v4l2_buffer buf;
+
+ CLEAR(buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = current_buffer;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QUERYBUF), &buf)) {
+ throw std::runtime_error("Unable to query status of buffer");
+ }
+
+ m_buffers[current_buffer].length = buf.length;
+ m_buffers[current_buffer].start =
+ reinterpret_cast<char *>(mmap(
+ NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
+ MAP_SHARED /* recommended */, m_fd, buf.m.offset));
+
+ if (MAP_FAILED == m_buffers[current_buffer].start) {
+ throw std::runtime_error("Unable to allocate memory for image buffers");
+ }
+ }
+}
+
+void UsbCam::init_userp()
+{
+ struct v4l2_requestbuffers req;
+ unsigned int page_size;
+
+ page_size = getpagesize();
+ auto buffer_size = (m_image.size_in_bytes + page_size - 1) & ~(page_size - 1);
+
+ CLEAR(req);
+
+ req.count = m_number_of_buffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_USERPTR;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_REQBUFS, &req)) {
+ if (EINVAL == errno) {
+ throw std::invalid_argument("Device does not support user pointer i/o");
+ } else {
+ throw std::invalid_argument("Unable to initialize memory mapping");
+ }
+ }
+
+ if (!m_buffers) {
+ throw std::overflow_error("Out of memory");
+ }
+
+ for (uint32_t current_buffer = 0; current_buffer < req.count; ++current_buffer) {
+ m_buffers[current_buffer].length = buffer_size;
+ m_buffers[current_buffer].start =
+ reinterpret_cast<char *>(memalign(/* boundary */ page_size, buffer_size));
+
+ if (!m_buffers[current_buffer].start) {
+ throw std::overflow_error("Out of memory");
+ }
+ }
+}
+
+void UsbCam::init_device()
+{
+ struct v4l2_capability cap;
+ struct v4l2_cropcap cropcap;
+ struct v4l2_crop crop;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QUERYCAP), &cap)) {
+ if (EINVAL == errno) {
+ throw std::invalid_argument("Device is not a V4L2 device");
+ } else {
+ throw std::invalid_argument("Unable to query device capabilities");
+ }
+ }
+
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ throw std::invalid_argument("Device is not a video capture device");
+ }
+
+ switch (m_io) {
+ case io_method_t::IO_METHOD_READ:
+ if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
+ throw std::invalid_argument("Device does not support read i/o");
+ }
+ break;
+ case io_method_t::IO_METHOD_MMAP:
+ case io_method_t::IO_METHOD_USERPTR:
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ throw std::invalid_argument("Device does not support streaming i/o");
+ }
+ break;
+ case io_method_t::IO_METHOD_UNKNOWN:
+ throw std::invalid_argument("Unsupported IO method specified");
+ }
+
+ /* Select video input, video standard and tune here. */
+
+ CLEAR(cropcap);
+
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (0 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_CROPCAP), &cropcap)) {
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, VIDIOC_S_CROP, &crop)) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ break;
+ }
+ }
+ } else {
+ /* Errors ignored. */
+ }
+
+ m_image.v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ m_image.v4l2_fmt.fmt.pix.width = m_image.width;
+ m_image.v4l2_fmt.fmt.pix.height = m_image.height;
+ m_image.v4l2_fmt.fmt.pix.pixelformat = m_image.pixel_format->v4l2();
+ m_image.v4l2_fmt.fmt.pix.field = V4L2_FIELD_ANY;
+
+ // Set v4l2 capture format
+ // Note VIDIOC_S_FMT may change width and height
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_FMT), &m_image.v4l2_fmt)) {
+ throw strerror(errno);
+ }
+
+ struct v4l2_streamparm stream_params;
+ memset(&stream_params, 0, sizeof(stream_params));
+ stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_G_PARM), &stream_params) < 0) {
+ throw strerror(errno);
+ }
+
+ if (!stream_params.parm.capture.capability && V4L2_CAP_TIMEPERFRAME) {
+ throw "V4L2_CAP_TIMEPERFRAME not supported";
+ }
+
+ // TODO(lucasw) need to get list of valid numerator/denominator pairs
+ // and match closest to what user put in.
+ stream_params.parm.capture.timeperframe.numerator = 1;
+ stream_params.parm.capture.timeperframe.denominator = m_framerate;
+ if (usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_PARM), &stream_params) < 0) {
+ throw std::invalid_argument("Couldn't set camera framerate");
+ }
+
+ switch (m_io) {
+ case io_method_t::IO_METHOD_READ:
+ init_read();
+ break;
+ case io_method_t::IO_METHOD_MMAP:
+ init_mmap();
+ break;
+ case io_method_t::IO_METHOD_USERPTR:
+ init_userp();
+ break;
+ case io_method_t::IO_METHOD_UNKNOWN:
+ // TODO(flynneva): log something
+ break;
+ }
+}
+
+void UsbCam::close_device()
+{
+ // Device is already closed
+ if (m_fd == -1) {return;}
+
+ if (-1 == close(m_fd)) {
+ throw strerror(errno);
+ }
+
+ m_fd = -1;
+}
+
+void UsbCam::open_device()
+{
+ struct stat st;
+
+ if (-1 == stat(m_device_name.c_str(), &st)) {
+ throw strerror(errno);
+ }
+
+ if (!S_ISCHR(st.st_mode)) {
+ throw strerror(errno);
+ }
+
+ m_fd = open(m_device_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+
+ if (-1 == m_fd) {
+ throw strerror(errno);
+ }
+}
+
+void UsbCam::configure(
+ parameters_t & parameters, const io_method_t & io_method)
+{
+ m_device_name = parameters.device_name;
+ m_io = io_method;
+
+ // Open device file descriptor before anything else
+ open_device();
+
+ m_image.width = static_cast<int>(parameters.image_width);
+ m_image.height = static_cast<int>(parameters.image_height);
+ m_image.set_number_of_pixels();
+
+ // Do this before calling set_bytes_per_line and set_size_in_bytes
+ m_image.pixel_format = set_pixel_format(parameters);
+ m_image.set_bytes_per_line();
+ m_image.set_size_in_bytes();
+ m_framerate = parameters.framerate;
+
+ init_device();
+}
+
+void UsbCam::start()
+{
+ start_capturing();
+}
+
+void UsbCam::shutdown()
+{
+ stop_capturing();
+ uninit_device();
+ close_device();
+}
+
+/// @brief Grab new image from V4L2 device, return pointer to image
+/// @return pointer to image data
+char * UsbCam::get_image()
+{
+ if ((m_image.width == 0) || (m_image.height == 0)) {
+ return nullptr;
+ }
+ // grab the image
+ grab_image();
+ return m_image.data;
+}
+
+/// @brief Overload get_image so users can pass in an image pointer to fill
+/// @param destination destination to fill in with image
+void UsbCam::get_image(char * destination)
+{
+ if ((m_image.width == 0) || (m_image.height == 0)) {
+ return;
+ }
+ // Set the destination pointer to be filled
+ m_image.data = destination;
+ // grab the image
+ grab_image();
+}
+
+std::vector<capture_format_t> UsbCam::get_supported_formats()
+{
+ m_supported_formats.clear();
+ struct v4l2_fmtdesc * current_format = new v4l2_fmtdesc();
+ struct v4l2_frmsizeenum * current_size = new v4l2_frmsizeenum();
+ struct v4l2_frmivalenum * current_interval = new v4l2_frmivalenum();
+
+ current_format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ current_format->index = 0;
+ for (current_format->index = 0;
+ usb_cam::utils::xioctl(
+ m_fd, VIDIOC_ENUM_FMT, current_format) == 0;
+ ++current_format->index)
+ {
+ current_size->index = 0;
+ current_size->pixel_format = current_format->pixelformat;
+
+ for (current_size->index = 0;
+ usb_cam::utils::xioctl(
+ m_fd, VIDIOC_ENUM_FRAMESIZES, current_size) == 0;
+ ++current_size->index)
+ {
+ current_interval->index = 0;
+ current_interval->pixel_format = current_size->pixel_format;
+ current_interval->width = current_size->discrete.width;
+ current_interval->height = current_size->discrete.height;
+ for (current_interval->index = 0;
+ usb_cam::utils::xioctl(
+ m_fd, VIDIOC_ENUM_FRAMEINTERVALS, current_interval) == 0;
+ ++current_interval->index)
+ {
+ if (current_interval->type == V4L2_FRMIVAL_TYPE_DISCRETE) {
+ capture_format_t capture_format;
+ capture_format.format = *current_format;
+ capture_format.v4l2_fmt = *current_interval;
+ m_supported_formats.push_back(capture_format);
+ }
+ } // interval loop
+ } // size loop
+ } // fmt loop
+
+ delete (current_format);
+ delete (current_size);
+ delete (current_interval);
+
+ return m_supported_formats;
+}
+
+void UsbCam::grab_image()
+{
+ fd_set fds;
+ struct timeval tv;
+ int r;
+
+ FD_ZERO(&fds);
+ FD_SET(m_fd, &fds);
+
+ /* Timeout. */
+ tv.tv_sec = 5;
+ tv.tv_usec = 0;
+
+ r = select(m_fd + 1, &fds, NULL, NULL, &tv);
+
+ if (-1 == r) {
+ if (EINTR == errno) {
+ // interruped (e.g. maybe Ctrl + c) so don't throw anything
+ return;
+ }
+
+ std::cerr << "Something went wrong, exiting..." << errno << std::endl;
+ throw errno;
+ }
+
+ if (0 == r) {
+ std::cerr << "Select timeout, exiting..." << std::endl;
+ throw "select timeout";
+ }
+
+ read_frame();
+}
+
+// enables/disables auto focus
+bool UsbCam::set_auto_focus(int value)
+{
+ struct v4l2_queryctrl queryctrl;
+ struct v4l2_ext_control control;
+
+ memset(&queryctrl, 0, sizeof(queryctrl));
+ queryctrl.id = V4L2_CID_FOCUS_AUTO;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_QUERYCTRL), &queryctrl)) {
+ if (errno != EINVAL) {
+ std::cerr << "VIDIOC_QUERYCTRL" << std::endl;
+ return false;
+ } else {
+ std::cerr << "V4L2_CID_FOCUS_AUTO is not supported" << std::endl;
+ return false;
+ }
+ } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
+ std::cerr << "V4L2_CID_FOCUS_AUTO is not supported" << std::endl;
+ return false;
+ } else {
+ memset(&control, 0, sizeof(control));
+ control.id = V4L2_CID_FOCUS_AUTO;
+ control.value = value;
+
+ if (-1 == usb_cam::utils::xioctl(m_fd, static_cast<int>(VIDIOC_S_CTRL), &control)) {
+ std::cerr << "VIDIOC_S_CTRL" << std::endl;
+ return false;
+ }
+ }
+ return true;
+}
+
+/**
+* Set video device parameter via call to v4l-utils.
+*
+* @param param The name of the parameter to set
+* @param param The value to assign
+*/
+bool UsbCam::set_v4l_parameter(const std::string & param, int value)
+{
+ char buf[33];
+ snprintf(buf, sizeof(buf), "%i", value);
+ return set_v4l_parameter(param, buf);
+}
+
+/**
+* Set video device parameter via call to v4l-utils.
+*
+* @param param The name of the parameter to set
+* @param param The value to assign
+*/
+bool UsbCam::set_v4l_parameter(const std::string & param, const std::string & value)
+{
+ int retcode = 0;
+ // build the command
+ std::stringstream ss;
+ ss << "v4l2-ctl --device=" << m_device_name << " -c " << param << "=" << value << " 2>&1";
+ std::string cmd = ss.str();
+
+ // capture the output
+ std::string output;
+ const int kBufferSize = 256;
+ char buffer[kBufferSize];
+ FILE * stream = popen(cmd.c_str(), "r");
+ if (stream) {
+ while (!feof(stream)) {
+ if (fgets(buffer, kBufferSize, stream) != NULL) {
+ output.append(buffer);
+ }
+ }
+ pclose(stream);
+ // any output should be an error
+ if (output.length() > 0) {
+ std::cout << output.c_str() << std::endl;
+ retcode = 1;
+ }
+ } else {
+ std::cerr << "usb_cam_node could not run '" << cmd.c_str() << "'" << std::endl;
+ retcode = 1;
+ }
+ return retcode;
+}
+
+} // namespace usb_cam
diff --git a/src/usb_cam-ros2/test/gtest_main.cpp b/src/usb_cam-ros2/test/gtest_main.cpp
new file mode 100644
index 0000000..97c3172
--- /dev/null
+++ b/src/usb_cam-ros2/test/gtest_main.cpp
@@ -0,0 +1,38 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#include "gtest/gtest.h"
+#include <rclcpp/rclcpp.hpp>
+
+
+int main(int argc, char * argv[])
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/usb_cam-ros2/test/test_pixel_formats.cpp b/src/usb_cam-ros2/test/test_pixel_formats.cpp
new file mode 100644
index 0000000..44a9a56
--- /dev/null
+++ b/src/usb_cam-ros2/test/test_pixel_formats.cpp
@@ -0,0 +1,49 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+
+#include <gtest/gtest.h>
+
+#include <linux/videodev2.h>
+
+#include "usb_cam/formats/pixel_format_base.hpp"
+
+TEST(test_pixel_formats, pixel_format_base_class) {
+ auto test_pix_fmt = usb_cam::formats::default_pixel_format();
+
+ EXPECT_EQ(test_pix_fmt.name(), "yuyv");
+ EXPECT_EQ(test_pix_fmt.v4l2(), V4L2_PIX_FMT_YUYV);
+ EXPECT_EQ(test_pix_fmt.channels(), 2);
+ EXPECT_EQ(test_pix_fmt.bit_depth(), 8);
+ EXPECT_EQ(test_pix_fmt.requires_conversion(), false);
+
+ EXPECT_EQ(test_pix_fmt.is_bayer(), false);
+ // TOOD(flynneva): should this be true for `yuyv`?
+ EXPECT_EQ(test_pix_fmt.is_color(), false);
+ EXPECT_EQ(test_pix_fmt.is_mono(), false);
+}
diff --git a/src/usb_cam-ros2/test/test_usb_cam_lib.cpp b/src/usb_cam-ros2/test/test_usb_cam_lib.cpp
new file mode 100644
index 0000000..f080158
--- /dev/null
+++ b/src/usb_cam-ros2/test/test_usb_cam_lib.cpp
@@ -0,0 +1,59 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include "usb_cam/usb_cam.hpp"
+#include "usb_cam/utils.hpp"
+
+TEST(test_usb_cam_lib, test_usb_cam_class) {
+ usb_cam::UsbCam test_usb_cam;
+
+ auto supported_fmts = test_usb_cam.get_supported_formats();
+
+ // TODO(flynneva): iterate over availble formats with test_usb_cam obj
+ for (auto fmt : supported_fmts) {
+ std::cerr << "format: " << fmt.format.type << std::endl;
+ }
+
+ // TODO(flynneva): rework these tests in another MR
+ {
+ // test_usb_cam.configure(
+ // "/dev/video0",
+ // usb_cam::utils::IO_METHOD_MMAP,
+ // "yuyv2rgb", 640, 480, 30);
+ // test_usb_cam.start();
+ // TODO(flynneva): uncomment once /dev/video0 can be simulated in CI
+ // EXPECT_TRUE(test_usb_cam.is_capturing());
+ // test_usb_cam.shutdown();
+ }
+}
diff --git a/src/usb_cam-ros2/test/test_usb_cam_utils.cpp b/src/usb_cam-ros2/test/test_usb_cam_utils.cpp
new file mode 100644
index 0000000..a0ac6fc
--- /dev/null
+++ b/src/usb_cam-ros2/test/test_usb_cam_utils.cpp
@@ -0,0 +1,83 @@
+// Copyright 2023 Evan Flynn
+//
+// 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 Evan Flynn nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
+
+#include <gtest/gtest.h>
+#include <libavutil/pixfmt.h>
+
+#include <string>
+
+#include "usb_cam/utils.hpp"
+#include "usb_cam/formats/utils.hpp"
+
+
+using usb_cam::utils::io_method_from_string;
+
+
+TEST(test_usb_cam_utils, test_io_method_from_string) {
+ usb_cam::utils::io_method_t test_io;
+
+ test_io = io_method_from_string("mmap");
+ EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_MMAP);
+
+ test_io = io_method_from_string("read");
+ EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_READ);
+
+ test_io = io_method_from_string("userptr");
+ EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_USERPTR);
+
+ test_io = io_method_from_string("bananas");
+ EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_UNKNOWN);
+}
+
+TEST(test_usb_cam_utils, test_clip_value) {
+ // Clip values to 0 if -128<=val<0
+ for (int i = -128; i < 0; i++) {
+ EXPECT_EQ(0, usb_cam::formats::CLIPVALUE(i));
+ }
+ // Do not clip values between 0<val<255
+ // Just return the value as unsigned char
+ for (int i = 0; i < 255; i++) {
+ EXPECT_EQ(i, usb_cam::formats::CLIPVALUE(i));
+ }
+ // Clip values to 255 if 255<=val<=383
+ for (int i = 255; i <= 383; i++) {
+ EXPECT_EQ(255, usb_cam::formats::CLIPVALUE(i));
+ }
+ // Test outlier cases val < -128 and val > 383
+ // these will use the old method (non-array method)
+ EXPECT_EQ(0, usb_cam::formats::CLIPVALUE(-129));
+ EXPECT_EQ(255, usb_cam::formats::CLIPVALUE(400));
+}
+
+TEST(test_usb_cam_utils, test_monotonic_to_real_time) {
+ // Get timeval to use for test
+
+ const time_t test_time_t = usb_cam::utils::get_epoch_time_shift_us();
+
+ EXPECT_NE(test_time_t, 0);
+}
diff --git a/static_transforms.yaml b/static_transforms.yaml
new file mode 100644
index 0000000..df35b8c
--- /dev/null
+++ b/static_transforms.yaml
@@ -0,0 +1,25 @@
+static_transforms:
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation:
+ - 0.9040756200965469
+ - -0.04513508982503454
+ - 0.03903623145495232
+ - -0.4231858568611735
+ translation:
+ - 0.2377450563929882
+ - -3.954531077024356
+ - 2.896648104324571
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation:
+ - 0.023345895194435935
+ - 0.9217664604335917
+ - -0.3839850764247843
+ - -0.0485491779575187
+ translation:
+ - 0.035852423609577216
+ - 4.437697599274499
+ - 2.9021133125723435
--
GitLab
From 059fd1261b46ee41285e70a5e0fc45a5a04a50a3 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 20 Apr 2024 10:50:42 +0300
Subject: [PATCH 54/64] cherry-pick of localization preprocessor commit
---
.../multi_camera_aruco_preprocessor.launch.py | 102 ++++++++++++++++
.../solo_camera_aruco_preprocessor.launch.py | 109 +++++++++---------
.../aruco_preprocessor_node.py | 2 +-
3 files changed, 155 insertions(+), 58 deletions(-)
diff --git a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
index e69de29..6f14ff9 100644
--- a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
@@ -0,0 +1,102 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params1.yaml"
+ )
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+
+ return LaunchDescription(
+ [
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [FindPackageShare("calibration_node"), "/launch", "/tf.launch.py"]
+ )
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera1",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera2",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.0696}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera1/image_raw"),
+ ("/camera/camera_info", "/camera1/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.0696}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="localization_preprocessor",
+ executable="aruco_preprocessor_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera2"]},
+ ],
+ remappings=[
+ ("/pose", "/robot_0/pose"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="localization_preprocessor",
+ executable="aruco_preprocessor_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera1"]},
+ ],
+ remappings=[
+ ("/pose", "/robot_0/pose"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
index 7b3e4db..8131ecd 100644
--- a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
@@ -7,62 +7,57 @@ from launch.actions import IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
-
- return LaunchDescription([
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource([
- FindPackageShare("calibration_node"),
- '/launch',
- '/tf.launch.py'
- ])
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera1',
- emulate_tty=True
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera1'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.0696} # 0.07
- ],
- remappings=[
- ('/camera/image_raw', '/camera1/image_raw'),
- ('/camera/camera_info', '/camera1/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
-
- Node(
- package='localization_preprocessor',
- executable='aruco_preprocessor_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera1"]}
- ],
-
- remappings=[
- ('/camera/image_raw', '/camera1/image_raw'),
- ('/pose', '/robot_0/pose'),
- ('/camera/camera_info', '/camera1/camera_info')
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
- ],
- output='screen',
- emulate_tty=True
- )
- ])
\ No newline at end of file
+ return LaunchDescription(
+ [
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [FindPackageShare("calibration_node"), "/launch", "/tf.launch.py"]
+ )
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera1",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.0696}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera1/image_raw"),
+ ("/camera/camera_info", "/camera1/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="localization_preprocessor",
+ executable="aruco_preprocessor_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera1"]},
+ ],
+ remappings=[
+ ("/pose", "/robot_0/pose"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 5c6d8b6..01cfa21 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -194,7 +194,7 @@ class ArucoPreprocessorNode(Node):
pose.pose.orientation.w,
]
transformed_quate_iter = tf_t.quaternion_multiply(
- quaternion_iter, self.aruco_rotation_map[id]
+ quaternion_iter, self.aruco_rotation_map[id] # !!! ALARM SHIIIIIT
)
(
rotated_pose.pose.orientation.x,
--
GitLab
From 55a0dec019d29ed583084d03f6cfc0412764a7d1 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 15:03:07 +0300
Subject: [PATCH 55/64] Arucos in 3D
---
.../config/connections_config.json | 14 +-
.../src/androsot_approach_teb.cpp | 6 +-
.../localisation/localisation_node.py | 126 +++++-------------
.../multi_camera_aruco_preprocessor.launch.py | 18 +--
.../aruco_preprocessor_node.py | 100 +++++++-------
.../odometry_estimator/OdometryEstimator.py | 2 +-
src/proxy/proxy/Proxy.py | 2 +-
src/server_main/config/robots_config.json | 8 +-
8 files changed, 105 insertions(+), 171 deletions(-)
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
index 5a858f3..553c9cb 100644
--- a/src/androsot_approach_teb/config/connections_config.json
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -3,20 +3,8 @@
"0": {
"device": "ttyUSB0",
- "robot_name": "ROKI-0058",
+ "robot_name": "ROKI-0061",
"port": "1969"
- },
-
- "1": {
- "device": "ttyUSB1",
- "robot_name": "ROKI-0067",
- "port": "1970"
- },
-
- "2": {
- "device": "ttyUSB2",
- "robot_name": "ROKI-0057",
- "port": "1971"
}
}
diff --git a/src/androsot_approach_teb/src/androsot_approach_teb.cpp b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
index f01aa54..20eb0f3 100644
--- a/src/androsot_approach_teb/src/androsot_approach_teb.cpp
+++ b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
@@ -362,9 +362,9 @@ int main(int argc, char **argv)
// TODO: translation coefficients m/s -> parrots
geometry_msgs::msg::Twist target_vel_msg;
- target_vel_msg.linear.x = vx;
- target_vel_msg.linear.y = vy;
- target_vel_msg.angular.z = omega;
+ target_vel_msg.linear.x = vx * 10000;
+ target_vel_msg.linear.y = vy * 10;
+ target_vel_msg.angular.z = omega * 10;
target_vel_publisher->publish(target_vel_msg);
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 8dd8b80..d1a1d96 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -48,13 +48,14 @@ from collections import deque
# pose.orientation.w = quaternion[3]
# return pose
-class PoseSinCos():
- def __init__(self, x, y, z, sin, cos):
+class Pose():
+ def __init__(self, x, y, z, roll, pitch, yaw):
self.x = x
self.y = y
self.z = z
- self.sin = sin
- self.cos = cos
+ self.roll = roll
+ self.pitch = pitch
+ self.yaw = yaw
class AlphaFilter():
def __init__(self):
@@ -73,13 +74,14 @@ class AlphaFilter():
def get(self):
weight_sum = 0.0
## find yaw mid
- mid = PoseSinCos(0.0, 0.0, 0.0, 0.0, 0.0)
+ mid = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
for i, elem in enumerate(self.buffer):
mid.x += elem.x * self.alpha ** (len(self.buffer) - i)
mid.y += elem.y * self.alpha ** (len(self.buffer) - i)
mid.z += elem.z * self.alpha ** (len(self.buffer) - i)
- mid.sin += elem.sin * self.alpha ** (len(self.buffer) - i)
- mid.cos += elem.cos * self.alpha ** (len(self.buffer) - i)
+ mid.roll += elem.roll * self.alpha ** (len(self.buffer) - i)
+ mid.pitch += elem.pitch * self.alpha ** (len(self.buffer) - i)
+ mid.yaw += elem.yaw * self.alpha ** (len(self.buffer) - i)
weight_sum += self.alpha ** (len(self.buffer) - i ) #TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
if len(self.buffer) == 0:
@@ -87,8 +89,9 @@ class AlphaFilter():
mid.x /= weight_sum
mid.y /= weight_sum
mid.z /= weight_sum
- mid.sin /= weight_sum
- mid.cos /= weight_sum
+ mid.roll /= weight_sum
+ mid.pitch /= weight_sum
+ mid.yaw /= weight_sum
return mid
class LocalisationNode(Node):
@@ -110,7 +113,7 @@ class LocalisationNode(Node):
self.current_robot_poses = {}
self.aruco_observed_poses = {}
self.robot_pose = AlphaFilter()
- self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.25, 0.0, 0.0)
+ self.unfiltered_last_observed_robot_pos = Pose(0.0, 0.0, 0.25, 0.0, 0.0, 0.0)
self.robot_pose.update(self.unfiltered_last_observed_robot_pos)
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
@@ -129,26 +132,10 @@ class LocalisationNode(Node):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
- sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
- # print(sin, cos)
- pose_sin_cos = PoseSinCos(x, y, z, sin, cos)
- self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, z, sin, cos)
- self.robot_pose.update(pose_sin_cos)
-
- if self.debug:
- # Extract quaternion from PoseStamped message
- quaternion = msg.pose.pose.orientation
- # Convert quaternion to RPY
- rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
- roll, pitch, yaw = rpy
-
- rpy_msg = RPY()
- rpy_msg.roll, rpy_msg.pitch, rpy_msg.yaw = roll, pitch, yaw
- self.rpy_pub.publish(rpy_msg)
-
- rpy_vector3_msg = Vector3()
- rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
- self.rpy_vector3_pub.publish(rpy_vector3_msg)
+ quaternion = msg.pose.pose.orientation
+ roll, pitch, yaw = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
+ self.unfiltered_last_observed_robot_pos = Pose(x, y, z, roll, pitch, yaw)
+ self.robot_pose.update(self.unfiltered_last_observed_robot_pos)
def get_x_y_yaw_pose_filtered(self):
@@ -170,31 +157,26 @@ class LocalisationNode(Node):
return pose
def timer_callback(self):
- if self.debug:
- unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
- self.unfiltered_pose_pub.publish(unfilt_msg)
+ # if self.debug:
+ # unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
+ # self.unfiltered_pose_pub.publish(unfilt_msg)
- pose_sin_cos = self.robot_pose.get()
- pose = Pose()
- x = pose_sin_cos.x
- y = pose_sin_cos.y
- z = pose_sin_cos.z
- theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
- filtered_msg = self.convert_to_3d_with_covariance(x, y, z, theta)
+ pose = self.robot_pose.get()
+ filtered_msg = self.convert_to_3d_with_covariance(pose)
self.filtered_pose_pub.publish(filtered_msg)
- filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(x, y, z, theta)
+ filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(pose)
self.filtered_stamped_pose_pub.publish(filtered_stamped_msg)
fallen_msg = Bool()
- fallen_msg.data = self.is_on_ground(z)
+ fallen_msg.data = self.fallen(pose.z)
self.on_ground_publisher.publish(fallen_msg)
- if self.debug:
- msg_3d = self.convert_to_3d(filt_msg)
- self.filtered_pose_3d_pub.publish(msg_3d)
+ # if self.debug:
+ # msg_3d = self.convert_to_3d(filt_msg)
+ # self.filtered_pose_3d_pub.publish(msg_3d)
- def is_on_ground(self, z):
+ def fallen(self, z):
if z < self.lowest_standing_z:
return True
else:
@@ -217,15 +199,15 @@ class LocalisationNode(Node):
pose.pose.orientation.w = quaternion[3]
return pose
- def convert_to_3d_with_covariance(self, x, y, z, theta):
+ def convert_to_3d_with_covariance(self, pose):
# Create a Pose message
p = PoseWithCovariance()
- p.pose.position.x = x
- p.pose.position.y = y
- p.pose.position.z = z
+ p.pose.position.x = pose.x
+ p.pose.position.y = pose.y
+ p.pose.position.z = pose.z
# Convert theta to quaternion for orientation
- quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
+ quaternion = tf_transformations.quaternion_from_euler(pose.roll, pose.pitch, pose.yaw)
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
@@ -234,54 +216,16 @@ class LocalisationNode(Node):
p.covariance = self.covariance
return p
- def convert_to_3d_with_covariance_stamped(self, x, y, z, theta):
+ def convert_to_3d_with_covariance_stamped(self, pose):
# Create a Pose message
p = PoseWithCovarianceStamped()
p.header.stamp = self.get_clock().now().to_msg()
p.header.frame_id = "world"
- p.pose.pose.position.x = x
- p.pose.pose.position.y = y
- p.pose.pose.position.z = z
- # Convert theta to quaternion for orientation
- quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
- p.pose.pose.orientation.x = quaternion[0]
- p.pose.pose.orientation.y = quaternion[1]
- p.pose.pose.orientation.z = quaternion[2]
- p.pose.pose.orientation.w = quaternion[3]
-
- p.pose.covariance = self.covariance
+ p.pose = self.convert_to_3d_with_covariance(pose)
return p
-
- def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
- x_ax = [1.0, 0.0, 0.0]
- quat_arr = [
- pose.pose.pose.orientation.w,
- pose.pose.pose.orientation.x,
- pose.pose.pose.orientation.y,
- pose.pose.pose.orientation.z
- ]
-
- rotated_x = Quaternion(quat_arr).rotate(x_ax)
- # print(rotated_x)
- # TODO check normalisation
- return rotated_x[1], rotated_x[0]
-
- def get_rpy_from_quaternion(self, pose: Pose):
- x_ax = [1.0, 0.0, 0.0]
- quat_arr = [
- pose.pose.pose.orientation.w,
- pose.pose.pose.orientation.x,
- pose.pose.pose.orientation.y,
- pose.pose.pose.orientation.z
- ]
-
- rotated_x = Quaternion(quat_arr).rotate(x_ax)
- # print(rotated_x)
- # TODO check normalisation
- return rotated_x[1], rotated_x[0]
def main(args=None):
rclpy.init(args=args)
diff --git a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
index 6f14ff9..6fbaea1 100644
--- a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
@@ -9,10 +9,10 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
- camera_config = os.path.join(
- get_package_share_directory("server_main"), "config", "camera_params1.yaml"
+ camera_config1 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
)
- camera_config = os.path.join(
+ camera_config2 = os.path.join(
get_package_share_directory("server_main"), "config", "camera_params2.yaml"
)
robot_config = os.path.join(
@@ -27,17 +27,17 @@ def generate_launch_description():
)
),
Node(
- package="usb_cam",
- executable="usb_cam_node_exe",
- parameters=[camera_config],
+ package="usb_cam_ros2",
+ executable="usb_cam_ros2_node_exe",
+ parameters=[camera_config1],
output="screen",
namespace="camera1",
emulate_tty=True,
),
Node(
- package="usb_cam",
- executable="usb_cam_node_exe",
- parameters=[camera_config],
+ package="usb_cam_ros2",
+ executable="usb_cam_ros2_node_exe",
+ parameters=[camera_config2],
output="screen",
namespace="camera2",
emulate_tty=True,
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 01cfa21..5d2ed62 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -99,42 +99,43 @@ class ArucoPreprocessorNode(Node):
def aruco_cb(self, msg):
self.get_camera_to_world_frame()
for id, pose in zip(msg.marker_ids, msg.poses):
- pose_stamped = PoseStamped()
- pose_stamped.pose = pose
- pose_stamped.header = msg.header
- # self.current_aruco_poses[id] = pose_stamped
- # Example quaternion (x, y, z, w)
- r = pose_stamped.pose.orientation
- quaternion = [r.x, r.y,r.z,r.w]
- roll, pitch, yaw = euler_from_quaternion(quaternion)
- rad2deg = lambda x: x / 3.14 * 180
- # roll is near 180
- # pitch is near 0
- # cov is equal for all angles
- # pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
- # cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
- # yaw - pohui
- cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
- print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
- field_pose = self.transform_to_robot_centre(pose_stamped, id)
- pose_with_cov = PoseWithCovarianceStamped()
- pose_with_cov.header = field_pose.header
- # pose_with_cov.header.frame_id = "odom"
- pose_with_cov.header.stamp = self.get_clock().now().to_msg()
- pose_with_cov.pose.pose = field_pose.pose
- # I think python links fuck me. upd numpy copy sigment fuck me. upd life fuck me
- print (pose_with_cov)
- self.covariance[21] = cov
- self.covariance[28] = cov
- self.covariance[35] = cov
- tf = self.tf_buffer.lookup_transform_core("world", field_pose.header.frame_id, rclpy.time.Time())
- world_pose = tf2_geometry_msgs.do_transform_pose(field_pose.pose, tf)
- pose_with_cov.pose.pose = world_pose
- pose_with_cov.header.frame_id = "world"
- pose_with_cov.pose.covariance = self.covariance
- # pose_with_cov.header.frame =
-
- self.publisher_.publish(pose_with_cov)
+ if id in self.aruco_rotation_map.keys():
+ pose_stamped = PoseStamped()
+ pose_stamped.pose = pose
+ pose_stamped.header = msg.header
+ # self.current_aruco_poses[id] = pose_stamped
+ # Example quaternion (x, y, z, w)
+ r = pose_stamped.pose.orientation
+ quaternion = [r.x, r.y,r.z,r.w]
+ roll, pitch, yaw = euler_from_quaternion(quaternion)
+ rad2deg = lambda x: x / 3.14 * 180
+ # roll is near 180
+ # pitch is near 0
+ # cov is equal for all angles
+ # pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
+ # cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
+ # yaw - pohui
+ cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
+ print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
+ field_pose = self.transform_to_robot_centre(pose_stamped, id)
+ pose_with_cov = PoseWithCovarianceStamped()
+ pose_with_cov.header = field_pose.header
+ # pose_with_cov.header.frame_id = "odom"
+ pose_with_cov.header.stamp = self.get_clock().now().to_msg()
+ pose_with_cov.pose.pose = field_pose.pose
+ # I think python links fuck me. upd numpy copy sigment fuck me. upd life fuck me
+ print (pose_with_cov)
+ self.covariance[21] = cov
+ self.covariance[28] = cov
+ self.covariance[35] = cov
+ tf = self.tf_buffer.lookup_transform_core("world", field_pose.header.frame_id, rclpy.time.Time())
+ world_pose = tf2_geometry_msgs.do_transform_pose(field_pose.pose, tf)
+ pose_with_cov.pose.pose = world_pose
+ pose_with_cov.header.frame_id = "world"
+ pose_with_cov.pose.covariance = self.covariance
+ # pose_with_cov.header.frame =
+
+ self.publisher_.publish(pose_with_cov)
def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
shifted_pose = self.shift_pose_to_robot_centre(pose, id)
@@ -193,19 +194,20 @@ class ArucoPreprocessorNode(Node):
pose.pose.orientation.z,
pose.pose.orientation.w,
]
- transformed_quate_iter = tf_t.quaternion_multiply(
- quaternion_iter, self.aruco_rotation_map[id] # !!! ALARM SHIIIIIT
- )
- (
- rotated_pose.pose.orientation.x,
- rotated_pose.pose.orientation.y,
- rotated_pose.pose.orientation.z,
- rotated_pose.pose.orientation.w,
- ) = transformed_quate_iter
- # rotated_pose.header.stamp = self.get_clock().now().to_msg()
- rotated_pose.header.frame_id = (
- pose.header.frame_id
- ) # "camera" ########## Choose frame id
+ if id in self.aruco_rotation_map.keys():
+ transformed_quate_iter = tf_t.quaternion_multiply(
+ quaternion_iter, self.aruco_rotation_map[id] # !!! ALARM SHIIIIIT
+ )
+ (
+ rotated_pose.pose.orientation.x,
+ rotated_pose.pose.orientation.y,
+ rotated_pose.pose.orientation.z,
+ rotated_pose.pose.orientation.w,
+ ) = transformed_quate_iter
+ # rotated_pose.header.stamp = self.get_clock().now().to_msg()
+ rotated_pose.header.frame_id = (
+ pose.header.frame_id
+ ) # "camera" ########## Choose frame id
return rotated_pose
def send_coord_to_robot(self, id):
diff --git a/src/odometry_estimator/odometry_estimator/OdometryEstimator.py b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
index 630338f..954f8d3 100644
--- a/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
+++ b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
@@ -53,7 +53,7 @@ class OdometryEstimator(Node):
current_odometry.twist.twist = msg
current_odometry.twist.covariance = [float(0.01) for _ in range(36)]
- self.last_twist = msg
+ self.last_twist = current_odometry.twist
self._odometry_publisher.publish(current_odometry)
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index b9a9336..9ea91db 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -77,7 +77,7 @@ class Proxy(Node):
self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
- # self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
+ self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
async def disconnect(self) -> None:
if self.writer is not None:
diff --git a/src/server_main/config/robots_config.json b/src/server_main/config/robots_config.json
index 9cc60ef..19b7fd7 100644
--- a/src/server_main/config/robots_config.json
+++ b/src/server_main/config/robots_config.json
@@ -7,17 +7,17 @@
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [-1.57, 0, 0]
+ [1.57, 0, 0]
]
},
"1": {
- "aruco_ids": [10,12,14,16,18],
+ "aruco_ids": [10, 12, 14, 16, 18],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [-1.57, 0, 0]
+ [1.57, 0, 0]
]
},
"2": {
@@ -27,7 +27,7 @@
[0, -1.57, 0],
[0, 3.14, 0],
[0, 1.57, 0],
- [-1.57, 0, 0]
+ [1.57, 0, 0]
]
}
}
--
GitLab
From c6535cdd4bd817ffa67f47d16539463c5fcb24dc Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sat, 20 Apr 2024 21:56:46 +0300
Subject: [PATCH 56/64] GOVNA COMMIT
---
.../config/connections_config.json | 2 +-
.../src/androsot_approach_teb.cpp | 18 ++++----
.../config/static_transforms.yaml | 30 ++++++-------
.../aruco_preprocessor_node.py | 42 +++++++++++++++----
src/proxy/proxy/Proxy.py | 4 +-
static_transforms.yaml | 30 ++++++-------
6 files changed, 78 insertions(+), 48 deletions(-)
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
index 553c9cb..9759959 100644
--- a/src/androsot_approach_teb/config/connections_config.json
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -3,7 +3,7 @@
"0": {
"device": "ttyUSB0",
- "robot_name": "ROKI-0061",
+ "robot_name": "ROKI-0067",
"port": "1969"
}
diff --git a/src/androsot_approach_teb/src/androsot_approach_teb.cpp b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
index 20eb0f3..39f3c09 100644
--- a/src/androsot_approach_teb/src/androsot_approach_teb.cpp
+++ b/src/androsot_approach_teb/src/androsot_approach_teb.cpp
@@ -70,11 +70,11 @@ void fillTebConfig(teb_local_planner::TebConfig &config)
// == Robot ==
//# Robot/Carlike
// Maximum translational velocity of the robot
- config.robot.max_vel_x = 0.1;
+ config.robot.max_vel_x = 0.14;
// Maximum translational velocity of the robot for driving backwards
- config.robot.max_vel_x_backwards = 0.02;
+ config.robot.max_vel_x_backwards = 0.14;
// Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) [0.0, 100]
- config.robot.max_vel_y = 0.1;
+ config.robot.max_vel_y = 0.18;
// Maximum angular velocity of the robot [0.01, 100]
config.robot.max_vel_theta = 0.1;
// Maximum translational acceleration of the robot
@@ -88,8 +88,8 @@ void fillTebConfig(teb_local_planner::TebConfig &config)
// Minimum turning radius of a carlike robot (diff-drive robot: zero)
config.robot.min_turning_radius = 0.0;
// The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
- config.robot.wheelbase = 1.0;
- config.robot.cmd_angle_instead_rotvel = false;
+ // config.robot.wheelbase = 0.1;
+ config.robot.cmd_angle_instead_rotvel = false;
// == GoalTolerance ==
// Allowed final euclidean distance to the goal position
@@ -355,16 +355,16 @@ int main(int argc, char **argv)
auto vy = 0.0; //strafing velocity which can be nonzero for holonomic robots[m/s]
auto omega = 0.0; //rotational velocity [rad/s]
- auto lookAheadPoses = 3;
+ auto lookAheadPoses = 10;
test.getVelocityCommand(vx, vy, omega, lookAheadPoses);
// TODO: translation coefficients m/s -> parrots
geometry_msgs::msg::Twist target_vel_msg;
- target_vel_msg.linear.x = vx * 10000;
- target_vel_msg.linear.y = vy * 10;
- target_vel_msg.angular.z = omega * 10;
+ target_vel_msg.linear.x = vx * 1000;
+ target_vel_msg.linear.y = vy * 1000;
+ target_vel_msg.angular.z = omega * 100;
target_vel_publisher->publish(target_vel_msg);
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index df35b8c..770d528 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -3,23 +3,23 @@ static_transforms:
name: camera1
parent_frame_id: world
rotation:
- - 0.9040756200965469
- - -0.04513508982503454
- - 0.03903623145495232
- - -0.4231858568611735
+ - 0.9553798467029665
+ - 0.05845252865982777
+ - -0.0036991582918384875
+ - -0.289515054245924
translation:
- - 0.2377450563929882
- - -3.954531077024356
- - 2.896648104324571
+ - 0.5095772336627481
+ - -0.7895029743512599
+ - 1.7309347486778708
- child_frame_id: camera2
name: camera2
parent_frame_id: world
- rotation:
- - 0.023345895194435935
- - 0.9217664604335917
- - -0.3839850764247843
- - -0.0485491779575187
+ rotation:
+ - 0.9697033003887658
+ - 0.05525964453102179
+ - 0.04329297270725113
+ - -0.233982049344836
translation:
- - 0.035852423609577216
- - 4.437697599274499
- - 2.9021133125723435
+ - 0.05312842279884265
+ - -0.972345994627029
+ - 1.6195570157578052
\ No newline at end of file
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 5d2ed62..3c48e46 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -5,7 +5,7 @@ from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, PoseStamped, Tran
from androsot_msgs.msg import FieldPose # Assuming you've defined this
from tf2_ros import TransformException, TransformListener, Buffer
import tf2_ros
-from tf2_geometry_msgs import do_transform_point
+from tf2_geometry_msgs import do_transform_point, do_transform_pose_with_covariance_stamped
from ros2_aruco_interfaces.msg import ArucoMarkers
from tf_transformations import euler_from_quaternion
@@ -118,6 +118,33 @@ class ArucoPreprocessorNode(Node):
cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
field_pose = self.transform_to_robot_centre(pose_stamped, id)
+
+ to_odom_quat = tf_t.quaternion_from_euler(
+ -math.pi / 2, 0.0, 0.0
+ )
+
+ to_odom_quat_2 = tf_t.quaternion_from_euler(
+ 0.0, 0.0, -math.pi / 2
+ )
+
+ to_odom_quat_3 = tf_t.quaternion_from_euler(
+ 0.0, math.pi / 2, 0.0
+ )
+
+ a = field_pose.pose.orientation
+ pose_quat = [a.w, a.x, a.y, a.z]
+ itog = tf_t.quaternion_multiply(to_odom_quat_3, tf_t.quaternion_multiply(to_odom_quat_2, tf_t.quaternion_multiply(
+ to_odom_quat, pose_quat # !!! ALARM SHIIIIIT
+ )))
+
+ print("ITOG: ", itog)
+ print()
+
+ field_pose.pose.orientation.w = itog[0]
+ field_pose.pose.orientation.x = itog[1]
+ field_pose.pose.orientation.y = itog[2]
+ field_pose.pose.orientation.z = itog[3]
+
pose_with_cov = PoseWithCovarianceStamped()
pose_with_cov.header = field_pose.header
# pose_with_cov.header.frame_id = "odom"
@@ -129,13 +156,14 @@ class ArucoPreprocessorNode(Node):
self.covariance[28] = cov
self.covariance[35] = cov
tf = self.tf_buffer.lookup_transform_core("world", field_pose.header.frame_id, rclpy.time.Time())
- world_pose = tf2_geometry_msgs.do_transform_pose(field_pose.pose, tf)
- pose_with_cov.pose.pose = world_pose
- pose_with_cov.header.frame_id = "world"
- pose_with_cov.pose.covariance = self.covariance
- # pose_with_cov.header.frame =
+ world_pose = do_transform_pose_with_covariance_stamped(pose_with_cov, tf)
+
+
+
+ world_pose.header.frame_id = "world"
+ # pose_with_cov.header.frame =
- self.publisher_.publish(pose_with_cov)
+ self.publisher_.publish(world_pose)
def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
shifted_pose = self.shift_pose_to_robot_centre(pose, id)
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index 9ea91db..89b2d14 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -8,6 +8,8 @@ from rclpy.node import Node
from geometry_msgs.msg import Twist
+import numpy as np
+
class Proxy(Node):
def __init__(self, host: str="localhost", port: str="1969",
@@ -74,7 +76,7 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
+ self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{np.clip(-int(msg.linear.y), -128, 128)}|{np.clip(-int(msg.linear.x), -128, 128)}\n".encode())
await self.writer.drain()
self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
diff --git a/static_transforms.yaml b/static_transforms.yaml
index df35b8c..770d528 100644
--- a/static_transforms.yaml
+++ b/static_transforms.yaml
@@ -3,23 +3,23 @@ static_transforms:
name: camera1
parent_frame_id: world
rotation:
- - 0.9040756200965469
- - -0.04513508982503454
- - 0.03903623145495232
- - -0.4231858568611735
+ - 0.9553798467029665
+ - 0.05845252865982777
+ - -0.0036991582918384875
+ - -0.289515054245924
translation:
- - 0.2377450563929882
- - -3.954531077024356
- - 2.896648104324571
+ - 0.5095772336627481
+ - -0.7895029743512599
+ - 1.7309347486778708
- child_frame_id: camera2
name: camera2
parent_frame_id: world
- rotation:
- - 0.023345895194435935
- - 0.9217664604335917
- - -0.3839850764247843
- - -0.0485491779575187
+ rotation:
+ - 0.9697033003887658
+ - 0.05525964453102179
+ - 0.04329297270725113
+ - -0.233982049344836
translation:
- - 0.035852423609577216
- - 4.437697599274499
- - 2.9021133125723435
+ - 0.05312842279884265
+ - -0.972345994627029
+ - 1.6195570157578052
\ No newline at end of file
--
GitLab
From e561bcec86d97f77c3b10f19dcd2147690927ceb Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sat, 20 Apr 2024 22:16:02 +0300
Subject: [PATCH 57/64] Add tested pic to real
---
src/pic_to_real/launch/bal_to_real.launch.py | 141 +++++++++++++++++++
src/pic_to_real/package.xml | 18 +++
src/pic_to_real/pic_to_real/__init__.py | 0
src/pic_to_real/pic_to_real/ball_to_real.py | 115 +++++++++++++++
src/pic_to_real/resource/pic_to_real | 0
src/pic_to_real/setup.cfg | 4 +
src/pic_to_real/setup.py | 33 +++++
src/pic_to_real/test/test_copyright.py | 25 ++++
src/pic_to_real/test/test_flake8.py | 25 ++++
src/pic_to_real/test/test_pep257.py | 23 +++
10 files changed, 384 insertions(+)
create mode 100644 src/pic_to_real/launch/bal_to_real.launch.py
create mode 100644 src/pic_to_real/package.xml
create mode 100644 src/pic_to_real/pic_to_real/__init__.py
create mode 100644 src/pic_to_real/pic_to_real/ball_to_real.py
create mode 100644 src/pic_to_real/resource/pic_to_real
create mode 100644 src/pic_to_real/setup.cfg
create mode 100644 src/pic_to_real/setup.py
create mode 100644 src/pic_to_real/test/test_copyright.py
create mode 100644 src/pic_to_real/test/test_flake8.py
create mode 100644 src/pic_to_real/test/test_pep257.py
diff --git a/src/pic_to_real/launch/bal_to_real.launch.py b/src/pic_to_real/launch/bal_to_real.launch.py
new file mode 100644
index 0000000..4922f64
--- /dev/null
+++ b/src/pic_to_real/launch/bal_to_real.launch.py
@@ -0,0 +1,141 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+import os
+from ament_index_python.packages import get_package_share_directory
+import yaml
+import json
+
+
+def load_yaml_file(file_path):
+ with open(file_path, "r") as file:
+ return yaml.safe_load(file)
+
+
+def create_tf_nodes(static_transforms: dict):
+ tf_nodes = []
+ for transform in static_transforms["static_transforms"]:
+ node = Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name=transform["child_frame_id"],
+ arguments=[
+ str(transform["translation"][0]),
+ str(transform["translation"][1]),
+ str(transform["translation"][2]),
+ str(transform["rotation"][0]),
+ str(transform["rotation"][1]),
+ str(transform["rotation"][2]),
+ str(transform["rotation"][3]),
+ transform["parent_frame_id"],
+ transform["child_frame_id"],
+ ],
+ )
+ tf_nodes.append(node)
+ return tf_nodes
+
+def create_tf_node():
+ node = Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name="camera1",
+ arguments=[
+ str(0.0),
+ str(0.0),
+ str(1.0),
+ str(0.0),
+ str(0.0),
+ str(0.0),
+ str(1.0),
+ "world",
+ "camera1",
+ ],
+ )
+ return node
+
+
+def generate_launch_description():
+ ld = LaunchDescription()
+ camera_config1 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+
+ # tf_config = os.path.join(
+ # get_package_share_directory("calibration_node"),
+ # "config",
+ # "static_transforms.yaml",
+ # )
+ # # [TODO] run launch from calibration_package
+ # static_transforms = load_yaml_file(tf_config)
+ # transforms_nodes = create_tf_nodes(static_transforms)
+ # for node in transforms_nodes:
+ # ld.add_action(node)
+ tf_node = create_tf_node()
+
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ yolo_config_dir = yolo_dir + "/config/"
+
+ (
+ ld.add_action(
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config1],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera1",
+ )
+ ),
+ )
+ ld.add_action(
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera1/ball_on_image"),
+ ("/image_raw", "/camera1/image_raw"),
+ ],
+ parameters=[os.path.join(yolo_config_dir, "yolo_roma.yaml")],
+ )
+ )
+ (
+ ld.add_action(
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera2",
+ )
+ ),
+ )
+ ld.add_action(
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera2/ball_on_image"),
+ ("/image_raw", "/camera2/image_raw"),
+ ],
+ parameters=[os.path.join(yolo_config_dir, "yolo_roma.yaml")],
+ )
+ )
+ ld.add_action(
+ Node(
+ package="pic_to_real",
+ executable="ball_to_real",
+ output="screen",
+ emulate_tty=True,
+
+ parameters=[{"camera_frames" : ["camera1", "camera2"]}]
+ )
+ )
+
+ ld.add_action(tf_node)
+ return ld
diff --git a/src/pic_to_real/package.xml b/src/pic_to_real/package.xml
new file mode 100644
index 0000000..41f23c9
--- /dev/null
+++ b/src/pic_to_real/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>pic_to_real</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="xatc120@gmail.com">nakuznetsov</maintainer>
+ <license>TODO: License declaration</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/pic_to_real/pic_to_real/__init__.py b/src/pic_to_real/pic_to_real/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/pic_to_real/pic_to_real/ball_to_real.py b/src/pic_to_real/pic_to_real/ball_to_real.py
new file mode 100644
index 0000000..058433a
--- /dev/null
+++ b/src/pic_to_real/pic_to_real/ball_to_real.py
@@ -0,0 +1,115 @@
+from soccer_vision_2d_msgs.msg import BallArray, Ball
+from sensor_msgs.msg import CameraInfo
+
+from image_geometry import PinholeCameraModel
+from geometry_msgs.msg import Pose, PointStamped, Pose2D
+
+from tf2_geometry_msgs import do_transform_point
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+import rclpy
+
+from skspatial.objects import Line, Plane
+from tf2_ros import TransformException, TransformListener, Buffer
+import tf2_ros
+
+class Ball2RealNode(rclpy.node.Node):
+ def __init__(self):
+ super().__init__("Ball2RealNode")
+ self.declare_parameter("camera_frames", ["camera1"])
+ self.camera_frames = self.get_parameter("camera_frames").value
+ self.camera_models = {} # {"frame_id": PinholeCameraModel}
+ for frame in self.camera_frames:
+ self.ball_sub = self.create_subscription(
+ BallArray, frame + "/" + "ball_on_image", self.update_ball_pose, 10
+ )
+ self.camera_info_sub = self.create_subscription(
+ CameraInfo, frame + "/" + "camera_info", self.init_camera_model, 1
+ )
+ self.camera_models[frame] = None
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+ self.ball_world_pub = self.create_publisher(Pose2D, "ball_coordinate_world", 10)
+ self.tf_from_camera_to_world = {}
+
+
+ def init_camera_model(self, msg):
+ self.camera_models[msg.header.frame_id] = PinholeCameraModel()
+ self.camera_models[msg.header.frame_id].fromCameraInfo(msg)
+
+ def update_ball_pose(self, msg):
+ self.get_camera_to_world_frame()
+ if (
+ msg.header.frame_id in self.camera_frames
+ and self.camera_models[msg.header.frame_id] == None
+ ):
+ self.get_logger().info(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
+ return
+
+ if msg.header.frame_id not in self.camera_frames:
+ self.get_logger().info(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
+ return
+
+ if len(msg.balls) == 0:
+ return
+ # in camera CS:
+ ray_ball_arr = self.camera_models[msg.header.frame_id].projectPixelTo3dRay(
+ (msg.balls[0].center.x, msg.balls[0].center.y)
+ )
+ ray_to_ball = PointStamped()
+ ray_to_ball.point.x = ray_ball_arr[0]
+ ray_to_ball.point.y = ray_ball_arr[1]
+ ray_to_ball.point.z = ray_ball_arr[2]
+ # header = Header()
+ # header.stamp = self.get_clock().now().to_msg()
+ # header.frame_id = "camera"
+ # ray_to_ball.header=header
+ camera_cs_world = self.tf_from_camera_to_world[
+ msg.header.frame_id
+ ].transform.translation ## or with minus
+ self.ray_to_ball_world = do_transform_point(
+ ray_to_ball, self.tf_from_camera_to_world[msg.header.frame_id]
+ )
+
+ plane = Plane.from_points([0, 0, 0], [1, 0, 0], [0, 1, 0])
+ line = Line.from_points(
+ [camera_cs_world.x, camera_cs_world.y, camera_cs_world.z],
+ [
+ self.ray_to_ball_world.point.x,
+ self.ray_to_ball_world.point.y,
+ self.ray_to_ball_world.point.z,
+ ],
+ )
+ self.ball_coordinate_world = plane.intersect_line(line)
+ ball_world_msg = Pose2D()
+ # ball_world_msg.header.stamp = self.get_clock().now().to_msg()
+ # ball_world_msg.header.frame = 'world'
+ ball_world_msg.x = self.ball_coordinate_world[0]
+ ball_world_msg.y = self.ball_coordinate_world[1]
+ self.ball_world_pub.publish(ball_world_msg)
+ # self.ball_coordinate_world = (ray_to_ball_world.point.x, ray_to_ball_world.point.y, ray_to_ball_world.point.z)
+
+ def get_camera_to_world_frame(self):
+ try:
+ for frame in self.camera_frames:
+ print (f"Camera frame is {frame}")
+ self.tf_from_camera_to_world[frame] = (
+ self.tf_buffer.lookup_transform( # check inversion not allwed
+ "world", frame, rclpy.time.Time()
+ )
+ )
+ except TransformException as ex:
+ self.get_logger().info(f"Could not transform world to camera: {ex}")
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ ball_to_real_node = Ball2RealNode()
+ rclpy.spin(ball_to_real_node)
+
+ ball_to_real_node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == "__main__":
+ main()
diff --git a/src/pic_to_real/resource/pic_to_real b/src/pic_to_real/resource/pic_to_real
new file mode 100644
index 0000000..e69de29
diff --git a/src/pic_to_real/setup.cfg b/src/pic_to_real/setup.cfg
new file mode 100644
index 0000000..50e9991
--- /dev/null
+++ b/src/pic_to_real/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pic_to_real
+[install]
+install_scripts=$base/lib/pic_to_real
diff --git a/src/pic_to_real/setup.py b/src/pic_to_real/setup.py
new file mode 100644
index 0000000..61eb98f
--- /dev/null
+++ b/src/pic_to_real/setup.py
@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = "pic_to_real"
+
+setup(
+ name=package_name,
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
+ data_files=[
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*")),
+ ),
+ ],
+ install_requires=["setuptools"],
+ zip_safe=True,
+ maintainer="nakuznetsov",
+ maintainer_email="tatus",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
+ entry_points={
+ "console_scripts": ["ball_to_real = pic_to_real.ball_to_real:main"],
+ },
+)
diff --git a/src/pic_to_real/test/test_copyright.py b/src/pic_to_real/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/pic_to_real/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/pic_to_real/test/test_flake8.py b/src/pic_to_real/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/pic_to_real/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/pic_to_real/test/test_pep257.py b/src/pic_to_real/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/pic_to_real/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
--
GitLab
From 290a430ece6308ecf981257cfde01af419561333 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 21 Apr 2024 05:31:04 +0300
Subject: [PATCH 58/64] solo strategy
Add falling to strategy
abc
---
src/strategy/strategy/solo_robot_strategy.py | 268 +++++++++++++------
src/strategy/strategy/strategy_node.py | 46 +++-
src/strategy/strategy/strategy_test.py | 39 ++-
3 files changed, 250 insertions(+), 103 deletions(-)
diff --git a/src/strategy/strategy/solo_robot_strategy.py b/src/strategy/strategy/solo_robot_strategy.py
index d9d79c2..d0a0721 100644
--- a/src/strategy/strategy/solo_robot_strategy.py
+++ b/src/strategy/strategy/solo_robot_strategy.py
@@ -16,13 +16,11 @@ class RobotState(Enum):
WAIT = 1
GO_TO_BALL = 2 # Approach local and global
APPROACH = 3
- STAND_UP = 4
+ FALL = 4
KICK = 5
GO_HOME = 6
-
-
class CmdType(Enum):
GO = 1
MOTION = 2
@@ -47,8 +45,12 @@ class RobotInfo:
game_state: GameState
role: Role
goal_pose: tp.Optional[Pose2D]
+ home_position: Pose2D
+ is_fallen: bool = False
+
# Add home position
+
@dataclass
class Cmd:
type_: CmdType
@@ -67,38 +69,42 @@ def angle_betwen(from_: Pose2D, to: Pose2D) -> float:
# Only forward. Russuian go! Russian go!
x = to.x - from_.x
y = to.y - from_.y
- return math.atan2(y, x)
+ return math.atan2(y, x)
def approach_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
- go_pose = robot_info.ball_pose # copy ?
+ go_pose = robot_info.ball_pose # copy ?
goal_centre = Pose2D()
- goal_centre.x = 0 #FIELD_WIDE / 2
+ goal_centre.x = 0 # FIELD_WIDE / 2
goal_centre.y = FIELD_LENGTH / 2
goal_centre.theta = 0
# FIELD_WIDE / 2, FIELD_LENGTH / 2, 0)
- go_pose.theta = angle_betwen(
- robot_info.robot_pose, goal_centre
- )
+ go_pose.theta = angle_betwen(robot_info.robot_pose, goal_centre)
robot_info.goal_pose = go_pose
return Cmd(1, go_pose)
-def kick_action(robot_info) -> tp.Optional[Cmd]:
+def kick_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
# def calculate leg for kick
+ dx = robot_info.ball_pose.x - robot_info.robot_pose.x
+ dy = robot_info.ball_pose.y - robot_info.robot_pose.y
+ theta = math.atan2(dy, dx)
+ if theta > robot_info.robot_pose.theta:
+ return Cmd(2, KICK_LEFT)
+ else:
+ return Cmd(2, KICK_LEFT)
- return Cmd(2, KICK_LEFT)
-
-def stand_up_action():
+def stand_up_action(robot_info:RobotInfo):
return Cmd(2, STAND_UP)
-def wait_action(robot_info) -> tp.Optional[Cmd]:
+def wait_action(robot_info:RobotInfo) -> tp.Optional[Cmd]:
return None
-def go_home_action(robot_info) -> tp.Optional[Cmd]:
+def go_home_action(robot_info:RobotInfo) -> tp.Optional[Cmd]:
+ # approach to home position
return None
@@ -106,100 +112,175 @@ attacker_state_actions = {
RobotState.WAIT: wait_action,
RobotState.APPROACH: approach_action,
RobotState.KICK: kick_action,
- RobotState.STAND_UP: stand_up_action,
+ RobotState.FALL: stand_up_action,
RobotState.GO_HOME: go_home_action,
}
distance_epsilon = 0.05
-angle_epsilon_rad = 0.35 # 20 degree
+angle_epsilon_rad = 0.35 # 20 degree
+
-# Other metric ?
-def is_ball_near_to_robot(robot_info:RobotInfo):
- print (f"Goal is {robot_info.goal_pose}")
+# Other metric ?
+def is_ball_near_to_robot(robot_info: RobotInfo):
+ print(f"Goal is {robot_info.goal_pose}")
print(f"Robot pose is {robot_info.robot_pose}")
- return abs(robot_info.goal_pose.x - robot_info.robot_pose.x) < distance_epsilon and \
- abs(robot_info.goal_pose.y - robot_info.robot_pose.y) < distance_epsilon and \
- abs(robot_info.goal_pose.theta - robot_info.robot_pose.theta) < angle_epsilon_rad
-
-def is_robot_at_home():
+ return (
+ abs(robot_info.goal_pose.x - robot_info.robot_pose.x) < distance_epsilon
+ and abs(robot_info.goal_pose.y - robot_info.robot_pose.y) < distance_epsilon
+ and abs(robot_info.goal_pose.theta - robot_info.robot_pose.theta)
+ < angle_epsilon_rad
+ )
+
+
+def is_robot_at_home(robot_info):
return True
+
@dataclass
-class StateDict():
+class StateDict:
states: tuple[RobotState]
state_transitions: dict[RobotState, Transition]
+
attacker_state_transitions = {
- RobotState.WAIT: StateDict((RobotState.GO_HOME, RobotState.APPROACH),{
- RobotState.GO_HOME: Transition(
- lambda robot_info: robot_info.game_state == GameState.START, lambda: None
- ),
- RobotState.APPROACH: Transition(
- lambda robot_info: robot_info.game_state == GameState.RUN,
- lambda: print("SET PARAM Approach On"),
- ),
- }),
- RobotState.APPROACH: StateDict((RobotState.KICK, RobotState.WAIT),{
- RobotState.KICK: Transition(
- lambda robot_info: is_ball_near_to_robot(robot_info), lambda: print("SET param off")
- ),
- RobotState.WAIT: Transition(
- lambda robot_info: robot_info.game_state == GameState.PAUSED, lambda: None
- ),
- }),
- RobotState.KICK:StateDict( (RobotState.APPROACH, RobotState.WAIT),{RobotState.APPROACH: Transition(lambda robot_info: not is_ball_near_to_robot(robot_info), lambda: print("Set param on")),
- RobotState.WAIT: Transition(lambda robot_info: True, lambda: None)}),
- RobotState.GO_HOME:StateDict((RobotState.WAIT), {RobotState.WAIT: Transition(lambda robot_info: is_robot_at_home(robot_info),lambda: None)}),
+ RobotState.WAIT: StateDict(
+ (RobotState.FALL, RobotState.GO_HOME, RobotState.APPROACH),
+ {
+ RobotState.FALL: Transition(
+ lambda robot_info: robot_info.is_fallen, lambda _: None
+ ),
+ RobotState.GO_HOME: Transition(
+ lambda robot_info: robot_info.game_state == GameState.START,
+ lambda _: None,
+ ),
+ RobotState.APPROACH: Transition(
+ lambda robot_info: robot_info.game_state == GameState.RUN,
+ lambda _: print("SET PARAM Approach On"),
+ ),
+ },
+ ),
+ RobotState.APPROACH: StateDict(
+ (RobotState.FALL, RobotState.KICK, RobotState.WAIT),
+ {
+ RobotState.FALL: Transition(
+ lambda robot_info: robot_info.is_fallen, lambda _: None
+ ),
+ RobotState.KICK: Transition(
+ lambda robot_info: is_ball_near_to_robot(robot_info),
+ lambda _: print("SET param off"),
+ ),
+ RobotState.WAIT: Transition(
+ lambda robot_info: robot_info.game_state == GameState.PAUSED,
+ lambda _: None,
+ ),
+ },
+ ),
+ RobotState.KICK: StateDict(
+ (RobotState.FALL, RobotState.APPROACH, RobotState.WAIT),
+ {
+ RobotState.FALL: Transition(
+ lambda robot_info: robot_info.is_fallen, lambda _: None
+ ),
+ RobotState.APPROACH: Transition(
+ lambda robot_info: not is_ball_near_to_robot(robot_info),
+ lambda _: print("Set param on"),
+ ),
+ RobotState.WAIT: Transition(lambda robot_info: True, lambda _: None),
+ },
+ ),
+ RobotState.GO_HOME: StateDict(
+ (RobotState.FALL, RobotState.WAIT),
+ {
+ RobotState.FALL: Transition(
+ lambda robot_info: robot_info.is_fallen, lambda _: None
+ ),
+ RobotState.WAIT: Transition(
+ lambda robot_info: is_robot_at_home(robot_info), lambda _: None
+ ),
+ },
+ ),
+ RobotState.FALL: StateDict(
+ (RobotState.WAIT,),
+ {
+ RobotState.WAIT: Transition(
+ lambda robot_info: not robot_info.is_fallen, lambda _: None
+ )
+ },
+ ),
}
+
class Robot(object):
def __init__(
self,
role: Role,
- behaviour_dict: dict[RobotState, dict[StateDict]],
- action_dict: dict[RobotState, tp.Callable],
- start_robot_info
+ state_transition_dict: dict[RobotState, StateDict],
+ state_action_dict: dict[RobotState, tp.Callable],
+ start_robot_info,
):
- self.pose:Pose2D
+ self.pose: Pose2D
self.role = role
- self.goal_pose:Pose2D
+ self.goal_pose: Pose2D
self.state = RobotState.WAIT
- self.action_dict = action_dict
- self.action = self.action_dict[self.state]
- self.behaviour_dict = behaviour_dict
+ self.state_action_dict = state_action_dict
+ self.action = self.state_action_dict[self.state]
+ self.state_transition_dict = state_transition_dict
self.robot_info = start_robot_info
+ self.cmd = None
def set_robot_info(self, robot_info):
self.robot_info = robot_info
+
def update_pose(self, pose: Pose2D) -> None:
self.robot_info.robot_pose = pose
-####ALRAM update goal pose
+
+ ####ALARM update goal pose
def get_pose(self) -> Pose2D:
return self.robot_info.robot_pose
+ def get_cmd(self) -> tp.Optional[Cmd]:
+ self.tmp = self.cmd
+ self.cmd = None
+ return self.tmp
+
+ def set_fallen(self, is_fallen):
+ self.robot_info.is_fallen = is_fallen
+
def set_ball_pose(self, ball_pose: Pose2D):
self.robot_info.ball_pose = ball_pose
+
def set_game_state(self, game_state: GameState):
self.robot_info.game_state = game_state
+
def step(self) -> None:
- for state in self.behaviour_dict[self.state].states:
- if (self.behaviour_dict[self.state].state_transitions[state].condition(self.robot_info)):
+ # print(f"dict: {self.state_transition_dict}")
+ # print(f"\n for state {self.state}: {self.state_transition_dict[self.state]}\n\n\n")
+ for state in self.state_transition_dict[self.state].states:
+ if (
+ self.state_transition_dict[self.state]
+ .state_transitions[state]
+ .condition(self.robot_info)
+ ):
print(state)
- self.behaviour_dict[self.state].state_transitions[state].action()#self.robot_info)
- self.action = self.action_dict[state]
+ self.state_transition_dict[self.state].state_transitions[state].action(
+ self.robot_info
+ )
+ self.action = self.state_action_dict[state]
self.state = state
break
- self.action(self.robot_info)
+ cmd = self.action(self.robot_info)
+ if (cmd is not None):
+ self.cmd = cmd
def change_state(self, state: RobotState) -> None:
- if self.state not in self.behaviour_dict:
+ if self.state not in self.state_transition_dict:
print(f"Current state is a terminal state: {self.state}")
return
- if state not in self.behaviour_dict[state]:
+ if state not in self.state_transition_dict[state]:
print(f"Has no transition from {self.state} to {state}")
return
- self.behaviour_dict[self.state][state]
+ self.state_transition_dict[self.state][state]
# HUETA!
- self.action = self.behaviour_dict[self.state][state].action
+ self.action = self.state_transition_dict[self.state][state].action
return
# def do_action(self):
@@ -207,46 +288,66 @@ class Robot(object):
def reset(self) -> None:
self.state = RobotState.WAIT
- self.action = self.action_dict[self.state]
+ self.action = self.state_action_dict[self.state]
+
action_dict = {}
behaviour_dict = {}
+
+
@dataclass
class RobotTopicConfig:
robot_name: str
localization_topic: str # "/robot_" + robot_id + "/... "
low_level_topic: str # "/robot_" + robot_id + "/..."
approach_topic: str # "/robot_" + robot_id + "/..."
-
+ fallen_topic: str
class RobotFabrica:
def __init__(self, config):
self.config = config
self.robot_configs = []
self.robots = dict()
self.init_robots()
+
def init_robots(self):
for robot_id in self.config["robots"]:
+ robot_name = "robot_" + robot_id
self.robot_configs.append(
RobotTopicConfig(
- "robot_" + robot_id,
- "idk", "idk", "idk"
+ robot_name,
+ robot_name + "/filtered_pose",
+ robot_name + "/cmd", robot_name + "/plan_goal",
+ robot_name + "/fallen"
)
)
role_set = {role.value for role in Role}
- if ("role" in self.config["robots"] and self.config["robots"]["role"] in role_set):
+ if (
+ "role" in self.config["robots"]
+ and self.config["robots"]["role"] in role_set
+ ):
self.robots["robot_" + robot_id] = Robot(
- Role(self.config["robots"]["role"]), behaviour_dict=behaviour_dict, action_dict=action_dict,
+ Role(self.config["robots"]["role"]),
+ behaviour_dict=behaviour_dict,
+ action_dict=action_dict,
)
else:
- default_robot_info = RobotInfo(Pose2D(), Pose2D(), GameState.START, Role.ATTACKER, Pose2D())
+ home_pose = Pose2D()
+ home_pose.y = -0.5
+ home_pose.x = -0.5 + 0.5 * int(robot_id)
+ home_pose.theta = 1.57
+ default_robot_info = RobotInfo(
+ Pose2D(), Pose2D(), GameState.START, Role.ATTACKER, Pose2D(), home_pose, False
+ )
self.robots["robot_" + robot_id] = Robot(
- Role.ATTACKER, attacker_state_transitions, attacker_state_actions,
- default_robot_info
+ Role.ATTACKER,
+ attacker_state_transitions,
+ attacker_state_actions,
+ default_robot_info,
)
-
def get_robots(self) -> dict[str, Robot]:
return self.robots
+
def get_robots_configs(self) -> list[RobotTopicConfig]:
return self.robot_configs
@@ -255,11 +356,11 @@ class Strategy(object):
def __init__(self, fabrica: RobotFabrica) -> None:
self.fabrica = fabrica
self.robots = self.fabrica.get_robots()
- self.ball_pose:Pose2D # Pose2D x, y
+ self.ball_pose: Pose2D # Pose2D x, y
self.game_state = GameState.START
def set_robot_pose(self, name: str, pose: Pose2D):
- # try
+ # try
self.robots[name].update_pose(pose)
def set_ball_pose(self, pose: Pose2D):
@@ -270,19 +371,12 @@ class Strategy(object):
self.game_state = state
for robot in self.robots.values():
robot.set_game_state(state)
-
+ def set_fallen(self, name:str , is_fallen:bool):
+ self.robots[name].set_fallen(is_fallen)
+ pass
def step(self):
-
for robot in self.robots.values():
robot.step()
- def is_have_cmd(self, name: str) -> bool:
- # try
- self.robots[name]
-
def get_cmd(self, name) -> tp.Optional[Cmd]:
- if (self.robots[name].is_have_cmd()):
- return self.robots[name]
- return None
-
-
+ return self.robots[name].get_cmd()
diff --git a/src/strategy/strategy/strategy_node.py b/src/strategy/strategy/strategy_node.py
index f7b6dfd..97cd4da 100644
--- a/src/strategy/strategy/strategy_node.py
+++ b/src/strategy/strategy/strategy_node.py
@@ -1,6 +1,6 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, Pose2D, TransformStamped
+from geometry_msgs.msg import Pose, PointStamped, PoseStamped, PoseWithCovarianceStamped, Pose2D, TransformStamped
from sensor_msgs.msg import CameraInfo
from std_msgs.msg import Int32
from soccer_vision_2d_msgs.msg import BallArray, Ball
@@ -24,7 +24,16 @@ import paho.mqtt.client as mqtt
import numpy as np
from dataclasses import dataclass
from solo_robot_strategy import Strategy, Cmd, CmdType, RobotFabrica, FieldPose
+from std_msgs.msg import Int32, Bool
+robots_config = {
+ "robots":{
+ "0":
+ {
+
+ }
+ }
+}
class StrategyNode(Node):
def __init__(self):
@@ -46,19 +55,22 @@ class StrategyNode(Node):
for config in robot_configs:
# in perfect world tf but pose will be okay
self.create_subscription(
- PoseStamped,
+ PoseWithCovarianceStamped,
config.localization_topic,
lambda msg: self.strategy.set_robot_pose(
config.robot_name, self._msg_preprocess(msg)
),
self.queue_size,
)
-
+ self.create_subscription(Int32,
+ config.low_level_topic,
+ lambda msg: self.strategy.set_fallen(msg.data),
+ self.queue_size)
self.approach_pubs[config.robot_name] = self.create_publisher(
- """Approach message type""", config.approach_topic, self.queue_size
+ Pose2D, config.approach_topic, self.queue_size
)
self.low_lvl_pubs[config.robot_name] = self.create_publisher(
- """low_level_msg""", config.low_level_topic, self.queue_size
+ Int32, config.low_level_topic, self.queue_size
)
def step(self):
@@ -66,27 +78,33 @@ class StrategyNode(Node):
for name in self.robot_publishers:
if self.strategy.is_have_cmd(name):
cmd = self.strategy.get_cmd()
- self.send_cmd(cmd)
+ if cmd is not None:
+ self.send_cmd(cmd)
def send_cmd(self, robot_name, cmd: Cmd):
if cmd.type_ == CmdType.GO:
- msg = self._create_approach_msg("""What inside""")
+ msg = self._create_approach_msg(cmd)
self.approach_pubs[robot_name].publish(msg)
elif cmd.type_ == CmdType.KICK:
# Need stop approach
# then send kick command
- msg = self._create_low_lvl_msg("""What inside""")
+ msg = self._create_low_lvl_msg(cmd)
self.low_lvl_pubs[robot_name].publish(msg)
- def _create_low_lvl_msg(robot_name):
- # We hasnot kick api right now
- pass
+ def _create_low_lvl_msg(cmd:Cmd)->Int32:
+ msg = Int32()
+ msg.data = cmd.data
+ return msg
- def _create_approach_msg(cmd: Cmd) -> PoseStamped:
- pass
+ def _create_approach_msg(cmd: Cmd) -> Pose2D:
+ msg = Pose2D()
+ msg.x = cmd.data.x
+ msg.y = cmd.data.y
+ msg.theta = cmd.data.theta
+ return msg # what is it
def _msg_preprocess(self, msg: Pose2D) -> Pose2D:
- return Pose2D
+ return msg
def main(args=None):
diff --git a/src/strategy/strategy/strategy_test.py b/src/strategy/strategy/strategy_test.py
index 5b5004c..b575de2 100644
--- a/src/strategy/strategy/strategy_test.py
+++ b/src/strategy/strategy/strategy_test.py
@@ -15,7 +15,7 @@ solo_config = {
}
}
-def test_strategy():
+def test_dummy_strategy():
fabrica = RobotFabrica(solo_config)
strategy = Strategy(fabrica)
strategy.change_game_state(GameState.RUN)
@@ -24,7 +24,6 @@ def test_strategy():
strategy.step()
strategy.set_ball_pose( Pose2D(0.,1., 0.))
strategy.step()
- print(strategy.robots["robot_0"].state)
assert (strategy.robots["robot_0"].state == RobotState.APPROACH)
strategy.set_robot_pose("robot_0", Pose2D(0.0, 1.00001, 1.57))
strategy.step()
@@ -32,3 +31,39 @@ def test_strategy():
assert (strategy.robots["robot_0"].state == RobotState.KICK)
strategy.step()
assert (strategy.robots["robot_0"].state == RobotState.WAIT)
+
+def test_full_strategy():
+ fabrica = RobotFabrica(solo_config)
+ strategy = Strategy(fabrica)
+ strategy.change_game_state(GameState.START)
+ strategy.set_robot_pose("robot_0", Pose2D(0., 0.0, 0.))
+ strategy.step()
+ strategy.step()
+ # assert(strategy.robots["robot_0"].state == RobotState.GO_HOME)
+ # strategy.set_robot_pose("robot_0", Pose2D(-0.5, -0.5, 1.57))
+ # strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ strategy.change_game_state(GameState.RUN)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ strategy.set_robot_pose("robot_0", Pose2D(-0.5, 1.0, 1.57))
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ strategy.set_fallen("robot_0", True)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.FALL)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.FALL)
+ strategy.set_fallen("robot_0", False)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ strategy.change_game_state(GameState.PAUSED)
+ strategy.step()
+ assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ strategy.change_game_state(GameState.START)
+ # assert(strategy.robots["robot_0"].state == RobotState.GO_HOME)
+
+
+
\ No newline at end of file
--
GitLab
From 3aa0bb35571d2dd4cdc99682e04e6f3f8551706a Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Sun, 21 Apr 2024 07:20:51 +0300
Subject: [PATCH 59/64] Fix launch for pic to real
Add tf from calibration node to launch
---
src/pic_to_real/launch/bal_to_real.launch.py | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
diff --git a/src/pic_to_real/launch/bal_to_real.launch.py b/src/pic_to_real/launch/bal_to_real.launch.py
index 4922f64..ed2273b 100644
--- a/src/pic_to_real/launch/bal_to_real.launch.py
+++ b/src/pic_to_real/launch/bal_to_real.launch.py
@@ -5,6 +5,9 @@ import os
from ament_index_python.packages import get_package_share_directory
import yaml
import json
+from launch.actions import IncludeLaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
def load_yaml_file(file_path):
@@ -137,5 +140,9 @@ def generate_launch_description():
)
)
- ld.add_action(tf_node)
+ #ld.add_action(tf_node)
+ ld.add_action(IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [FindPackageShare("calibration_node"), "/launch", "/tf.launch.py"]
+ )))
return ld
--
GitLab
From bad5004516752be09c409becefd3e6da566c402a Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Sun, 21 Apr 2024 09:27:24 +0300
Subject: [PATCH 60/64] Simple approach to launch in the game
---
.../config/static_transforms.yaml | 30 +-
.../launch/renamed_multi.launch.py | 58 +++
.../localisation/localisation_node.py | 146 +++++---
src/proxy/proxy/Proxy.py | 2 +-
src/simple_approach/LICENSE | 202 +++++++++++
.../config/connections_config.json | 11 +
src/simple_approach/launch/approach.launch.py | 80 +++++
src/simple_approach/package.xml | 22 ++
src/simple_approach/resource/simple_approach | 0
src/simple_approach/setup.cfg | 4 +
src/simple_approach/setup.py | 30 ++
.../simple_approach/SimpleApproach.py | 338 ++++++++++++++++++
.../simple_approach/__init__.py | 0
src/simple_approach/test/test_copyright.py | 25 ++
src/simple_approach/test/test_flake8.py | 25 ++
src/simple_approach/test/test_pep257.py | 23 ++
static_transforms.yaml | 25 --
17 files changed, 939 insertions(+), 82 deletions(-)
create mode 100644 src/localisation/launch/renamed_multi.launch.py
create mode 100644 src/simple_approach/LICENSE
create mode 100644 src/simple_approach/config/connections_config.json
create mode 100644 src/simple_approach/launch/approach.launch.py
create mode 100644 src/simple_approach/package.xml
create mode 100644 src/simple_approach/resource/simple_approach
create mode 100644 src/simple_approach/setup.cfg
create mode 100644 src/simple_approach/setup.py
create mode 100644 src/simple_approach/simple_approach/SimpleApproach.py
create mode 100644 src/simple_approach/simple_approach/__init__.py
create mode 100644 src/simple_approach/test/test_copyright.py
create mode 100644 src/simple_approach/test/test_flake8.py
create mode 100644 src/simple_approach/test/test_pep257.py
delete mode 100644 static_transforms.yaml
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index df35b8c..d5dbcaf 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -3,23 +3,23 @@ static_transforms:
name: camera1
parent_frame_id: world
rotation:
- - 0.9040756200965469
- - -0.04513508982503454
- - 0.03903623145495232
- - -0.4231858568611735
+ - -0.5845781463562191
+ - 0.6842680837945385
+ - -0.33536892934797113
+ - 0.27851976865363226
translation:
- - 0.2377450563929882
- - -3.954531077024356
- - 2.896648104324571
+ - -4.102433424332044
+ - -0.33554078957982525
+ - 2.8603490712788853
- child_frame_id: camera2
name: camera2
parent_frame_id: world
- rotation:
- - 0.023345895194435935
- - 0.9217664604335917
- - -0.3839850764247843
- - -0.0485491779575187
+ rotation:
+ - 0.6689080181465836
+ - 0.6427557356753467
+ - -0.2321647577020393
+ - -0.29244940211395004
translation:
- - 0.035852423609577216
- - 4.437697599274499
- - 2.9021133125723435
+ - 4.394749353486249
+ - 0.018674504462683372
+ - 3.04352171951904
\ No newline at end of file
diff --git a/src/localisation/launch/renamed_multi.launch.py b/src/localisation/launch/renamed_multi.launch.py
new file mode 100644
index 0000000..1e59a2d
--- /dev/null
+++ b/src/localisation/launch/renamed_multi.launch.py
@@ -0,0 +1,58 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ localisation_node_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_0/filtered_pose'),
+ ("/fallen","/robot_0/fallen"),
+ ("/filtered_pose_3d_robot_0", 'filt_3d')],
+ )
+
+ localisation_node_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose'),
+ ("/fallen","/robot_1/fallen")],
+ )
+
+ localisation_node_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose'),
+ ("/fallen","/robot_2/fallen")],
+ )
+
+ localisation_node_op_0 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose'),
+ ("/fallen","/opponent_robot_0/fallen")],
+ )
+
+ localisation_node_op_1 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose'),
+ ("/fallen","/opponent_robot_1/fallen")],
+ )
+
+ localisation_node_op_2 = Node(
+ package="localisation",
+ executable="localisation_node",
+ remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose'),
+ ("/fallen","/opponent_robot_2/fallen")],
+ )
+
+ ld.add_action(localisation_node_0)
+ ld.add_action(localisation_node_1)
+ ld.add_action(localisation_node_2)
+ ld.add_action(localisation_node_op_0)
+ ld.add_action(localisation_node_op_1)
+ ld.add_action(localisation_node_op_2)
+
+ return ld
+
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index d1a1d96..4cac4c5 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,5 +1,3 @@
-from ros2_aruco_interfaces.msg import ArucoMarkers
-
from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
from std_msgs.msg import Bool
@@ -48,14 +46,13 @@ from collections import deque
# pose.orientation.w = quaternion[3]
# return pose
-class Pose():
- def __init__(self, x, y, z, roll, pitch, yaw):
+class PoseSinCos():
+ def __init__(self, x, y, z, sin, cos):
self.x = x
self.y = y
self.z = z
- self.roll = roll
- self.pitch = pitch
- self.yaw = yaw
+ self.sin = sin
+ self.cos = cos
class AlphaFilter():
def __init__(self):
@@ -74,14 +71,13 @@ class AlphaFilter():
def get(self):
weight_sum = 0.0
## find yaw mid
- mid = Pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ mid = PoseSinCos(0.0, 0.0, 0.0, 0.0, 0.0)
for i, elem in enumerate(self.buffer):
mid.x += elem.x * self.alpha ** (len(self.buffer) - i)
mid.y += elem.y * self.alpha ** (len(self.buffer) - i)
mid.z += elem.z * self.alpha ** (len(self.buffer) - i)
- mid.roll += elem.roll * self.alpha ** (len(self.buffer) - i)
- mid.pitch += elem.pitch * self.alpha ** (len(self.buffer) - i)
- mid.yaw += elem.yaw * self.alpha ** (len(self.buffer) - i)
+ mid.sin += elem.sin * self.alpha ** (len(self.buffer) - i)
+ mid.cos += elem.cos * self.alpha ** (len(self.buffer) - i)
weight_sum += self.alpha ** (len(self.buffer) - i ) #TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
if len(self.buffer) == 0:
@@ -89,9 +85,8 @@ class AlphaFilter():
mid.x /= weight_sum
mid.y /= weight_sum
mid.z /= weight_sum
- mid.roll /= weight_sum
- mid.pitch /= weight_sum
- mid.yaw /= weight_sum
+ mid.sin /= weight_sum
+ mid.cos /= weight_sum
return mid
class LocalisationNode(Node):
@@ -100,8 +95,9 @@ class LocalisationNode(Node):
self.debug = debug
self.robot_id = 0
self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
- self.filtered_pose_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose", 10)
- self.filtered_stamped_pose_pub= self.create_publisher(PoseWithCovarianceStamped, "/stamped_filtered_pose", 10)
+ self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose", 10)
+ # self.filtered_pose_cov_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose_with_cov", 10)
+ # self.filtered_stamped_pose_pub= self.create_publisher(PoseWithCovarianceStamped, "/stamped_filtered_pose_with_cov", 10)
self.on_ground_publisher = self.create_publisher(Bool, "fallen", 10)
if self.debug:
self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
@@ -113,7 +109,7 @@ class LocalisationNode(Node):
self.current_robot_poses = {}
self.aruco_observed_poses = {}
self.robot_pose = AlphaFilter()
- self.unfiltered_last_observed_robot_pos = Pose(0.0, 0.0, 0.25, 0.0, 0.0, 0.0)
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(0.0, 0.0, 0.25, 0.0, 0.0)
self.robot_pose.update(self.unfiltered_last_observed_robot_pos)
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
@@ -132,10 +128,26 @@ class LocalisationNode(Node):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
- quaternion = msg.pose.pose.orientation
- roll, pitch, yaw = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
- self.unfiltered_last_observed_robot_pos = Pose(x, y, z, roll, pitch, yaw)
- self.robot_pose.update(self.unfiltered_last_observed_robot_pos)
+ sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
+ # print(sin, cos)
+ pose_sin_cos = PoseSinCos(x, y, z, sin, cos)
+ self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, z, sin, cos)
+ self.robot_pose.update(pose_sin_cos)
+
+ if self.debug:
+ # Extract quaternion from PoseStamped message
+ quaternion = msg.pose.pose.orientation
+ # Convert quaternion to RPY
+ rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
+ roll, pitch, yaw = rpy
+
+ rpy_msg = RPY()
+ rpy_msg.roll, rpy_msg.pitch, rpy_msg.yaw = roll, pitch, yaw
+ self.rpy_pub.publish(rpy_msg)
+
+ rpy_vector3_msg = Vector3()
+ rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
+ self.rpy_vector3_pub.publish(rpy_vector3_msg)
def get_x_y_yaw_pose_filtered(self):
@@ -157,26 +169,40 @@ class LocalisationNode(Node):
return pose
def timer_callback(self):
- # if self.debug:
- # unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
- # self.unfiltered_pose_pub.publish(unfilt_msg)
+ if self.debug:
+ unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
+ self.unfiltered_pose_pub.publish(unfilt_msg)
- pose = self.robot_pose.get()
- filtered_msg = self.convert_to_3d_with_covariance(pose)
+ pose_sin_cos = self.robot_pose.get()
+
+ x = pose_sin_cos.x
+ y = pose_sin_cos.y
+ z = pose_sin_cos.z
+ theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
+
+ theta -= math.pi / 2.0
+ if theta < -math.pi:
+ theta += 2 * math.pi
+
+ filtered_msg = Pose2D()
+ filtered_msg.x = x
+ filtered_msg.y = y
+ filtered_msg.theta = theta
+ # filtered_msg = self.convert_to_3d_with_covariance(x, y, z, theta)
self.filtered_pose_pub.publish(filtered_msg)
- filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(pose)
- self.filtered_stamped_pose_pub.publish(filtered_stamped_msg)
+ # filtered_stamped_msg = self.convert_to_3d_with_covariance_stamped(x, y, z, theta)
+ # self.filtered_stamped_pose_pub.publish(filtered_stamped_msg)
fallen_msg = Bool()
- fallen_msg.data = self.fallen(pose.z)
+ fallen_msg.data = self.is_on_ground(z)
self.on_ground_publisher.publish(fallen_msg)
- # if self.debug:
- # msg_3d = self.convert_to_3d(filt_msg)
- # self.filtered_pose_3d_pub.publish(msg_3d)
+ if self.debug:
+ msg_3d = self.convert_to_3d(filtered_msg)
+ self.filtered_pose_3d_pub.publish(msg_3d)
- def fallen(self, z):
+ def is_on_ground(self, z):
if z < self.lowest_standing_z:
return True
else:
@@ -190,7 +216,7 @@ class LocalisationNode(Node):
pose.header.frame_id = "world"
pose.pose.position.x = pose2d.x
pose.pose.position.y = pose2d.y
- pose.pose.position.z = 0.0
+ pose.pose.position.z = self.robot_pose.get().z
# Convert theta to quaternion for orientation
quaternion = tf_transformations.quaternion_from_euler(0, 0, pose2d.theta)
pose.pose.orientation.x = quaternion[0]
@@ -199,15 +225,15 @@ class LocalisationNode(Node):
pose.pose.orientation.w = quaternion[3]
return pose
- def convert_to_3d_with_covariance(self, pose):
+ def convert_to_3d_with_covariance(self, x, y, z, theta):
# Create a Pose message
p = PoseWithCovariance()
- p.pose.position.x = pose.x
- p.pose.position.y = pose.y
- p.pose.position.z = pose.z
+ p.pose.position.x = x
+ p.pose.position.y = y
+ p.pose.position.z = z
# Convert theta to quaternion for orientation
- quaternion = tf_transformations.quaternion_from_euler(pose.roll, pose.pitch, pose.yaw)
+ quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
@@ -216,16 +242,54 @@ class LocalisationNode(Node):
p.covariance = self.covariance
return p
- def convert_to_3d_with_covariance_stamped(self, pose):
+ def convert_to_3d_with_covariance_stamped(self, x, y, z, theta):
# Create a Pose message
p = PoseWithCovarianceStamped()
p.header.stamp = self.get_clock().now().to_msg()
p.header.frame_id = "world"
- p.pose = self.convert_to_3d_with_covariance(pose)
+ p.pose.pose.position.x = x
+ p.pose.pose.position.y = y
+ p.pose.pose.position.z = z
+ # Convert theta to quaternion for orientation
+ quaternion = tf_transformations.quaternion_from_euler(0, 0, theta)
+ p.pose.pose.orientation.x = quaternion[0]
+ p.pose.pose.orientation.y = quaternion[1]
+ p.pose.pose.orientation.z = quaternion[2]
+ p.pose.pose.orientation.w = quaternion[3]
+
+ p.pose.covariance = self.covariance
return p
+
+ def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
+ x_ax = [1.0, 0.0, 0.0]
+ quat_arr = [
+ pose.pose.pose.orientation.w,
+ pose.pose.pose.orientation.x,
+ pose.pose.pose.orientation.y,
+ pose.pose.pose.orientation.z
+ ]
+
+ rotated_x = Quaternion(quat_arr).rotate(x_ax)
+ # print(rotated_x)
+ # TODO check normalisation
+ return rotated_x[1], rotated_x[0]
+
+ def get_rpy_from_quaternion(self, pose: Pose):
+ x_ax = [1.0, 0.0, 0.0]
+ quat_arr = [
+ pose.pose.pose.orientation.w,
+ pose.pose.pose.orientation.x,
+ pose.pose.pose.orientation.y,
+ pose.pose.pose.orientation.z
+ ]
+
+ rotated_x = Quaternion(quat_arr).rotate(x_ax)
+ # print(rotated_x)
+ # TODO check normalisation
+ return rotated_x[1], rotated_x[0]
def main(args=None):
rclpy.init(args=args)
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index 9ea91db..873f00f 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -74,7 +74,7 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{int(msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
+ self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{int(-msg.linear.y)}|{int(-msg.linear.x)}\n".encode())
await self.writer.drain()
self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
diff --git a/src/simple_approach/LICENSE b/src/simple_approach/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/src/simple_approach/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/src/simple_approach/config/connections_config.json b/src/simple_approach/config/connections_config.json
new file mode 100644
index 0000000..3ed7c39
--- /dev/null
+++ b/src/simple_approach/config/connections_config.json
@@ -0,0 +1,11 @@
+{
+ "connections": {
+
+ "0": {
+ "device": "ttyUSB0",
+ "robot_name": "ROKI-0060",
+ "port": "1969"
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/src/simple_approach/launch/approach.launch.py b/src/simple_approach/launch/approach.launch.py
new file mode 100644
index 0000000..bf7a25d
--- /dev/null
+++ b/src/simple_approach/launch/approach.launch.py
@@ -0,0 +1,80 @@
+import json
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+from launch_ros.actions import Node
+
+
+class LaunchUnitsCreator:
+
+ def __init__(self, connections_config_path: str) -> None:
+ self._config = self._read_connections_config(connections_config_path)
+
+ def create_launch(self) -> LaunchDescription:
+ return LaunchDescription([
+ *self._create_robo_pult_executors(),
+ *self._create_proxy_nodes(),
+ *self._create_approach_nodes(),
+ # *self._create_odometry_estimator_node(),
+ ])
+
+ @staticmethod
+ def _read_connections_config(connections_config_path: str) -> dict:
+ with open(connections_config_path) as connections_config_json:
+ conf = json.load(connections_config_json, parse_float=lambda x: str(x), parse_int=lambda x:str(x))
+
+ return conf
+
+ def _create_robo_pult_executors(self) -> list[ExecuteProcess]:
+ return [ExecuteProcess(cmd=[
+ 'sudo',
+ './roboLinuxPult/roboLinuxPult.sh',
+ connection["port"],
+ connection["device"],
+ connection["robot_name"][5:], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
+ # found in Taiwan April 2024
+ ]) for connection in self._config["connections"].values()]
+
+ def _create_proxy_nodes(self) -> list[Node]:
+ print(self._config["connections"])
+ return [
+ Node(
+ package ="proxy",
+ executable ="proxy",
+ namespace =f"robot_{id}",
+ parameters = [
+ {"port": connection["port"] },
+ {"robot_name": connection["robot_name"] },
+ ],
+ ) for id, connection in self._config["connections"].items()
+ ]
+
+ def _create_approach_nodes(self) -> list[Node]:
+ return [
+ Node(
+ package ="simple_approach",
+ executable ="simple_approach",
+ namespace =f"robot_{id}",
+ ) for id in self._config["connections"].keys()
+ ]
+
+ def _create_odometry_estimator_node(self) -> list[Node]:
+ return [
+ Node(
+ package="odometry_estimator",
+ executable="odometry_estimator",
+ namespace=f"robot_{id}",
+ ) for id in self._config["connections"].keys()
+ ]
+
+def generate_launch_description():
+
+ connections_config = os.path.join(get_package_share_directory('simple_approach'),
+ 'config',
+ "connections_config.json")
+
+ return LaunchUnitsCreator(connections_config).create_launch()
\ No newline at end of file
diff --git a/src/simple_approach/package.xml b/src/simple_approach/package.xml
new file mode 100644
index 0000000..9ba73ec
--- /dev/null
+++ b/src/simple_approach/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>simple_approach</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="root@todo.todo">root</maintainer>
+ <license>Apache-2.0</license>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <exec_depend>rclpy</exec_depend>
+ <exec_depend>geomtry_msgs</exec_depend>
+ <depend>ros2launch</depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/src/simple_approach/resource/simple_approach b/src/simple_approach/resource/simple_approach
new file mode 100644
index 0000000..e69de29
diff --git a/src/simple_approach/setup.cfg b/src/simple_approach/setup.cfg
new file mode 100644
index 0000000..819b364
--- /dev/null
+++ b/src/simple_approach/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/simple_approach
+[install]
+install_scripts=$base/lib/simple_approach
diff --git a/src/simple_approach/setup.py b/src/simple_approach/setup.py
new file mode 100644
index 0000000..2def61e
--- /dev/null
+++ b/src/simple_approach/setup.py
@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'simple_approach'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+ (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='root',
+ maintainer_email='root@todo.todo',
+ description='TODO: Package description',
+ license='Apache-2.0',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'simple_approach = simple_approach.SimpleApproach:main'
+ ],
+ },
+)
diff --git a/src/simple_approach/simple_approach/SimpleApproach.py b/src/simple_approach/simple_approach/SimpleApproach.py
new file mode 100644
index 0000000..486a296
--- /dev/null
+++ b/src/simple_approach/simple_approach/SimpleApproach.py
@@ -0,0 +1,338 @@
+from dataclasses import dataclass
+from typing import List
+from math import atan2, cos, sin, acos, asin
+
+import rclpy
+
+from rclpy.node import Node
+
+from geometry_msgs.msg import Pose2D
+from geometry_msgs.msg import Twist
+
+import numpy as np
+
+@dataclass
+class Obstacle:
+ x: float
+ y: float
+ r: float
+
+class SimpleApproach(Node):
+
+ def __init__(self) -> None:
+ super().__init__('simple_approach')
+
+ self._goal_cmd_vel_publisher = self.create_publisher(
+ Twist,
+ 'plan_cmd_vel',
+ 10
+ )
+
+ self._filtered_pose_subscriber = self.create_subscription(
+ Pose2D,
+ 'filtered_pose',
+ self._localization_callback,
+ 10
+ )
+
+ self._goal_pose_subscriber = self.create_subscription(
+ Pose2D,
+ 'plan_goal',
+ self._approach_callback,
+ 10
+ )
+
+ self.current_position = Pose2D()
+ self.obstacles = []
+
+ def _localization_callback(self, msg: Pose2D):
+ self.current_position = msg
+
+ def if_bigger(self: Obstacle, other: Obstacle, init: Pose2D):
+ l_self = (self.x - init.x) ** 2 + (self.y - init.y) ** 2
+ l_other = (other.x - init.x) ** 2 + (other.y - init.y) ** 2
+ return l_self < l_other
+
+
+ def partition(self, low, high, path_point: Pose2D):
+ pivot = self.obstacles[high]
+ i = low - 1
+
+ for j in range(low, high):
+
+ if self.if_bigger(self.obstacles[j], pivot, path_point):
+ i += 1
+ (self.obstacles[i], self.obstacles[j]) = (self.obstacles[j], self.obstacles[i])
+
+ (self.obstacles[i + 1], self.obstacles[high]) = (self.obstacles[high], self.obstacles[i + 1])
+
+ return i + 1
+
+
+ def quicksort(self, low, high, path_point: Pose2D):
+ if low < high:
+ pi = self.partition(low, high, path_point)
+ self.quicksort(low, pi - 1, path_point)
+ self.quicksort(pi + 1, high, path_point)
+
+
+ def tangent_line(self, robot: Pose2D, obstacle: Obstacle):
+ # r = r0 + p * t, where r, r0, p -- 2d vectors, t -- parameter
+ r = [obstacle.x - robot.x, obstacle.y - robot.y]
+ l_n = obstacle.r / (r[0] ** 2 + r[1] ** 2) ** 0.5
+ n = [r[1] * l_n, -r[0] * l_n]
+ r1, r2 = [r[0] + n[0], r[1] + n[1]], [r[0] - n[0], r[1] - n[1]]
+ k1, k2 = r1[1]/r1[0], r2[1]/r2[0]
+ c1, c2 = robot.y - k1 * robot.x, robot.y - k2 * robot.x
+ return [k1, c1], [k2, c2]
+
+
+ def intersection_point(self, line1: List[float], line2: List[float]):
+ return [(line2[1] - line1[1]) / (line1[0] - line2[0]),
+ line1[0] * (line2[1] - line1[1]) / (line1[0] - line2[0]) + line1[1]]
+
+
+ @staticmethod
+ def is_intersect(path_point: Pose2D, target: Pose2D, obstacle: Obstacle):
+ if path_point.x != target.x and path_point.y != target.y:
+
+ A, B, C = (
+ 1 / (target.x - path_point.x),
+ -1/(target.y - path_point.y),
+ path_point.y / (target.y - path_point.y) - path_point.x / (target.x - path_point.x)
+ )
+
+ elif path_point.x != target.x:
+ A, B, C = 0, 1, path_point.y
+
+ else:
+ A, B, C = 1, 0, path_point.x
+
+ d = abs(A * obstacle.x + B * obstacle.y + C) / (A ** 2 + B ** 2) ** 0.5
+ phi = atan2(target.y - path_point.y, target.x - path_point.x)
+
+ x = obstacle.x - d * sin(phi)
+ y = obstacle.y + d * cos(phi)
+
+ return abs(obstacle.r - d) > 0.01 and min(path_point.x, target.x) < x < max(path_point.x, target.x) and min(path_point.y, target.y) < y < max(path_point.y, target.y)
+
+
+ def rotate(self, init: Pose2D, target: Pose2D):
+ yaw_i = init.theta
+ if abs(init.x - target.x) < 0.02 and abs(init.y - target.y) < 0.02:
+ yaw_t = target.theta
+ else:
+ yaw_t = atan2(target.y - init.y, target.x - init.x) # returns value from -pi to pi
+ return yaw_t
+
+ @staticmethod
+ def is_intersect_circle(obstacle1: Obstacle, obstacle2: Obstacle):
+ return ((obstacle1.x - obstacle2.x) ** 2 + (obstacle1.y - obstacle2.y) ** 2) ** 0.5 <= obstacle1.r + obstacle2.r
+
+ # TODO: refactor the function
+ def obstacle_avoidance(self, robot: Pose2D, target: Pose2D, obstacle: Obstacle):
+ if (target.x - obstacle.x) ** 2 + (target.y - obstacle.y) ** 2 > obstacle.r ** 2 and \
+ (robot.x - obstacle.x) ** 2 + (robot.y - obstacle.y) ** 2 > obstacle.r ** 2:
+ line1_r2o, line2_r2o = self.tangent_line(robot, obstacle)
+ line1_o2t, line2_o2t = self.tangent_line(target, obstacle)
+ intersec_points = [self.intersection_point(line1_r2o, line1_o2t), self.intersection_point(line1_r2o, line2_o2t),
+ self.intersection_point(line2_r2o, line1_o2t), self.intersection_point(line2_r2o, line2_o2t)]
+ path_length = []
+ for i in range(4):
+ path_length.append(((robot.x - intersec_points[i][0]) ** 2 + (robot.y - intersec_points[i][1]) ** 2) ** 0.5 +
+ ((intersec_points[i][0] - target.x) ** 2 + (intersec_points[i][1] - target.y) ** 2) ** 0.5)
+ index_minpath = path_length.index(min(path_length))
+ x, y = intersec_points[index_minpath]
+
+ interjacent_point = Pose2D()
+ interjacent_point.x = x
+ interjacent_point.y = y
+ interjacent_point.theta = 0
+
+ yaw1 = self.rotate(robot, interjacent_point)
+ #yaw2 = rotate(interjacent_point, target)
+
+ pos1 = Pose2D()
+ pos1.x = robot.x
+ pos1.y = robot.y
+ pos1.theta = yaw1
+
+ pos2 = Pose2D()
+ pos2.x = x
+ pos2.y = y
+ pos2.theta = yaw1
+
+ return pos1, pos2
+
+ elif (target.x - obstacle.x) ** 2 + (target.y - obstacle.y) ** 2 <= obstacle.r ** 2 and \
+ (robot.x - obstacle.x) ** 2 + (robot.y - obstacle.y) ** 2 > robot.r ** 2:
+ line1, line2 = self.tangent_line(robot, obstacle)
+ intersec_points = [[target.x, line1[0] * target.x + line1[1]], [target.x, line2[0] * target.x + line2[1]]]
+ path_length = []
+ for i in range(2):
+ path_length.append((intersec_points[i][0] ** 2 + intersec_points[i][1] ** 2) ** 0.5)
+ index_minpath = path_length.index(min(path_length))
+ x, y = intersec_points[index_minpath]
+
+ interjacent_point = Pose2D()
+ interjacent_point.x = x
+ interjacent_point.y = y
+ interjacent_point.theta = 0
+
+ yaw1 = self.rotate(robot, interjacent_point)
+
+ pos1 = Pose2D()
+ pos1.x = robot.x
+ pos1.y = robot.y
+ pos1.theta = yaw1
+
+ pos2 = Pose2D()
+ pos2.x = x
+ pos2.y = y
+ pos2.theta = yaw1
+
+ return pos1, pos2
+ else:
+
+ pos1 = Pose2D()
+ pos1.x = robot.x
+ pos1.y = robot.y
+ pos1.theta = self.rotate(robot, target)
+
+ pos2 = Pose2D()
+ pos2.x = target.x
+ pos2.y = target.y
+ pos2.theta = self.rotate(robot, target)
+
+ return pos1, pos2
+
+
+ def all_obstacle_avoidance(self, path_point: Pose2D, target: Pose2D):
+ self.quicksort(self.obstacles, 0, len(self.obstacles) - 1, path_point)
+
+ added_poses = []
+
+ for i in range(len(self.obstacles)):
+ for j in range(i + 1, len(self.obstacles)):
+
+ if self.is_intersect_circle(self.obstacles[i], self.obstacles[j]):
+ self.obstacles[i] = Obstacle(0.5 * (self.bstacles[i].x + self.obstacles[j].x),
+ 0.5 * (self.obstacles[i].y + self.obstacles[j].y),
+ 0.5 * (self.obstacles[i].r + self.obstacles[j].r + ((self.obstacles[i].x - self.obstacles[j].x) ** 2 +
+ (self.obstacles[i].y - self.obstacles[j].y) ** 2) ** 0.5))
+
+ self.obstacles[j] = Obstacle(0, 0, 0)
+
+ for obstacle in self.obstacles:
+
+ if self.is_intersect(path_point, target, obstacle):
+ pos1, pos2 = self.obstacle_avoidance(path_point, target, obstacle)
+ added_poses.append(pos1)
+ added_poses.append(pos2)
+ return added_poses
+ return added_poses
+
+
+ def control_poses(self, target):
+ positions = [self.current_position]
+
+ if len(self.obstacles) != 0:
+
+ while (positions[-1].x, positions[-1].y) != (target.x, target.y):
+ new_poses = self.all_obstacle_avoidance(positions[-1], target, self.obstacles)
+
+ if len(new_poses) != 0:
+ pos1, pos2 = new_poses[0], new_poses[1]
+ positions.append(pos1)
+ positions.append(pos2)
+
+ else:
+ new_yaw = self.rotate(positions[-1], target)
+
+ yaw_position = Pose2D()
+ yaw_position.x = positions[-1].x
+ yaw_position.y = positions[-1].y
+ yaw_position.theta = new_yaw
+
+ target_position = Pose2D()
+ target_position = target.x
+ target_position = target.y
+ target_position = new_yaw
+
+ positions.append(yaw_position)
+ positions.append(target_position)
+
+ else:
+ new_position = Pose2D()
+ new_position.x = target.x
+ new_position.y = target.y
+ new_position.theta = self.rotate(positions[-1], target)
+
+ positions.append(new_position)
+
+ return positions
+
+
+ def _approach_callback(self, msg: Pose2D):
+ positions = self.control_poses(msg)
+ print("HELLO")
+ # TODO: fit constants and create params
+ v_linear = 50.0 if len(positions) > 1 else 0.0
+ omega_z = 50.0 if len(positions) > 1 else 0.0
+
+
+ if len(positions) > 1 and abs(positions[0].theta - positions[1].theta) < 0.15:
+ omega_z = 0.0
+ if len(positions) > 1 and ((positions[0].x - positions[1].x) ** 2 + (positions[0].y - positions[1].y) ** 2) ** 0.5 < 0.02:
+ v_linear = 0.0
+
+ robot_coords = np.array([
+ [self.current_position.x],
+ [self.current_position.y]
+ ])
+
+ goal_coords = np.array([
+ [positions[1].x],
+ [positions[1].y]
+ ])
+
+ M = np.array([
+ [np.cos(self.current_position.theta), -np.sin(self.current_position.theta)],
+ [np.sin(self.current_position.theta), np.cos(self.current_position.theta)]
+ ])
+
+ M_inv = M.transpose()
+
+ coords_ = M_inv@(goal_coords - robot_coords)
+ phi = atan2(coords_[1][0], coords_[0][0])
+
+ self.get_logger().info(f"PHI: {phi}")
+
+ twist = Twist()
+
+ twist.linear.x = v_linear * cos(phi)
+ twist.linear.y = v_linear * sin(phi)
+ twist.linear.z = 0.0
+
+ twist.angular.x = 0.0
+ twist.angular.y = 0.0
+ twist.angular.z = omega_z * np.sign(positions[0].theta - positions[1].theta) if len(positions) > 1 else 0.0
+
+ self._goal_cmd_vel_publisher.publish(twist)
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ simple_approach = SimpleApproach()
+
+ rclpy.spin(simple_approach)
+
+ # Destroy the node explicitly
+ # (optional - otherwise it will be done automatically
+ # when the garbage collector destroys the node object)
+ simple_approach.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/simple_approach/simple_approach/__init__.py b/src/simple_approach/simple_approach/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/simple_approach/test/test_copyright.py b/src/simple_approach/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/simple_approach/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/simple_approach/test/test_flake8.py b/src/simple_approach/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/simple_approach/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/simple_approach/test/test_pep257.py b/src/simple_approach/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/simple_approach/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/static_transforms.yaml b/static_transforms.yaml
deleted file mode 100644
index df35b8c..0000000
--- a/static_transforms.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-static_transforms:
-- child_frame_id: camera1
- name: camera1
- parent_frame_id: world
- rotation:
- - 0.9040756200965469
- - -0.04513508982503454
- - 0.03903623145495232
- - -0.4231858568611735
- translation:
- - 0.2377450563929882
- - -3.954531077024356
- - 2.896648104324571
-- child_frame_id: camera2
- name: camera2
- parent_frame_id: world
- rotation:
- - 0.023345895194435935
- - 0.9217664604335917
- - -0.3839850764247843
- - -0.0485491779575187
- translation:
- - 0.035852423609577216
- - 4.437697599274499
- - 2.9021133125723435
--
GitLab
From f5b6b1da9944d882ef754ac2be38043a7e40aab2 Mon Sep 17 00:00:00 2001
From: Iashin Prokhor <iashin.pa@phystech.edu>
Date: Mon, 29 Apr 2024 00:23:46 +0300
Subject: [PATCH 61/64] Localization preprocessor fixes, launch vision with
approach: robot go to pose with huge position error, drift
---
src/androsot_approach_teb/config/connections_config.json | 2 +-
.../localization_preprocessor/aruco_preprocessor_node.py | 8 +++++---
src/server_main/config/robots_config.json | 2 +-
src/simple_approach/config/connections_config.json | 3 ++-
src/simple_approach/simple_approach/SimpleApproach.py | 4 ++--
5 files changed, 11 insertions(+), 8 deletions(-)
diff --git a/src/androsot_approach_teb/config/connections_config.json b/src/androsot_approach_teb/config/connections_config.json
index 9759959..4b056fa 100644
--- a/src/androsot_approach_teb/config/connections_config.json
+++ b/src/androsot_approach_teb/config/connections_config.json
@@ -3,7 +3,7 @@
"0": {
"device": "ttyUSB0",
- "robot_name": "ROKI-0067",
+ "robot_name": "ROKI-0059",
"port": "1969"
}
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 3c48e46..fa86c98 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -106,7 +106,7 @@ class ArucoPreprocessorNode(Node):
# self.current_aruco_poses[id] = pose_stamped
# Example quaternion (x, y, z, w)
r = pose_stamped.pose.orientation
- quaternion = [r.x, r.y,r.z,r.w]
+ quaternion = [r.x, r.y, r.z, r.w]
roll, pitch, yaw = euler_from_quaternion(quaternion)
rad2deg = lambda x: x / 3.14 * 180
# roll is near 180
@@ -115,6 +115,8 @@ class ArucoPreprocessorNode(Node):
# pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
# cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
# yaw - pohui
+
+ # self.get_logger().info(f"ID:{id}, ROLL: {roll}, PITCH: {pitch}, YAW: {yaw}")
cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
field_pose = self.transform_to_robot_centre(pose_stamped, id)
@@ -133,8 +135,8 @@ class ArucoPreprocessorNode(Node):
a = field_pose.pose.orientation
pose_quat = [a.w, a.x, a.y, a.z]
- itog = tf_t.quaternion_multiply(to_odom_quat_3, tf_t.quaternion_multiply(to_odom_quat_2, tf_t.quaternion_multiply(
- to_odom_quat, pose_quat # !!! ALARM SHIIIIIT
+ itog = tf_t.quaternion_multiply(to_odom_quat, tf_t.quaternion_multiply(to_odom_quat_2, tf_t.quaternion_multiply(
+ to_odom_quat_3, pose_quat # !!! ALARM SHIIIIIT
)))
print("ITOG: ", itog)
diff --git a/src/server_main/config/robots_config.json b/src/server_main/config/robots_config.json
index 19b7fd7..951a0a4 100644
--- a/src/server_main/config/robots_config.json
+++ b/src/server_main/config/robots_config.json
@@ -21,7 +21,7 @@
]
},
"2": {
- "aruco_ids": [20,22,24,26,28],
+ "aruco_ids": [20, 22, 24, 26, 28],
"aruco_transforms": [
[0, 0, 0],
[0, -1.57, 0],
diff --git a/src/simple_approach/config/connections_config.json b/src/simple_approach/config/connections_config.json
index 3ed7c39..1c0933c 100644
--- a/src/simple_approach/config/connections_config.json
+++ b/src/simple_approach/config/connections_config.json
@@ -3,9 +3,10 @@
"0": {
"device": "ttyUSB0",
- "robot_name": "ROKI-0060",
+ "robot_name": "ROKI-0065",
"port": "1969"
}
}
+
}
\ No newline at end of file
diff --git a/src/simple_approach/simple_approach/SimpleApproach.py b/src/simple_approach/simple_approach/SimpleApproach.py
index 486a296..9ed3f03 100644
--- a/src/simple_approach/simple_approach/SimpleApproach.py
+++ b/src/simple_approach/simple_approach/SimpleApproach.py
@@ -277,8 +277,8 @@ class SimpleApproach(Node):
positions = self.control_poses(msg)
print("HELLO")
# TODO: fit constants and create params
- v_linear = 50.0 if len(positions) > 1 else 0.0
- omega_z = 50.0 if len(positions) > 1 else 0.0
+ v_linear = 100.0
+ omega_z = 100.0
if len(positions) > 1 and abs(positions[0].theta - positions[1].theta) < 0.15:
--
GitLab
From ba8a9446ec86295da9aaca1fabdf71861f6b15bf Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Mon, 29 Apr 2024 13:11:05 +0300
Subject: [PATCH 62/64] Fix comments from gitlab MR
---
.gitignore | 2 +-
.gitmodules | 6 +-
is_symlink | 5 -
src/androsot_msgs/CMakeLists.txt | 32 ----
src/androsot_msgs/msg/FieldPose.msg | 5 -
src/androsot_msgs/package.xml | 22 ---
.../calibration_node/calibration.py | 70 ++++-----
.../calibration_node/gui_window.py | 16 +-
.../config/static_transforms.yaml | 22 ---
.../launch/aruco_preprocessor.launch.py | 139 +++++++++---------
.../aruco_preprocessor_node.py | 52 +------
static_transforms.yaml | 25 ----
12 files changed, 106 insertions(+), 290 deletions(-)
delete mode 100644 is_symlink
delete mode 100644 src/androsot_msgs/CMakeLists.txt
delete mode 100644 src/androsot_msgs/msg/FieldPose.msg
delete mode 100644 src/androsot_msgs/package.xml
delete mode 100644 static_transforms.yaml
diff --git a/.gitignore b/.gitignore
index 2323435..629878d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,5 +5,5 @@ log
*.jpg
*.ipynb
-
+*__pycache__
roboLinuxPult/
\ No newline at end of file
diff --git a/.gitmodules b/.gitmodules
index a9c077c..4ba9475 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,3 @@
-[submodule "yolo_openvino_ros2"]
- path = yolo_openvino_ros2
- url = ./src/yolo_openvino_ros2/
[submodule "src/yolo_openvino_ros2"]
path = src/yolo_openvino_ros2
url = https://wavegit.mipt.ru/starkit/yolo_openvino_ros2.git
@@ -15,3 +12,6 @@
[submodule "src/ros2_aruco"]
path = src/ros2_aruco
url = https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco.git
+[submodule "src/usb_cam"]
+ path = src/usb_cam
+ url = https://github.com/ros-drivers/usb_cam.git
diff --git a/is_symlink b/is_symlink
deleted file mode 100644
index 5a5de0c..0000000
--- a/is_symlink
+++ /dev/null
@@ -1,5 +0,0 @@
-#! bin/bash
-
-if [[ -L "/sys/class/video4linux/video2" ]]; then
- echo "It's a link!"
-fi
diff --git a/src/androsot_msgs/CMakeLists.txt b/src/androsot_msgs/CMakeLists.txt
deleted file mode 100644
index 738cc18..0000000
--- a/src/androsot_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,32 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(androsot_msgs)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-find_package(rosidl_default_generators REQUIRED)
-find_package(std_msgs REQUIRED)
-rosidl_generate_interfaces(${PROJECT_NAME}
- "msg/FieldPose.msg"
- DEPENDENCIES std_msgs
-)
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- # the following line skips the linter which checks for copyrights
- # comment the line when a copyright and license is added to all source files
- set(ament_cmake_copyright_FOUND TRUE)
- # the following line skips cpplint (only works in a git repo)
- # comment the line when this package is in a git repo and when
- # a copyright and license is added to all source files
- set(ament_cmake_cpplint_FOUND TRUE)
- ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
diff --git a/src/androsot_msgs/msg/FieldPose.msg b/src/androsot_msgs/msg/FieldPose.msg
deleted file mode 100644
index b4f478c..0000000
--- a/src/androsot_msgs/msg/FieldPose.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-std_msgs/Header header
-
-float64 x
-float64 y
-float64 yaw
\ No newline at end of file
diff --git a/src/androsot_msgs/package.xml b/src/androsot_msgs/package.xml
deleted file mode 100644
index 2e66974..0000000
--- a/src/androsot_msgs/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
- <name>androsot_msgs</name>
- <version>0.0.0</version>
- <description>TODO: Package description</description>
- <maintainer email="xatc120@gmail.com">nakuznetsov</maintainer>
- <license>TODO: License declaration</license>
-
- <buildtool_depend>ament_cmake</buildtool_depend>
-
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_lint_common</test_depend>
-<build_depend>rosidl_default_generators</build_depend>
-
-<exec_depend>rosidl_default_runtime</exec_depend>
-
-<member_of_group>rosidl_interface_packages</member_of_group>
- <export>
- <build_type>ament_cmake</build_type>
- </export>
-</package>
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index 9f808be..dfe9a2b 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -6,8 +6,6 @@ from geometry_msgs.msg import TransformStamped
import tf2_ros
-import rclpy
-from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import tf_transformations as tf_t
@@ -44,7 +42,7 @@ class CalibrationNode(Node):
def _init_params(self):
self.declare_parameter("world_tf_topic", "world")
- world_name = self.get_parameter("world_tf_topic").value
+ self.get_parameter("world_tf_topic").value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
@@ -57,7 +55,7 @@ class CalibrationNode(Node):
self.aruco_size = self.get_parameter("aruco_size").value # m
self.declare_parameter(
"camera_frames", ["camera1", "camera2"]
- ) # rclpy.Parameter.Type.STRING_ARRAY)
+ )
self.camera_frames = self.get_parameter("camera_frames").value
self.declare_parameter("path_to_save", "./static_transforms.yaml")
self.path_to_save = self.get_parameter("path_to_save").value
@@ -74,16 +72,15 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- #print (f"!!!!!!!!!!\n Header is {msg.header}")
- tf_stamped.child_frame_id = str(id) #self.camera_frames[0] # add add seconde camera
- # tf_stamped.header.frame_id =
+ tf_stamped.child_frame_id = str(
+ id
+ )
self.current_aruco_tf[msg.header.frame_id][str(id)] = tf_stamped
def calib_cb(self):
pose_ui = self.ui_window.try_get_position()
self.get_logger().info("Timer callback called")
- # Huetta!
if pose_ui is not None:
pose_ui.header.stamp = self.get_clock().now().to_msg()
self.get_logger().info(
@@ -91,29 +88,28 @@ class CalibrationNode(Node):
)
for camera_frame in self.current_aruco_tf.keys():
if (
- str(pose_ui.header.frame_id) in self.current_aruco_tf[camera_frame]
- ): # [TODO] pose_ui.child_frame_id - has int type????child_frame_id id is number aruco
-
- pose_aruco = self.current_aruco_tf[camera_frame][str(pose_ui.header.frame_id)]
+ str(pose_ui.header.frame_id) in self.current_aruco_tf[camera_frame]
+ ): # [TODO] pose_ui.child_frame_id - has int type????child_frame_id id is number aruco
+ pose_aruco = self.current_aruco_tf[camera_frame][
+ str(pose_ui.header.frame_id)
+ ]
self.match_pose_arr.append(MatchPose(pose_ui, pose_aruco))
if self.ui_window.is_calibrate_ready():
self.calibrate()
- # del self.ui_window
def calibrate(self):
- # use only last
translations_xyz = []
orientations_rpy = []
last_used_quaternion = None
buffer_cores = dict()
for camera in self.camera_frames:
- buffer_cores[camera] = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
- # buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
+ buffer_cores[camera] = tf2_ros.BufferCore(
+ time=rclpy.duration.Duration(seconds=10.0)
+ )
time_now = self.get_clock().now()
for match_pose in self.match_pose_arr:
- # rot ebal pythona
buffer_core = buffer_cores[match_pose.tf_camera.header.frame_id]
match_pose.tf_ui.header.stamp = time_now.to_msg()
match_pose.tf_camera.header.stamp = time_now.to_msg()
@@ -128,14 +124,9 @@ class CalibrationNode(Node):
for camera_frame in self.camera_frames:
try:
- # buffer_core.set_transform(
- # buffer_core.lookup_transform_core("aruco", camera_frame, time_now), "default_authority"
- # )
- # print(dir(buffer_core))
- # print(buffer_core.all_frames_as_string())
inverse_transform = buffer_cores[camera_frame].lookup_transform_core(
- "world",#match_pose.tf_ui.header.frame_id, # world
- camera_frame,#match_pose.tf_camera.header.frame_id, # camera
+ "world", # match_pose.tf_ui.header.frame_id, # world
+ camera_frame, # match_pose.tf_camera.header.frame_id, # camera
time_now,
)
except Exception as e:
@@ -150,7 +141,7 @@ class CalibrationNode(Node):
quat = rot.x, rot.y, rot.z, rot.w
euler = tf_t.euler_from_quaternion(quat)
translations_xyz.append(translation_arr)
- orientations_rpy.append(euler) # Not true
+ orientations_rpy.append(euler)
last_used_quaternion = quat
# for multi aruco calibration
@@ -178,23 +169,20 @@ class CalibrationNode(Node):
f" camera {camera_frame}"
)
self._save_transform_to_yaml()
- def _add_transform_to_yaml(self, translation,
- quaternion, frame_id, child_frame_id):
+
+ def _add_transform_to_yaml(self, translation, quaternion, frame_id, child_frame_id):
self.dump_tf.append(
- {
- "name": child_frame_id,
- "translation": translation,
- "rotation": quaternion,
- "parent_frame_id": frame_id,
- "child_frame_id": child_frame_id,
- })
-
- def _save_transform_to_yaml(
- self
- ):
- data = {
- "static_transforms": self.dump_tf
- }
+ {
+ "name": child_frame_id,
+ "translation": translation,
+ "rotation": quaternion,
+ "parent_frame_id": frame_id,
+ "child_frame_id": child_frame_id,
+ }
+ )
+
+ def _save_transform_to_yaml(self):
+ data = {"static_transforms": self.dump_tf}
with open(self.path_to_save, "w") as file:
yaml.dump(data, file, default_flow_style=False)
self.get_logger().debug(f"save config to {self.path_to_save}")
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 0c59463..54df0d0 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -29,44 +29,34 @@ class GuiWindow(QMainWindow, WindowInterface):
self.node = node
super().__init__()
- # Not working
- # self.node_spin_timer = QTimer()
- # self.node_spin_timer.start(1000) # mmm magic_number
- # self.node_spin_timer.timeout.connect(self._handler_ros_node)
self.ros_thread = Thread(target=self._handler_ros_node)
self.setWindowTitle("Pose Input")
self.setGeometry(100, 100, 300, 200)
- # Create a central widget
central_widget = QWidget()
self.setCentralWidget(central_widget)
- # Create a vertical layout
layout = QVBoxLayout()
- # Create labels and input fields for pose data
self.pose_labels = ["Id:", "X:", "Y:", "Z:", "Roll:", "Pitch:", "Yaw:"]
self.pose_inputs = [QLineEdit() for _ in range(len(self.pose_labels))]
for label, input_field in zip(self.pose_labels, self.pose_inputs):
layout.addWidget(QLabel(label))
layout.addWidget(input_field)
- # Create "Calibrate" and "Next" buttons
calibrate_button = QPushButton("Calibrate")
next_button = QPushButton("Save Position")
layout.addWidget(calibrate_button)
layout.addWidget(next_button)
- # Connect buttons to their respective functions
calibrate_button.clicked.connect(self.calibrate)
next_button.clicked.connect(self.save_position)
- # Set the layout on the central widget
central_widget.setLayout(layout)
self.tf_queue = Queue(
32
- ) # AtomicQueue(256, wait_strategy=BUSY_SPIN_WAIT_STRATEGY)
+ )
self.calibration_ready = False
def _handler_ros_node(self) -> None:
@@ -78,7 +68,7 @@ class GuiWindow(QMainWindow, WindowInterface):
def save_position(self) -> None:
try:
- # Why ros specific in ui???
+ # [TODO] Why ros specific in ui???
self.tf = TransformStamped()
quat = tf_t.quaternion_from_euler(
*[float(x.text()) for x in self.pose_inputs[4:7]]
@@ -109,7 +99,7 @@ class GuiWindow(QMainWindow, WindowInterface):
try:
return self.tf_queue.get_nowait()
except Empty:
- pass # debug log
+ pass
except Exception as e:
print(e)
return None
diff --git a/src/calibration_node/config/static_transforms.yaml b/src/calibration_node/config/static_transforms.yaml
index b67237d..d66dc0d 100644
--- a/src/calibration_node/config/static_transforms.yaml
+++ b/src/calibration_node/config/static_transforms.yaml
@@ -3,7 +3,6 @@ static_transforms:
name: camera1
parent_frame_id: world
rotation:
-<<<<<<< HEAD
- 0.9553798467029665
- 0.05845252865982777
- -0.0036991582918384875
@@ -12,21 +11,10 @@ static_transforms:
- 0.5095772336627481
- -0.7895029743512599
- 1.7309347486778708
-=======
- - -0.5845781463562191
- - 0.6842680837945385
- - -0.33536892934797113
- - 0.27851976865363226
- translation:
- - -4.102433424332044
- - -0.33554078957982525
- - 2.8603490712788853
->>>>>>> simple_approach
- child_frame_id: camera2
name: camera2
parent_frame_id: world
rotation:
-<<<<<<< HEAD
- 0.9697033003887658
- 0.05525964453102179
- 0.04329297270725113
@@ -35,13 +23,3 @@ static_transforms:
- 0.05312842279884265
- -0.972345994627029
- 1.6195570157578052
-=======
- - 0.6689080181465836
- - 0.6427557356753467
- - -0.2321647577020393
- - -0.29244940211395004
- translation:
- - 4.394749353486249
- - 0.018674504462683372
- - 3.04352171951904
->>>>>>> simple_approach
diff --git a/src/localization_preprocessor/launch/aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/aruco_preprocessor.launch.py
index 16c71e1..79d37c1 100644
--- a/src/localization_preprocessor/launch/aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/aruco_preprocessor.launch.py
@@ -1,78 +1,75 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- camera_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params2.yaml")
-
- return LaunchDescription([
-
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera1',
- emulate_tty=True
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera1'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.170} # 0.07
- ],
- remappings=[
- ('/camera/image_raw', '/camera1/image_raw'),
- ('/camera/camera_info', '/camera1/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config2],
- output='screen',
- namespace='camera2',
- emulate_tty=True
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera2'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.170} # 0.07
- ],
- remappings=[
- ('/camera/image_raw', '/camera2/image_raw'),
- ('/camera/camera_info', '/camera2/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
-
- # world frame publisher
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
- Node(
- package='calibration_node',
- executable='calibration_node',
-# parameters=[
-# {'robots_params': robot_config},
-# {'camera_frames': ["camera"]}
-# ],
- output='screen',
- emulate_tty=True
- )
- ])
\ No newline at end of file
+ return LaunchDescription(
+ [
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera1",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.170}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera1/image_raw"),
+ ("/camera/camera_info", "/camera1/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ namespace="camera2",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.170}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ # world frame publisher
+ Node(
+ package="calibration_node",
+ executable="calibration_node",
+ # parameters=[
+ # {'robots_params': robot_config},
+ # {'camera_frames': ["camera"]}
+ # ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index fa86c98..5356212 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -2,7 +2,6 @@ import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, PoseStamped, TransformStamped, PointStamped
-from androsot_msgs.msg import FieldPose # Assuming you've defined this
from tf2_ros import TransformException, TransformListener, Buffer
import tf2_ros
from tf2_geometry_msgs import do_transform_point, do_transform_pose_with_covariance_stamped
@@ -103,7 +102,6 @@ class ArucoPreprocessorNode(Node):
pose_stamped = PoseStamped()
pose_stamped.pose = pose
pose_stamped.header = msg.header
- # self.current_aruco_poses[id] = pose_stamped
# Example quaternion (x, y, z, w)
r = pose_stamped.pose.orientation
quaternion = [r.x, r.y, r.z, r.w]
@@ -114,7 +112,6 @@ class ArucoPreprocessorNode(Node):
# cov is equal for all angles
# pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
# cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
- # yaw - pohui
# self.get_logger().info(f"ID:{id}, ROLL: {roll}, PITCH: {pitch}, YAW: {yaw}")
cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
@@ -171,13 +168,6 @@ class ArucoPreprocessorNode(Node):
shifted_pose = self.shift_pose_to_robot_centre(pose, id)
rotated_pose = self.rotate_pose_to_forward(shifted_pose, id)
return rotated_pose
- def tf_camera2world(self, pose):
-
- buffer_core = tf2_ros.BufferCore(time=rclpy.duration.Duration(seconds=10.0))
- buffer_core.set_transform(transform, "default_authority")
- inverse_transform = buffer_core.lookup_transform_core(
- transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
- )
def shift_pose_to_robot_centre(
self, pose:PoseStamped, id
@@ -226,7 +216,7 @@ class ArucoPreprocessorNode(Node):
]
if id in self.aruco_rotation_map.keys():
transformed_quate_iter = tf_t.quaternion_multiply(
- quaternion_iter, self.aruco_rotation_map[id] # !!! ALARM SHIIIIIT
+ quaternion_iter, self.aruco_rotation_map[id] # I believe it's true
)
(
rotated_pose.pose.orientation.x,
@@ -234,49 +224,11 @@ class ArucoPreprocessorNode(Node):
rotated_pose.pose.orientation.z,
rotated_pose.pose.orientation.w,
) = transformed_quate_iter
- # rotated_pose.header.stamp = self.get_clock().now().to_msg()
rotated_pose.header.frame_id = (
pose.header.frame_id
- ) # "camera" ########## Choose frame id
+ )
return rotated_pose
- def send_coord_to_robot(self, id):
- if id in self.world_robots_poses:
- msg = PoseStamped()
- msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
- msg.pose = self.world_robots_poses[id].get()
- self.robots_pub[id].publish(msg)
- # self.get_logger().info("send_command_to_goalkeeper")
- if (self.team_name=='starkit1'):
- robot_msg = f"{-msg.pose.position.x} {-msg.pose.position.y} {-self.get_yaw_world_rotation(msg.pose)}"
- else:
- robot_msg = f"{msg.pose.position.x} {msg.pose.position.y} {self.get_yaw_world_rotation(msg.pose)}"
-
- def count_robots_poses(self):
- robots_poses = {}
- for id, pose in self.current_aruco_poses.items():
- robot_id = self.get_robot_id(id)
- self.get_logger().info(f"robot_id: {robot_id}")
- if robot_id == -1:
- continue
- # forward_pose = self.get_forward_pose(pose, id) def rotate_pose_to_forward
- rotated_to_forward_pose = self.transform_to_robot_centre(pose, id)
-
- world_pos = self.tf_buffer.transform(rotated_to_forward_pose, "world")
- if not (robot_id in robots_poses):
- robots_poses[robot_id] = [world_pos]
- else:
- robots_poses[robot_id].append(world_pos)
- # find mid of x,y
- for robot_id in self.robot_ids:
- if robot_id in robots_poses:
- if not robot_id in self.world_robots_poses:
- self.world_robots_poses[robot_id] = AlphaFilter()
- self.world_robots_poses[robot_id].update(self.find_mid_pose(
- robots_poses[robot_id])
- ) # transform aruco to forward
-
def main(args=None):
rclpy.init(args=args)
diff --git a/static_transforms.yaml b/static_transforms.yaml
deleted file mode 100644
index 770d528..0000000
--- a/static_transforms.yaml
+++ /dev/null
@@ -1,25 +0,0 @@
-static_transforms:
-- child_frame_id: camera1
- name: camera1
- parent_frame_id: world
- rotation:
- - 0.9553798467029665
- - 0.05845252865982777
- - -0.0036991582918384875
- - -0.289515054245924
- translation:
- - 0.5095772336627481
- - -0.7895029743512599
- - 1.7309347486778708
-- child_frame_id: camera2
- name: camera2
- parent_frame_id: world
- rotation:
- - 0.9697033003887658
- - 0.05525964453102179
- - 0.04329297270725113
- - -0.233982049344836
- translation:
- - 0.05312842279884265
- - -0.972345994627029
- - 1.6195570157578052
\ No newline at end of file
--
GitLab
From fa8f2929789a52e9fd1f4db9b4a41eae71c5d198 Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Mon, 29 Apr 2024 13:19:47 +0300
Subject: [PATCH 63/64] Delete azer robocup project and apply ruff lint
---
.gitignore | 4 +-
src/azer_robocup_project/.openmv_disk | 0
.../.vs/After Tomsk2021 disk image/v16/.suo | Bin 193536 -> 0 bytes
.../.vs/ProjectSettings.json | 3 -
.../.vs/PythonSettings.json | 3 -
...e9c5f4ee-dd77-40c1-a281-22adb6db7ee6.vsidx | Bin 395301 -> 0 bytes
.../FileContentIndex/read.lock | 0
.../.vs/RoboFest2023 disk image/v16/.suo | Bin 49152 -> 0 bytes
.../.vs/RoboFest2023 disk image/v17/.wsuo | Bin 16896 -> 0 bytes
.../.vs/RoboFinist2022 disk image/v16/.suo | Bin 107008 -> 0 bytes
.../.vs/Roobofest2020 disk image/v16/.suo | Bin 71680 -> 0 bytes
.../.vs/Tomsk2021 disk image/v16/.suo | Bin 151552 -> 0 bytes
.../v16/.suo | Bin 102400 -> 0 bytes
.../.vs/Tomsk2022 disk image/v16/.suo | Bin 63488 -> 0 bytes
.../.vs/VSWorkspaceState.json | 12 -
src/azer_robocup_project/.vs/slnx.sqlite | Bin 118784 -> 0 bytes
...afd4ce3-dd99-4473-ad13-6824332ea171.vsidx" | Bin 49913 -> 0 bytes
...bcad96e-e241-4ca9-8356-de26ccdab90a.vsidx" | Bin 12819 -> 0 bytes
...2dfad02-59fd-4c98-a6ad-51863ac0b212.vsidx" | Bin 215133 -> 0 bytes
...fa13618-942f-47e9-9248-324ed8e367bd.vsidx" | Bin 13898 -> 0 bytes
.../FileContentIndex/read.lock" | 0
.../v17/.wsuo" | Bin 93184 -> 0 bytes
src/azer_robocup_project/Error.txt | 3 -
.../Init_params/Real/Real_Thresholds.json | 1 -
.../Init_params/Real/Real_calibr.json | 1 -
.../Init_params/Real/Real_landmarks.json | 43 -
.../Init_params/Real/Real_params.json | 17 -
.../Init_params/Real/Real_params_2.json | 19 -
.../Init_params/Real/wifi_params.json | 12 -
.../Init_params/Sim/SURROGAT/Sim_calibr.json | 1 -
.../Init_params/Sim/SURROGAT/Sim_params.json | 16 -
.../Sim/SURROGAT/Sim_params_2.json | 20 -
.../Init_params/Sim/SURROGAT/wifi_params.json | 8 -
.../Init_params/Sim/Sim_Thresholds.json | 37 -
.../Init_params/Sim/Sim_landmarks.json | 43 -
.../Init_params/Sim/Sim_landmarks_field2.json | 43 -
.../Init_params/Sprint_params.json | 4 -
.../Init_params/strategy_data.json | 1 -
.../Soccer/Localisation/PF/ParticleFilter.py | 455 ------
.../__pycache__/ParticleFilter.cpython-39.pyc | Bin 13434 -> 0 bytes
.../call_par_filter.cpython-39.pyc | Bin 1413 -> 0 bytes
.../Soccer/Localisation/PF/call_par_filter.py | 56 -
.../Soccer/Localisation/PF/pf_constants.json | 22 -
.../__pycache__/class_Glob.cpython-39.pyc | Bin 3973 -> 0 bytes
.../__pycache__/class_Local.cpython-39.pyc | Bin 38344 -> 0 bytes
.../class_Visualisation.cpython-39.pyc | Bin 1774 -> 0 bytes
.../Soccer/Localisation/class_Glob.py | 143 --
.../Soccer/Localisation/class_Local.py | 1434 -----------------
.../Localisation/class_Visualisation.py | 46 -
.../Soccer/Localisation/deviation_report.py | 45 -
.../Soccer/Motion/Head_Tilt_Calibration.py | 74 -
.../__pycache__/class_Motion.cpython-39.pyc | Bin 28248 -> 0 bytes
.../class_Motion_real.cpython-39.pyc | Bin 29327 -> 0 bytes
.../class_Motion_sim.cpython-39.pyc | Bin 10409 -> 0 bytes
.../compute_Alpha_v3.cpython-39.pyc | Bin 4616 -> 0 bytes
.../compute_Alpha_v4.cpython-39.pyc | Bin 4816 -> 0 bytes
.../Soccer/Motion/ball_Approach_Steps_Seq.py | 131 --
.../Soccer/Motion/ball_Approach_calc.py | 169 --
.../Soccer/Motion/bno055.py | 93 --
.../Soccer/Motion/class_Motion.py | 974 -----------
.../Soccer/Motion/class_Motion_real.py | 1082 -------------
.../Soccer/Motion/class_Motion_sim.py | 343 ----
.../Soccer/Motion/compute_Alpha_v3.py | 181 ---
.../Soccer/Motion/compute_Alpha_v4.py | 199 ---
.../Soccer/Motion/kondo_controller.py | 856 ----------
.../Soccer/Motion/kondo_controller2020.mpy | Bin 9317 -> 0 bytes
.../Soccer/Motion/motion_slots/Dance_2.json | 31 -
.../Soccer/Motion/motion_slots/Dance_3.json | 1 -
.../Soccer/Motion/motion_slots/Dance_4.json | 1 -
.../Soccer/Motion/motion_slots/Dance_5.json | 1 -
.../Soccer/Motion/motion_slots/Dance_6.json | 1 -
.../Soccer/Motion/motion_slots/Dance_6_1.json | 1 -
.../Soccer/Motion/motion_slots/Dance_7-1.json | 1 -
.../Soccer/Motion/motion_slots/Dance_7-2.json | 1 -
.../Soccer/Motion/motion_slots/Dance_7.json | 1 -
.../motion_slots/Get_Up_From_Defence.json | 1 -
.../motion_slots/Get_Up_From_Defence21.json | 1 -
.../motion_slots/Get_Up_From_Defence22.json | 1 -
.../Motion/motion_slots/Get_Up_Left.json | 8 -
.../Motion/motion_slots/Get_Up_Right.json | 8 -
.../Motion/motion_slots/Initial_Pose.json | 5 -
.../motion_slots/Motion_slot_Random_259.json | 1 -
.../motion_slots/PanaltyDefenceReady.json | 6 -
.../PanaltyDefenceReady_Fast.json | 6 -
.../Motion/motion_slots/PenaltyDefenceF.json | 7 -
.../Motion/motion_slots/PenaltyDefenceL.json | 9 -
.../Motion/motion_slots/PenaltyDefenceR.json | 9 -
.../Motion/motion_slots/Quick_sit_v1.json | 1 -
.../motion_slots/Soccer_Get_UP_Face_Up.json | 1 -
.../motion_slots/Soccer_Get_UP_Stomach_N.json | 14 -
.../Soccer_Get_UP_Stomach_N_old.json | 11 -
.../Soccer_Kick_Forward_Left_Leg.json | 19 -
.../Soccer_Kick_Forward_Right_Leg.json | 19 -
.../Soccer_Small_Jump_Forward.json | 11 -
.../Motion/motion_slots/Spot_walk1.json | 1 -
.../TripleJumpForFIRA2019full.json | 1 -
.../motion_slots/TropleJumpForFIRA2019.json | 1 -
.../motion_slots/TropleJumpForFIRA2019v1.json | 1 -
...0\276\320\264\321\203\320\273\321\2142.py" | 252 ---
.../Soccer/Motion/path_planning.py | 794 ---------
.../__pycache__/class_Vision.cpython-39.pyc | Bin 1732 -> 0 bytes
...ac_line_segments_simulation.cpython-39.pyc | Bin 3476 -> 0 bytes
.../Vision/__pycache__/reload.cpython-39.pyc | Bin 7448 -> 0 bytes
.../Soccer/Vision/class_Vision.py | 42 -
.../Soccer/Vision/ransac_line_segments.py | 294 ----
.../Vision/ransac_line_segments_simulation.py | 165 --
.../Soccer/Vision/reload.py | 257 ---
.../Soccer/Vision/zhangSuen_Thinning.py | 115 --
.../__pycache__/strategy.cpython-39.pyc | Bin 21870 -> 0 bytes
.../Soccer/__pycache__/utility.cpython-39.pyc | Bin 1543 -> 0 bytes
src/azer_robocup_project/Soccer/pid.py | 56 -
src/azer_robocup_project/Soccer/strategy.py | 680 --------
src/azer_robocup_project/Soccer/utility.py | 58 -
.../IndexerVolumeGuid | Bin 76 -> 0 bytes
.../System Volume Information/WPSettings.dat | Bin 12 -> 0 bytes
src/azer_robocup_project/Treshold_Utility.py | 57 -
src/azer_robocup_project/button_test.py | 152 --
src/azer_robocup_project/main.py | 182 ---
src/azer_robocup_project/mqtt.py | 237 ---
src/azer_robocup_project/pose_designer.py | 1208 --------------
.../pose_designer_config.json | 1 -
.../pose_designer_log.txt | 1 -
.../simulator_lib_directory.txt | 1 -
.../threshold_tuner_server.py | 258 ---
...320\261\320\276\321\202\320\260 2023.docx" | Bin 17452 -> 0 bytes
...0\276\320\264\321\203\320\273\321\2141.py" | 2 -
.../calibration_node/gui_window.py | 2 -
.../launch/multi_camera_calibration.launch.py | 1 -
.../single_camera_calibration.launch.py | 1 -
.../localisation/dump_aruco_publisher.py | 14 +-
.../localisation/localisation_node.py | 11 +-
.../multi_camera_aruco_preprocessor.launch.py | 1 -
.../solo_camera_aruco_preprocessor.launch.py | 1 -
.../launch/static_tf_publisher.launch.py | 2 -
.../aruco_preprocessor_node.py | 9 +-
src/pic_to_real/launch/bal_to_real.launch.py | 4 +-
src/pic_to_real/pic_to_real/ball_to_real.py | 9 +-
src/proxy/proxy/Proxy.py | 4 +-
src/server_main/config/field_config.yaml | 7 +
.../launch/camera_calibration.launch.py | 2 +-
.../launch/multi_camera_server.launch.py | 2 +-
.../launch/multi_team_multi_camera.launch.py | 2 +-
src/server_main/scripts/camera_calibration.py | 2 +-
.../scripts/camera_pose_calibration.py | 22 +-
src/server_main/scripts/create_calib_photo.py | 7 +-
src/server_main/scripts/is_symlink | 5 +
src/server_main/server_main/server_node.py | 16 +-
src/server_main/server_main/strategy.py | 2 -
src/server_main/test/logger.py | 1 -
src/server_main/test/mqtt_kondo_dummy.py | 1 -
.../simple_approach/SimpleApproach.py | 3 +-
src/strategy/launch/game.launch.py | 3 +-
src/strategy/static_transforms.yaml | 121 ++
src/strategy/strategy/solo_robot_strategy.py | 2 +-
src/strategy/strategy/strategy_node.py | 24 +-
src/strategy/strategy/strategy_test.py | 3 +-
156 files changed, 170 insertions(+), 11701 deletions(-)
delete mode 100755 src/azer_robocup_project/.openmv_disk
delete mode 100755 src/azer_robocup_project/.vs/After Tomsk2021 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/ProjectSettings.json
delete mode 100755 src/azer_robocup_project/.vs/PythonSettings.json
delete mode 100755 src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/e9c5f4ee-dd77-40c1-a281-22adb6db7ee6.vsidx
delete mode 100755 src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/read.lock
delete mode 100755 src/azer_robocup_project/.vs/RoboFest2023 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/RoboFest2023 disk image/v17/.wsuo
delete mode 100755 src/azer_robocup_project/.vs/RoboFinist2022 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/Roobofest2020 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/Tomsk2021 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/Tomsk2022 disk image experimental/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/Tomsk2022 disk image/v16/.suo
delete mode 100755 src/azer_robocup_project/.vs/VSWorkspaceState.json
delete mode 100755 src/azer_robocup_project/.vs/slnx.sqlite
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/1afd4ce3-dd99-4473-ad13-6824332ea171.vsidx"
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/6bcad96e-e241-4ca9-8356-de26ccdab90a.vsidx"
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/82dfad02-59fd-4c98-a6ad-51863ac0b212.vsidx"
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/dfa13618-942f-47e9-9248-324ed8e367bd.vsidx"
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/read.lock"
delete mode 100755 "src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/v17/.wsuo"
delete mode 100644 src/azer_robocup_project/Error.txt
delete mode 100644 src/azer_robocup_project/Init_params/Real/Real_Thresholds.json
delete mode 100644 src/azer_robocup_project/Init_params/Real/Real_calibr.json
delete mode 100644 src/azer_robocup_project/Init_params/Real/Real_landmarks.json
delete mode 100644 src/azer_robocup_project/Init_params/Real/Real_params.json
delete mode 100644 src/azer_robocup_project/Init_params/Real/Real_params_2.json
delete mode 100644 src/azer_robocup_project/Init_params/Real/wifi_params.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_calibr.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params_2.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/SURROGAT/wifi_params.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/Sim_Thresholds.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/Sim_landmarks.json
delete mode 100644 src/azer_robocup_project/Init_params/Sim/Sim_landmarks_field2.json
delete mode 100644 src/azer_robocup_project/Init_params/Sprint_params.json
delete mode 100644 src/azer_robocup_project/Init_params/strategy_data.json
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/PF/ParticleFilter.py
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/ParticleFilter.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/call_par_filter.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/PF/call_par_filter.py
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/PF/pf_constants.json
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Glob.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Local.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Visualisation.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/class_Glob.py
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/class_Local.py
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/class_Visualisation.py
delete mode 100644 src/azer_robocup_project/Soccer/Localisation/deviation_report.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/Head_Tilt_Calibration.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_real.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_sim.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v3.cpython-39.pyc
delete mode 100755 src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v4.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Motion/ball_Approach_Steps_Seq.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/ball_Approach_calc.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/bno055.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/class_Motion.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/class_Motion_real.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/class_Motion_sim.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/compute_Alpha_v3.py
delete mode 100755 src/azer_robocup_project/Soccer/Motion/compute_Alpha_v4.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/kondo_controller.py
delete mode 100644 src/azer_robocup_project/Soccer/Motion/kondo_controller2020.mpy
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_2.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_3.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_4.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_5.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6_1.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-1.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-2.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence21.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence22.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Left.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Right.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Initial_Pose.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Motion_slot_Random_259.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady_Fast.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceF.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceL.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceR.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Quick_sit_v1.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Face_Up.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N_old.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Left_Leg.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Right_Leg.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Small_Jump_Forward.json
delete mode 100755 src/azer_robocup_project/Soccer/Motion/motion_slots/Spot_walk1.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/TripleJumpForFIRA2019full.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019.json
delete mode 100644 src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019v1.json
delete mode 100644 "src/azer_robocup_project/Soccer/Motion/motion_slots/\320\274\320\276\320\264\321\203\320\273\321\2142.py"
delete mode 100644 src/azer_robocup_project/Soccer/Motion/path_planning.py
delete mode 100644 src/azer_robocup_project/Soccer/Vision/__pycache__/class_Vision.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Vision/__pycache__/ransac_line_segments_simulation.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Vision/__pycache__/reload.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/Vision/class_Vision.py
delete mode 100644 src/azer_robocup_project/Soccer/Vision/ransac_line_segments.py
delete mode 100644 src/azer_robocup_project/Soccer/Vision/ransac_line_segments_simulation.py
delete mode 100644 src/azer_robocup_project/Soccer/Vision/reload.py
delete mode 100644 src/azer_robocup_project/Soccer/Vision/zhangSuen_Thinning.py
delete mode 100644 src/azer_robocup_project/Soccer/__pycache__/strategy.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/__pycache__/utility.cpython-39.pyc
delete mode 100644 src/azer_robocup_project/Soccer/pid.py
delete mode 100644 src/azer_robocup_project/Soccer/strategy.py
delete mode 100644 src/azer_robocup_project/Soccer/utility.py
delete mode 100644 src/azer_robocup_project/System Volume Information/IndexerVolumeGuid
delete mode 100644 src/azer_robocup_project/System Volume Information/WPSettings.dat
delete mode 100644 src/azer_robocup_project/Treshold_Utility.py
delete mode 100644 src/azer_robocup_project/button_test.py
delete mode 100644 src/azer_robocup_project/main.py
delete mode 100644 src/azer_robocup_project/mqtt.py
delete mode 100644 src/azer_robocup_project/pose_designer.py
delete mode 100644 src/azer_robocup_project/pose_designer_config.json
delete mode 100644 src/azer_robocup_project/pose_designer_log.txt
delete mode 100644 src/azer_robocup_project/simulator_lib_directory.txt
delete mode 100644 src/azer_robocup_project/threshold_tuner_server.py
delete mode 100644 "src/azer_robocup_project/\320\235\320\260\321\201\321\202\321\200\320\276\320\271\320\272\320\260 \321\200\320\276\320\261\320\276\321\202\320\260 2023.docx"
delete mode 100644 "src/azer_robocup_project/\320\274\320\276\320\264\321\203\320\273\321\2141.py"
create mode 100644 src/server_main/config/field_config.yaml
create mode 100644 src/server_main/scripts/is_symlink
create mode 100644 src/strategy/static_transforms.yaml
diff --git a/.gitignore b/.gitignore
index 629878d..b418f9f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,4 +6,6 @@ log
*.ipynb
*__pycache__
-roboLinuxPult/
\ No newline at end of file
+src/calibration_node/config/static_transforms.yaml
+roboLinuxPult/
+
diff --git a/src/azer_robocup_project/.openmv_disk b/src/azer_robocup_project/.openmv_disk
deleted file mode 100755
index e69de29..0000000
diff --git a/src/azer_robocup_project/.vs/After Tomsk2021 disk image/v16/.suo b/src/azer_robocup_project/.vs/After Tomsk2021 disk image/v16/.suo
deleted file mode 100755
index 81f151c0ef8c3127c243d3986e98e2591f16bfce..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 193536
zcmeHQ2YejG)n6AdU;{SB#@G~rX-1ZGP0iTGS9B^?IrYN9aaCIBny;u~Q(`)V9t;=)
zUjl>@dIw_+n9zemiy;sUn2;DkhlGUk{ofW}(Vet+C;2RA=YH>IcXoIBy!YnKn>U+Y
znX%>n?wR{O6X%vPlbD}=-jmrqDtvO(H8Y)I_QySi{q*zCKhvWLfTIBgfV}|-mfcQX
z0}YH1zYvqdlrdiT0cHR#C*93vcH7?0G5sS!6kmMuz3V~8<_Q{l=Hy>6Aw?h4%~T?F
z7p}^f{sdnp51P$P+@3yFb@DeOPtL$4x-aL%^eJ6G<3<hw$XbADL|s?`LN<lBhk8e(
zk-iJjd?rLy$ApoJn_*FYg3hRv4G0MgO)@3~=}(x7kcj}P<#%GBenSHlZVKSXs6I+#
znOq6N4+GG(3jV%;0{|BQ=>3D>?+G{%Fb&X*IA_2oe9?mIdGM)@Ex4vQXXE-Pz>xro
zvk3k|;d(B7Dg%X|fcKBZ^<Dr<XFnnQaKS$YKE?N>@SMIOr^US;-|FF?48ItDK7jfp
z|3>8h(73RfxUU50;=;u*st{f+JbN_BJL0?J&EJvZ|Ktn*EsuZnpPgy^i~V&E<cr1|
zjje;D`uJhErtwejNg4n29*uwUqsKqpi{qbg3*FQBCp@48(D<kH#qqxY_w){pf5JU9
z{t2gu<DZ_<`{Z&Ft_8%8|5Aj}_<s_>zsdj0<3CaxXP~Wq0ibp#I1krn1I_`QD}<j9
z{{p~;fL{VG0bC5A_b(NE@%u>3^3`~#0ctnm9VQ85WjFXl!-+2-oIo_5XfcgXqVvSp
z(>F9GX?zpSr!h)2RO|zyKL=sN9}s<~u`7m0`-chP^Wh%>ApU^x1hFSH)(H;~KR{!g
z@I@Yg=sL|Y6ac~<grC#^;wuXQgj0zA6P_U)un2G*U@>3`U@72uz%qaqKy;t@1_PiB
zU<8x{DgeY65dB{cAReFwPz$I7m;m*F20$aA3D68^0YsOv1NSEY%zzUCCjl&glL1zM
z4PXa20OEJ&;kpaJ0=lEZ>A4s8zNoMOu1^Dmgs@ZL_W}9=147s-@K*v>3HPhvpDtWe
zoaD|F?$3sQ4&Yqj{(Sfs0#w3SzX;bC11<rG`MnC)R{(wmxDv1iK+mog{I&3J09*(7
zHQ;&xJ^w%WzY(r)gnujGX23&$TLARVo$zl5{2p+J5Ox>*y8-tA9tPY8xF7HU;6Z?x
zuSanG7~pllCxFKR>j6&y=$$9w{{!$WU?boez#jng>^b<)3)e5ge+lriaQ_PYO@KcM
z_piZ!5AX)yO(ASD>gz4x`Y-U`5w8C#_<x80KHvkuheFs#@c#z*Sh%NrY!j|Oh5t{$
zXMi68UjV)Yd<FO#K<|DF|2x2c0sjVk5BLv&p8X*BKf(VQKofQydXkUnVLa%G0r*nq
zJ@h%bgE0R<G#rjV5?r+0gJJ$I#|Os<&zq2L5b0K<rw7r48`0}&4i>_QpqWu{n0V+B
z&3`7rKNcW0{mYR49`rFB&HHGU7r@-hig}n3`EcVth|$f@Or6L{2>8K=H|S0JkK}3*
z(w~KR2**iH|F?K|>Av`bGaz0nsYi&p73q6{349pACj19+9|BP7kyQC_>0g2LsVs8<
zQq!k?ejs&hIJy1*x7&X>`9lk+YY`}EA>UOJ{vVTu{NtC1iEt7*Dd`g}Pv(&Rgd^~f
z`aZd2??s1Sj+@;HX~9WJzl47^;XkE+B%V|Nq^5r@-rXw1CMEq+;oU@DI`#iMM0AP}
zStGEIUBGp{z%vdG7v=*G`+x@t1M|2i9~jyP4C&x7VVJ{$6vfgs+P??iiTF2_^odf0
zfUyF|hZni-LOjb*{|CZ903fyg-FSC*0J$1)Z_P~3kn-k4Q3C$W#Q#U7QGO4ix=M)d
z1W+IchvSLrors)Na}<%6$nsApjHm?3Bl*^d<fSBy^!@mpcxG}^`d=Z(Y4h*L{~wF=
zi3e^4NUi@x7<-or5lKm3!*TKPDf9P~KBX(V#Barhl8oUzAxKL4h1@J7HV*MFo%Bya
z`h<hXNlRbNzntJ7=}%}uIL$Gr4idaFB*=>N*9f6f(pL>b0jT^%h-)!GYWlC@UE*oU
zNlQOD1)%aTMLbIYQq#W%@9u#*ASWe#^$7i+(iI&a>+t`bcuYJvIce!D`Ii&?Q~9ZP
zXaSUm1g{JUx)&k)3FQ~_E&KnQH6X43X@>M6F@X9X)m#+<$QQ$h^FPPKRQNA7C*4UL
zBc*>l9#sOQrvDAzotV=2Ck&U^A$?G1UUnok{dIUh75t~>q*=++r*uV^_^sH`4Y(!Q
z068h`pB($pJf3g=`G|}1z(qR3-yojT9F&}t^hI8U|04E39sYL(qIg?~MoRk0dp}?P
z>4@tnfYkDzg?Ce_|K!x4Pya9>ZK>(^;(d}Gk&{yXqT%V^gcA^t1t2y33-Ina0CLjO
zPu>GX{BsiGlA3-PWB(m4b*bqm?}0S_DP7SC(GKgKj`UZjlz#FaK;=IXaV-Q;9)?X=
zcsPjkR|(fr>R+{ewlFkyvHnvj|GyATD*azM%KZPMDW<O)f&LJFAP!Y({qKqTPX+(0
zxRnhgkiex=|9c~Sk~x#Rc7h)3BWBEjgWSwNh-)iWB+Nr3lk}vVo2gbnE<kUP?tl%@
z!-bl${?UawEXhtuhU><>HV0uOD<Ju*8MD{;=N5#C&n-d@dIbrDA8Qxa*`7Ri@x;dI
z@9y?{-(yGp<0w~7L*4b|z9!EHmtX$O+NFP@5{Nxd^eIfbyJ!cNe`x(!qUt9f`bPuI
zrt_cq&rJtR&VOk2PuKL<cWV#ppYZ6KD{sH#<oy(7C3jeeb;z*9v2`P5{F4+gnM()%
z1reDjf0CAdNwVZ2_viT!l2J+>e_QY_%|XdYOMm$JX940N3MDmtlKrK||0wW9Vn^jC
z>O?gpHGSfJMEM7;3r)m5t)WZ?>;@p&*c1TC==KEc1(*t$2G|=w-_Y^}@jWvD`vGPG
zC_EbGAdF;la{==bJg0XL6T*pqJOV(nRN|$M1{?!e05}$q3m_Vo2gnCdol~t)y;J##
zLJ}UPen7pIT8!$1WT%S&#{o!2L2?3;7towx89)or0rUU^pbTIHlmjXNG_R-vEC&#u
zUjwKG)B#L@dO!o95zqu^2DAX8>!Abp#3PvjM5~EcA%5v(fE8c^*Z~fJ_#L$m$v0U*
zcT_k%_u}3c6&AqtX@HOrb}IZnKtEtW2&3_~5<qmH;#dv;bU-xz)<QFhUkf9=?ZcWB
zt<0%V&nnb&4kxFikR0TS)(bm<sm-V%(p(IoM1F*kY?i(=qekd0@|#gZ4XCkl)OOSM
zCl*ekNt%x~e3g$<29c(hd0(&o;_T&X@3fxa{KI7}d0TFL;VX8J1yvml{+Y|R{OXF0
z$aDe>7_fSR`Q1UEcjYMx!R3)3_a{DEwNklIS+de-eQ6Z&e|q&tl6I28^E9#sccPTn
zj2`GjeL7ILO{ga~SIc^Yh_z;Ap2{%)2BcsE4esPxGr&o`Y5kFM6-K*IzJ_y<e^1y4
zFA!cpx01wv9*%bprw`zgr$2z+L%q|@znJ8o(x-F}!E>qUQ~wvEi%-7><@F#{(u0m(
zizS+s$Sp+rgxmH6NKHQn?-H&c7oR?@Zcv-|U^MyA=KN}NbWZs1#D5LaCtOWwlM};-
z^AE*C!tLaw(7!?n{AWAjlA1m{A+`BO;Ry0)uHeESSEv#GNI&u4!aLH+KP>+<p$%~n
zZkJkqKhh_BOio(*M9C66k^d3vAn{wVp`@$5MF{#$hWI0M`Vou&_!UROZy}68o+tKx
za_K?SF1u-=#rwNJ>5MwV>FYLH>aOkhTm2!Wt0o<B{a>gkV);d1C|*ME8ib1S|LFYl
z6e|pjLo02B19G^PQRjBb7QxK1xsJu!b}oh>=$QrlLIj`F42NhUUn*B+>6JG=bjLS)
zK5_YDziV6lx2|LU@WW{r++OK?xbT=WH@#1chJ;c*nqm7%>3>DTkAE(g#KTDK|Gnsc
z+-4zo($Y_^fsoGp<20n7iu|K!`1#KSE6PFqo7D1Og!G9&BPXr=!_GfqdI=xv@c(Ba
z{cnU<;^|K-*Y{~{o#yc)yPP!SU&K-DpCtXn^e=fnNd8Q7kQ{*2`X^eoaU%ZU;>%C^
zV8rwIw{u~`*Frk$|3oiD`kN?yUdzmZTsS!WwBhnoB=A2}pHj=;I!yXJr$Krag#Vo2
z*!}ns7UQ{m3XOJ1Kjr>MyxL?Tl18LKvj*yMlnSNXfG{dE@jmt7LWwhG@gKwtjMoy3
z<N*2)6K3O<cp9nYkLLfPIjb7vgtP!@UPChy6H@cyN9|xl2+eHbM@2g2KNxwXxiq=q
z(r@AN5dyDB5;?K#^o0X8)5VQX=)a^~{|6ymar{ZaKWc6!&1HAUAMqc0gi_s;8}dSe
zfIlPbO(DDmrJ%Vc@%coH`!VyN+9zG4k>(y0#o<E#-woGN(?0_EPwan97-4z90tQF{
zic!uYfCe+(GJp>MrSOX|`!#TLU@iPo+!062<7DVRO`!UuHkFz_)xCgzP<HB3p_qO^
zJ&$;h0sQm*-GDpVkGLFf(~LgiX#5|CTawvGO`qm#M$`d08VdyE&J^y?hJOy=T;cwF
z_!k0X{qJ2*|2vxhO;3K2O#VYyoFo;rPQYJTxL!@t1yS2P2rMhgeePX8|LsdwPpN&Y
zWAVA48Nd7SnA`sO+D9{r-re%Z+n=pJzlJ*Db}Ym9*PnkT%)`6qCY|)@Bk#U@VXA8v
z9&0e<4CuY$_r&u{mwh#IdVC}cC5D#9%aP4NXTTKjxmdUJNVCCb4||;6P|#e?hAP4~
zbA!+3>tRD?tqZB<H2FNi9+g6+%yF>6o*dR=?R1(OeRjJuU=D@?){wJvAm2YARiss<
zP%KoILa5~qI;Cd73{I4V+>gavh4>EAeJ3i%?*!*(D!+bXDzmQs3&&NB*G=A3Hp#tU
z*&cszvwxm;aMxmE>4Hn2r_zfpBKpbYg|RcI(OiE@um6(tD}*0$ID8!tKITjQU;S<U
z2dsYVmcCV+&-E3b``Z~g?^OJF`J~TJcHI51m;ZVT35ti5oTR(4y&&id2kcIB!0Gn|
z&70P3TEFT3Kds*M*q<)c<okaQ>N#5LZ^XO1xz#w@M{WVFa)ORhoBF`Ri8~BPA{&u7
z%Huz?@&7BdwmAOJ8q)GRcKj<AD)_;l0s<Ot|B1LJEOH+x-Vm3c2LBn&e^XzK)-&h#
z*$_VJ<mAtZbe^L#0r_jp2#kJ~?D{f1xLJ5u2d=XlGmlR2obh{nT880!#kh(8BwoG@
zK)6o~AI`rN4@Le@kJ6J~dl0KeG{@;1l8fPD;`TqKT>j|#m%{&RhL^u`@^=yM51(89
zsR6v_XTpop(jQ*_O6zd6{wB5l>yZ8zDWyNG{3Ye`Ymj~_@?Xs`{gCEgwEjrzj#A5C
zg!D!Ex7cn{e3~~%d2eTY=P~4c9pG`mdcYHa4S*cLQ-G%dCe+Kb@WuQ;hihVPUI4rZ
zcnR<_;E#Y;0Gj}R0=x=%4e&bP4FJt;X^y)Y@D|{0012tz0lW+NE8soA7Qp*}4*(wm
zwgNr^{0;Ci;O~Hc0JZ@>0elMhCxFJz=K$)%Ujn`Yd=2;p@Gamwz`p?0#@_?}1NZ?z
z<@!nRC!(x$f95FiAF0UCX$C;j6`s#J=wpA*ewVW~S6{r)c-kZHbiMgNuJ`y=*6;fc
zeZ=z2U#}!0yS-H?*W`*dMC|zen*6FYAJ=Vpr(d;6Z`-OVIp8?^-P8Bp?}E?#hre0#
zN}%BBuP%JHfWy#y4D?Sivms+yt4`fp;VEhDROI@qO9sn~4sA=JuGP_HRM<70u10H}
zyS2nu&}LHghFvuc747=!worq$;?+^CzjXN5jTxq&SpLue+Uw;cF~k{DJ9UCWiF;$C
zr4WmOfAUd>#0rb`Np(7$L)T*#PvYN5oIWq{ssq<5uDB$V!qWQdGDJoCAyU&%mHw%R
z*FT(~L)vvIq<=IjfW{;_Y4lIQzntJ-T>qoxBB|xiLHdshaSbPbY~@w~yvU;;oCz-n
zjn9HA4gTu@L_#t+a>J#sM=#_h(H`iI&~EH7a#9P-5s|~Dv;S~5(j}gaoYeG1871{c
zKXN5H?Ej9zOk((O{wzEc({JM9GogM1h?ST2QC*Yllb^pSA%Udco!F(b{uSN-sn1At
zyCVHR1l9V#O#h9^#1BDkxbX@=W^_(k5~JlAGqZT|(#e|1`pL&luAZzE2zaxmzV`n2
zem?QjPfvJdweyXlLnpnr^|Zf#Ykui3Tk=1B=F551dE>hor*qttEPa0ANt}9`vHZbr
zfhB<=5e*OWV`ff;fP)BA83s$4ke5E@+GB1hy5!*>PM-DHVGrj7o2M+xJD2_4qaR<W
zRIj^jv+W3VqTC4b!yvGWNOQ~>h2reY5|sg-&){_;GpxTtA<yssuJx$8&EK8*tBY3O
zV><q9-`Z8zxphBowOsm-)o0xG!}5y~<(R_}<`@lw*c>O<Jvj8ScklbdQ}^fZwkehU
z*Yf^vYKB-x>%X-Ayf=V4c<j@}cOC?DEG_);u>V^<%Jcu(p#7=r|I=W_EtyM){vC<*
zMg1RX{1=BY5@vz;ujP0mBDaJu#RZlj{fC7xY3Xa?Vu;*Q`n9-i0!ZzD6-b}vrsU%F
z4~WN4NB@)PsMPci$GeBqfPqV%ero!ksnmZo{}Z2n4M$r3cl1A{(hr{I-EwQw%DoHk
zIOUMfikBXA>_yif@XajuuYza1cJ(=@o^t+?77}i39|>ZM3^2p3AKjnf{55+0B0K9J
z(W@!ZYzp!6$#{u!ZcWA}5P$6*?@m1AbKL`9{qcd{Up?1w`Mb}5SF+c0N4<a1F;`yL
zJnd^sqFSXj9s2%;4C^nNnbQdQS&X>(8Q)USwY?ybPP{@TjYTX@5-XL8VcK5Vi@!+o
z{|IPl#9A;5GC7c?EAS)cl>UcvYrPjQxuQ3+{+s4%ZH+I)`n+bl+Zqg7_?r|KZcCKK
z;?m7^$ZYKDVLEF_m0mVv@mpzAgt?LRm>ZiL8tTfmO>`rK3!5eOD|dUgOr;tU$E)Z|
z+y4<?e>7hwO(~Mdb#B+-)hGux|KZ2!Yw!E=y8{n@F0kz%_iS8JbLGqL%vhN`eK8*+
z?LSFp{6!`cJYFlfdf|bE_D=Sg{Zz|;e#^Ayz@0_My&Qhvnj;=NOL2d|NJ8Tw^~39l
zWxD?&8h@oD|BGGCm5R+Hn#f`;PXbRc8L%6Gc(W-0;*n^t>Ry1UfN22QJGu{GU%+(0
z3;@{@nF-h*FbhEZ-2s3(fCB+@0rLO{0S*Qn0yrEXrcM5Q+>=a*c(bDbBqKWpumF$`
zh<=x@3lOFNC;_CGqz2HshWK7Ft`|q$i{V83N7L0KjYc7zCiqLxQ%Jtkit9E&JD>w_
z1j5YlPXv(tpE<ZM1duHr8-O@W2mCzvF8G~*E&%P7=mzuv+yD>23-AGGAHfPh01yO(
z0JOKg7tjaj2aHtS(e8ip_)mZTbT-C+JpVf$^}lI6=V08B4{t;#>wib|&!x;iNdleB
z(fsphJS4m&wf=9zyR=40PFniOdAM}u|6!ynuD`TFkm!cKtrgO=9{iK;x0H1!F7x;!
z`?FMTn$t=xKjHN~gb3<6Z9~$5AwB4@uo@7Zn|#cXiJwZO{Y!hECr+4&xJU;{YWhT{
z#DQW%45V4k+jFqP*MOrYjTEO4&T~jlJbUx;zb?Dx_n+iX`S7BDK6v$voNd=TR+`@U
z*bsb7`T5UW2ch(%9nGRGsM`VLo-9I;ejV8qCEi@e>4n;v3IjK}NJWX13?Y(YbnLd_
zKJ#4NAE&sknfGJYgBvcs|AKSpx87NF)7@K6c;rsoN4wRM%#Ev{m<;x?nbW^ivOd%T
zM6?F%${8~+xz_m9savN%{rN2~zBEsF(Z-KTLd=T3(=Rw<&z8+A5)~nh`itJCo$qkE
ztYLS^67u+qv>Hu*$QcX;ouLrx?F>?SBRcdYu3pI}H*F&RoHeXH;0yX(q5Kv$7`D0_
zLtzK&%Wp#Lt*qDK>sy%9;tT{aXIi2x%2&{DVU8Xd2?v}@yv{I`U)>9HOktawwJ&!L
zH2Hd*-X%7L+EwT(b}5yPLWNaroknZO{t=VPuk=FM)@8LjgQJluamdW#kyF+CyrF>4
z?REz88^Yd3p-v6fkaY?oIAU>S&<Gu2AzW(@vAxcvJr;}3YVSei8(F8@F@G_>q&f>$
zdQF(kAu@OTI~qjP=yQi@{@KbpocKa)KZ-wb#QRyf^vb-w!_OajxKT^isA;!8jbNJH
z5w;>dVcH`@HcOa0e>kr|WBJRcD}MM*?HcFd-%qQ!w)EGBoP8g=P@!x1XF~afs0`f{
zX8k4<8Jf>&HBQp?qj<$eTDcMzvTJ~b%)lTXAY2QILF(zW$zCNC=$(a^9(~#avlicP
zdUK)W;5XktcEd{-oOt<ZXRRG<{I-1cgHNLUhTswoW6~P#aQt7q#hm$*PvV6uWgG#Z
z<DU36wnUZ98&%L^!N&yDT!#G8<`udp75i%36B(l{M4IN_sS~dHeu1`f)_ceHZf-yN
zA0K?J52z=GHr~AEk=OTl*f<46j-0O|Xr3pKWmp1Es~ZB1rIUP2tWAX*Gn7ANihna)
ze-lMsuh0L_-N!8a>Agdiw=aBgZs51)T5c%Wbj_jP9H~C|>L(s8-aS!^lOBIlrusLT
z?%&O{{)Qj_(d(bltBL%$zj^hoUz(0T`RVgcv~6hqL{n@m&b|5ZCk|fKGjDeN3Rm8#
zi7G9ASHx)AUrKNO6sw7L^!uTgAHU+-XP66rb?S*{ZalB@$JOucaqHo~{`P|It<Cu_
zo}H*niKU~75YX`Uv(n3dto$q0`8OTjK)QgF03`Df<v%1hAo&l;ccSG#6do=Aq5F9G
z56ON=&XY*~L$aSk0VKbf4>$}!vLK3w<UHh){3jP6%72a(!WY8N6Rzn!QT}rr!btu@
zVTFQE@}Ckwsc?The3Az(0W1Y*0m}gNjSjvZKysXFfDup*pl6lvs{qS|`x^MQ!nFxL
z$$uJzdy?CbtcPSZ(efXPhh#gV{D<!8T)dM3bZvug7p|RxPx2p<^U!mW|Bx@rf9RUz
zKP2}d`48QT@}DztPqLqt0FwU<0O<KCg1-v>slxSY_{rt3vi$ddLH;{b{zL1(yw)8t
zM&o7wH@Z0I2tSYHXgrAT45$D1zp(#X*8j=){D%oM(3x@o)B(kQHJnef2wE2*HyZjs
zwBEpVeu?P^@ILYV<izi!_`9P1Cut`BFYAA1d;Ky~`%~+(JpUYi{zRfSl6{hGMA9TB
zYa&EnI|V_*=6$*bKE3^O>%xJPSKC<i;LCdzU3KfaLd!i*?z2HLIroA@BXqDx)!SRb
zw%dzJYM19`I{%BO|Km{qD+Y&Ajg91@>3@9r|JU@tq^-XsuTiqUv^O3f1|S-e{LRRd
zL0oPWZl$Cz(yB-dk*jphA2<!4znwz*$+a(e`}@-o*HHke<)?k(v^S5Ol=3InKIPI+
zx&QYf-7mP*8RiW5zW}TTko_0R8woki0gye5^8n`qE(8$$|0RI5(JuyE0=N`#8Q^lj
z6@Xs>t^}+BTm`rqa1CHB;93CbH~kuLJ>Ukw{{emj_$}Z@z)gUg0lx#>0=N}G>xH)i
zeh;_<a3|m{z}<j*0QUm!1KbaI0PrB-A;80cM*z<Nr0_T5JAc6Ygd=t}_oG-3g3o{l
zjb49@T?HL()e#X0j@Exq#`S&xsxKjUMDHNs4JrL!xt&`b8ZD*20KsBg4Si#1$kPb=
zN(h#ge)17W=^ul*$^lZ#Pko&FGdXGL7xFJB_^0$KT?L*=O`rJP&0<`oDXnCg+|~g%
z8w}Ah)<i_Hu*Me*H8}0CapN9<=^F>FIR)uyE|&S6{dKNt7}zOuJ4vr{N%EH$djes-
z&*QOr9Zk;uP&JZ>U+7|X--A*lw<j(9&7eP09^HGdsl-!}tU8Qk9I{Dm$F`sle15Ms
zzr@0y=Sp@SX#Zf2u$y(lgdnbuf!#5({j~t!)PO^?3kxwm<GHlae-X7+<>ab|d1Z
zl;|uz+Q)}s#5uAb;o%mQcw2b`h^+<Mv$SD}w{S*FI5qg54^?8VJ^Fh(=SYn=;!8vJ
zw5fD-h62AEJyN10kT|7Jdn)(1>V|EF7rk@O=3gpxx~A`nUfuG+z@Ksb!@Z`H{2PvZ
z|M`oQy7r1!e|_DP|5|<8YbxDu+MeBu4fwpA&g-Fzo2-FOX9z7Ea`^(D`0C%0Ee&z>
zWN0Byn6Pi-0L-&^@*6_|tcjj*qITrYTGk(OdV{Qub+e&?{IY(hom(0ei$$GA>_>Y}
z#?5a3#jwddUTT9Wy;gVFX|XVSP%7Iy>+ZOjK}nIDL3i|c?@3dsFK~@V16S-sJ8=D+
zWc@P!C+kn7P9o0CXms=X%i{UBN=`eIT5qV6@xP4!#i2VM@&7jG^>41dN~wFc`GmLo
zmbN_I@sv_G_|ri1t*gJ<c0!;?XV-ofoYOh*?0au2bp@|qYxABptMLU>iLUsvHKm7r
zp`Lou{YAPxTGnnk<lpx+e0Hoxw<PDouj(JW{%N8$uTGuVU-I!~zc}Z#BHj6yT-IWk
z_lRcBeM;R8N5A>*TPHDH`(CWn`RdzlxM1&fD?U1<NO#x#L;kw)o<sZgZ7b4E|KaZ6
z{O#mpI||-Z>CQTG;lNw#FZtbbtwp-4&;4oJ%P(#*{brt8x2ERL|2@z4_rEkBs?wdj
zWXrm%5BiUP*;18m;e&s^c)h9V6%W#)5gUCSPoMY4b80`a4WUr9ESf&&gZC_fv_=IR
z5F&jh?tr+9ApB_hP3ypkxeT)LnV8F<{E-Xcd2%kpjPz+IODR%_?PZaC^YMuPBJ~mP
z$xjrPEIiS>Qo~ZI1u=iqaq$!&!rAyBC5z?NlGh?B&5X>eC9g%YH9s=1mZ&ep^J?+@
z+4wAlbpClfw8?P%^Z0C^Ao?rKB{Cl~GX9tGzf`;~^*tH?j~^;VCw`^Jd8AJ>qxmO0
zhs9nPhm=3Q6#bANX~tz~hDw}d`k%f;a7S>NqeuS}&1q;%k|H^s-p5<H96$Jsm!QUv
zO<tW|4CO6(j`uRLn2(aon%d~~T>yXbGC10CLRKGn?siw83#3a)M$2}MR_et{1dXNu
zCo`p8gS@@l@jjZUTmh`=&<-cs7(}Z`;}iFh?AnbIxKJWqMn%4OzbloAFX^t)PDABL
z=zg5zPXJAJ_WA@ta+&Vl5pi9LAHSl-f5N`fPEsNH<1YZ|en0dS`SeP{&*EtpH03Cf
zR8$P75|f5UwsRc2Zzp<p)o@lE`OS)MapxtILu-j}<R8ypZvPicygeZ5f^>MIbC8d<
z$jC{R`M@0!%a_ql#O;!lgJj_@j(fClE96A|NbiyC*+#S=EeNA?<!HMj?RD>p${Af!
z&f8ZletYZpYmDnZUNmLWT|Jy;Np!o4JvZ*tgtyV((RHjOqmNtBCytla_5H&Bf3oGm
zSMG7#j&<6#PQSeEcXSmUvzRndMX9^LsK*irefE6ef}&k4Wh6W|A+vr=`**Khw`Sj_
zcm8<y#5F$seg|K_xOnELMc14+Z})9KwVZeOm#@4<xuYfOXh&h>;?D!m#|aie)S=`8
zB#WRKky9iUV!exgZ2dPRD?Tj<hzD55+OH+W4Htg2OV*jFO~sUAeJX9_+#J!;Tc-Oz
zGu7W6UjJCiD5f6p_~cpB=bp2EZvPcGZFAoKz`A+e+e&`D>8Ahu_LVgYo?Q1dEs`be
z0mQdUwRAVsvK;N5s6j)~+2(BAIONyTy72~$L>i07LBc|3XK1k=Mn->&nN1n?|IF*Z
zlG7XraoCM;2x%7){p3$WAio*gvm1a_%8~D;?N10BM&6H{gEs5VEgQGCKEB^lf6GgE
zZT#f($8WuP-7i<acJD-O(bn&3&iy+oAVFKCqJI~y|ExhRDuL7J^g|71lzF(L(+&yG
zX}M=w0pn>ArpF4T0yR%RJ>FggD&W9~=XEBM<Im`R7;Aei+vg|Tnf3k?O@-s_x7pCQ
zu;U(<!bC&SS3FVuuj^)qXP<t>nc+(3Pg5T5n051O51s$@=5ue%z30VCUQXOQc$K4#
z33#3ZpNH)>d~^VF-vq-V1%9V!RmQSnO_?%JSFBOzDU}9Ap0-S>%~L3}3Vo@<Xf*28
zt5(7SM$pb_`z>9mUu33Z?}BElZM2TOTX{+e=k%I9b&;_&PopVR=ank7+B{XMQL8bi
z$_k4WMXPSfUZy=()=M@XCFb<$?B#T1yADzdv3_SP#JFmNYM<Q-1zKyAxlk@Bg+e@)
z^+rfgh;p~jHm=GbSUKtO!5Wdp9<kLUeMH}ty>_sLLm{8nLdKd>sPt#Em*H3xRN=Hb
zEKMxTPU$fiY&hr?#*0!Y$o->TkcK*4oxa&*UQWHi+@RNKs#%)_YS0$bK&T5f-J(<|
zbmj)TjUI5m&Kh*$I)z-{GupY9ex@uA9(7tA&LG?Ag*hteT|E1!CXq3sAnQq?4Cjwd
zwZ|Od>R??iX8>CoSgTu^<-EilC@YyE8H;*}&WkFAdU<ek+9(}U**?G1>*?iCDOtt_
z8=FGDFUct1W$qxhUj)q&>H}-t+8R@JnWd>rTVrmhw!rFJYB?I~RxM7Ub}q=Mb`s1e
z>d;m-EoRmeCWB-SODbLL`O(gdbnGh-zZkB`WHh5~L#*BH#3Ty#2~()o$Ff(HkdwA@
zg#rVXUe2x~JD4i%ej<CBk7Yvq)=-zl@3wlq*jkiA?cbZd0`##i7MMzCf)w%$K`I&g
z;vBndT+a08F}i}rrbS9s3CSpw$|8qKRjSH!s*4?Y8l~NlXH^sz=Q(XwyVb6-m)Px^
zRl*0QDSn{lq#!AzpjJvw!7$!u(EG+6adF}($$k>>dFbC`8~Yd26oP|nE7q)~_yB%B
zf4!1z(bYBdDVCRa`m}Jhjm=$U&7HW?)2+5suWg5?8+3XD5L3am)iyRLDz$;2#$HrU
z0S(^zW~B}v^mq65mbBM5Q(%?7tgFM`r_~yQHMlJ{Xzh;P73K6nLj}(5(6t!5D*9U-
z4y&%e#b_#2IXoTeU}4bOQ`)7dwYB!>nnN|Zc13N!&Q)z((bTRhYVItrD%CI7;&|G=
zfl#%^qi40b%Zti-gY5+dU00Ezg<VnE;)4a<cAuiEz0+@O)Ks>riksd3P;XgBx2r?d
z);w5KSU6aiTO2H2UNY!k(d=?n^%XmEi|i$(%E97bNw}Ex1qTY3hr$}CHe_sWujndv
zvekh=e@#;vptsahTUx8D?=y94!rtQk?vC>MGE<YH)ZNz^DmAgL%B}{#Z6H{#>vI%?
zlP<O?4TIHX${tUt(pIi^lxfRreb&lOg<e<T>hZB5ho?l@Qr=^0_jGs~0v#)SMY+9(
z5@Tnxp{S)#UtQUz*Y#BeI_=8bK6O=F_j0Y);p;aBbgW9-5YpQkm4^O6X<<`$H#?|z
zuF%%B)U*v&)#yz6wg!i>xV_O=RM%Zw-`1?F@2%_z)agCV1D?wE-foY+N$azPT?NhF
zwhotGq4RdBx~=Sr<vq*0YN|RE`hvRZI*+x<Y9A~OwcDCKJr$*uP34}7?hborxHxPJ
z_Jv#htUb50wjhw}>oYW)>T7DPMq7KXvBqSx+uVb>xea~gx=vfJy5H9~U@R-`s#?K1
zO~vibhCzF<uSVSxsMa{^`~&WG*5~&a1_q3#c6Dus)!15KsA}o1GP<?CfWEde(ACOX
zJN#v3tj>o@?x@hNXdUo7TtRh{zptuBTisJu>RHk5?hFj7OB)L`C8l1Nx7O3vi257o
zYH4@4429Lca(20vT~Y2WbhOsA3{-U%G}Q#kTbGv_f=$NGp02Q3QJ`J!DJ&|gE>`t5
z6b*J4s=5n<Y-3TquFuy|)az^NbA{`4oy&D)g_<&BS(&S{yu?@nHrJ>wQ@Y#A?cqLC
zi>)iDDR8amwi@c{8Wnb{qQchPS?w<j6_qsljg@|vDbQ5g*yzwyRkqYxOT*=Z4ZWV`
z*6QL4U->}QAluvKtyC)2^%xLUdZn{<plhJIzFgDaVQX?V*lVn{zLv0}-WO^%s0TYL
zy+sXe*3$k`dst)gd2L0SqHa%bV{KbqwMw_VT2resbSq4?E|0yUBe1;ERkeJe+1afM
zY1=gg?fqqyzKT*?Ax?*O##-Oe*RHTFuWoM9>w}7t(zZckRg=?RZ*6x68XOe`Y;kw5
zyTYa)=y8{KRI7EK0-dS^by{!eY%i%F^s3#O9#2`YutZnXQrlco*wR?!3wG3$v06pA
zskgGcQdc*)!s=~k7!3Ei)V3a@)?3)#UDd{Rv5oq$udJ|BRbSZaFI9C8_V!wJc6U#i
z&r?x6P*7c8*4ymYm514qqEdx^g(2J<R24hh8rsyIjf&a=S9L#Y>My8Jv@1h?WvJIz
zRNZ2#FI!P<YVWM`Ra$jyNkO?up$}H5x=I~BRc>9_7N{(*?rREZ6-_Ojy7qo|WpA@l
zS6EZUT6-G|b*AN&)$M`mKATOct!--z*Zaa{ExA3NxhlKAZ$Q)6)@CX!FtKh$N!VRn
zQ|xv0G&pVTMY%1xR$WzxNn7pAt*Wf(Z7wcouWV75>9uXnetUUgU$I?VThidF8!T*P
zRc@8PFkq`_cj|Pdxm~rPl9tL)y<TT+Z>wsm4|cZpsrqZX+smtM6{Z!=(gL=|Z}+$8
z+?Cb2h83Bd@vZT!(6s5BN~?n%xh>vMzr9D}E@lTx>Rqg^hwaQ&G?%oru;KoGtJbM=
zg%wIyjYm=69SjnF*ETk{)HN(G)VEhw(%~>MZVod6stAV)IvlW~vq33b8^)b<ai3VB
zWNoI5!I(xTlYW`>%cNf>{W9s7Nx!Dk)o87Ax0d({+Dxk6u&bt_qFrCz7Q*~#tVw@G
z6gyCg>)8Oq_)%(@A}B$WXG-ixIUbq3%j8`q?=pFp$-7M6vq|3PM3MIZt`8*gzRRfL
z%t@xl<I0pBh+V16axyYAAu|&)Ga)k*GBY7F6WL@Y4vAtW@^F1PF%#L>(2P-Q;<W=(
zR0iKW<}2$cF%QY}HTEgUEQ8E4$Si}*GRQ20%raz?WuQ|bL{}?Z7_z?;F#&1jf>O|E
z-G}EH#LsMF$29D~BxJThW*cO-L1r6dwn1havdK2w7-d`OK3vlw2Zr&v>2@F5vm4o~
z+nAV7$Y}rXSi~uF0x~Bca{@9aAaepTCy-4}AfmvqU<0i=klP6a2nluz(vEO<3Ic@v
zqc<{&ACHz8VVL~wfFxw*L1rFg=0Rp2WadF;9<s?ixVP_XWSFJ6_7U?iE{#u24TfN(
z%-J~r2BRUd2q9&2Fk>5~%mc_gfXoBPJb=st$UHzcd4TrqM=cz-p$*q$YkDV8S`;Se
z8CZXvnc&;eQY=iKyRlC}W*lV3L1r9e#zAHrWX2(zjKe%yY7pF!@TUTH9>yVN@gOyO
z)H4%&JIZ7|D=El~gUmR{jDyTL$c)2yVjSvl_8OfROXrUgbP8vAg>XDx3r+$n!nt57
zocfi_9m`CdKu;F|=zPFr??s1i#?2{0fVA{WqCbzhM*1<Qp~XB*a`P_2Q%k=nNlXlr
zPWpdDY##_QN-4h@Es)HmlRllQ{i+b*u4hfNWqVaE*udhNPR28g!?RbW#~<2a%5>pr
z%p;UpK$!)USwNWulv%*>!~*VmEWmaS02i}`1t`N<pNy2sVMNp856W5)P1|FR8cY98
znHtN~Sf<7@HI}KdOpUX-k`qmh4;F3=JKxEP)1r@CEC4-D<#@(a-<N5#Oq*reEYoJ0
zHp{d*o3uIFrsM+9=0U)i&y-U+LpWxpOdm>{DW~?POpRq~EK_5d8q3sJrpDQ%#?don
z5?gai*B6ZTdE<3{r?;lX%zDCP_P}9DMR2`$%vZ-oORWt)d)R}6e1qmh38e6<OpRq~
zEK_5d8q3sJrp7x9H5MuJn9PkGtjpyLfON7}w=xA2M$_XzOg=YG|4o@1%hXt=#xgaQ
zsj*Crvq_Ej5J@nTvw=1n%mo-89PPQWbSpkS%%VNLmXI?TviPlmpfivH<(`%B>*mDM
z=?wEUHnGZVfy@@jY=O)c$ZUbk7G#qx*v=K;VhVta6pzUyJz5W2lLD!aS-_?blTXs)
zlaT(VOpRq~EK_5d8q3sJrpDQ%#*3rKaRILB93jK=*_)&{`s{XRz#Mblpvh=9Spy-~
z?sghkcPOQK`-~x9c4azemzu>LmXOR#$h?HiOUS&0%uC3;L^gSeC{}`bW&*>Is=DE^
z?C}yIr`zdqh5`eYUN#uEy4gW%i1m3>5CzOl9z{o{Y(!R~mRSXvRghT)nN^Tk1({XI
zCaZALc3xr51`n<k#40?IJyt=O(+g(&{nk*I#qYLyy{xx01$JS2{D&uH$|MY*uQ85Z
zW)@^-L1q?YW<h2aWM(0o%))kF0T&10RKnTBEZmztW}%ODu}~uvgbOL~2Q!9zS)VC`
zumfV0*#MahklBEph7EAR`g;y6y|YXZmf)?h1fRoMfDbbcg!vdVY>z8o2fh?ndfa>P
z3v;&OL(BleS`cbxn()MjZ-cn5!S{TqlF=~v_`ZbuFJ;u+&&=4F08%yM?EtRLz`taX
zTn`)aX2ygtAFR*^kQWc~Vni<7utgul^)XC8;woYmaH+6dJU%?}F*`#k`jIouE>jvS
zTEN4VX45XvHg@z8@AiJjFfZ)_WnwX!Xf)9Hd3_fs4}S(l4_e9(>_C`g*T4%w?tG11
zrImUyifA+ixHCg+Tr0h~3-mrJSAe5zAy6C-C=RvQc*K2HuD^Le6N$EpG*-OdmCD4I
zbk}GnI(x^CF-3eA-}lh{IOi3xVLAF`h^fFEH0l~K25o=|_>t3cJdhK8IEUlLJZSkI
zj(1amd&}XtISR?)dbbrE7Nye2Y3R}SJonX%uP6?}zE&<B1JbY~O&Sr2azd&9B7^TE
zCF$YHXHK&u3|9~PA}5q1f_Nu}lTP9v0O^)^v~t|Y)vTSXZ{nJLXkFsCy{LQY30Aa1
zkg>!(v7l|79G9LCDmj2s<UoQ!j{M{K%kBSSiMIztU0j6g=p5w7q?0cx2hn+s&Y252
z3?Sz;gCOc><@&Z0A%3o0UR+sm?Z;a_T-lkfsGQLy<-C2>;<vYczs9)!<3&>@-POaL
zEEV0Z)Q(bJ^mlYEoCzR&-p9>&D9<7EbU#xE2;tchhOgY?FawJG^0r^oRdmc^ZfJat
zG;YLRL_L;3=(Fbw7ZmMcDP#J%37Pd{>P6SCTeEM|JAb@;;u@cRzk{z|Ts-sBqHE5Z
zxBIrATFyKC%U9l_+|k)y(T>8%#p9HGrVn9$u3jV+AXx;>h@5JnB2x4GRc!q?Br85m
z3&Utq8enGlPGpKEi)OA{x_~YT(K!>v8J{A5Po*VXD2~`_?rvj-^-nlFGt3t~iqpj-
zk58U8eeOBy=k{N5(>CYr53HNly{+Wen{N8gZ(mun;K_AQ?@wiqZhg9@FmlAV2Qi8h
zW3O^NCrZWRtHrqXqvE~HQf6;vFW}%Ah@l1z<>tO`#D8QQ^0I(#yg?(8#^Q09Vbj@d
zT2MA4Q--tYHf6p3D^VJv%!J)2?<BXNZq|rUnn5=*&7jr|z$)d)chmMKvG3<I-S~>I
z|ExQ=Y~0%V_<l?MEic`*@srOVzxC#Izg+#=y%V)XTfeJ0_wT5H1Z_d4;-XuH-b&_v
zifTO#YEg+@b2`+e1~bY$+$n**6u8oI&$PIr5ugR=AqP^R=IN)$+pEy74vhHNb{mR6
zqv;txZb;HZed5(Dx8I#vPM`WmG**eGsb-8DFP<uxLj3p<bj1_Z|GI8=c=qX6oEffk
z{xs#`j#)Rq_R#rXZ$9_N+<RWU<Yj7e>Icz|!f5ntOu+LT_&meR#IpmK`?9wLBIqRq
zLjh~Z**TEH8pti#%e2SJrlS1$boO!@tM(CdICP-G*4$a`FANoxH2RH|ewQiGRNL6-
z&{S2n)LKi!<%12qp61r-;tF5+K-D1I+vcrQD%JHWho?%fbhZw34OG{cYx+BEO|Axe
zjkVU-5?0jvLd^#CU`M65sG-eT+Fxo9YfL_`tw>YU?dff-ZL6zR>EvZ#c^NpH%fQnJ
zD+=yd__F{M#<TpmptTMubLjC+|D9HM3g~wI&WOZP$ba^caLiUj9u;#zDHP(VtT#e}
zLX^9GwsBPkK{>O>=XLljcAqyC@VVV7Z3ew7d+lHgheAFtXM8G!N`E$c8IEN^D#+|~
z6E@#k^w>vZ1J{?@Mw<IayC4m9x;lNc$-JC;gSkPk(^RuIi^~_Vm<B>!sOc7^LZLG^
z*lqNH^L5ss6W1wJ|2RB@I{T<b7^X2yYmoJ%Q0-%J?q|F?)s-txDdC)6lcz2+mgZ?R
zh3dRgg;txVDm7{~231*Mv7%_zSmY(%YRt}(msHHkk5@Z>JKrgGW8K42IlE`4I=NsM
zvO+PgH^GkkZ8_snRTv8?6U3D&cG<X;^vz>*1&vLMl&X?Nibba=l|>Gfs#KNdR2Mt)
zG)lW8&#EXc&U4zVcB@@uFR|M-tAr0qQ~W@!Nc97y<PQwveFnX6G$#fw?8Hn}&SPf=
zJ2ZA#6<t<EmsQbs8dY?8DWb8+7wo7hW3`HKQ*UK?rLJyph1J{8Fc|K2sck(*t+%kf
zyQ+=tVjJ~gUs++Ns=lz-U#jXH?CrJc?Czd2pQoaDprE?Gthd>(D-W|JMWqV;3PZRz
zs48~0HMFTa8x^$$uIhf))L&4cXjg{(%22OQUW&-(QpBO#*BlPppcJkR<4(G`PpnX~
zHdDr6Orw)YzfAgN(l3*KSqniX{n;e_6;Lz~T`4ZO&FJGtscDL!1XZ3Xu^;7lWb!VP
zcbUA)<XtB3GI`G?d7mTFcK89nfdIoUqlPmlnI4ZTQ+6PBQ6S67$jpSyOvucH%uLA4
zgv?B2lbIj`_@bj@?hXeSvag|;xia|P{Hnj8LT();<{^2$#y$m^Wsq40nPre!2AO4$
zS%z$~3}gsjbhW~TVVBqhw-3)Vh<oR3W5+b?z$9e0L1r6dwn1haWVS(O8?wnZ+!$qB
z?LJ(ST?E7U+;qDS?b(g&)oo17CuFq$cP!$RIRTjykU0UF6OcIpnG?t+ClFC!Sg?WC
z9LVhi0)#|XAp)tHGTJ|StD!~sc(lX_!{l!VBq1{oGV>ra4>I#0GY>NJkWJ>ny?tLJ
z!z{(MkC=yXX?$X8lxf<A#3F=pHdGOYX)vG7*TaU)T35&!$Z7I<f;}pQN}1zegFQK{
z$J*&M3(AdS8>P$x$UK0|1IRpp%mc_gKsI@RYelnD%wZeaaO?`%n%@bO7Bjd0Ix~Ug
z*wIofOrE>3PeEoJWX3^e9Aw5pW*lV3A)AcDJX&fH+>!970(Kt8A!hL)HG9-E6MQ?$
zWIZb>$c%%`ILM5H%s9x5!+2sG%uMR%7xm#}Ll+>~35Kelj@r65U%h49-i22mUHj!~
z<3U$eKfGq^y3WCyH?I$DeV?d$^!apjt;MN>bf%zquI=u4=cxqe)y1Dmob32dspqd|
zdirMX=w+Z0M?@Nh10qA*iKTRuBpnRhiK-6a*i1T1m#?4`ANzK{b4^n>uQMh~Kl%Bm
zizhFgteLEzeB9*f$*PKWr%qRz+f^GXX{ij=>vh)lwyLK3U}tNes=v0oy}a61VOrrV
zEnsW>c7Kb`U0I!LSiy#jWvx1OZ-u9%wNsJnt1cNVGdi>_g}PQpmr-HQCga#b-HUSz
zHn8yNWK6?2JbPuj-xqXR9L^xy>E+HPGG)5(lqf=>KjmJnS#`>G9lBg(>`I#oV@%
zL(f?KMcqfQS0C<t@8H8G-9YsjKV&HkCsc;fDMMJ>qf?z@JL%BVUKio?)7Ue{H;l>o
zn{+xle?B@L^Gm}<g;O*MlAna0{1ndU(@Uez=j4atqjPstT=&hT+pc@UR(JgK?52wO
z%@>wmYW?tonfpF@+1bD4G^mDFT@zP(RDaUXip~^Q#hx_EpTpW5bOwTEtIcfi*~1>E
zHxx8WU&)N`Q1TRD+)V-K9NtXMZyq|LQ|r<1?3Cj_8Bf#zY(h0gr?>N07TnSK_70G0
zI{mo^*d}t8^S#UGzkSK-DYb8PEI#)$<99zEbK5^(`)EedyIUT4`?K}u*U)M2L(r-R
zF?)fr=nUy{v`vVq0FI?vC9D>IUih0^^xsWfFlS<&<Lkmd-MXmsjU)cD_V~XYkTBvz
zqkA;Z9*>;tJY0{8yhZr)sSMkHfRRFFlK08`GfY3eJ%|DZKxLZHR?^N7r7zq7YIkD$
z20%x9xiJ~$`Wnj&!WU0h=V{Kw-2K;__U+dn9N+WMrQhCmNI~a2>-%PXcl&=ozHXZ1
zG|PsZM9r_rQ$)E{D^<m+g&NH@uPgVsHFRLJ<*Oslt3Um8b>CsuC;s`jMBj`3GXBKs
zRK`m53F(nha^<VuJxWS8d^D%uTCjH8(PdMo&A<DqzPo(?_;$(9|245^B+3m}82pVi
zK2mRwSSlG!@6F8coIqwhF};#(%CP^I_t@G!9iE0j#|mFjZm*%l*x76-YU$HgSGMVO
zeN}-@yE3;=UDejTT&s2X`i%h{tI{@v^tMK&p+8Vs*wo$44(gpNv^6a?ZG%-cvIs~P
z0m&ku@gxG;^+W(fFhn;SN+;wDV|_AWD(78Jk3O`MK3otzw-5Tl0lU*2aAJOCPXA4L
zHa6~NV=^`N`8|e#0i&s1UE5(bwiXzwTDq%@Zmln%udNJpwX)U@e_0u;^My)H9TnOY
ztpk3CE2wVr_f^$st9!~yJuBMXoq<7hX=8z=#MJAOsj*CrWon#Ffjm!t`Tv838^g|b
za`Lo>YKzGlfIJlRxB&FH%pGI{pul#YHx%%>-6SY&)Yh1)%PdW0+8T2v@5{7Vrp@w9
zS*Fc0ZI)?sHfeLTP3Z-o&4Yk3pD7oEz^+_*N(txmnml!pu{2MkDOBf`Dzw@>RjE;{
zF{sK4ixowyVrI(pp|qKDYH!NaSf<7@HI}KdOpRq~oK0#hOpZB^#MYeZ+65B~ixl{s
zqE#8oiZx}*JYBIyou^b96nWY*r51~zT7|w;VKf@`>QyTj*RVmmk##$ROIJn<l_UB+
zb$+L}rp3&9!esWyVM#@By?4x4$4XBig;!;2EK_5d8q3sJrp7Wg-dU)zNSViEZtP%P
zE@uGRd92l~Ol58y|6%gEar$q{)L5p*GBuW|u}qC+YMf1KyoX4FnVb!aa6K1bcyP4m
z#?tq`_%Mt1^jbpBV94SZmyM-LwPz*#x;gQ5I>S7TO{_9oAhQKBTOhLqGFu?C1=(Z^
zwsQrzm;#_f3dUrT9<7J1NoA5A|6%e;dirn5)L5p*GBuW|u}qC+YMdH1&V2v!$oqds
zvKLg#e&gTk&FMeB{=h*$R^}}J=Ru1ey8oep{io-Cby;KZp;fOCjUQ@3gF>0OebIBi
zymz)~zWL`#hnB8hw%b(?ygTv54|)}UxO2*7*Zt$|w=Db97x6JE_>IxxhT_N`=~XK=
zg$s*{R~oJEpz}(6c_bOjFa>-rtgas^7|c+?ZqaDk6tM_~f82s$mTc@P9{n-}v7g<O
zVu_L)FmfO^7k;$Up0}02ay#XfDW*U+qB8IQe#8<)J}j^SLAGXCm}~NKdE@u$AAZq>
zcTP~)CT?z;T~~9_BllllaPxtWzmT`_g#T>TExqB{zbERG5>An*^s*#)vcoonxHDgx
z#fZ_}5kI>26UdH`N3hj1+T$v^4~A}B+rIjj5B7UMxw`*~Q(pVxQq2hm=`a7&O<(F?
zyX?o;Pk%2_oBaIqPe2_tctj_u)3piy?=#$gAKU-NuKa?nqU9?dnK$#9u8IFX_0IOA
z=I;5)I~sQ}J5W;ZVs$-iXRe~Tq@{%o_xD@1PMs^PP`YY7iu&$gP)n|{xuvdQd7-|&
zvT_O4P_&!FOn}DH;;5hkT+=zShUc><q!o@@;f`=;gVqr1^O{XYGq(AJSi9Q^H7T6Z
zlEP$e#*i<&GL_j%&EgJANS;f|b4htFDbFS4xuiUoOl>YXy1YabE5ST7fnjK?lHsxJ
z@e(1Y+v#zJ0t1#_HW;?L*+Gt<NP&Bpn>>n+OxcL6L@l!lGOHl73Nou8vkK#uRj4}Z
z{woIl@wWYsXMcG8cO1!$myfcL$#@}IBfFhe$hh?I!<>audc6XYE(*24;w1|^mLXi}
zIcXdV*;)Wsv6d?{k!RMyUMR`?YM2&SLnVn~7^#sAkk5fC4f$WT<!71NZ|vb($ANzf
ztW#1;kquEFT7qnf26oaKAzQg**q)b4Wzr``wvd~;ywfN2GY{;rMr@jkryY>2H&U;$
z@;eu5KQ;TWVp>a?y_vlvoOcj!8#vw?Ys9*4ybWFx4i_*pCe(a@8<S+q)Hw?B?_}#g
zjob*O{Lx4q-cIt5AD(pY>J`fzXVu>N)ou^o#C);#kO{Ttc;9@X;qrm?yB7oq)5PN?
zhPfM|sg~c4^3&K5ThqdwWI=c<(yH!DUby4+=LX&gy%aiN!YQ+FeE0NEn*R9P|6Drn
z(0v-cmIIzklpAi@Xw^z(=|Tm+V8pK@Aw;cQSi%RLgAe1^koZ#Rz0XZL>C;EvefL5;
zCKiIAl0U_XlU-txF^4&ZNpO7haJHdS?Te|_Wqe;UqNQVNa^#WCA*|Ttn@?sMuef-{
z7weAt!PIlW5u1Owc&+WL(+?>v=qz5pa9QQ+2^UPb6}hB#6FGgIQ^b|M$VwlslAfQD
z=m3#gMvF5#%vb=h0>u*YB2-I4`_<jgFFELr7nhwiu<uE4ezo$&<9nyww*R~%Z#wm*
zW9IJm)V9YGbs6qd@J#JrXNrGzi6cKwqUz<`oDClM3V`8>?1>5lnZ9t0tKS;xviRLr
zub1_9rXVhu9{=G<nF<Mp&(|17FAED~VSy|xkc9=ZuwdK@3w8-+foBx>{{tp5%xq#7
z?#&*v(8s!1Y$FpE7gFF4W(@hVK2ru^2gE3|0Xr2N5I^UOKktO}vHXy;(OlAk>5m(C
zU6}sR88vD_el!1wrgP>mTk+$+pWZTWVe@N8z4qSc4NpwCYTq{|{&M!rkFR-y##;RB
zy;%6x%%5LahKNZ)i{?|L#+FEwMd6LeLJfo%)l3<((8QF*p9dK)9~sN}aco`D*)}26
z4M`UGGoidF5FOq)mQpURKFmm%M6Hxubdv9VUby#3o0k@^d-Kc-U-*;Pbj!t0HGddf
z|HHG+*Jiv^a)>rj!=U<eAYWDBNBWfXPmY^3i!<m|D~mP73zaH}H9}|<Eo3OE%Nj<~
zn;cnoki6|1FiqD1^f+zMgd8qMx1uw;e7J5vsE*N~3z3BS;SBZnev#7m@h5oApS%o*
zaW0x%GTF@JOjb<RPCgzXg*cwG5XW;CPF{j1g_CobnRuqhQ!RWt70ike8N`q8S(Cnf
zW5!?dF1zv<-(PUYg%t-L^0$w}-@kp8?%wxLQ0CqB%IYr@HM6*#T4@-pE>re+N|m;9
zwWCa1UhA_~b}IC`3RjPh4LLj|$`<)l0{K*e3Sap^)gas3=B-pJ)%7Zer%JDMwhnX+
zRM(el`a5h*t_FLJwbs`XR@D1K%?9;gN2Rx@q0L&_Uuq9)Og^uzNK@49>20iStE*P&
z<Y~Z8F%5{8AM%p=Fh{*es7dG8negufMWa=N$XZ@J*N}YGA^OC$$Xh(#>Rh^Ex8p8-
z@=Mb<f1mUH-A``5^bpnTSyu<NZ~gu2<{LV@u1bWrC@k8M{GM`ZK~4ua`pch0+XA{p
zG)%`~d^<3`i1CR6@b@~Dh?#)ch&dsj+%z)0*k8liVfx?a3gx$O%07*uu!HsG*OT5)
zXrRgGa|iRASbs48wv_Py(ABDJ%YRYWc-D-glynMS81-sL>ksE_8S-xe|3VBCEh^i=
zK+!@BF;O;wys0id<<m{S`O6zmpP{<PedT^Rr?=m_&vjpX+_3g(^>>#X(nebNL(4E+
zo=JCKe6CQt3SGr6rP5KTu&S-QQ8!qUr1hI%>m#b^o5HN$WVQELJDvHBVISGtG+0Ac
zis7)u^lqaw6oSY%NKcmbSS&icjC&*NbUR2}p4n?^KCLG5oXBuoV%qGnP`wREgGLCA
z;@I^|$_6=c{VB!#GZmSgKd$7MOh1OM|Iw+39c=u!_uk^BHH$9S1?K#C>$92`)%o|`
zaM|mZ>~#mnuMFv_qp^M|uE37G+IzF}4SQwF-8a3cIP$DNop^HVnY%6c_=FQ@Y^XY}
z_@#!R?>AJB@mUPyH2FNi9+iSy?Fsheu+fWnH9j%*bk~0~o!=x~|4P37D*F6*aT@nd
z!NlDB8@rw#yrF*8<@a9yt1Ty8dC_sV`_8!Nm>1^WS<AkisA|BEvrG<vv;jIX4&tc_
zo!iY*9Vea-jOjco-FeK-DdGR2^O6@*`hF<=NH(vLN*60DdYg+2+ACYsWqNI!v)^7`
z*jH@V)|NE5>IMs2S(RJmFAUgry)y!*QI{0lvG8XBC=3!o&b71u6HI@tfuSS3lPPhC
z$6QN2@cK#9)F#KYo0oSjdFI~R>z6&9|5*32`@ekcgFiN2ldI}K;c2Sccof;ghRj+Q
zHe!u{D!JWTyix?evP$NwnQjm?8U&2-ed80`1}=T@|E9is$A7=fJ*chsh3|Gd^LHE7
zEq}b~fbd@ur3_2jBsxj`{N;#jVtt0=H=6%WPmYqT>EiWoL>`__rH-7v{86uCmUZ5h
zcW*i&cjcqM|Dxi8FHSt@eD(RyDlUKGmH%G<abu!t6U9J^RV&rmqFb?aBeKJMXlo9~
zS2YXV5Q!FfX_7Rum#U^=@>q4lkH2^lNSlml8fJ}Z!ezN9A2?IlQB|<&!^ZayEc>wT
z^ryGJIOU!rPJH*Qb9-LhjYD+tV{ZFkizeop6e(=-QborqvIvne!}l5LSq#(m%3l0M
zn*T>&hUdethvOmw@Ds}~lSRMQd-0MhdL#N*9+=?`nx&iEi&d$J#!j@7lUV0Sus9$N
zqg5+Ql*OXj*mcNgypEVXoNBIw&EWy72S?Vi9&=-JLqlD;wux@YS|P&oyMsRO%2NzT
z@6j~$4wLbn>B|serRmXfgXkv~v~R?g0B`T9DQW(-m`3^;gD_;wm<9PT#Uu?tGZqh=
z+}ehjITdReLC6OT_!SCcfPp?H!)#0%b0YPyq1;*K*u9a<G0nmQ$a8c}n9<io?r%8V
zUxq10Nh*?-MA|48F&@M0iP-*-G`1a8FH)%<zY^n!!VVGI$1q2uEPu>;S%z!lRQ2m2
zdqA05QYup&#)uTXdWKsIdN8H9$6`zjrSY^HN&3!VZfvtS#w;SM<mo@d9D#bt{&-H-
zZzGI$gqKOovGE!U*&l(!_1fgvo8PS=TDM`C-OwtdgTJ-toksLcvAuUk%ukI;U!_$%
z-f|4tpdl7b1xYi(Xj%di%R&56Rnl@u!`=}w8^aVKzTYQ}Z%5&8QSLRe)Q!OeHFNqU
z+6H16I)XU+cwqR^BSq_iS95+n>n)iKqebSQ%~)|E$yYg~(M{lDM#gaS&_*xqv?Yt+
zYVnFxQM$mP5jN-oZg69o7Cp;mm}Mvt@t4C1Tck|(7|eG@3uBfdKP8Ay#55Yje3DS>
znXWh_R!CuhB^hR2)?0S?(MLQZX;)Zc_-(P4NI>|Tte0RU?HPtS3i&*H6!V$N%r16+
zAA_w+J3r1%-0Jc3O|oe9>rt%L^kjZEaJnJ<__c20hY5FxJv0Y-JXy$NrsFu-${_>o
zKiYwpZ250T@e#v~WE;f7K5%7)QL}nlM|?HwxlWyC@}rOD`6RvI_f|*GWDiHV&P=IX
zW21}4FuBOdohjvHq&ebPTg79TImmOS5)x6)$$+LZO!oP**pU&Dd?nr&EM;FT!_X1o
zRU>dQB2N^(WR$Ge9*_9ek7{2@M-w%+rebAmGzT9}tCwFi%sxhmEyGBy&1-^?Wd?p1
z_|ToZ3^RY&F_8$rC*K~+Fk}rf`8+=eJS}P)nK?e1=WooMj=*rXUU{37e;t!Pl<7P_
z8J15pLWY-99*Ww#YD~(QgI1y0W)RY3UW$?2?kaJ`wDiQQXZg4D8~Y%4*`HCTQrpQS
zYe%9a*&oZvN|Jm=Epi9R%0(w*B$o-NvPxpa#tQ?0=QaBfFB#PyO^$fzOq%*ADF^av
zAcT!$XWC?=LwrvBI=$D+{fiti9i*KpeUGdR9fa7D<FZt0vk2U&5#vtGCn=)bnDucU
zk<Sd3w2L|27dgE->p6|3dhzlTF|QMm*U??I7(0$Me!h9YPSGP`>zU@BWa)#2FKXp6
z%tGWe9o{e*&e9^jTT<#_L$&L~_0jlUKT@^hl4*^{Ywbd37`c5RrQD7*>q&=`O-4DA
za~#Q4zmij%2!2T1cB-F_+PXGhy=B|pg;yV4`{iomL048kyk_gV&cT~EuMcc}A5}F(
z2`t^!n{3M;dPgq<jS!I=L0Cu)mc*xI*35%JMq3CYW~g|icH(2-?su+f>gIJB>hFrA
zC--X2sym)sz4h}eOyM^x=C+j_ddBK6>OOkC`f%ra2Ol=+hD1$9VNlXfE{TfQ_(@b%
z8^%O`E>q4ub`F?qHIr|ON9XRQxbB-vw_W#yt?u~e*-aJmn=dTC)cWBEGxvS+va^3n
zqJ<&7o%jdd+0fjH=2ByU9b&OKKSxZa89~-|M~BBzLsD48ly2v7N)fX|z?7LvCu!D{
zY}*I}so!Xop%nR5M@bQnL(G}-lcKgA!%RnPmy9Cq7>R?9e$Go1gQ!Q*P9tr;j>y8x
zaZPL8d|#I`qEqY9@9dQ0KXp2qcpTlkeE!>)te#T)R>$IVKQn&!<1x4W^R<s=6urCU
zk+(lve}2vRX!s$xA+^(7-rS=9ZsLMD6YCsb7yjwiMWt^X@t3v7|BblDp%EuiZS3K=
zxODe$C&DNr^{9w!iuzQh``-p|=7^|Ekd7|$=$>Db4$n4pjy1A2)5P5U*PQn4*B>0;
z^U$T=-gZbq=R52BW_@@2e?Pu%n&ULfhMYvr&mVIYQ94x>s}^cB*SxOW<JQoD&6cl@
zJg@%r)75>4S)cgl-x7T<_RIKPI;o76#QfrwuX^_=DcSJRoPKM;+HFUdO`SIX?yLIl
z^8Mr6B|rby#Ga8TH=+?gnu8?MMNMyQEw)dm!U<&76Voe6ruJhq*H9g~{YRQF$mhS2
z5lP<8vS`m()R4<YpOVji%K`5zRVqG`CWd_en{=BzJ4&u5pZ_MF|0dgi&+hq&nbL1*
zdq8$JDiW(7wzK6<oKK9bo4gn^W7>O0yjyy6C^4;c<}Q4yvHWUec9mGq!-Sc>Z2w)h
z|4z0SXw5%%A8lrRX{Z*-$RiulZDM^%XdtTS%l6-aRk9@&j-T&J*@+<AfApfgNVY+`
z39|i1Y1%|1&6`HX9kTsLUMio7{l{qjg<tWN?Z3(P-yB$ij%Z|({56@A?Z3(P-$4A*
zHKY>Rm@3<UOY8ivOxHd#)E^NyJ;?Uo1ijOUC80!G9YZzLvTgrOw*QsR`fG$YmF<6}
zcWz;(cq>+J5m{jz+M20pt^}j_p4goz(Kf9_wYf+?7+!vtJl1U6|B~%L$@ZTjoc_o?
ze%G`Alzd-IA_`uYlC7j&(YDlh*y$Ca2Z=R~cpFNiyC-Bv>3;E+tYrI7O;C`hvx!9f
zooENa__K2~+LoSn6$@CIYFmqoOS1i^(bkttrvI1z{vn2u?LU#d`$WcmcLn=T<HdT?
z__tG)eO@hAqac2DIkL|r`bqpAShBgaGg-14&E4+l>8KC4>yCj=$Bwf9G@flGB(q6F
zmXyY)U8^%w+CwJWfAVhMlSrF1)7iV8tPhc0qg~emRHnSRxM;*%%98Cr@wS9B(ISoC
zla;C#=upkQv9SO2zhI>)`JU<IYXGE+IkJr_+5QvR*iLSpD%m_Vk?o6QQo&u>{!{Xv
znW@F1OzA<aWu?*NuA_Il^H!N@XP0Oziq3vY&z4m3HG@>@GO;bK8&l%q#@<dua%(@M
zD?i^+d%qZlcE^t=+f~``5s~$jWb5k;Ll(i(;h~e^VlCpkC8e=2RJ%sD|3tdj5lgI*
zD+_1i!wX9+v=@s%H!Iqnux$S&dTl9@{g-T6Y~kq`oj4%UF<!%kVP;^&WJ-%;t<sp%
zu=P8<CDWzI<LGcA#oz|@INM4rJssdVniAU`tqsqhBn-15C5$%~)IS*?9a+zz^VevX
z3*ikiQ5JjBg&w6CP5a-iNW;zTvE&V*8L;k4);-BCksqn^yUXKez|r#LSc^u{PsF}6
z7kS><R@9Tv;iK&{Mcxw2G8JVRP27~8ggSPIFT+egu6DN7h)CVV@0E?N;aCe;3^NlY
X8BNZ2Ec<u-O3H!AJDnR8nMeOW-cX@>
diff --git a/src/azer_robocup_project/.vs/ProjectSettings.json b/src/azer_robocup_project/.vs/ProjectSettings.json
deleted file mode 100755
index 866f1e1..0000000
--- a/src/azer_robocup_project/.vs/ProjectSettings.json
+++ /dev/null
@@ -1,3 +0,0 @@
-{
- "CurrentProjectSetting": null
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/.vs/PythonSettings.json b/src/azer_robocup_project/.vs/PythonSettings.json
deleted file mode 100755
index 8dcd4dd..0000000
--- a/src/azer_robocup_project/.vs/PythonSettings.json
+++ /dev/null
@@ -1,3 +0,0 @@
-{
- "Interpreter": "Global|PythonCore|3.7"
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/e9c5f4ee-dd77-40c1-a281-22adb6db7ee6.vsidx b/src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/e9c5f4ee-dd77-40c1-a281-22adb6db7ee6.vsidx
deleted file mode 100755
index d2d0d8ae6aae45865565fa2a8e453291eae3999a..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 395301
zcmW)|?Y5)1vMqRebdT=mC_GL{!a_*mo0HVNLJ}Wo3?$%XlO%%&*?ax!r|y3p+6;<f
zrDcp!#EcnJ5!R;tzy9mL{_FqyfBtVc{?~uGbGQ8;@L&G_{^#_+{!jSdUv5KxA@UcZ
ze^tR>75}RmJ+~3hTRlWPtUdfMk5G?#9z`ClJ^J+M#|vtYH6Huvg}oQ9UikL7=ZVY{
zg_osX>3JpcO74}yEC1#F=se|lD)7|osl`)Yp8D<8%Bu&jp1k_w)w`!lPb*Iko}N7Y
z@{IC~^-S-X#WOF@E?#53ruUk`YbLK*yjFRw_1eK}C$C++&U)SC^~UR;UO#$|-W#kp
zz;W`PlQ&v#gyYMbYHwP+Y4fI+H!E+p-rRX}@6CfZPu{$Ezh2(rd5iUy)?0dSnY`r%
zPr_T3w_0!Qy>;@|*;`j{-Mn@8Hsx)dxAoq(c-zZ+DepCUui1O8-k0Zn_1@d_-ktY`
zlf`?#y^rVZuq$u3-bual@XpgazrD-zuIu*KUpKdPx7*`}$PLvu)ZWl^L-P$SH_UH%
zz6HT85Vs(?adhMC#`#U4n}}~h-6Xoj=`D$FX?#nQTbAF7^!C4>H^pws+|+QZt6QyZ
zHJmKB`gPOlriYuJZW_+NZYI7Nbu;>A?9KEyv);^pv%}3UH~YCY>ee*3roT1Qtyyl(
z>(;7Ut8cBnwbQL#Zk@e#{jG0q{qxq3w|=`l*zGah26Y?kZRl?UoPTb^etW9hv%fv3
z+o*2i>o%*~+}!5jHjlS?y3NaN-fxSzE%LUg+hT9abX#7xmEBf-TkUP_Z|iVd*W0??
zR`~b*wu#$jZ(DcU`rG!rZOiSYZm;_Gnr~m~_SN6s-R<4qKH~P-Zo9bc+wBN$2frQ1
z?O*8jZ+-i>x&6EOZ!G^?q<`Db-%bB_Xa63izeoG`uh-wJLLNb$f+r5o8o38;(5*p#
z!``942lV%d{{DsjUXj}fLKLAAp%bBRL<k}j5r#;K$ccQ0?k&2nh<a`m#fWl51)>ts
z2+;)53{i!sL9|D7Ky*TMK@`Ssh<*?~5d9Zo7O|g*&4_I%L?~R5U`R+5F_f<Elm7>y
zA01K%DS?zkszz!+YD8*AYC-A?sSm0VsuijSR8OdeKX;@R(iZ6f=?Uot=@-&(q<3T#
zG8UO0nHRFZko}+rp+=#mN6mnm2{j99UZ_Q=Rj9S7olv`=PNB}CZbCgmy+M7C`UmQt
zs2@?kp`R0a^yo36!J?r@!+?efJr#QP=sBSgp;4jHq7lw#G``SOqsgFYMAL$%4b2G6
z63r2s6`C!Y8#MQ59?(3Yc|pGr`t?E!LW@O9hn5~K6Ixyf<_D&7)o8V7?a<nzbwKNc
z))}q8(7K{^Ln{p1(WcO5(bl1@N85z91#K_1z0pgd*N9#-dadZSp)Z8K6#BC0t4D8y
z-U_`t^oH{VeGvLk^x4tpLOVh`MY}?~Mf-&I4gE&wH$}f+=+B7$Z0OG)bP{y>=%nZj
z&>5n$L}!K01Dz*2ztMG}e|hL%U{Dl$7<+ImvG)Ue3VRxR7JCi$I_wSD8?iTGZ^qt&
zy%l>K_FmX~V|2qP#7JP2Vw7R@10#h|jgiL4U}Q0BFlsUCF&Z#>Vl-kjVKig(3!??2
z6{8KK7e-$g!PPz(?HK*R=wH}v0md9-fpLU!jB$do!nnp*V{9;PFzzvaU_4;_#CXDZ
z#(2Sa#dyQ`jqwNL9pm4aAWR4*J|=feC?)|W43iL(5|av(6Z;DLEA}^hC+;%?IC2~a
z97r5QIEZnO;2_08hJzdj1r8JrY8+@B7#uus@WjE2gAE5C9PF4A%qiv!^A8pTi#rw+
z3x-9A1;;{QA+d<Ch_OhpNU_MU$gwD}_<<$C(#Miw$*|;D3M?g-5tcEQ36?3A8J0Pg
z1&#=gD2^D8IF1C4B#t5+#W+fEl;SAEQGsKEV;{#9#~jB3#}SSboDiH)oN$~7oWP$c
zPBNV2I3+lxIORAka7J)OamH~bahBjL#W}?}#|6O!#RZ&5tO!;VD~^@G3jU0+im|G&
zs<ARy4Oop>%~&m1eQ-^19pIYdTHspY+Tyy$^?>UM*DqY}SQD%j)&^^fb%*tU^@Q~c
z>m3_{jgO7O#$wZBvtjeX=7Y_SEy0#zYp`9g{lfNx8-^Q&n;tg<ZYJC;xFxt%xYf8F
za0`FFaQngSjyr-og*%J83HSevdxHBr?kVmA+=sZAxYxKhxbJcQ#Qli-8TT9hN$}4*
z{yF0Z!H)nx1b#^Tu=vsA$Akxg2ZaZVhaL~`=ZuFBeiHoj@l)Za#m^o;C;VLT^TH#+
z;~kF_j{zP99u*!f9w$7$@c6;wj;9(=22Ty120Sfz+VD*9Eb$!SImUB>=M>Kwo^w1G
zc&_lQ@T~D{@!a6K$Mb;a5ziBz7d*f4eBk-Qzg~FZcoBG!cu{!Kc(Hit@G|0M!pn@8
z4_*VjGQ5U()p#{{wRmmt+Tpdw>wwn@uQOg(yl!}X;q|~9$D6>L#9M?ng*S`04sSi)
zo_L$^w%~2U+Y4_WyzTf!@Jr&C!mk#;M*LdwYr|g*e<}Rc<F~?Zi{Bl7!}$Zh2mJov
z55XTFfA080@h89^jz0o_B>qJB6XTD@pC|r|_%q|r3x6)W6TDNr2Y3(h&hakrF7Y1W
zJ;uAjyT<zy?=#+4yl;5F@Na^D&-nL-4}=elj{qMW9~vJ99~K`CK3aSX_;})D#K(+}
z1s^LuVEh{&AABi%J@GZ;e{u3xMLb6S8z2WF^h(_36LKen5)u+p67qwPgOJmqk?#|q
z6JHQt5<exrCcYtlL;MHv2jV}8zY_mV?#SWRU68vZ_k`SQavxmq2PC7MP(i39G$u45
zbaB`g2ogvVBqaEU1d0ST2^tctgfYTu!ZcxqFiY4#*g_cm`{l4O6eN@+Oi7rLP?OM*
zuqEM>gd+)O5-ucMN%)79H^(^2fRrbxD)J8}{|GWI<ly8G<dE>q$Vth`$a#?S^fC5v
z;A8G%0l4O4<>MNh*~cxogpWrbFFsy<y!$@(eeV0x_a8p{>2u}_*tstfU!=as-9v&G
z<=vOmmw_)sUvgibenk8z^ds&^rymnPzWZ_M$ETlgKZQN^RpzVQSGBJezgB*2eLeZc
z_l@#R@0*WrcE1_?X7T&G-=F>?^dG{1NdNKnhu(ks{xk5O%70G&X#HjKSLLtPUweOr
z^UdFszghon{SWa!cmE^&kMuvI|C#-r`1{@8NB=iWBfX>lxuf%r@f|<!c)sKHj`utM
zedo+gub_7UC-N?$yNK^1zl+n&x%BUn-6g-v^B$e|nB3#U8A4Ltlk=YOd)42SxGQ;A
z@m-~NmEYBHuf@Gi?)7lj``xI!v3IlI+t0n-?;W}K{`~>Fybt#NB=@JfkNbU&?sI&f
zllz?B=j=Y`_qn*w(|wWm)!x_bzN!10-aqsGv)y<9{u}oB{(GY?mup=n=Sb?EB1(~>
z$WR3T*iqC|G*C2C^oyc}qLreJqBrmupfA8zDEdPk1qmQ(LZpP?2Z0TO7DOosNDzl0
z_CVC3c&2!zcmoumfuMn;K}3U?1_=#P8e}vmXs}YooLg{8Ih>r%KLn+MQz593R76z7
zR3ucSRAf{XRGiL%B&Cv5DX5fGMpVXBCRAor=2R9`o-`sfqBP<(5;T%DifNS4D0iM0
zQyOy`3mPXhKAj^blqQ@e2~BdE6f`9?r8MO<jcJ<EjM9wLoYEYQDJ>{1I90F<sw7p0
zs)4GNs*S2QRUcYYS}R%)w0==ds1B%}sNShj)L3eIYF^ZS(T31Q(Pp9TKzl-aO?yN8
zC+$btZ}ed)z{Wu704F^iX8N@Bxzdr)QP5G*@kOVaP6M4Kog+HObWZ4;(mA7ZPUnKo
z6I~QtEM4H_Lsv`Jo~|2Rce)9>#dK42>*&_g?Mb(ZZVTN$^d;y^(l@2=kiISbQ2LSd
z6VuO=en$G4>E}gvLU&5{knWuBg6@*;G2NeZAL%~ReWN>!Iq7do!PtJE^mx)^qQ^>)
zlm5#Kun4dOdJ$j~;0EG%fPX@44)6ex3<3s3f&ezo0^vDy1ltjC&I2I=Ap;QwA`V0n
zh%^vcAo4&IfhYrU2GVm-Dt%{bk_D0nQUp>4G74lI$Rv<yAhST`fh+=92J&<!8L=SZ
zK_r4m22m75aS$azlm$^9M1?chm<O>4Vj09y5GO&L25}a|xx;L*<%9=`2of12QINzz
zk^~97R0PQxq%25zkcuFUf;0)zG)S``<3T2ZOa@sJWO<MkK~9652YDRiNsy;Oo(Fm1
zoTK1DA%h|fiYzFK;GaHFB2ZDF;y`7A(t#QRH3n)9Y7x{bsBKUWfhK_#ft~_m1Jef$
z>_gD{L92p(41P|*g9Hx|JY?{A1dlTqR50|xQw7gH7)>yq!BhwH6f7cGY_RmfYJ(My
zTd=-@O$3_?wmx`?;8h175qxCu(ZS~#eCFV@2A?h1MX<|YSHZ4>eF*kB_-%vVGxG?e
zGouG%9ymR7+sQa$oG?xq4;W{RbH)YZlJSV~nDK=1l<|!5obiJ3l5u#;Hxq~n!USc4
zF%dGsnGj4!CL$(cCK4u6CNd^+CJH7>Ccc?OOcEx2CMlDgNx`IKGGa1jGGQ`hGGj7l
zvS6}g@?;TW5n&N!5oeKLkz`TCqL@Vqi&7S4EXr9_u&87)VliQ{&tl5rfW?f(oW+90
zlEo2=V-_bYPFb9>IA?Le;*uqVB|b|iOG1`#mI#(emP9OxS(30MWl6@8oFxTIVDL9f
z2}>zUIZFjgB}*fg#w<-(nzA%wY0lDur6tQemQj{*mI;<gmPIU!S(dOYWm(3uoMi>e
zDa!+vbCwI1OO{70k6E6uJY{*t@|@)bD+ntnD>y3zD<mr-R>Z7GSdp?KV@1x2f~lG*
z&6HuPVX9?nVCu=#$kfc#!qm#t#?%*6A66??x2*12J+OLW^}^~Gt9Paq(>2qYY0GrS
zbkFp_^poj{>4oVR(_c(~nBJKY%qV6wGnSd2nTeT&nHMu3W_D%`vjejuvkS8?X1|#I
zu!gZlvBt2bXU)Kxi8V887S=M>D%NV&TGkG%omjiD4ztd(u4moEx{LLa^_umD^$qKv
ztRGoFvwmUy%6?Al=gfYt>`}9aWsjacCN?NGXf{|j^lX^eQ?aLI&z?Qud}Ys_jf9Pq
zjf#zyjT0MZHon+Yv#DXzgG~dQMmEiC+Sv4BGhs7jGh?%4bHwI?&5F&I%?+D7Hur2E
z*gUd%V)Mdg7{9amV84E`UoZCSi!F*RHCrrOTDJ6T8QC(iWoFBZt%|Lht(L7FTYI)X
z*gCLvWb4G%nXSLry0I08?QBzQvux|w*0b%&wux;E+g@z@u$N*l!(KgmjqEkE*UDZS
z`%>)7vag<fE$ofiTd}ug?~c8D_8!=KW^efSi+vFLVD_QxBiKi>PtQIB`z-9UvCqYJ
z#CFVf%67qa$#%u|f$a<1H@08wH)X#q``xkM1N*(O-y1u>+10b_!LET_3%jlm0X4>u
zw})7UI1aH2u@12baTDT4h=&kALp+6efn+JfAILXC{5!mhFob;x!!_hA6g(7ZD00WI
z<vo-%lwl}&C}k+iP@Z8#!Wi~@7}GF5!z2q+9;R`a@vz`wwS+1RYZcZ*s8y)<(Ad!Q
zp_xMS37aKsm#`nh!4C%-4k{dMI85PihEEkfC&%AM8jf%>hvO?8&v1H$vkK=XoTqSx
z^F3T-xY%&%!)0<bf3@M-hbx@V;kt#}6u#E*s}H}H@NL6)AASh;V5i(WIU*o6a#V73
za=dbaI3bYLI0^<ZOhQgdPJVDQL59G|37G(}e4zE*5%iV&AMQicN1O(na$spd!hmA|
zodW&@go*!$Dsjh<YECt$4W~V)1E(XW6Q>KOE2kT$AD|8p(RonwU<J?z1P4$Cs0HW*
zKm?=%6a$a~4B||4w(=13@Xo^!rUxE351%|-IY*ph&I#u}+zsae=OK5S;9PPZaUOG?
zaGr9Wah`Kta9(nLa)G%ZTu?Vq2reWS5f?ER2^T3B85cPh1s5e32Ny8@n@h|k;gY%w
zOTne&GU77kGT}1iGUGDmvf%RI^5pWHM}$Yz#j!~6Nb)G+QOu)+M=6go9_2hLcvSL8
z@#y5yH;)OAsk`4;@L2LV;&II5gvTk5GalzWE_i(M_?st$C)9ndMDRrNB;rZTlY}QJ
zPcok5JSli`^5mPRgr}6Jf~S(F5l>^DCOl1fn(;K}X~ENzr{6pyJfrRjWrAmtXA#e0
zo+Ug>d6w}k=UKtClV{&NCp@P-7d)3dk9Z#QJmGoD^Ni;?&kLTHJU@B<%?s)tb|H8n
zc@gm<=0(DblouH<a$Xd?IC=5SOUcWamk0k*aYcBYb4z)1@+aX>#-9gYfAJ6JAHhG8
ze<J>g`6uI_oPQMmH2mQF5d4t*_{q<|_(}Nb^Yadw1U~~u7S52LB|j^E9{l{n&y%0u
z{Hpmi^K0SP%CC)oQ{fTe!4Vd#PJsw$@dA|sodOeqS3wA*{VwNo2|RF8U>}fymxBBd
zE~AEA`V_t|d?9=(d;{sI@Snn8A-5EFA?{M#6G-{Qy%+a~xUb?46dXBX3kK*?9mr%L
zAxQ*HA*m3w7E}vrfanXl0$B%AF6alSwx9=)U=bJ~XChdEegWA6!UXIG2o2B_pdCOa
zfF%F}fB+BiTQDV9EtrNNDp(^}D_AesD%j>U&X9{xiqME~5aCmVi{PH%NO0n^dFSEa
zrQn?4b>J)DDq#9x&|srrd|+5$F<=ca&86U#;7^zg!B4?2!M}y@gg`<Nm(000!zP7@
zgouSmgh++Rgvf;`geZkLh4>cIgGCIN`GH5mSSf{!gp7qugiM9Zgv^C3ge-+Tggk}(
z7Ln)DM4*1)!Eo(JiYO9METTk2sfaQW<svFXREp>n(YJ^_5fhhIg1f_w#8SkOh+`2a
zB2Gn|i8vQ=A>vZRr-;8r;)#T~95e|XJiv`7QY4W`Vv!^wNkx*0Bo|2`l2Rn6NWMkt
ziIljU6?iDX34GC1iZl{wEYd`zsYo-C<{~XbT8i`(>9<=r&4|c+m*xWez=O%8$Rd%&
zB1=S;iYyaZF0w*orN~Z^eT&=^Idvo#unL}HE=3-RJQjH(@>JxR$a9ewA}>XLiu@ZE
zW<^0<4h_HtkGzngh(r;KA`wL@icA!_C<;-OqG&|%C5lrN-=aira>_%Lr}*(h{HR1V
zi0Ub-QB*HceZ)VNP(-M^P*f;5*FqU#YSAe%PvX~0EK0mjaUch!hY*JpM<k9~9KASx
zienWg5vLFG2_*NB#)*@OGZbej&L85e#QBdn4{`nx=P52KE+ej|xK{B^rAMR($5MKy
zL<H8)BswL=5)+BF#4D(rk`SjwIo%CHdMU{tkiDF2@6;<HeJOn<{aX4)`i=B^5R#<-
zl>Q2;ko0$X$MViW<&pOoBpZ1*@;=J@EbovQBbR7GKyXq8C=nnu#FAzp3P4UST_O!*
zAQyrZRLNkF!BYmK3|1MuAZvmg3Gy6BS|AUB>;kd{$N_-d191ml4zyb`C0Q+*mdr?I
zC2J+?fSXG8Q?fy_MY2`0FUj7(rGW7`o)QWfMlxI__arAya&r+HQeg<Lz+xbFfn!1F
zf`9}u27(6!20(m3bAV~U;!6q-Aj~N%4p6|`!3)6&!KuJc!0y40!7jnnz<Nq4Dk)AW
zzNPfwIXW@MT^-&4TO?&HWg=xNWhP}VWg%rLWhLb)BTq(1M#PCXkc2@X1uutpM3IbQ
z86`4GWt7P%mr)_3QbzEWZy9?sCQj^us0)w)myaVE$1+Z2oXR+paW3OR#-)r;8Gp;f
zlL?W@-HAhx<UybY6oETRBALW8No11BB$G)llR_q?Oir16%hZ!8br~T<b$}eW{WOwk
zEYn1$sZ2AO<}xj0TFUg4nI|(Mv%3?YAcF)J01yRVH;ZHz%Pf&uDzi*xxy%Zgl`@03
ze9PREIdNhYPzJy=cs6+?^H}DI%u|_XGS6jR$h?&KDf4excuv-W+!Xi)fFC@_B9cWc
zi$oTwEHYW-vM6Lx$`Z-)kmV_VRI(DXddO;!)hMf%RDo1R)~(c7YAQ98mdbXKy^({G
z&q>a`T$o&xT#4M3BQg$R38=Q9WrC;%dI@C4@+{@~L!O5`Pk9k}QF$48&GLH7ZxVU1
z)5t3$?}Wwd2%Qlo5#|wIBZ4AAoH9p%M3<58M|TAxW<-&bVIb88y5uB3Fp4;$NksFA
zYS6YKx<&L8(H*o8$ERGL4^{&SIV9MSHbXuPxh|x#kg4j3nTRzJYa`Z2Y>L=d6v`+B
zI^{*2I7N_$zzbcha0Cu$8N37(3g{8=9^f=UR?0|tPB?U-4nhjpCPWkn5&-%D;sDBk
zvjCki?2N>BB)v!yr!qP>2G;^_0b2%B2I~Y<1M2~6fC+{<D<io^@{Htn6nRlZ9V>KK
zhtaUbQItec8bw(Y<xx~bQ5i+wQS3SW638ROG;j_W6~|GWL~$C$Srq3{Ttsmh#b=aw
zQ9_*32`U9JO$dPCNpQ&|j*=uw(kRKIB#)9JO3Ent5hd`J?<n=6lsZ)dm?;EI@I1JM
zG>+0FO4BIKqBM`vB1+3BJ)`tH%DgC}j>AG|1s{c*&EhCaqAZQFEXwjIE26B7vNOuQ
zquc}8)v;X&!r;yDwemR1lPFK4Jd5%?%8MvJqr!^{>KHKuYVdgY>O~wCNmQg!kwrxw
z6-88(QF%n=8C7Fcy`qLh4UJ9$D+v)OzL1%nM|7UiWnz!UUKx95j7W@VjLsO77_TuQ
zZq30hz(5XHy5&}1#=eRDHNHn6$H#XKl6p*$ll_2<JM|gx1|EML(*)$;m=-auL2L!Z
z6J)qJSV5%%$q7^>(1t+k0R;!78IWB-RRPf?W;$kV%%+&Z$lq}Yq{E9jaSF6cPT)@f
zGniZ8`at4=pab&;Rt<C+Xz>{f4|t(V2pneu%mWhurUJ|Zm;^8b2=)-a&scg8cwExp
zU>a}{%miQ&Fb-e~U<i-}z~YP}FOCSfmdh=iL4$#UHGwgKeZYLfEW+HtbBB@NaqPi&
zb}5MaNVpt~gDr{UG>)@4&f~a<<2sJdIR1_kFHVRP)FEF18U=U^<_TkyBu>&e$>JoB
zlOj&aI633wJ5IegC2>k!as!+Tz#D8CE}SNDn#O4sr+J(faeBt-cbs`~MjbB$fCqbr
z56_Y~OXDnyvpmj<IQx!sF9vlVk|f}65Cp&{;0E&~&eJ&0;yjP@A}+kRppMx=hyag)
z+b@#1NaG@li##r1Kp7X`acSdniOVCdRczw8z2cL=LP2~6@p;CVNj#c(W#XMssgvM7
zAtWKbTSg#dLe9i*lDmdgndH7E6gh<$ay7Sx0Z|BC6CM(*G|ZrEf=uaxl9LocA_U<N
z6g^PuK#VJr5GP@pglodRgcG+q;j&`L`GA2!90TWuCk-hdq;rs`K@JAVRhjTJ;ophy
z5<wuBa75MdW`F^h6QHU<J%LIBI|RaaCelkJb?Kvv>kt(IDq!9qC_<crxCNmIqRW{?
z9sse^CLKxw0s^7|qQLY5pa4LC+k-QMqn=6ZfrmH=(-|ljA6ONb09XJ_Dohwm20ZRF
zNjz7`kPx@t;=Tji8GI^S4u-;(CP|hgd6E=KQYOinq+XJeB&9C91+EPd8(<7<3I?WW
zl4ePoCuxzSF!DReyd<MeJ%v0NSUJRYFg>_lmL^%2WO<SmNp>c==azaPQ3l!$!5<J2
z%o9E{Pm?@L@;u3lB!^+&N#P|0fkh#gQUk#UngG}dh75OCq)Cw_MV=HzQk;%HlzmdZ
zlk!N)GdW2Di%VyfoJVpk$+afm->FBTGBQQ5#6(~T*)24=Mcgv=b?UFFzrjjgdjFkL
z1S@1Nad+!pKt{k(+{zQI?xX=u0}YzEODvph4azdeyDAOWlzS;BZrRG|BOrM|<^dE2
zKpmV69v-Cqkk3P|4oP^K@-r14WScHEahVkWKR6;hU`S3N*?{~3QUVzBok}m2)TJ+u
z$w6=cXN5@sCIrj|$O`a}GnL<I<fRdH%Cw7-5c430fKS6*K?s4s0B{dbex}iP8hdF>
z+}c_UvKaswfE1t)ct6Z4I6pWxI4!v4na1B~LehjfsoU8)STI;8m>rlD%s)&&%qz?p
z%*2@{-)Tx+YUOSYJ{7J7BVfzYG*8naP0uv_h6Oz*en9#KJPsloAPra$49T)I%hRk#
zvog(8nw@F(o#xbu>yW4c<AWFom;|;47t6CW&(pj}^D`|-3TuWSr9d(VtPtWP;1<{;
zd|;8KMV=N#T70MFkv>WKWa;xr$26VVbhYWh(?g_3pB|6&B<V@hOQhG5c{KA%r~=Fg
z$p{=_nZJZ}@yvf_cbwftcCTT@H@kl_ieM$xi8pS26i6sUX84L4Rsl1*!SWa^cV)o`
zR#0HYA`91ylZ^YYp6ONzoG1plCXiML=Abcw)4;O<#Rt?JkZH<{pPBG8L0vNHa&HLu
z0K?!|@cbYhg$xs@C&{D_IgeX#aJdAe2f%-U7XVs=|G{Gh`U{K}Mt)}zbs4SWL%<pU
z%Ylx-BmijuDga^rJBz7Po?L8$xCH?Tz#cppW(lw!a2$~OJ4;BG&@2g^K;;k(&<bD(
zAO;*ACKLP|d>A~l%#t%pzO$5MDRokovuv<hFh?*lm~)tNm`IqUGlOMb_m$vY;cno=
z;41JkY<ZRySq1~Xvz)pm6iC5=%LB)O$OCW!CIdtAJj;tLKeK{5sSI*-Ap1aqAT$Bq
zfK9<QiaaZdtT?ms$ds2gG&^{9Kq^nNlV;bFdo=gX9MPQoofDE13S|(v|Ack@oFcbK
z2*Mkr`)-jNC=~!$0Smo(pz>gK8h~5Af<>b&57(R%w^+?XP&}PB;G{{QU=SVw^Fa6m
zMGw?Ekm1Ul|H%273qKb$7lBg|oWcTR4FV~kBiKHCW6%*m8h~Nnxg@#tb4ha<IK9E;
zbwKMN<N}U@m%y`u)CIDU6S%umBOLJs(gy(<P!@a$o*ZyK;BzqGJCCVT5FL*JjsoNZ
zf;J#A_!&H1h}jU8&piIl6Y5k(7rG!ULA-%j0%#4s2r~d+4G>)B$q5kYlt>3&08D^5
zfFpno;IA-C;I80`XP%KfyXP6rv%qPV&WOP}!T!L!z;3`GU_N0wVYXoKcb-!xqPTAb
z_YL<1p9e3)R^<7a7k*w)r+R|Q0_hqs*#d|LL=V6Qum~7c<i(knNB(2T{~7Xw=Ld)}
zG{2U@qlH%%-dP}85K<60hK0WtcU4g2mVZG&brlbg`2jTs<}$&eenD4Q2Zcq=B3uhj
zKxcO%tCOlBH3a$$8ZksFKqD|ixGzw1NFn?}+zUaS<m0q6px8iyAesSUfvLj1!?3fE
zq>z3g?}elwJvi0YCHX+cfi6MZ18f7MhHnXZ=64ZMCmp$z2B<oaDTs=Id|>kM41lSh
zz>=Nf>*y&^PvDfmwjgW*G=fh!Dptg2k&q&xP8D{H1lR`92%rcMS^+-6Z{R^fWQAeh
z5c!;P>|zqc8wey29su+KV!^lIVFSXRMfzQ2q{yffnEwMzE^ly<0ayS|4lW3D0v>o4
zIRVdcT9>m_utl&lupzJyn01&mn6n>6{!fv^$nT;cMM0e^=Ds<6H@J1UE%->d77T^$
zEQ;@<Jc{P!G_!*j2PkK>ILqQZi)$%8THc!Swn0(0tHweUiWSQ6RdQAe*g;(bX%f@~
zkU2oP03HFk)9FO<_yh<m?NIhXIjHgpyDjIbT&D7E%LA2v=JL-UsO>7xvOJIfLF5OX
zVDaV$FMo*mhxDCX_oD*E?nf2<sQMpjgp%+dUH4<?f2_9hyvpO1msMU<d28ie6+#sv
z6`~a~6>2KfR;aJg&k79{dRAzz&{CnbLaz#aDzsPVH<aX7m{(X-SXMZya9rV}!fA!G
z3hN4+3a=IZRDr01s0#9mlZw-dvx=7$|5Sohf`V}Dv<i__q6EFKlBANflDv{}CC@4%
zRm7_(t)jAud6n=gqgBqUf>)JSRd5s)s2x>QRcTc<Rkc*rT2))6hN|8xqbjpjjjEcy
z>Vv9><5)fJ)g!1Lef4-$gQ|wUda7!K-B#mNjdL}As=2S`rCL<AOx5zKmZMsO3ZC2A
zR~zgLXe!mKu3l~RrK<N(y+74QR-b3}8LQ7+?P0acY9FfuRYy=AygKIUSgT{Jj!$)*
z)xW-7MIJBz5#>J>|EK!=Q|<rs<3A7i&(QxbMF01*|KBULmk{Mq<(0}i6(S0q3KNC7
z!jZzU!kNOk!UfdhD%>l)Dnb;wD?*`~P!Xm`s7R@NU)_6kpP@2I(Kpn5sNkcRg0*?Y
zEUc6()+_cCR+?dvScOJ~o8rV(IdS1ih6QmoYi@CZD^V&@DRH<uVo6*Ll+!o4lBJTB
zt1ONP$U6TiRDmjE;>z40RZfUXs7i@SeU-uqaMq_v50w#D@t0AR36)8e9V#cT05GQz
z*i<f5E>(W0|G=Uuges&ea#b9v^i+uytQu7nOs#VB2UE4G>QKtKb_XbJu8FFZs;#Pf
zRS&A3RK2QtSM?uNUrHxRYo)Eyozf4b2c;*ae<{5w{Ze{Y`f|n5hA6{9(pDx?CWdP(
zqm?nrSY;YzdS!kpGbuAGvnuma=1ZAfnM0XN*{8CjvR|r6RAW`ssAf{lrkYE&NVP<@
zRJEmQE7dC1TGh6y9aTH2c2(_LwU_E!)eov4RllfyRsE)ZPU>ORqgRhz4W8@jfb#8u
zxH2@T!Wcp|a5V@uNHvt;0cucc&}y)1Xw=ZEp;yC04TBmcHOy*Q)v&4IrG`Tdmm2=9
zo>)DJtLAtr^{mxXtEW}ZRy}+5e5&WHo~wFp>Up_t5-1xVi5h)1Qdj*kN;RfxEY(=4
zQK_+3qgJC)qg7+8#!ijB8XsyL)wrs0Q{zjGyBgtA->&?8rfL>ymTIolY}MSWc~bMN
z=0(k`nlD$Mz7SVnvM9A^wHUQnwKQsJ)zYiwp_WlClUf$FY--una;X)m6|0rF8a*gW
zSxdDlwOY0IYJI46QtPbNRe__eFSQ<Oz0`)(hSf$~O&^r8Y)WlfZANWYZJpYBwM}Z9
z)wZZ@RokYvm)bsR+tqfd*PvdjdTr{<s;^dk_3CR-Uz2)!>W$PJt2c3Vg6}}Rg?dZ%
zF4enIZ>8Q^y?gZ@)O%6yk9wc#eW~|1)Z?oUfx1E{rBNTDK2m)m^(ob-QXi{6z4{F5
z^HiTN^|{pUsU4{utDUHws-3A_soko5RQs&<RqdDhP1SFvep_{5b#QeEbx3tY>M-i)
z)$vmugF2q-nAP!7Cs7ydLS0&2MqQ1%dUZ|en$`77U8}k_b$zL8uf4NIq(-zxbB)#-
zoi!#k7B#MG+}C)m@m3R3ld|^x+7~s|HT~3duLE5}F`26<s-YgJW~OGgW=+kyn)Njs
zYWA$z2<0+0o1wI&W^2v1n!#J%HT%?Tufwqp-!&($UqQeT7EpOD&RXJHl3LPQ*0mh!
z2-gv*V_e5|osc@Ab&|q@b6vsytgE@MRzUN*I$Zsxs%xcdWol(>HGm!0YN^#)*Fz2H
zVr^}k+P1arYdh3-sqI?Zt!|gPz3Q&7`?l_fx_{RFSod4~)4J*>K%fCm`g)k^;Z+Y;
z{haFOT#vRMCkIYbT~DT-9`!WT(^5}cJ-zDbtDfHV^r>fB&qKX<^@8e!)C+aBREw$?
zTQ5_+yz1quS6r{8UTM9udM)c!*Q>49wqE;s-RkwI*Q<Ut^{cO6n`^stU0=|A3CFVD
zw|c+ogV%?s4_P05{pSzBWPR23rR&SoS5seYeRcKK*Vj;AV|~r_wba*IUt4{B)z`be
zKJ~S0k7$o-L^Prr6Sv@i4K#H?ivlzNXhNWR-qpss;`LJdz7B*AG95HfQl?oAHDOTf
z1vOQgJvCdPXi2k8v$qyR3m=xowcuJ3Evc4)PN+_}E<;`Z(G_&CK_CQJ2gn2D0`vix
zSiyUBjde|Q9qalDUq)-Ib*uGL>q+ZX>zCGVtuJjLAb73^4q96*(NKPFsjE=4p|(O>
zscq>Z2E5xEZ5wTSZ3k_i+K$>z+RoZ8+OFDe+P<_swY_xH>vq>2a=kJg(H(UGr_;K#
zy6biK(A}WBNq39xPTgI)Z*)KEe$fw4KahT4{UG{5^&`*^rXO5CgnpDRWIbyA(E4HY
z!|F$?AH9Bz`myTArXO$pxLiydtR8wjO!}$x)99zwlhISFr$J9oJ&k&r^fc>f(bKA@
zm!659nVxq&U;5WeFG?>)FIF$TUM9V4dc}GrdZl_bdbN5T^*ZVGk6tgmVZ9N(QN1y}
zl`fEOR&TA|I=%IJoAfs8?U&vby{&rN^!C!*p|?|SmwpZU_0+FXzZU&k^_SFNoBq1=
zo9H*yZ>HZuzomYc`fc?4)E}%rM1Oq!f%0k(niQe^Nbf}NK2%%lUF&_(N3Ac|VPdVm
zI(_x}8uT^kYu4AIuT@{0zFzwJ($`yGyZNg!9x)y@UTM73AOcO829*Y#&>#nGYz7nP
zA~Tp9EDV+gM+O&;Bj6gkp$s<A-DGfMaBFY}{YVD?H27igVDQu65gLRHo(*20QODrT
z;FrN)2EPsd7`#IViy_32yCDH|bQn^)Rw`(H@VW7Y@gw7>#y7@ajK7+@Fn4X}Xy|I_
z28Ec0X~PVZEgJUIu*tC5u*I;|unlV9Oejn!O&FW7Fkx-N)^KEm=ZY%@H$ob@n;4nc
zm=f5(XDV(gVJc~=#8jE7jHwz^t)_xN2oMMa$Qm_3XonaKq0!XX)D^&`se4nuz}t-`
zu4WUslGa8WqdPD@uskp(qjz{xjzhW48q*uI7<0KEQ|KGB#8t{eu_u(ySz)X+HZnFg
zHZe9eHZwLiwlubK6wGRO0c(wIjBSnWjU9{~jh&5Mj9rc0jQuk9ZS2R`)7Z;2i)pQC
z2h&cbT}-D<H#r{HYj-1kWBS(ggXy29A59N0Ev8>hznT7J`Y-eIWd>`8-8_{UJu_M}
zu4a6h@iG&3)u~f$CT%8brp8PUGd;~TnrSuD%S>NpdYkETY;m5<e3=ECg_wo9n$|^{
zr8J8&i#CfjOK+CRtk|r?tkkT`tf5&;$Y#u{&DxuFGwW{F!>pIth}o#wI<xg=o6NSG
zmo=}!yq@N@n%8Dtf6Ny%U%mMd*GBW9=EKdWGP`GX0$nu*Y87{GAYs~1vtQ;w=HTWK
z=8)!y%+Z_Ur#Xo^sW}hew&to0;N=DLX3f=@t2fufT!Xou<{Hg4nQJ!JVy@L(o4H=*
z`ZCwsfS3LIZT~{nBi5tVE1?bE{%tKH7Ey~z=-0LAgmzZwN3|GROe_v8<`xT!rNxoO
zvBinSsl}Paxy6OW3VIYBV7s<MYjI<74{d(X+6P^H(5Gkd1dVpkI|toy(C}vQ7xb-J
z{IPh4o-s>^B|bD+SrS@OTJn$eed}}U3+qekN7gscL1gcdr2&+uSt=||EUjH+a7`VJ
zr6V+2K#PT?n+=p@u-7hVxRPaSSp(I^mi15~3>Cmo%M0bXP=*U7wl)+tlr|hK_bext
zGs_RlFDr-@z7^Dp&<buPwNh9qZA5KMY)oweduUT?)6izrW}(fA&8f{pTTokswnVlZ
zAj!8CvK6*fU@KuOX{*Rqxvd&VbZxajHfyWRR$sPy+y8~E@~na*aUsrSIFM;T<^Tx*
zFnD0(z_x)p0}lpvds=k?Hv)QL>zCEm>IUMO)jfnF`=2la&~G)o1orWlHJ&xdb$=RS
z4Rux9CbULaQ(992l(=x}umC(6oU;c91HZCn1tYfR01LC$11oXN%ME&1J6JnfJ6pS0
zyV}OH4YG~0jkQg0o541dZ5G>Xwt3msvn{eMu`RW2Y1_)S%C^?Fy=|Yioou_<cC{^x
z+--Z=zP0^e`_cBR{W;m6FZ-bOvD(LF2hR@34%iOF4!#|zD?cBk9V$C0M+O0Y;MRu5
z4!s=)J4|+%?XcQmv%{AiPCH!oiR=^GC$UdzpUkC+z#yL1J}3K}?Q^xy%|3VgJni$c
zqi08CM{Gx8N9s!5$Iy<#j-^X3fp-Af!IKzUJN9;b+HtbuYRAovFFRg#s_oR+X|U61
zr^QY$J0m+|I}<zmc4l@C?JVqE+F3dN0#9^q?A+OTu=8Z+#m=jpH#@@{FZ+wwU)273
z*<T0zCWl>+Yv)~vU3|Mxy9BNbe+liv?IP?V?NZvMvWs%G2=Ex@YH94!+NHP4V3(&|
zM!QUQneDRJWwpy@mzQ0>?DDqDZWs8JKXy6oa@p0hE3zxLE3vC@*SlS*T?4x^yM}fZ
zc9nK5?ONIOAG<2Y!d5Vi&aS;(A9fw=I@)!z>ulG>uB%-)yT0st*!7QHPrJfLeA^A%
zjo9sOH)=O#x6*D3sJ$a}5J@&`x5jRr-FmzIwA;gOgWaBX8|^mPZMNHDx7BW&-ClP4
zvfJBkAG_^#JM4DZ*I-{y`<m^S2daSmvi7UDUz>e<_KobD*f+IrW#6@ZTl=2u`+`1c
zXlSq>VL!@#wEbB7>FsB>pUr*_`#J6Bvd~?<6T4S-ukAkDeYN{(4`dH+4`B~!kH{Xe
zJraAA_UP=<+vBG_W_zsm*zKP`_9XUX_AKpr*o)Xp*h|_gvR7=c#9pbrG7EafRRf=e
z5Ng34uf|@jy*hjK_8RQ<wAW~_$zHR)7JIGs+U)hR*O$HC_WIasx7TI={cgOnL8L*n
zK_~n!K!ZtxRf8AkTW;{D!FxkULs&z~hWu#A(NNq#Q7}~v)eW@`?Hf8bbZO`c%_j}3
z8>Sm(8fF{TG^}k{2b~)Y``NHZ!-j@E8#Y3(1$0m}>{r8<hQS-RhP@j0)v$NNJ`LL&
z_Pb$M6QU*@pxVCSsNuX3sFA2iuBJK~RX0jEss|Yb6o*Ewje=APH#KcEM0y1w5D>7j
zrm-WKI9N&3pr*l1Lz;#*4R0FJG_q-I<F>wO)}}d|=5oKH0Zs7Gs?tNQ0O-{={m}Gd
z(=ScGHT_r9f13Vk2IWv~XqusKhN&4|%~LnepRRR&BCf|}(#<q9)6z_@W?q{`G>dE&
z<xp^$8o<cSYc|wuq}lvtqs<mJTitBNVdOS9+uCefv%Q+_XtuN2uI4o~ud&(t=HSgC
znnN~6)EsehB+ZdFN7fvDa}rRqnyYRg(E)CC!58Y9t8cDHa}CY)Y_74nrskTPYiX`m
zbG_TY{%Ze1tw&mqwqDtKXNyRSXp7DklNO5>_bq<5cx>_75(2%=Eh$@nZ7Ftw8LeAt
zTKZ^%6<Uv=xu|8jWu|4eWlhW4mh~<B*|MQ!BecP^42ErOh}zJ!;n;>#8_umDt)Ng0
z-3rx;rIiHAD_f~rxwH|5>ajMaP`J~UAc8k-^=PZDt=_F7t<oULfgA>UR;%V#ZLNB>
zwQTDIa^qGL*IlGlYe3I~A1H!fL&2>jt);Hz$d;|GTB{xXba)LoxVCm{?Vr}3;7c&^
zZEM?hXxqK*NZZwIXWMRQyRGf67V^k`Z2PtCw|21YFttzR!q<q~5srO3PVKn0<K+~@
zX=tahou+nL+G%U2tDTp2AyDzxF3N?wrEiz1U2(gTcBSpg+BIy~vR$io?c4R$u7BFC
zb0P6Hw6CRov-U$=Q}TzlAJ*;!x|7=-PL_5*+x=<})E>M&M0?2gsN17&kEK06?XkCi
zhV~-uMca#ks0kWLd+GLS+N*D`M|;7m&-R+yYi_Tlz1H^HTCkJr=senaW#^q8A|1MS
zh<51gFzPVraL{4iVbNjP;mGCI?uQTH_Xv)TkdF8rp&bc3Qg%M>DDUXfQRsbA&}`N*
z(=pqzregziRdsBEo+#*d>O$OwvJ0awjJq)H!nG5m6SNaSC(bT9yApLjcvp$8lI|BN
z6zLT06o+ExPQkw<ok}~Ebt>;v(W$aiO{X568bJE&)ZD41Q!8kQo%-t3(J2s9rcOUQ
zJ$FWR7IvUern9!QeP@TxK07;hc7YfMA*ZuDpl#PYyUy!6)xie(LEm(J+x0`&k6k}^
z{nGVo*KgfJbpz=J+6{d-T-`*viFQ-iO=UM#knwa=-%Ud|J-cb_rm35jZd$u(>!w#X
zy}Nnn=BfMj>K5BAQ@6ajm3C{{ty{Nw-G;gicN^(8+HI`c!fva&t?stD+m>!yyA6h&
z-F9`ay4%<8@VWza2k#Ei9kM&3?uff1>5jQOX?KzCf}`p{Ji9;wG2LamtLd(;yZY`L
zx@+vNsk`RxTDxoOu6O_6qDKUNAgRX$e(Itpq$g$X%ifQm)xW=s{x17_)KlJ5(No#e
z$^BSJAo}0|ovzT)+J~YKWgo_U*gykSFK91GFLnPz_7(4|3Koj`3S{LL6m!tNLAeHr
z8B}A?e?ia$X|k`&zOF&+14XZ|7tqu|7wa|Zwd!>Z!jPLBC(3~S@&xsy*DHu1ph5KJ
zFZV-b@RLYRBOtx;UGuj~-33(R_9p60+?%pDRc{m|w7nT2*@L{UH!n!TdUHaq1sM*o
zWT2S9`G89S#p&(4Z?IF3Ty*LCX6l;_khE_}-%`Ltm)EwfGnTgR+b7@!KmfRU-@d?C
z!4><S_I=RzVc-973uUl9WdHvY_a<<X6lMB<i=rap0g4yuP#%@pU0q#SeauWj4c6V+
zl^L0n5mlAh*=0B|0}Ko^;~XjuC?1HntFGsI>*}hguqx_$A1fZa3SM~P?W!p3daW1w
zf1c*mAG$K)-)W{lRrT#RA~WL6$SaOF-Uu8HL7+j58gbzJLEs05Zk$4RTX8vO9kV3E
z89(&=u<C~mwwuF_ANKsv_rur^6F<~`h+PJLIP$~P566CJ;iQd@px~@Uxt#T`FhLo!
zEvvqo_0_zu7JRkQS2YaQar6WIVQ7n^K908{9)ZgDRo_>EuR>obUq!x(eU<o1`^xxg
z;H#mp#=f#X9L7YhAC>&5>_>G!YWPu;ZTiUfqtK6(9}WD-(qT+o_v3~icl<c;W8=rx
z-#qXWmjf>edgIUy(D4)BPXa#){baMBBz~g(#Q2Hzwc~5o*EJ5&=oT6czV7la)V{C#
zz7BjH`daxq@^$QM?Q7%ffv-otw!SIA8y4Kv_{PQ5f8UgSQ^PoqZ(6?T@cnOaH~YTn
z`zG*B=$ptlv2V0*jBf_M8Tlsj&5CcVKhXZL;SW3h(D&1#pE`b8^V1Hi52^1jH~i&}
zza05_#m_xIulo6{zmoW47mhUi@svL<`QvGST=vH^{<!9kPxi-~{BiD&$38})aCO+w
zx3O>a(xkIAsh6g_r3JOrX)Q(mQj{#kou#<9q+3fmS{j5)1HCkMm&TJz<Ef=_X=yyY
zG%hcV^M0Y$FS`9Ae#d>M(07V`$L%}KzSHeHOMOT89n*K+zB|)*YyC;xpYr-8-7jTy
z;WO>^%MEsdmb?A3-!CVa#o8~MetFcNiTg9UU#avfUcXZ9doKNgaKcr&Uv=pqqzV@r
zm43AboA`b;>DMd$dc9xw`xpb>)MsaEqtkE1{pKS2-_a-AZ@c|A8o+HdHTxaE-%<Te
zf>s1-{Qb_T-^u#D1h1~X@3Py~U&7_s_hX!-e&6l)r!cVG?~nUIr?2pd`jP5KNnbmC
z?e=x4k2%D;#s;PK`#SFHL0{*6J??AUH&NeY{gK-rHTomJpOyQ0rJsBKJnN6${<zdf
z8QMnQHqpP^x9z^|_O0KyLEnabtNJ$T+qiF&zSVte`sa;;f*TY{L7^5D#zE1AN4cOl
zhMO?B;tCu$a6FD5(!moPIknlYr9UP(V8VSL29APfA~+HX91}Q$z)1twrE3azI&fzK
zw-&hdz}*x~I>F>*FgX`YhVUK|l&0a;A}9qx33H4wEQ!$%%<gWpq0D*Li$U2B$^m9F
z2j$H{Il(;SplmQz81sQK={J~}4`z~JMh7#)U}h9}lYv(Xs`!*)h!E5(L0t!R3{zp~
z9)l|A<ZcIzUeE}Gh6);S&`5%Y4jKjnZ$Tr&;0;Dof+qTx3qh+Gw49(-;*9N9h#qHj
z2M6s!&@Q5jHfXy+dopNG1?^JME(h(Ipj`{vb@Xxu?G~H$Z5_1ppgl$>SI{|;4kqEd
zj@?Wy9d)pOrxJ9$pfewI7J|<DptB+9Yz$UAVmJVc!5?(SvuguA1gOiS5{GB%68Z>&
zP8f6)+S_OnqnU~(WzZQ0ojmA_(Xt9U=LcOk=$6o!3A#Phs!_E?^$xWP4(IaqgLh1u
z{S9at_`Sdn@npnP49^`rG6H`X_{%}R5cG>d-wFDqpx+7lKCYM`DA4g<-~@q-vxQR<
z1T!2Q4w^yG3W7E}FoH!s^7$79K^z205a=Kn1wk4FSrFtwU<0)}o{Vv=2ty61nW`X)
zf+!B6B#3knnIIYj(I|*)5Ep~kg-4ShE_3`DT_f>g5c@$K1qp_uoj|*RE(N+A=vts#
z>^RXrGt_yYZD0z4DF((3%w%9nfhh;3#u03Eu9z;~OM%gW$pVuH#s-5<FbIP|5)5=O
zFu|}94E<o_2Wb($zJk;Z(o&F42U#)5+#o9jSvkmRK~@j4R*=~suLQXl<UR++Eyn9w
zfo-GnCO}in_5zFWrNH)a^nncns{$JZHV*9Oz$Ss!fi;001a=tMQDD=+W`VWAd83dX
zRBK_;4U46)IEJIW&~f3GE_5oP<Au&b=&TQ&4WZKvomS|y*-_;5__B4v&`Cl^!&evl
zTZOJ0y5-QVg>Dp1hGD54mI63xfh(49+6kw<aJnCs%N)rsx5IKbEXQFv3ClVx55n>&
zoGD;Fd^qEUGdlEKOjr%Qsn9E7YG3FzFjp<C&V<!^SjGQ-IM)m7Dr~qML~5Yt8!ZpK
z>X*VsKWqeH15t6<NWz8=8wR7%VIvEh?XcMin;2wT3|k?Fn!*;khA|LQ4%@Y`-9Wc^
z*iOQ>4%_HJDTJLOovwA<uro<-H*liR@xo3OMo(eq6xtxLSFz1bvQ7|oLiBTmoe2F$
zVMn9?CG6y3XN*3Gu;+%oQrIhpJumE4!`^Jzn+tojuos8DB<$sAX`nV1`T?pAp&#R2
zj(2M4r{U59Zo)7ugkdpUEzmHW4#Sx+^un-8+Xv?Ahv6U$vrsvq>V(P<RTL^6s!<rV
z!zc_R6-IFwC1Iq)$b``_jGZuc!`KVsY8cOk@%k{{5XKwBxJF|N^!CNwFb={v3gaY<
zO&AZtI1A%EjBS{7R%=g1dZE)UJOI(9gq~psq3(rxG1UD~4?~@Yrpzuwv&eduiNZlC
z9F)UBKxGFi9K_*3hXWH1Y&a~1!*V#Rg~M(*)Zx&EqeeJN!n75p-7xKiX&j~yb(q>P
zD~6d9W?q;jVP?Y|bvHL0m&0)_9Ot1egtiJxkPt)JwiVhodMiWQ4Q&tgyU+%q4MQ7+
zHV$nPS{+&w+F@u%p-n@Zh3Ab_!BvHlD%4bAtP1C=qN|D}RjjF^QN=MFS;7aUavbHj
z%9&P9MLC{ws>+#F&Kzw|oThSG%4xH6)akMv>@2cI=>$AKj#5qpw|elGryQ*u10QzE
z87gN44|K}Olv`A;tK3QDPSN#(TZ4}?HCa)UT1{2eRH#ZtRjSgqrPNZTt}69Z$*9r@
zu8!2Sr>0wKdYNV~<)$jPRJqNr*>YEvd#b$1%4~T_mHVn3sB);v3T{5&xdSda;B-Tk
z4LoS5@=%pWs+_8Fre=z2#!)j>HPcixp_+-+j8>INxK2=&n(|!bO)77S{-3;Q<;^G$
zftZE6NqM=dI;!fbYDrbgs_Lm~6;u9Hb*yF!YSvY=B{f^dq$#!Fs)b3lFs&9MwQ+;0
zJE}gd>J?S@RK2R|T~*hrZWOH6HdR!ksTwVXF|G!l?jcNeR3nCYjcORx3{*2xO$^o+
zRLfDVGCQGLi>#z`L^)9{qgt727gW25QBT!&RlBU(b=BTPYoK;hwOgv)X1{g2$9jD`
zQ0-8)6~@U_J5g<|+D5f=)gEIcNwv?T|7Q4t<yel3u@-t#>C|Xd)M;`2jUxf*0zvyA
z;7!?4=;>CS7(L7A@I}w8>Wow;Rh?XQ#^|F|-8P#MUG$HfsCos}E25`K^<32}sopgD
zj8xB4y(&$XdTn;A_ZHda=mn~$RnMqiu6iq~hel~})uyS%es{l%b_kji%HNENzVeOo
zmz9qSnWOry>ep32Kot=6GZmCnP*y<|?|Zgj__ECK0L6ntg<6F}XwNo{8Yq>~$7JNH
zsG_14TRD+ZQKBNPBBP>#ibixN6E{>GsCb}~hDsu(i%L66y9y4&bXn;djjb>spu21y
z={_G<8k@yR8>I(I=Sq*2w#pQgDJtVA<0?~9W?GrDG8JV!Wva@|Dl@Olf->us*`Ulu
zWopXQX~tz*%CtG)U>4c@GJ!HmnMj#fnM4__j8P_6W<{B?GL~L52QGY=(f(`D<S+$V
zY=ck@8*125BVVPHDlMsW24<LWuBoy~m6cRBqq4fnD=PO?URAlTa;0*u^08Wp)yj$*
zyJ}og<6Mo$%2sGrW}C{kFffZ@5M{gQ7*lo;jY5SAiw%`k%0|k@%5GLRQC2H!lpQEL
zR5n#MQ`W-yGJN1hj?1B4_|tStkz0eiw5Z&GqayeZis%CfzJ4OO(TV88#YC0)s1`=`
zN>ulvdNpdeFe`+CAnfg6ng-J``XGn9(54%~g&Pf<yr@}?nzg9e;8;y_3C2KCQ%B7_
zYQk8d9JRctRgGHpsMUbA3am_EEE2W)FzJBlM${U>pn=Xu+x4j3h}s>DYO{Bz9YyUy
z)Xp&A5p~_DTZ_8gh#kQOy|GcR5cQm>=SIC!)GMP)H0m{?UN`FXqMna##E6}PHe$bt
z7y0$bZ$*Ba7a$w4XoBWZzZUg7QQt>%1x=2qe;!?*25uCT=+lUG?luRV0w1-;C{U;l
zMy%xNC>TV+FbY!CmZH#!SbZr)VL1vL(Q0)8FZC!4_%7wkHNtx_3X>?*5npUWyiTI<
zyhs)33YyP%H&T<4Dn)7<kJm`eM9Pa)HBz&Ynv2wYq-v3>M`}}~8hB>W;Dt{aJRo@g
zv+IjDEp9^Gez?7&NX60WU4+XwQiDhhBb7&LB~oKtD=5<ljX53+G{avUMM)ItC^Aue
zViY@3>_+i)6xXA;L9?2;9mO41gi$HLws9QANfhfS#`c3K&Z2l6#g;zl;no_CV-tAj
zEJaB<N@`J3kCH}|v^i>rPJQ&_qKhu^(T-&O0*?XYoWxO*M2U_P6D6Z4$)Y5Wl9ebK
zM~RKJ6KOZnQ;{x3dOFhONPCf<kMu&MH%7WnlOGr+=uV{lNXL=Zkv5SYL^_Lf9%)NY
z`3C;B;nCKOOo>jaOf52v$aL6KXna=f4URsJOcEI#850>B!Cm*D$0|P_QG+-dB+)=e
z0}~Ca(XbN@{b*E-Mou(xqtRqEDn+AmH0nengrk)Z;;9ymbTl&2$VRCXrEZj#qO=^P
z)hO*mX&j|GN^O)iB8yKmvMrcYL>TI{UG#HD=<Kv;YJ`!EBda58B0GvtEXD;lF4W?p
z8y8D)u@*aS>`cbabnH}O$BUh6?99f_eC)J2I!Y(Pis#db;X*fdPmEnRb|+)E6uZ;#
zvKqU!*d51{PCVI)C&PGB$CD<8Bhe{X>dnCuVmxi)RsVPJi3aaua3aPQPdS9Euecn;
z<re&B#pUIA#))UD@eEu(C9&tmUMcp<u~&&bFZSxOw<-4W*c;Q?dDVsE33_~lZ?Gyt
zEZv{O!SJk0Z=jqiKaS^Ix&`GF<%M`|9M8LS5lY{XE**u^L!?W0p`0P+#p|o_2A8fw
zIhDwZH&o+|E+(188@+gAHLh1+2OrmA>e`~YN~6c_F}!LT2~1{T{~9+2FawR7Zrm)v
zdNOVf;#N6sg}mxp_#gHkFhXjq-T^SbDAA1>W-xT>wC(M*;!c}G&7B^bfE|SaV2sOR
zNHXq>F@hL(3-sv@@5mgt!+=~Z?nW3xihBhN5}+rY1JsM%crlB8H})rEzZCm3@L>@9
zwb*aQek=Cd=o5_n2t97-FN^&&_E+M*8~00bzlUxDj&HM(jJwms0jkJRta`D+LkhMz
zKAr-&S8;dYuE50|M<ZNJxD?{Jgp(J?oj4A7H^)gFk7%Kcaq`6H&7Ig-7h+wEwZoBf
zy%=jh*8NzIV{K#O#-<dTT5P)PWi%!p1o1$}0~3#;c$CChA<i87b<3)8R*$o0oW*g5
za2;nR&hj|3aqh+WT%6b9yc6esoG-<B5|2wTk&bN>hI_H?qW>~RYuhS3U1BJ1H^(-K
zt&XjUaXZ*-bJ5-G;xn~*^Kf(GZcfbRL4yPP1Ap_dw>kGW+axKtNuib$-K02$MF^52
z{vX4wJlw&<dwb%ziBm}&FLA0g&32lJ)1pBWeSFiMcjEYo6C_TUI0}xf6DLU=4Ik5q
zGfJE^aoxnNC2l=&Hzkvk$>dy8@{&@Rl#-;RlhQbu_Gs%(r-ASZ2n#8?@PjA5q`Z`r
zL-@Z-%A1pNl9V<4(<S9WGE+=us>#fJGSf<Cf@CI4W>f-KXcaf9=)`jqZ!+<w60eld
z)ln_+PEI_VRNbUH3BS2XwUkt+;a7z&)~dCndNRk#szFi>G4Ca*J4xM3>dmAMQ7830
zX%v!1k)FBXX|(~*88DWF^%h24F~Wv!WAtZ)tdY@v9VY1-_Toun2=nfwktGcp&eElK
zt4Yg<Rsj1;m_@?&F`?xm45tc7y9i6cr0pi{5-j<W_Ds^QChZ!{-f7&1KIu4V>!h8-
z%8IVRJ1%{8(z>WdyLi}a!ibFaF&F{Hh+n|gWJmF3LaPqgK)_gGh@pB~&~vaEV{b5}
zZ_+xW=VP!i>1m8tt-f<G@_`W!jA6hk4TB9`4j!^+AARn4BjLi=iEk2rnD}YpFDHE`
z>6g$soAkY;@1x%{>F4N?OoCz(I7#3pK?%Kn>`F&pNmxk2BKkFw&`rV;IuY2v&K4=^
zWQ9Z(>F->X=*y7J540}OLO{*Fmnc6`I#Gi}<%t@j0!L@JoFIj27HpznuL=V?)Ob*p
z@e_0}u^NF_JMK}uGBsXTcr7K-Fo{M<lqC^v4;PQNByQ0f1$RDg09>*-c`#1FJ~q)#
zqTNK75<Qb>FVWRRHxk{X-3o>Rv`KWH=#@m<#1s-!OpKElH!+imDJ7<ym`Y;2#8eYg
zOH3WMw6u0HiyT8RIx&O9<cYBftZrcfGYAsY=m$C(m}J<XMa<AohIq+0l98W`k|b>=
zX^>>aBy*C?O|nvwm6NQ-c@J4T$^0ZUNtP$sILU01SCZUI@@kULCApvEOUcSgGIo=3
zDH-ROCjsN}#I_O`B-(Cbdl=J5>{4R;i479;Ls*5{1RiWCgT(5@n#7J0n<h3(tkoyh
zu~v{SxVli%g_<smb<xFwCAwI~VkEja#?ld3=|MX#mR`_KMLV8$s@j>=&YX7UwX=~^
zG+??8%VJnN!obJra$MS3<hTmD%+V=GNBn5kphkno>Smq?`n5N3Nw1w0-r=>g3_tJM
zvGC*$SM1s?X?I$?HSN~5yGgraJz3RLo}R+*vMzOX$>(V-X<agOqCf5F=?!|itEXc<
zopGi}xkbYwjEb+i{$z(ex+Lk-GuGwJx}4~;*5#otTRl^t1O6F!ko5FSRnIi^Ow0>p
zM#B}O_KNWM20w1Picd{1OzDMby>WxCaVi~V$LV@S*Q>hj>$=ADFukdQNlLoW(v2==
z0bza)=G$PNiEa!rtwM7W0t}a7)DHWmMONQhiEcMw=c?O7u26)9O=#xm((5@*5@Bcv
zCy^E1@pPv~du<xk(O?edZtPp7tr#kRn|TS-C<@j~NC$>NR(CtP8|iKaBMg{O=pKfy
zF~p8vj7DR03!}vvBfA)p<zyPnFyH_u#wjtNhyg#eHBqU=SQkdBQtdD6zKc;Dx*hFT
zX^n+pvA))Qqx-oITnqr9PanPZw9;a?HF}LR9pvawTpc<qYSqvRl}Jq4fxUSbtq8OS
z*otM{7q3rN(dfsNRTESr@X8NZpN$h8=XmGo&57QOd&$MC0?%%p<hXjYc5(W(p4PgC
zqUSUq-J)F>^+sKIq;;mX<w8$b76wZw7#A+&;agq9kv2Rc!(poNw5e({tIeD?HO?Z!
zGz-|s!!j4fWv0u4J`=J|jc0?=+N9cK+T_|;jdi6k<!I2<gBH!u27w-gdJyYDq6b<J
zTO1x7MS7IzEYz8zF3YaY3py|Ad|JafV(#g@s`FW$w`c_nL$-WT=f2LD*jbKdJ*x0|
ztZ}Tlp@sfRtXEd_*wy2j9*?yxX<NgL5LoWRf|%xSwySMV+eHjZqOV=s5WNCu1fbf2
zM;b~(Tchnz+mW`ZwwWoorcg44nkkG;(KW>>tWji&HI`O!jD=^gU<}rGF^+4vWDAyE
zsTjvIPSrSboMPoPIb(p!guv{sYn&cOL7l#F0^@}2hge-m!#JDy9OWVwSe=4PP#9-q
zoD>T}7`JF#*SM3$Eg5&(xMkzc7`JBJx|y6blNB@RnaPk3;3?QgS2#5RR?_I(C>c{q
z;jA1!%FVQArrV}m=47vOlXDl!5WA+_<6vxg$&~x-UM<Ibgws#1G4y*og5z3vr!_Ne
zGZUH_Jl)~jv}`J#sZ@<uFkaDkF1$AyuVOsUcnF*|-h%OJ#<QkcFx4Xb)0t|?RBNVc
z&1}KU7UAp4%vv*7FmpvS=bAZd=3O&iGV|6fxMrbb7OYwC8v1vzW`k=sl*|TeHo9hG
z$!xTkW^C&CR7~A7^|q;Z*+Wy0xl9>WG{TA>4VSLp;o_H*JU!E>n#QbY%$Y{bH0qq|
z)@X9xMx$*SJ=5rOXod6r(9MH6b-2JW;caLdscB@Uv1}T-X^b%k%rqOAtz?>s!OWu8
zqG<)D6`EFrIUc4p;^et@i{_Iwqet7Zz=cOTuA#v+%%E$WTGwfEvPh?GIz0~N&;%A;
zO4t~-r+81|eu7mcZ6#s&XgWEJ3r)Ag;c*Q6bR*MEOm|><c%5QFHZ0tPg-u+uI7uh>
ztCl@x(KCxRvv{&utaBP4?P)NyjxKXFWMjUBY1%R{i@8}G!^DL1{AjKLs|y%=(vZUV
zeU2~DlmWe5X#1l6j{6(K@I$i{nSO)TpnWumT@#c{Fk^xW#vo}Q%0W1UXBZlxttiJE
zFg6ey_NAhC5`B4S8)E?$_*!mqmJICoqaMeB+2w(52zD-@d5@+wnzCqQns|y65MgZ!
za~rm8(11Y`B|%#Pm3Xe=lDPEfpUjw~!g-3U>Y;uXu#L`Y3bsMz05@~N=ptUCM!SYD
z!m`mbM%Rq4(}<PN?$~H!^vJ9}L5v>b{x`<utVf;?9DKkQKmRh$gB-MJk}yza5VJZn
z&}P`68SD^t3NVJnZ)DP<NnNZqVA7^Z+a|@X1%nm0GORh{8Z1x%7t~oDGZ;9Im&GQ_
zO=eAAFnQ7B6_a}=ubO<;<TaBwIe#ekO}=FEJ_r5K%b3S}zM`*jWyOqLGcK8NZpLF{
zJ!7lJ&Kf&s?7XoH#;!MZgRvViansl)CLtM2q_8m5h4Et#wo5RnFm}mcOdbP|Hbl=Q
z`Zf&OaH!wmwSbba17nBAj*LwWo`=>B3htm73><gh;xjWS1%uU96X2SBFynB61#jb^
z>cR!&pgJ?CRyc{gJ{WAOV+!t|k+2AxlW?xjndQyKpc%4%v>9V|A72#B6--paq^Uu>
zGibvgyEy1LgN{4slm?yhpi>)k>VwWH>`$d-GpsG$LAL}u*Fm>F=ynI)e9&8D$4n1r
z$r<>qfgivqhn=r}!t>@2If0eq;+Q)EYvq71rN9hAXAruBurvs(gRnLT>w~aKLuL+!
zq2m@qR)aw}9)##T#T2ggKy?PnAE-XMum|jzMlUli3d?m7v2t4M3}SZ>m*_z}t_@Z@
z4hC@>X3+z-!{b4m4B~tc+d<+ClG1=py9TF)C7nUyqq;vx622r8%O#1lJJ6+pu5;e4
zZVq&3pci2X!y(_*ngQOWxC{pxrwS{!;r-zbObL(gfvF8l9aD8N6_WE~OlQC+h>w9{
z+>L0C<0csxJuqfq1_P4~OpcR1ScwOiLy7rnwmmGk!$N6TEDfg<IJ@Wc8akjiL$5Hb
z7KhamymJn#v%^`en1sP+)Z)#s6%Sje%RiV)#&IHwJM5H(oz}3^9(KCJ&Jtz_pbMOx
zWSy8*{*E4Y1~8!>cFr4ilVQ&p_DaKES>_h|EllX43Ax{c!4vw`hJMUeg0FGI4Fh)=
zlsNyG#sqd4Im5^uMx|lY8b<A5<fCVE7{zE(3?n^^ESiGDWPr*aC*I+*wzEUq9TnVB
zp*AXvM@4s3ERBk_QE`mr-$sth+00IR<b)$99yxmCn30o?oNVN}Beyo1oa4N`QaYN>
zM&-t+oQ!6I(To~-?#L^Ryz!{&j<DEBbv&9ajp~(A!yh#QHf|bv)G(t)Hfjz=Eg!Sv
zMxElQ(-?J{G>Y%|m>4ix?2H!u5$qKGHjLgdq>n)Zwu)#|Wk!KJ3QC+y9dt&4kJ0zl
zK1B@sxfr?|soF?2N6L<(B05Y*F&gCVC@GDSD(7A&ol)YWK{(RRNV_9l8tL*#*G4)Y
z8Gke?kMNh4N2wW&-O;!-viM-Gc5{TOina}d{SgYy_C|JbWc`utk8Ci)5UW+_%N*Hw
zWRsEABWp%BOAEEMFiwkwwCG}G$Fx{Wi({;-hsE$x$K}j#r^#uKTtSVCld&t*(O6_F
zbuzAB<W8k-Idx~!$;ouGnofr4q)sQ(bgGz6jnb)%%MX?+X~|1Vb7?6{r<>`tNvBgz
zB`-IzxB^yC;M{_87xuhqc`+^fX?ZCv2WdHEZ)-VD%bU}3l9qK!@BhPerp_^$nNeDC
z;R_ya-07UV;-!^pTA58Nb7^Hhtt_OK^=V~8TG^OZ8fm4OR$6JLlUBNE#ipLiHJ9km
z8s4kn*xO6JYU<6V-dyU<r(P|kPhre-Z>QcU^~R}Z)2hogn&?g!Zgb)5wvtx8v^tko
zH>A}XU2ar6X*Eu(Hl20T*-5wxO=qj=>|8p#KAo+lv-Nbgoz8aB*={=Lay6SdFP*EU
z>$~(LPA??Yv_6~G=hOOzw7wA|PHA1kmj_*pH^Q_zz>MXzHJP?bT<WCdrLAh(YH%t>
zD@og!h2Kg$?X(l59i4WYX}6PhyEM4#MrjXoHDK?kQeUUONrTBWD5XIs4Sbk`q@l}o
zb;4R2MyaZ#%1c!>RkK_hEq1XW71!H|XVTb9<7yfsd@hX}Y24)GkGRiX@OYdilW9^)
zlMb5VT!2M)(AGj5Bh^`I3aKfgmYf<FHQm&Bsi~%BHZ^mpnNQ6^YSyP_Luxjrrp4JL
z#!pR<nm9EHUS?S8Cmn2}UxCp$O;^{k%qwZ`rFknIyXkl`9hcK_Egk2nEv42=Z7a3y
z)OJ(rr#67?0D9<C8>d#M)}%H~ZI%_>tWe4dwX87Cif&dcWyM-n9AhEL%yF?CW9E37
zQ_Y;&%$dua`OI0!ob{QrA>+D(SP0P1oIdRloG5cN>kCerIeF%~T)fMj;@U)RIdf++
zx0bm%7AngotJ!3jP3mki%chE0Pb-^R=8AKrxvVtGO3T@FGn+<@1I`k-)<{<FvJ(Xr
z5V&u_gh&iI&Wy5|G^@DT>f#4krIuA}<`px~&AdsjfJ7(kwago5p3SOmMz_)Q`<zv4
zS#_LMZ8qy>vy&M;*w(T+H=C<v^+Hy!Wc7_%y-RDOdWeC|tgcz>sgJUHmcbVc-G~HP
zW0cWL0^A%#S<B5@lUb|8rFB|f)~aT$4lR}1oviI=op#m<Fk>q7JDIPtU@{9zS<uM>
zABIO+=yHvwa3%|DFwKE|Nv0~9@-kJ;)GYl&aKOHksX-PEGAxF+I)IVIUKUrgcs7gY
z=s+TFa&}SdXK_D^gDgg`*<_ZKvZRwGK6=p58N`+Pw43P?`cN|6%Cw*9&1msvI?vc*
zM2oAFnZ=B?Vf1IC8amDfF4v68D_QPkc{R&xS?*`~QZ{z8aXB03*?64UQf584uYik!
z%(gH;HN&(g+r_kl%r0iuhq)H)Q!=YEj0;)xk)zW~quQ9+G_%=q(Oq_H%ahCWeo<;J
zmqzq~F{73n&E<w!?l{XGceyjU+?iVLl$JZS<xU+QGL}2-<xaBP(aW9ja>p(&HkSQ(
zIVdd$b~#eZQM4RSE@OeCWOb=46D*r}*(A$@aM}8Kp^+Dwd7+&bx_P0O7Z4KVg*Y!H
ztjQJzd0~_n(!7x6#bREZ%8QM>*vyM9&buvkIhO+V@<l%{_VZ$p7sI@$@?w-1<Gh%#
z0|0g5;vg>$^WrEkrg;(REa%R2?o@Ke%boe$Y35EVcOtAMnL8O*N}R6b(_TJZ&8KJc
z>A8Hmolgh(G*(%d$}6S3QqH*&LyDCWa&Ic<VgqHat^k+q^tL~ndvm!rPltsb9M{2<
z8Jxny4fJGQEpY_~xOJ~q^Xgn)ozJW5^Xdk8h|XtA`D_JldGonaK3B=-=JL7ud~PA1
z+mO$}GiE8DujKRVb9l*IDCG;4d|@G980YIt`T9z}em-BnAzwevH<a=Xm3(6<ui-PD
z*Sx$oo7d*@+WNc}atx%F<Zyu1Xu@+x-t6SfAaCIl<*g)d4f6I3U7NK1ygkS}LEahW
z-5~F!oCdcT<^E*uPvw3o_oq3<+4o`MM9Ts+DPa1%G?gz!Ijxqb^8R$*@8qz)g?|*9
zX8QD!LbEgc6Xj|;R}I*g<Z6Indkn$SfeJ^n{5+2G7^201K@Oh;$zq;Fxo&Xo9eaWX
zXhNgSnVT>-37R_jsFaTu^R$$wGkNOeX*Ex0^R$(xah{rd^?{pbUY<4btc80c&upGg
z=XoX1y*#hx`Fx(Q&+{<PlRVdXp62`;=VKQuu;t@gJ|3?WOK^d-(u!8vjg@w^((_k>
z&Po_@En-z&iIbH$TS+=ANwlJ)6;t2>#U@%Ap!WzX6*pE^+AAx)m6Z@($mmZ(GizmK
zFg~$9b|1t=j8~V;9J^Cvw={OA$L<VWlp%9>1#3)>Cnvc^2mJkWJ;-o8sj<i+m+XN1
z|0&JYk4t5`b1pT;r3fQh?4K?n9L>0#YkF{{xB7UxK|}TFXgsaQ<;J+&8kgJSa(7(r
zjmwKPU@r&bay%|4<1#iF@lAyX$&AB=1-Ob2I>3_g$(8ZR4X&|_<v}*#qQ_$FSZ{fn
zONFdn+4Lv9dYR(t#6^h<am?$iNsD-16X|%2Wiv}-6fOVGw*}W0YPMLj&ZKpE*3s6j
z*^;s4mMv>rvDPbCuV}p~ERSJ5&w7j2i><fWdL!$NZM9&lMO&S+)v~28{&`!i+3MKN
z7VK=%&Q|SQ!Oj)!e8J8a?LxsW;Mdyq1-rgz*H7CGMZ2+R>vh}kY@=!$v$in@FISwY
zPlr=5VueKvohZR665G3tfo%*e+y*sgY}2#Ns%_5N=A3OdILnicY<wEMVsstN9X@p7
z1PGo}FaeZ4N@xs=QFn|Ope2MiKOf01T?D~T44o|WY}@CUJjN5+19}MHv}c&%Y__XY
zc`+5%c7~YUN|!?LD}%YDt1}^KS_}I@4C8YY!nM7U?akO;-S!%sYznJe&SXQ>0Q1f4
zV$m+TcCl<1YnGF#+MJDwIYWy+O#l{SySUjdCU((U-=Tkk)oCXj7hRoqVV5Gi6x*c%
z{TB2)Fy*&>W&0R4ZE!wo;M0hJ?S9vWB^x$4!xa-CVA2T70`!c+m{Jw2Dq7Xx%vTk$
zuT&urW<xOCfeQrK*}yV}W-hcdTD9}A(a^>&*Pn?!dd-h#Z9He=c^fa-cs>0ptqy*3
zsuN~fz?yh<)R_Y>96ZkL=7CL0Hkq*;LBud2M(;3Z&d1Ofy)nU~2|MoDBTw^x*dW8`
z5@t;>f>`YYXQzc8tiBja*3{{}iT!Bo?V^<gyMzX8n&7p$+FVBi+YUWDtlHr${XD@@
z3frPS+KP5mvZDs4_Kr9o3#}=R2#u^w3pOp<)V1lPO-nYNL8Tv+c$?O2+M*qFYHe1q
zS<z;$%_ePDve^u3RH!`JtY$NKlY%c6pXSop*k;z^0cQ(X|J+)~+PbwZYuh-ZFs8OP
zv@C9pK_O$Ld!W!dP-q`0^bQmj4;1_Zh44T@9VkQx3h{x$<^u(NpkNLZO9zV01I5;X
zV*5bRKTr$~6ypO$eV~{fsMHQr&OhLtf1rB)0o^&T6`j}aoY(cw8>Q#Fh4V|r$0V(L
zEKRKbJr2KcIyBh3ZO`77Ei3I^r|myy+s=J^S9;s`E$%;cCE0cAuCurATV;3K-hEs#
zYTefDd(U3C{hTdlY+EVs+P!V(IghXmx*^5C;rFcXUbg<~v%Ykzzej;n_wU=cYp2}@
z-O9Vq{Tnjprk7s#fxlaU?w)NsPdn4@+q31YZKv(qwddTwMU7vd{;or(fZcuWzB702
z{9E#$c*_az{JRzKcW&QjcW>FV<($1M$+j&!R)6eiTXt+ebx--My}NcE?5rG$68Oss
z?AWq%>p5HYoW1wpI;@=nHx$Xib4SsvodV{}J=^x4xogMPgBQV?DDcSbr*D^w;ubFf
zz5SdOeC^G=0uKEt_&*BR)3<Niv9)@WC2^=v!5<X3nODFXl>mCUVUTN9&&6MR9IQzR
z+*pCXb~mo60ypPwIOGcK-m`t@A0Es%dF9>c*?g!K*o$}KzHMimYqxIMx8;^n;G8Yn
z|F$>ajSAq9)CYF$M@3}k88=S$=Kp^vDkQsi?cHX#Zri*4jGf!|{7sbs=y}K$u&3?X
zdHVJ<ZqDgnEv7@Sz>ZyKl=nSq-_1P<heCn9+t1m*V+$&S7Ih%hjrT~+@MfuTC=}4U
zPCISeo|Rx1AG>?E?AyL;=gMh2w(Q+&dpmZW`d2QB!qUI}-F3p7R$x`9gKMz13JkaJ
z-T%i@`OAtNb@AGiz}9V#*uHu)?4E7AckQ|97Ry>G5O-H_MeJb33{{dH`)=|9u~w%5
z3dCFX?Aw0Yj&0q8<v+283hX|e52d~PwxFGR)6b<tr~sZoD}OxoH=X03J)xdmdhemS
z4XFU$OS|!Ex>=8)8#Fn@3ZU3;Zbd*!;t(saS_xZK;pP^?S}A~4NwJ<4R;|MFj0Zg<
zYpnnlVe<~70$64hOS>IT1y<L(JZuVJF&nN1w(;;PfK_o0rvg~X3JZQ8?3P^1y8x#E
zi!5Quzr(2j)(||b*8moz!^*seu>`P)8`gb4tO{V2(8H?$);Bw>3SeE<g~O=;R@cG;
zqKEMsz`|aYgTIdVZ+qXNtdjk)Lc;n07AnJns8|#FU?s5T3Sc44!>a%may)DbU}bqO
zI(m2&z{>Lne-y9fHNX<!BDsh26nI#3@8HjYH7)@z5PJCT0-ORYWjud)6~M~c2Up<P
z|GjokLY4rQTRiw(u*M2tiE8iQcfs55`TjL~7oY^NFf7){JlMNn%@x4=@USZ2d06rH
zaFzg;`8{}zXsu5{b@niq0G3`qc#UYSOMuIzAGT{?7V8dU8Tf;@3)Zp(pa9mu{VTWO
z2QOZ`2N4y(YS;(A2G&pktiO#l)91W{pMo`101HiXdGWao2fYZ_Tmh`{d^i=@P??)O
ztfv5reOC@|2~_9)N+;DCUjta0yn67L(V8oOb?gu8MTk|n53d5VmAS)v7tDFH2d|W_
z^))az`&ask*Ia?Q>cRU7*Ia?W)<v`C3T&7^j8DQ1-rW4c?7`cHYgz&uy!CUHTTX#+
z*Q)KrsT?$K?B2a+*B03NVhH`Tzic=l1DE@MeaP$w{-OeU-?rU*t={(Vo6_PC+7J9e
zft|Z5=$^f~Cf0|(1a2^2xj~zop~l)O0Q-$CI}WbJ8Y=)3ql4!^u?7n4ItL~M+iYXU
z?lZU8N6g;h3M_8hveg>cjo8+zb@{3-%}r}@=<kBFckSG|i`FCi_UziR<0h?V{$R;*
z=ug3)D}ebJmGU_^c~>5K1<qN0Alkh<c43l8d&|z#wqd5%O&j^&A_~kNZUyEJw*vEr
zTY-hcu7LG!QiWUOB3wUx*cG_x8}XJYftx%56BEaO_uYr8w_GYDfAS#eZQExDyKQ&R
zu5)aA+v%8*we8Q8x#bl=L*>TL#lxV$pI#5Qi~_;7)A#+!asT06cuQRa$@VkOya^4~
zxCA&2ZTprTHiiNIUoL_*R^Wy^WA)u^6U=_Y%}|A5(T%RcHC7<rvUAIhedo#@h`E4U
z&%JT(xD;-A1#EW<riT8p3?8%XJ!|<Oinrak1iJq(-#^|4x2OWa;Z`8I<rUbxfBR`?
z+r5~R`iMV$0^d@n;D)&mcJ-YEALd(7MIP+_lX6(&Yv4~6(EE0s!(Iz@BNYyh0v1zI
zMTuKRfj+N<?yfzL+_Gn@<(mmVG8O4CD6slw`lAN7i~{-`_FOIPKL?eg8_MC2SHT)9
zVD@a^y<;15M#gZ=-hffV#?$xj*m27#u<QRS@W;&gFj(^v_zMdB$unt<6}aL(SG@0v
zw_W*!E8czO)2n~6o8-_duzSnCGwp87yxzHe=NW%7&p$qf`OhIV?cTk8>tE0BPco-)
z2s^9)FyHlW)8Y^+uz%n79ozT)O%5myZ3$p%-T#|uc!O{=3gZx71ERp5Ej#yaISrF9
zcW$$Lx1F)-(c~bC+%gKFO0qgD6eaZMx~zc$+ji{Qa`W$pL$AQ2&)l-}484EbPHWEG
z{?{J{heUyM_wL(v&br~Q9q?hZ&fj_Zu07}6F!j~nxpmv4w(Yqgrni6l*8lSuI5f|J
zn^a&F>uvk?;fc7nj0tG}H+O^_ifO<5{^0P#?%R*a-?;YoJmRl*QvX4l=RWC!e|O8s
zh{F*(`2X=JIus>v#Y?Za<jTig`S>d@z^m_FSNz)*m#ka;_tGogcje<D(0@^R>#oxt
z^@pRrG1tBR?=OKXUUtQ$S6;~X?z$`9bLF$Hyy%KcZZf|c=e_#>AyME)FMaRNA2k0b
z$3F}Bg96_f-)c4H_7Z<<kl!QlJrdvB;d@7s?<Bm7@K}(0ABXR4@x2$m-1h{08Gc{k
z{e<@yt`nXpEP%|%6?sxPB`gW2g=LWXRYdl{+u%Dd@`7-^aD#9o$o)<d`D9^TxJh`5
z@FBv6uqkYTJkGArmvDO8;&Jzd0mw4kj4$)m_%hxI2Oz^oBB#O(Wcn*2kHMqxJsn?e
ze<r>xZ@OEeyc=Jp`*7hNkl}p%aJxt2%lHHMvYZ}^FXJDFFT)>?FZH}g<R=TC0&@Fj
z;LCKLi!b+k9=_b~`S?=L7vXycd|!<(<Nq07#=jO{?su`sZvvU#rTB8cx8cj}FT<DH
zzeD&=kn!&p`8^`PAG{~NSK-TazlJaOyBc3^|4os<1yY|MiTq=b`CNzZ(fIya<llna
z|9au?K<ak{0=fNdK*k>_ye-Iljsm&eG2orS6F_c%KjAu%+dTlpXX1gv2Z1c-Ns*^O
z9)~A#RX7VW{q-Vm0GZxNAoHn9_$CQ&fK0b5vM=#V!am6KBB2JE&H&_o%ObA`9}40#
zVexZUaJR@02YI{~i2OL=<AqNUK2dm~@JYgpgijVeMfg<V(}YhKK129S;j_R8BLBCF
z{5Ij+g_jB6A$+IsUBZ6@dED<2`Mn^^^#dY*Q1~H`+kHgje;0mK!apYR$AzB|ep2`!
zAou?i$n>w2@Xtv2=Op|p3IC$-OA`MTk-rM^ynO>?I^UM??+Cvu@jnpxhamI)k;p$5
zUMsu~<n{J*kmvo^Ah-Xm#9uG`oy6Z7k&Hi5csr1K+yTTU6Gwx*pN<Fl{<t5A&%`?X
zF#begLHGa=pNR*F{9utC;iQC52}{ChVHu>}vm(z4=Y<Qx^&s`EiF^`>&qN(Rd}l=>
zZw9GnBGf_ya{rNVS;BMSif{}bgYC`&x&1j3zEijhq`vz=d?p@&AD+)ggUt7Ugr5&`
z{|iKZyu?32_(b7_!Y6?|k53o*8Nz1@p9S)L`h1aJAbg?lMZy;g|4H}~;Y)=t6TV#d
z3gIh-uM)mm_!{9q3tuaIo$z0TuNS^S_(tKy;Dey|l^{M7pOx^>2|q9Vg77Ng7lmIE
zep&bx;a5R?Ca%U0%l}*89C$nmo#FQcc|PtXyf?`5T_^I1!UD+sA1rcF=m=fmB*=72
zAh)l8_)N^=hxyEjyk6uD!i~b3@Fd~M!n$yi@D$-ggbk4SwnT0VJHoE8CtMWz!X;rJ
z<b6F9c_d6h9?uGRNAOGu-!42$c(!l{$a35x@?POSka|8s<VOl0CE<?-na^V+{IS9d
zKzt^ifFIshPZs%U!e@f4>wQ%CJCOM>F_!zSK+3lUd4JqN!jG2lV?@4_$aeve#KdvJ
z<3a9sUy;{=Os^n(Ajo_k1oC+}4RZTA37-e?nb?RQ?st-~E<8op0P&gV;D`Ay3i}}U
z3qgD)Hsgo+B@%8x9{<xN{29V$O8h^7JRi@O@D~VQD0~sf^#2KDzApn=&aamE*9iX^
z<aVzUzFzo7iN6@+{%-;CnRqLHxc%Ef>U$Z;_;*S8yM^zS`1gVMOngY>4+}p6a=*(#
zruT7>$Mb2B<@^PZ>0bp>@2`sdH4vYPZ{mmhe@DW<EBu~>|3LU73IDP1T9Ent48&*R
zmlFOf;jcmJ^IH&~)y$dCt)T?>I|`&;cK}(BKMrL4@e+Oy;XOef$NfR3ccO$Bgbxrt
z5M(;jAk(P`XF={a4>G?6;d<c);YN^p)<JH6iiAHz!rLNugk6x^EsE@m+!ux*)7vcZ
z30Q;tbcuha@E;`p*%JRrk^ci^8|)|)7LVtSAoV{+cqieVg?AC&Rd}rMIFQe+`+>aw
z9t=`%2V}X<i0pyPXAY#^8$jl_5riren<Ts`;VqEqbVTk7d%{JbFI*D#g#pO@mB^7W
z7H$?MLM=2P^-D$0K&F!m#~_d6VG_SZ!cP-<t8g30bhe9pmhf!h4v^{Y1bJOOQg{K#
za=QrR{!athu6mxxF9KQ4uLP;bYe1&+S`eR!i$#93$ZrE#&Tj{q-n&57pFS%5EXZ`e
zAp8=@?Y|5%-LFdc*Fm1=A4>c+68~e6=kF&X|4igxflT+eAk({E;(sr^6%La694R~s
z<Z<5xKg{=7ko(^q<o?GC?*VeZ`-psk$m@g;1gVcJ@sq+S39o?Me@?>Zg$v;E2oFH!
z8-dhgGsyF^3{sC=<gF5aI>`3fevtb;3gmX@O8f!g`5-<MPs9)7F9f;$lSO`t$WI5U
z|Feb9m-rWe)aOMazZm3kyiDYm3tu68CCKz&19HFDO8DzUe!a+V5WW#)zL!Y+ze@O9
zg>M7#nRqvT*gpLakv|JE{m+BEF24ry_^t+dz5D>gDWA9w<aYl9vM=ja*qHg<7DUj*
zF(CE53&`z`75VNU_rH(G_XC;!{Xy>kK#?B=GMy60?W-Wun+LgF4di)jNq84z`7MFW
zFOcvE#Ajk8@-oQ$a**4Ng;wHE6>bB$-RU6HJxk=XMcyUyZjkBh0jb}9ksm4Xj}o3M
z@#jhWV?=%&$m4ku$owu6K3Vt_koiAD_-x_xB>wrr7YJV{e39_QAoY5w@D(8SeU-$&
zTKF1?f1Sj?Uc%oXe53GUkm<ce_^%TFR*^3g`5nS{g4F*#AoG2n@B_jRfjsUHgG~3M
zB3~~27|843(;)Z166E!8707hI0&@Sag53Y>B7Xzq_TLiu+rsY%zY8**YefDL$n^eO
z!hZpB|6fY{uSEWh$iEd{FZ`Xv^P;3aw*pu12awn4o$<rx{=Gn^dmrI_L2iFP5TA)6
zeyG>9$O|C1-vCm-lR<7@7jBaHhX|Vz-V(M!mP=3KeUSID0ip853dr(34diyGgG~Pn
zkoj&Go-N@!gy(?Ve?Q3m9wp)D3Lh;z05YBPL2h?}@Cg$CMB#-J{$!BbKUL&si2N+!
zvq9?j9N}|?|0wa#2l1JBIew_$>p<rF29W!`QFyWNO~N+|-y*z3c&YGTLF)B3k>4)7
zO!y9v=jYuZ+jW<NEZ0v-_-8=o|5@SZgr66F0pxS^dmz*KA;|0JI*`}RuR!Mi8<6Sz
z7UXgLUSw`W`PLw}I}+q}w*{Hb(IBs@yMo;AZW4YkklWo`cpnL02XgxdgG|p6P6|sP
z_nQ`$g)_p6&=XdLv%)#yyl_Fd9%R0CkvD<Vw*gX*7Rd7H3Hu=R2tX9Y#6ZF`311fG
z!WEF|JXB;W@)nU#1)2YuB5xOY7s&k{F5C;^GjTqCnEzvhj}=}Ze4Oy{AoqWw$QOdF
zpZt@^FOm3{fxJIoFX3+hdAx5H`7OdrK#nE74`jL@knj%*KLoPe{$2Pn3I90A>+%Ya
z`+WxF^>G!*<Ng}R<GEV+b&&O;Yel{e<bMAn@=t_675+^4bCBEpugJd?{z~|3iT|z0
z*Ngmnka{11LZBYE7T!j9B*=7cC-UuuM@jh6Adlm&5`L`kIFQeS`+(g4zQX&1-2Ooz
z%cTS|-)Ui4I3uhGJ&@_liaaOs0?6__Ny1N-@J-<Abu8gcklXb@<{wJ95=Ig}5P2vZ
zflOyv<XpHS919;Rw8Do8w}9M#D@grM7kRtLXN$ZOWI5~-?iM~=xJS5GxKFqrWV(+8
zd3@)I{1}OUEXe#HFY*(FPZVA#e3I}Y;gf|=0h#{OM1DHR`{2bO(|xJ%6~b48EZ^6I
zY$sg?^0?k1d?(2J>pdX#{UFHa$R{NJlOWUml*E4)Wd5I*_%De3Md6o)UzPZ;flU9K
z68>!o|Bmpx!ta6H?uQ~@Bm9x@$HHrc*MYoVe*tp8UrYFJK<anB@b?nVmlDH|09WrT
zkjHgfk#8sQM~Qp~;nBi73Xc)qNqA@BT|lOPtjNa+?*=lTdx(5bk?#Xi@B4$e<tJQ`
zr-dF!J*&c5;hb<@xBxPp8p!-lmhif86Ug)061fd>yROJR;iAw7xxErO62>6+OC)|I
zawhT1!W`uBK2&5Ye3)>H#BT+e-s!?Kg=c|0?z4qEgy#r%3U>*2gUoL)$n^GuyxtFp
ze7^9pAk)17#3eEDGzou(gg+DHaX%Midd~-$&Pyczr4s%MkzWboGw~*o-wg74e7ne(
zfz;=nBEJh{y6*vbzCIw~9|XDGheiH~@ZW_W6<#j<nDFDmPY6E=GM)bv`BTDA3$Kv)
z&xrh4kv}i;7lc<y_?JZfvhXXyuL{2=yc%S_-w=LF_#KJ=uJC&j{(X^uApD{58j$a&
zpNjl5;m<*q+b==p^J|f>m-ydFI3I2-*V}+R&qsnhA9ob_7~!2jrgImO?<zc2c%1NV
z!n+HP7v4j7PvO0U_ZHqqc!Kc0!ux^Dcb&*53JVhcK#<p;EAo`YmxR;8vTz1udLGE@
z`D6*N3pWW*5k3TD`W=wvxhUbja7ow~2EtINgpn{7ZWbm&Ei}S`a3~xJQ(-1t7Useg
z;aK=kp%p$%xJ7uX@HF98kmYl_$Y%)86mA!uB|KZWLwJsGr*M~WH^_3_53(E|CE@1^
zA1yo}JP)KEj}iH?!V83t6Fy$}1mP2f7Yd&wya?oV{Y;UcCHx1F`8-GD=ZgG1;R_`E
zg(AOL_!8mEB>v^XR|sDzd=<#_UnBBAi~Kr}<#DmdZw7fE{wv7xdAsnP!gqsA=e@%B
zOZbOG{)q6SAn)VLg&z}s9OV1*vmom!Uj>=o)gblwHpqOxC*j`*na&SHzDDF9iF_@{
z^nMA_2JL86rkLI_AfNxoflT+FAdl+=kmdgX33ou|HwALPX^|_!S&;e93FkqkdlJa>
zP67Gc>q-2g#4C{L#US$^Nc>Rb9Ar9MKyG&$NIgy$ZWrzl?gF{p!$I!%NRWCxN_ejD
z(ZU15^FXHiSdjZa4x~O$lK6`x{3#+oRroaF(?M?kY>}S>GM#@E`FSG0K;#z+UnG37
z#J@!3mx}yykzXNvrSMh4SA*31wIaVx_%FiO3*R7oqwr$kn}lx$na-sm|0_uS-zM_g
zg_lYAJ4JpM$o<{}^7ua}@gJ7>j|l%=!Y>#3V<LY-<WGwHpCW%s_-Wx4!YhTJ0jb~T
zME<<+3&N{}Uj(_`mqq@H@T(Gjwa8x=ena?8iT}3nyTbpH`0opUApD{58sU$GKNemq
zyiWMvAoco*@MpqbfGo$~fV^&h3-UTX0*!O>_8|AWgYb?Zw>w67C*hqzo|j`qK2CTy
zkozAmyr+cUOL%YLeS{|n?+bFj`wLG5d0#(B;vXz53LT*<oCK-Qw6G%Kp0Fz6^B~jR
zDB(5XNy3vs>aj`q5D9Mxn-bm@xg+cfd%{Jb4^po{<Pc=~k;t)dvoH~AklPJJ9tuao
zRG5L>E*E)4<cER{guh(mSAu*#zFv4KScd#{ko`wL2YLQ~1F{};TXZ7g--)|}s2Wb3
z2;LoB0QtT+1?2V}klPuM>92r1UMun$Ag`Y@h1-Q^f!yvKko)ZiX`B39km<e%<ap8>
zM7{)MzVDLoPk_|(^AdhFNIiZG^8Ee}$a489$ozi>^1A<(#9t5cc#cIT-2Qlw+uZ}?
zb|-+;>wzHOQ&S-IEld0?$nEDq?!N&<k`s#{^;Z%f31g7yr4s*83AZ5AJyqn>K<d2>
zq<!2YCHzq!^Epp=frLL!<O@Z9lJFwolR>8cG!VD?#ET^S#ln9AneNL$ZucsX`M(Ba
zdA=3oalTFXcHw0pw||$&|0aAl$n)}1kuL|C-p57$gz%Ff_y3g0p9XoIeFfz9Uz7N6
zi2N;(dVE{>9f|)hk-snef$)dIYlJ@%{#bae@H&wB{g22$5&35#|6KSB;r~kfuR!Ym
zTM55j!hbKk6;2-YJp$x*M~Zw~;q8RC7ak?NgYanK9YOAYXOP$7@e+Oy;XQ@-65d;Q
zAK?ig_rE{L<2(`MeKaZYQ^JyP8sv5}LQlf0!dc;*#4m`vUbsQHQCJh6Bs>|UKBtKM
z5RmybMQ#b(!j7;j?19|g7kNq87X}iqK;Fk%!VQS3>BJV0_u+Q11zs%iZvuHATnf_u
z<yw$-GY%@V^WYeq1^)r8f*%4s@TVZ3qbJ@Cby=_tI^Yhl06r7sb?_{Z$Ne0Tdb}KD
zey^7B*MdARZvnaeCBjR=BIGZECxX8Mna>G$cySKLNg$t_8OZz&fZXmS;5zU<5`Hzv
z?XL%!{_ljp7xE>*>+5zP<8Kc#-3No~z%F<qcm~LPo&r7yd^7k!@H5~Pcs=NX555Pk
zF)#*MK92#J-b+B9zn6i`=jFmzfGm&KfGnR&K_oSCIfz4^_zK85Ek6U9&as$$z<3A5
zshX&PIFyM9<aTF*_XZyio&dfKWWKKeneQt>o{!f^_-jEP&nG4RKR`Z@z6kQTe+8}s
z?}fzi?}P_ZuMW8S{sWo+c98l%0_6UW2k#5M9K@qx;u4VgUkXy6w+i15vi<aaiT?n|
z=jbOvrgIg@<NSfhKL?r55je<oU;(5a9?0-b!oDyC`CfV`$o#GFVG@2i$b9yId{0~e
z@;tvv!ru(?`g{k-<M|xO?Y;%_`aJ4BH{^S~@Btvp_rW0ZD}ubf${>$xBgp-GA}7Kz
z$m7@{JOJVrn|LNj{ay_6_+BURmw~*`-wm=n-wU!FF9(^=XF#U+O$on7_;Zl?-x?PO
z)4L1E<GUBg?Hvi92bu1Akk5@%K<*cUOg{yg->D$=+X-_2^FgNjRFK=h5M=$}UqD_r
z?*X~rM?t3lF_7i+X^_YBISKy)$nv{N!oMo=*Fcu<H$Yx@KL?rK?<M>QoFpF4(ID@u
zJA>T*Sde<%A7r}gK;~NnsfP=4`)P?UOL#?O59Ib6MBXT@fxNyR0&@R0$n?4(kKcgI
zZvax?vm|~8$a38Wa=%A`JpS`R>i;B=dOt<tp8+zRXM(&gUJ0_?-z4F023N0Bkb1sP
z!rw3afbfGL^}QTq`kw%)?|+E=X^{GS7NmY(k@&BHJn!EJsmBjMp6_3R+>Q???sprI
z@kfe$l*o4gncp!W^Sd+1<0ydC*9E!%G|2SkL2kbRq`sR#rh5v={TsrTgtvtq3Ga!#
z2(nx@gG@J*@MU2RGM|TnEdSFad@IQ7cAv=mLFW5Nka`{fx!+?z>i0yD`dtVzou`64
zu4jXM{=Ew1cK-r0oi~Vl3CMDNw}ihJWIFE?zF+tOiT@DD<M|}W<N2(Fe;%Y>UjmuW
z*CqU$Ak+Jn@Y^7_`!C@&68<BQdR;5>b;AD^{*Uk{!k-F%2C|%PgO@DRy*<d|ISypL
zcLRByt^-*fMG1F69)}0AycR$n&nA%j`ylsU0;yL3a(@l7+=n33Jr(43r-691O*{r<
zdQXt>CxYDnNg%g>j)XrKWIE3m`2`}s807orVvza0RpfsMd3+xSsrP3<mh%@urvGJ$
zzZ&Fo<{FU4@e2w6HOTy~2f5#^*WIA^Z9wL8G|1=J-9aAD@gTRq7s!0?3-Y)MAoHIB
zneVi42IO|LAop7kZUnjANg(%s2*~&rNIhea`Hw*6n}Xba8Kgd2MLr#5{23tgIa}l%
zAh+KMGJKE7`#^5LU*t!E%=ghC!w(406P^z;oyUrNf$(u4!=E7X6G5hPk?^S!{xsp!
zCHz?+_j|U4KS%gn34fl*&j*>_izNJ?gf9iD=gWjI7rsLHO5v-7uNJ-rWI4Yc<oSOa
zNIfnCSw8;;^89}QWcnWzen|LX;YUE~b-BnN6MkIy3E?L}rvINHx4%N<&wxz-v%=2_
zKQH`(@G6k|e@WyogS@`J4Kn<@Ak+CCNWFgqGW=SQ;nxZO8)W!TME<GpXTqNge*tp4
zUyA%I;je|i5&l+qJ;?2UFET5o%=cEpBZRjW-bQ$&@V3I+flTKpkmYno2|q@7C*hqz
zZg*GVaT0zv;oXJD3-1B4yw-_)BFOv6k#JAKtHN2~9LRhYgc~G$BS`&D7P$`cx!4i8
zE9?mug+9o9W05xt6QKrq{xYGJ@P`SvfYj$Sk+%xBflPOY$ma-m3U`6r@8Kfv0gr=x
zp@d%q@_3#K7QstFK8N26-U@u5@cqIMfP60fC&>HdN|1VA1u~y6flTkKAnPC3fVTpF
z1Ty|wk*@=}|4$|S=feM$_+JWtCH%GUH^Se7%;)zYpMysgZm7TA59EFAf;^6Skojx`
zsplym@Bg;QOW>^__rW8;P-G>Hgt2flcm%f7!hwVjg(G1KQomf}72y~>0^wG;MR=NU
z8>kWgPVgk~r(g}7e!va#ncxP<uK;<zE(cElzYU@&CT{b<8`_um1J^;`1oHS!0gnJ1
zA~%IC3Gay974{_D2XBr20tpX=3cM%6PZ#+NunhTmB0nGGelG&&!2bkY@E0Idow(P7
zZqRQdcz4JO#Gy{?2JxAA9)9ivULyP?h*$H(4?*5<KLL*gZ}Z?Aj_*kDHjs}3XTg&t
zybj(2vJsvQqKP{3MBz)pyFtDbq#hpyd0baZ_)kD?e>4i4<#<QoF(AwH&ceG&_^}|5
z`|h9v*1;peCdm7405YGmK<4`xkm<ibcnQeuJ`A$n_F0hi)9;A<Gmv`x9Ay6g3o^f7
zOZacVNyraE=G^YVAoDv3WWU5_kolbjGW|z`ET3nCEYDYi-0oc<%i&6p`g{judfyd(
z4`lw|2buphBL5gfkxd+jlg;h#2J$)Z0FcKq2Qs}LNIf1V@w-6g|2T>NM~QzEcqfFv
z8)Sa(1)1*qME(Fc4f)3sel5s+eh;#Ik3(VbIdFfF=}d#n{~;jrON3iNmcs$?4&a3#
z^ScP-@jVq}zE1<0-#fuOfFA;Rd{=_Z_gf&JkJm}~??gU&>W29Lk7fIg`+9ogIIf-2
zK!uQ|XlPjtB#F$H7SWP+R2oX6Xpm7+8If61NePuvi4ak?@FQ7~y+^;-+x`0M`M9og
z&ULPHKA-RRzPtT?hhX+^fSK0<v(Jf`eNM*g(+X?g#m4=xH1TCv631em-!${Hu@v!q
zOg~GEpTL}d17^S1G4<cjt(g7aM6PVFe=ze()hk$63HK&G4F9`7^Gz}H+F<r=r`^q8
zg4w6P4#1pmF!GA-HOu^L{I72;O@0yPTu+(bfdBQ4#mT>onfD3ioZn;m`vbGzpZb^n
zt^er1x<~Ucv#yX9)*@O|i)nE!fjLhpErYqoa=I_3?tZ$zR?v!=?}f^kb%$sb#}Cu0
zS`BkAjWFwuHGdrDJ}<<4|MkYy>#O}ReGb6%GZ1r+ml|J&`F^<0c#2NN)R}I4J*NIm
z#y2}Y(|8u<dt(vic|3;M_i@KpVftQUycSdc8GQ~je*@-yconnX8~PSz{rkorVb1fh
z@ea&;vD^H2`n~>usrQTif!Y5r%)b9(>h00|FiTuW3u_U~x?)<wd`aU{#-(*1^JR_8
z8Sjhfr=lK&c`k=x&RG+)&*7LpYU>etq}I{8nECaMk1}p(+(;W^_B~dcnQyKwFm+DU
zmgY~^R(cBN=W%b$?;jJeCf<wH@p-I<JI(Lipy2QDM`Ky?tuXI%2P}czFz4*9=j#R9
zLodYi*He3&@1uP&=j(4gKnG&h4K^O4Lv<MDy&Z#@cLk=;sg6&>TEt6@m+7OJ`Hx{k
z{1o#Zev4WE7v|@}J`5(VtVdve{xrjK*cP*Y2h92&m~-^Pyq|qB_j9rKb9|r<H9rjV
z^I^2{7#)js$lvGqVoX0zVLopg^i|CM?_z#V?ZSQVH}i!X70llU(|;Ar`Kx03tf7Zv
zo@;%_8z5h~d!3KvagdI~)SsfWF!ymUroTtcuf=@7Y<2t-O#Lr0_wto~jp^qb-Ho}A
zUop?`;Kl{>55eSXV$OdQrjKUY#_@BEyJPn2g?UZ`G5w8kd@`nwYcbDl8s<K3z<jP}
zIz9_)6R*b9*?`%13+CS6*PWRCzH$6_E!w1@-?EtgD`EPmiP^8V*1_~s7qfpO<HpEW
z++G(M_r(03G!nD_RhYh}V(w)oroZ`)FEV}{)8AU%fO&3PG3S5V{D+w5@iC^4otQqq
z#A>+r(FNnBu?+Enm_BM?>NUokvl*tZQ!w+|;eOZ~)8A0@<1p){W6poQ-hk=rW=tQq
znV+L`u?G1yj<3V)`x2(!yO{ky!Swfy@h`^tkFm*@#`JRl=KiW-`lzloF!xal(??y*
zJ)DKPmvb@Cvj=9sewfe8C`>;S^g73H!t688`~uAVEH(cmX8)&jwXVVZoOuq@&w8wf
zyD|6lBWAxpG3P3FY=Px5`&GvDR|EfhPMCd9a=b0(99=N`chzp%9kc&M#yz!{_SQby
zS1;CndWrVe0Xk3zVa_+ic&HB3OZ74xt|N3L=03(?p3gX);P_QK5wp)VI$5vNshE41
zrqeO?W*Fb7H(~DWF3jiae)A8QUxB%|C(S>Fsk2s}HUFHh)AgA5ZIkg!m~*{q{F-jj
z*YyqEs@pK1yPf#o_Y<bh_m2O7>F-xeUw>fk<uCmQa}WPIUg)@jxG?TZet%5e%9wtu
z8rR0WhmA4on_%i6i`lQa`4*V{PSlp>PsYq|ZN829(~R31pJ99!rfz%Vj*fRS?rhuz
zbN=&<FVG%(p<bjtwHKz(K6){xzy6qghZtX~BQWP4sh8_09j#+@ET*q3jVI_;I#I9I
zYjl!M#`H5qr(?d?Zo_=;@5X#TJY@VR=6hi^X5I^!=k*5WIlhBA*LLGiG3VKVxrd#Y
ze!nu_<@h)Ht?tJ3^@H(``jh!zjeo<`{lom<=KnFj$2fmXOCN=_uol7eTN3lVR?&PV
ztV!I=xH+cplQ4a?!umK2a}VP%pXX_q`?>{lFLz?@>p^`K^WHz9&*(<ojH$l`)7P7r
zzTd*^SGZ}xcri?!eK7O(#q4(oX1~K7udhdAzQ0c~K117M)?I+vr-xpMx!0b?y)gUq
zGrk1#b7U5#kJ~XnzaGZadmOXh^O*PORZL&+82=Bm?hDMi-!R`dd-F%o^ifJnWA39Y
z=KlA?^i{#}ikRor)VLXD{t1}p(iXFS2h8*7iRojYj?@X5`P20_%=>U3X1@n8eLbiT
znO|i5FxFxGP0YD>U_K9DVfOn8b1(m5_9?|5ePboe{92ekkJ95X`?tpI-v+b)X?nW(
zGcfmZ4(8nLu@;Up9*Y%-XJFpTTQT==pDxGj{~V^D&F0_7^!=IfkB<L~*{^Vmg1(Ak
z_AOz&H&!FAgXy;}=3GZ(&T}$W#Lk%dJu&w(NJn8M;u|pY7GU~Xg6Vq=rtVsO8goC-
zV)|Hbego#c_yW`KE=(Uk>pz%&N}W*f9`1|j=U~h}M`G^fc+9+0F@3am{31*rJ+&96
zk3M>_`F?mL`I|8JKL_)kFVZzwoA^^qpPysin{P1lf5v=H{=)2E;>3b^r7`=J!SqoU
zbN+qJ?~hf9n>c<nrvCAG0G?~U9j1O4<F1%}d*Xl31+#7#X5J`Ff8#OxUa1rGDxIiT
z>oqz_C+oGCb4)Rws?&73UXPhK!}vzM$^0$GGmU2(-)cO^crK>@`NnrUzQFiiy$^E_
zi!t}Q)ci8@Phk3a%KU2MwZ=~yKa2mpm*zL<^SV)A&=+-+zJz&SKEQnc{tr`c2j+Qv
zjd>2cF#UXMyc=`QpN)UPtp8K@VCob;so?n*$J8mIC3SC1-O^gdd|Bi2m^%BLuVB8C
z9%%j`t*i&@A$llgpTmr+Vm_}&V$Ru6kHehn1kCqDNAuk=@9V{m55hdZ5twt2!JKcb
zUZLZ3yk4mj^eUZ*>F*kyj5+rd%=g1=O#NH2GA_gHyAspyYJJZ9M$Gz6`U>WAwgof)
zEzG&MW9oj0+5cla6#vGo--B6Kq-DYUQkb|brcMQ|jOphv%=buL%y=Wry|uvXbAq0T
z*{`Lx!i=ASHL#!YC7632hS_%nrtd2-^(Pun!Sp*F^IT_P_MMNJ|A0P%*=LC^)n)o9
zrmx4a8g4Uw3;(+>Oh2FLH<&(t#C*U1jp=i*lMCjTz<eK8!OW|Pg|HT8pW1pP=HA<2
z>YszT&yJXLb;j)915>9DX5Jv<%P{jt8IL!<8VixXMkitRy;i4Sb>c;seIC(em~*Va
z%zs>;!0i7t=AK``ta}Nw|7)0aTQTe2G=3Kg5x<9-zuovl%)F2B5d6#dAI!YHS{2kS
zhS|3?CSMk_&wl0)G=DIrkE-Sm$J|?eEQCjC11wD3NSk0S;;xu|F2vk#Kgau<A8LLS
z=6O%V?02nB$E?2@b3gMi``)g1=$%-Y{Cup98!_u%!K{1J_<hWIKg87e2-DZ+n0<F)
z?%_Mk`d={X|HaJTgN3osDFyFybxghnW?gMeoqCvc$6)TOh2t$Tb=w%9;dp!VT`={!
zYB%k!=j#R9LodYa-`ltkX5WFvgD`y$F&?VJFuylV!1O&yr#e0ji;%y;cm}4<O?nGv
z-&-BO&G>f5@5J<RpX2xI1NxvoqziSCKCFxN5nY10#}$}<9>*g1r15IZJ+9HU`m{cy
z&+2o!PS@)O%-=uW#O(Jz<~*Mme}OswF8$8&UorRj7v}vc(z+lniRo)^Ev2P(A1$M0
zG3P6fMX>_rb5aHKbL)7_J||%MYOQBt=ADPxzX#^$-ayRv_~n@AJ{ePIJ{HA$^+C+O
z3$Yr$gz5Wb%zM2JQ~yIuy)X4A%zl4iQQU*+tN5t}mcgt$08{Tk%s!Q|DmF82fyIbV
z#_V?n=KkAZ)^|7ViN%O7HXel8cNnJbWte@28;>-;9P|Bni}6fMpY!w{EJpkQ<~&O<
zb(dr2J&CEm+Wa$Egm@ihzxBESGjAhS$1jY(!eYd`^(XVc82_nzF#8s1Q(y_qJ(Mxt
z7qj1fn0+c}B|Q++*TGl=PsE(3jrq2E78WPp0kdCc?T(q>8;fIqEQUjLIA;G5nEfx;
z(K^=gD=_cbY|Q$3n7;1BtebECKH~>4eJs)?nE8)7{+RLO#!q1Sc+z+^=J(Clu{ges
zS^u7HH~%SSpU*LU?Q(p#`5!TLf5P<ni~fd35Fd70LEY+DocIXLygKIVYeURFM;jk!
z++0sG-wLz7HRir9G~Wx;Uq8$~gRumTz?^Forv4R}e#c?vPc%Q-{I!_+Q;etTG@b7F
z4aPI{Moj&gdYkz<I#=gmzGogZeh9PQB3*3$5lnx}G5xQ^)PEB5zCDLI$2wiF8}xZh
z-4~2s)J>SWuR6X3Q-7=D+w@I+OW(#4%=-{?zdQ6ROugNh@15T<zmFF^z2NT-`(ggQ
zR}=I1kEWR4PupSoZ;$D-qj4wW&Y1Id$2{kej$e+|iRWPYxC^uXZcHEd>mtnhCAu8{
zyGP?EG4r1`UWe&ty>7tN-RSsBn0<F)>g>ks|1)OZujc>2)cMo=-<WxY+ZOawTuWi*
zmBy?ugVpdT%>1LxAB*X;Ip#bkVF_$we1>s*%)HK+{<>l6o{!n*0^<wuVB)cudE*_w
z(($V>`%E&v7E^aRrq1=oH)Hy|&G-(?y1Oy^F3|h1I`Kx#`WG<uUeVVvb>7DG^PX<U
z)cc?D4$S(`jK47c60_ge#=9`}zsKym-x&qt6)@*+fT`OAv){3fACG$zpKQK0rcPVT
z{Ij(^rjHKV5wrh!#+|V$@hJ0SFz1_qsdJ59=lE1CN&aR`KeI6N=NaFL+3zmQK6e}6
zV|*_jLH=d)ujppXey?NdZZm$z_<hVi|HGVfr+)4Dx0wCD(;qOu50p8xAYTr%z9Oc-
zO2&s6ABx$(nsIeZAGI)bYh%vcz_<~nPGiixV~mf*gNV;H-vQHa7fhXQdOl{qiyZHZ
zsdI_r0~{aX_;AdABXp!*uA_7`W}mT`eJAK7$0zHxdYw+ushID>*~YhG>dnFQdx!Zu
z^)8+7_`SyW>HYeEKBy1rLd>}r>r!3r_+z?4S7Q2EW&DIbsZU|Pf45;R{2d!(owEw|
zsgGIT0CS%WwUPNInD?Y9=D&-yb^Hv>-|;WR+(U0n-xoXH4^y|l`9YX@!*qoCQ92eg
ze;npHO~mwbHKzYNjPG=O0cOAZG3y@0)PLCVWthGn*EN{$-SwEd8}xbIi1~bcf~mIy
z^K<S`%s&5M>K8h@pugf;8dG;aJqWY!VVHfYYBjBn`5rp~Q@15%ejDSqdKTs$+8cMm
z^wk-&Uk~#=jW5CM*Wdgg<H4A9LouJjDdwkQ&U1s_jF~sn@mZMuZZkhuZ`V69>+iwr
zyFl;7oM#~(hA(33yo@=|7R<VB=HJHb`!1%=d&VCaZ^t~B-!bd{#?;&6_+IA}tS^k&
zuNr2b!!h%YG(HMbzk&J2=8wZt<WDwkgX!mV%>1@`2G$`S>G&uegV|@CPB1?aYm&bo
z)6XLFi!t+;>T=9Jk2${5{1fJ%#2So$V7whu?-Tvp{1=#e{}xmKd*fd*_x2~|Tz@;h
z2ea>9=X!sbd#-}XAEs3?bq~kXse`Fs-?)(;qfIgMPr}S=g{gOnw$@X%jh?2bYg;`-
z&(yQ@Y&{2au6CIHI-2jK=V2N0y^Z@|=3lIrnD4IxFm(nS57D7IOfS{TbhwVtk$O31
zztNbwR~V0X{7RjmSLsB|x@(Ll>14eY^E{>*PuJ`92AzR<zvmdw#eA<V!@Nf;G0$tY
z<7;#+rr&3EJ!bxkx*4<o>zL>JhHlkun0>y+toshL&o7woufH+p`xjHEaJz#2ied64
zFuz}v!>q4hz7l4?12F&ot#5pkHo)}V7*qFH%=+fq64OT;<1;b)osC)7&ir}iyI{UI
zFLiu`j>6QxLMLGQx(ZY8YV+6XG@XH&cO&NK+Ct2}i!t}|7-s!SO#LTyEvE1FnDrYl
z=h}o>|BCVJnDtvR^WVk#Sgd`)y5gAel9+X+wJc`8{T#2LmGnT&{DX}THLi;3r<(a1
z#x)(UjWroR)BIVO{m;Sl-_iIy%>Lbs&o{mZ^B!D+xu2nqUy273PsQ|gJ!bt)#xwO+
zor~G`cFewa8!y1z(?gi`i!tYU1oM5k$@nGAy4NuCwqokNY5qM-osW$_!_50qzrob~
zR(E55UX|%kkS~j=S01yjf*y#eTLrU!4a|Mib-V#)T_emr9Bq6oX20W%n;JLM=GsC}
z&=d6}+=umNV(Oo*=Q`fbxPx&=?WE^vXYHb0wHxM~Juvkz(q4}D);`)-FUHKj#JIl>
zz?^@u@emzq{xZzHjm4}Vhxz`v1=Gi^nDuk@4$S@C>G(aEb@w{{fbm0^c?*pnHeRfc
z=n|~Pyw{AkVD{T;yiMOU{|@GPZ#VuJ)8`IMA3GiY8ngc{^WPfp*6%RyS=o*S^U7iB
zR=|9o4#xCR1yjGe<Fzq$kHG9#$MK_#8#>+y^Lx?xm^v3a-qW}brcPhuOB^3yJXnY6
zaLoJ>#-lLL=>|+6H)1{yvoZb7#hm9Z;{}*L?$?LRFUG82qRTM*JZk(H=J)bf&A*1(
z=S|E#eSq2TW6XY^Vd{O3+3zdkU6{JxVAlU={%7;QV1AyI?NqS7yzZwJv=XN7L74rj
zXbnsswY4s$kNTK>kJ1L(5c6~RG~?5?EvEiinDysqJMG~3d6@mWnLl59INlSpZ!hhQ
z`MEY8bN*{E>#xJy;|$EcH|s3S`Q~Eg-KqCr>MqcGF?H8s)<27xzd>KrS1{`~W9Dta
zobL_H{_kS;`w+AK6U_Wib%%bYJN0w@0#o;E<6W5FuYSSo^PB#me_`tXt^er1x<~gq
zuV7vwOr0W_{z_o>E9H1;-ABu4SuLmKbzjW-3dR*Nbq~V--YYCeTpzP<18t;DF!$EX
zxCLf?OHAEUv<>F(Z|7pp+fLhS2dqYZgz-qs`q7wu$71@KVE$^ItWz-G-#202t6MSq
z+@^DMuFk`}FDr~!V)j|3PwHySzR&9OnDcFN{AJ8OuVDIp&3FsubNnS{{x|wPW}hE1
z^MAqI-`|e!)wy7PQ7wttzYJzwIn3`hwT+K-{3y)1kH+kGJf?ngOn)sLZ-uGT8Z+;7
z^XFjZovZCI{SPo6h^c=mrtSzGrDHMmuEflnWIRQ$*PAf=+^n}?MdAlB_pk)Beg$Tq
zRhaXyF~1J8em!Qt7tC+Q%zFc~?oG_RxAbj&2lIQ|PndbXn*YuCPfXo^&F?WT(xo6@
z0<*4^mci^(R?BI5%s%xn^%`Q<HNo_Gy!jT|((zL;_i%>s*~XnP`<$npwTpJuZkXpb
z9JAl$nCCqjbFWuo`nejj&s5_ZFrS~<#&eDD!R)(0@74SCetke6)Q2$pKWw~MAJHYc
zRF`4;TyFfBuE6yD6sF!<^H1wD`m8>u>vX+t(C2ldzMwDaCVfd?)>m}1zN)Y37JVIa
zj;+Sq^i6$B-^R>)*Z4hsUq8_8`k{WLAM5}06U_a5X1r5B*Dv%-{Yt;qUHT2CuieJq
z>G%4B{)nZS_mA<vy2pH}t_AN+Y261?x2$nFEwB6PewcX`Fy}eI{DI~xn?KZi6+O)H
zYL3^`+FA$m9@o`+T3?US2AKUD88_A@=8w_i%pb2!F`uWFn9ujA=G$Q2-?K1v&%u2D
zI%sFi{PQvWU7$VmLcIvHz8B_R`eN1%aD1Q+(!rSdLyd>&rFxm;BlL3fqm0L3p6?_~
z|I;x0-H7S)CQSWXjAt6pHojGF(>Xd<=jrWw2j>3nHNH>p*9S1?UTC~XAI9|Yi189#
zikZJ$SDJrZS7FYz7PI~t^UvyYx=z>Y27O*P>I;~9n~h)9*D$~Ty=(lQzONtXcKuL4
z!u0h&<4^Qc-Jzdh=6!Dbg?@=S|1RTi^jqDn-|6?5@BiO1_5L*fm;SB)IKIbtuWkkJ
zLm@4!MYO0E!}L?qcyBF*`8<@x>|e?J0hsrpiq>+xzT-z}18t~{w6Qk9oaY$hV=?b>
zbIiUcnQw`i*GgNPKULdco?|=X_S!)^Y9~DpQ?HA0SIl`X!rVh|^L@0hUhH^(;{ljD
zgNz635c9+IGV{ZYM`HFFZGH^qyyMMZg{gbB<JVyBWs3P}dcET}=nPEVn;oBNewOjA
z#<%Glo$L7R#&_tQdY8_})L&qHuij_=LF0#Xp)S&gbup&SQsZU%s4myXbOmPqD&r^g
zNqtIJ>l)1bXN;fK=X9N}*A4o-Zp1u~myBQ5S9CMx`MhSlMPJu99N(sIVd}l7AL=KV
z_jVVi-`$wM+x?089{)@Kc6^WVUfm1c*Alot^D1Nh{Z<3>@4?2H@4MqMeKyk;n4cS`
zVd|f*Z87_wg;{?-=6&mlS>IdxV%A-({qz#d`37O?kHV}U=lFOmOFSL3&kViU@mnzG
zzRP&N-i<lO1I7<x>MzIiwZi;L%>GXrKc%a6jjqMadk(XHz4;B8zFshXQ8(#J`Z88$
z-Z#eIV(R^1{G<M)KkF};{eHvD{|hT%x$_IoR}r)SfyPygt6}c(aLm2e#?(2|@p_ng
z4Ker995cVA`PP{4wKFmI*d9}-1Ex+V<MXt$cER-19rIlJVd@TYe6aClSc&`;%zvkw
zjaBe5O#PLZpBroOAbbn=#m_PC>vx!bzBga!f`YiP7QysU98<5P`MtH2md2wPZ))5O
z)9*>fEiw0S8s_uX-h2oA?;bGwbiv$XZ{v%#KjxkXWBMIp{!-)1bhwVd-1k`HEAT+#
znOGGc!~gCF4<_D%sr$P5t(f)i8o!VG5r2cZw_h;xf5r6u2d3U%=KnVSSNG~sFu#x%
z#;hxbc|K(@=iAru{V@I3F@F^1-i~*?DW<=Zj9X&ror3wCx5xC;5%awJV&0F-Fntd<
zKM8Z+H)1{y_d0$brk{r}>sDg=*^KG;P0ar9W9of?+3!RB*zuj1zP>d7HD>-U%=_`Z
z{({->56Az+)cYIr-d4P@;B!z7v+i(A-6Jvc>S}$=d5*{I(^Q*jb4<NXn01}acftIf
zWiVF8>o9-!e*kmN#h85`#jJZwSDJsycnxOXwfeN<>m7d{^Er40bKWh+Z)56zfLXuY
z@&D;(=6B)=j90&?;5pR6ypK&V>yO9(&W-7_xp50UK~L0^v?XSKD{XE5RBdDabj<fr
zC-diFo<mp1yXys*^Yz5^(@T46AMK0Tx1aXM+|Mw_FV)L*IA*`gG3!Q~AA|Whe7*S@
znEh|T+{<m4KIWLe1GD}v^Ye}G!MyKF%rDht`Y2}IW0*c3H+}-M-x|z5YxQZ&`?JCL
zdEKZl=!?1uQ}-3)&HAdohS~ph%)D*-w)uCBKhTf#6U^uKD@>o?V(NT{>Gym6!SP>=
zf7Rdgcg*wIgW11u&w}+uF!hRQ2~6FRx;N%Ilr=7=<#k`(PxsdfS`o9~ftdae!F(Sc
zt_?BukHPGFEM|UF%={BD<E`{G%)HYv<7Z)>dpFGYM_<f+4aUqHf;rzXOr4RK`lB)H
z$6@A;$IP2xe3kLlI?4QG%=#(DQ!(qN8(*(C=nTCPGw)`dsk8MqOdoSF^>4@Ydp9P3
z4<>&fCca-Ez|?)nc%d%RhcWN*qq+)H=PAtjpT>NDZpGByrf*{Ay@RR$9%lS~{Q#5y
z(D)<c|6%s~9QVgRFz5IeQ?F330*hkm71QFFdL^}#?t^)*2VmacDwyxh+E@sW&?7P5
zZ^vNvIUY0bWX!tLu@JV^GxSV73$y<@nDy<=cfh<)-5tNcd=JdLo|vD%BQf_m#__Rw
zg^ttln0+Q-)?clY9lutuGd~S;e>Y-zobUMEdXFx^{Qj^EGk+zf{u8<y)88|g{h!nI
z`aBjQzY+8NUNQfw`PVS(w_@tPg}L949RC=PCqAHeL7f9JaXaJo*noH<roU^=U#C-a
zs!r4CdcEF&+5aY;X?~XRZN_u3F!Sc=9eS7EjfEJ$M;Bn~-f#SXKBy02Va693Kdg)O
z5nZB7br}|B-Ev)Fex*LHt1$aNWxQI~=vsYRpV4RaIbEmgb%Q>y8}$XuelHontgm2U
z@~;}drd!Ovfm!#a`M30KeaG?ljX%)sSeSJm>HqXo%yZa*Rj^Q>g3oao%;&BmX1o#>
z!GkdG;i2ZM=wVoxd^O|hS_AX%tft1zv^i$}3C1U4-pf;rTkEOX28%G>7W4Ty7xQ`R
zfvIy5rq7<ny^Z@|5%T?vFTu<oU_4L<>0lj#Ip<}V@2@eK`<RGDa1y4EX_$SdV?K|w
zFy~!_S@)>p%Q5?|FkXrI_r*r@FPh(k*>^Lh{#M7|bo?#z9~f`P?6U(?f2aA+^$RRQ
z{%cIV@6G>!`M&<g_+Q<Fc@9PU7St<_MTqyt{G6|ZxrZv4eQROPe>kRoJ>&X%ls3?Y
z+DIE~6U@HHW6seGQ?I3QD{YP0?^NT{F!$IM55QrVp9hmLzvs-r%J>jw-9k+Lhm99w
z?%@gZPhr02U&8#m^#eQ*f5LKD_~L?lD2JJM7-oH4Jr1+a@!C|IX>-i}Cm5foCt=Qe
zD(1dV$DFI1<K3|+@rA}0VdnKV?xTJ6V(q7wXn#z<BQf>H=y=TYoP_Crvhnqp=RM2(
zY)t>RVSc|@gt@0RnEuyc`g$HyXCr3*i~5q|n=$)s!_0fr_yf%E8=qt9edGAIn0@wO
z>J{o&FkS+)UnxwT(z*|3URmRET3+|n{V@9-YFq`25!W!TskJcs9ASK<*1_WB8)Eir
zY`%#ejj4Z<aZ5c}TVd9<#ypSH&9^mwCT8DrjN3WhUOPB`p5tB2chzp@&o{on_(J20
zF#Gnwe18o#KLm>tk1!sI&4`y`-iK9~b*nM=^(^Ll?=8%8c-!&!^&`wV|L6E8`Y9GC
z|0U*}yL30^{rCm5?l(-`-;Mv&zcGF6bxFZ^AuWvQs|04ew3fs4e}Eo_nO76@{niLe
z;PIG!n_>2Cj;V8!`I9mIw#MRkhU4cr-VU>_J!W1fEP>}6Ux=yS)3}fJGv6Pxet-_t
zK{{B6=ujPo*>5<e{zy!JR~V1S)V)$C=v9tiZG4SR(#d+QUWcha)p(jt*X#8Loq?Hu
zlip%}CZ^w8&Ck(!j^D0#=$)APcVp@;(EIcO%=sQPUTC~XAI3cIWyX){a(xU_cct;;
znD71PjW=T6^AC)-WBU2n_<zPbj6c(z`ni6gU+P!-weHeyF#YT{{tmO>kLG{YUmgF=
z_z&Yhu>|uA^)IMX1oIx3z<dtMVeX>>=JS6j=KiYaVOmwIX?3lkHMN!=uC+1!AE|XQ
z`!zIfjOni_X5VI56EDR4oEm6;5a!>>6EXF!!Q8{OnDsYcp3@!1_hQbo7&Ctf=5w$D
zv(HM*?~m(^H(>7NMdMBSlD@33=w{6N_pu~?YJLZ%&KJgC>Q|U^@5cNb+iO5UAB8Z#
zuT;eRJUs%_Zyl|tM`6y}2uos9{O`Unzvr|zK2_W3X?nW0)iW@Co`c!1op#Vp+8MJ?
z7fgQx%?~y|1k=~0=7(eIj&gjoj?uB0{l*!O*DG~`UZoTDYRvcZjm9@&p8IW>=Xxim
z?tIM8+eMhZAI9{v)OeXbs>}5;U7;)Wab1O}|D^F#x?0y@`rBmulD>?2?puss$Gq3u
zG0*Wc^E>r({X)OguQ2=X(r@)U$G_Jf^hf;(v+ft;U-dWr-SIzhZ{`&lSa5GeG51(b
zD`Wbt?syGM-CA1P{1L`=v>xXE8)E7;H-7@=`L)J8r&BTW+G6hO9PNmC@A_fp4>3O+
z)6W%{d!J%_ljE~>9;WVHx&TWNFUIu01hfBA%zL!b_;Fo@>3f~=dfkAzug#eEe~WH)
zd>iH*@0$Mr^W48M{u*<xe=vRQ!PF}`s9;_NOx*)8;|CcZf;mrB;~I|F!kpu9t&N#i
z7jtioG4+lyZh@(LB4*#y%%6j)b1we(?`uq*UYPs21hZ~{@i5FeM`FI$uEU&jCT8Aj
z%=)>Q{qNBEn0@ZX?7z@>kv@!>zXbF1c7yrnb)&wZFJkt4$@pb`MK|lK`kHRh*D>q2
z>YMtu<L_YldEfkY{mA?$#yc?Q`V4a~Um1Umd0&6T?DI3G-fxcou76<e>mSU#J?8fs
zTrj@~X1oOE-pZTbAG2=-%(_a(2k3!%kXFXbJH+@<Ox<e6)g7;CTuTo(e}wUoT1V?@
zJ*}@tX#>o@jf@)`A8mY$9&7%1<EGk7n`;X_K~L0^F!$Ta_!Q$dnEg*T-&W7iGcoJV
zF+LYFzdh!8b~fKdyJ|Pgx(hJ(*wcJ3%({M<_jo+!?*=#EJ~$Wi92a2LJ%HKoA<XYH
zt1zG6r!jTbWA@pIdEPHL{wiku8|Js_HhmLQ_iap_56o}Je9pf%-i7%-{T&a%3PTFk
zRnh}7`yOO`FlOJwFy9ZgF#R22zMeM3tZRhnqlq44{#ZTEd{g6Q+8ndbNsgavzLlP$
ztugzbhS{er=6mUU$1l(xdLiciFTt!Es6#NH*UPaKUWMs@BJP7X7|+10yBV|JOw4`W
zgX#BvUFi73nCH11^LyV$^DpX4n0lLyU&HLX#r#(DZ|d9nuHzqI>VJw^w*$-I_n7{E
zH2)LkJ@_5--2TPVxcAV4d|6DLikN;5)PuDOrv71C)qD+1y(6^&<{p}0_CE%*?pWjF
zjhkvSZH{G_cRFU>8JOSyFTy;J0hphk<1wF;>oL!37H0o>n0@cULvc0cyw76xe@@q7
z=D%pX3A5kJ#;@pR^RHp<;|=p$G5u`U&oTS#a{L?p7E|{-<L@zbelq@9f5E&Ldk-s^
zUmDZzK3Yc0YB?>BwV8JuX8oC%zlZk5+{-}B-xJ4S_ML#a-|I2;Z*crB%>S<ENz6H(
z#=Hm5W9Dtdoa04($^6U4n=$pbn13C!{vAxc?dCt!k1)^aQ{x?&`aAUtOuer$b$&4a
zlm3GFJ}q`>!E-E)|J@hnb6ruZVCq)IJg+*Kd3CiOrhXISqcQW3#k?OausoiH`TTdq
ztnY>yzsR_!aX-vHmzuu}bB>Xix}zMQXnYOkeZCblZ!V_(Jj{L%m|uw5Z>jNe%)G}i
zbys5ceMUE7?sqfh{@=j-J^L%if5NQ)8FOF%VD{hZvVwI*G2_LwxcO3;_oNo)e;0Hj
z*2h6u4zIwxUsEuBO~cfgj@f6H@oY@L^NsJ;dvt;0i!uE_YJRyshX0)#|2sG4{ojh|
z;}gesVD|q^cbfk~zcT+d=6hz3Rv2E;@1dB_Q7ufJ!?iZ%d$X}|6U_5E$+)GSjOo9P
zo@xFp%zM`n^PIY1?yawJf83XNEath4$DDV9@zt2R*J1jaVtzVi-VNqwVEVrm^Zwj{
z>H9&<`o)f~z&y{*nDwt=`r3-AzfIqC{B6wq?=W?LG5;H8{_pw+rvJY&&%fA+g6CBZ
zv(LV|zgEPoucQZ<KghVU@gc^CV(zgfrr#sXABm}77qkCSnEj74-xM?d1kC!@nCI6H
zvwwTc`i{n(j5`~5!PM<;{sO&Fdt&PJ!p!T7*?$P;zDDR+%s%5V^T+FzIzg|(^g9VN
z|623c=@gx+(=hXA7~hClceBnkKg;-5%=~%gZ`V8YPR#oGnDZ>a+}lFTxt3$ztF@T(
zt;fuN9#eOdZZ`iK=Kb4-sq?<$A7S3t&viHE^ZmQ=ULy-W7yDxNt&CY;4Re3hwT9Nj
z^l>=m^K_i~<FzT~JT3Gj^DXsc^R10f)i&na8lRzO>RFh5&%yF|q4|rnr}om`nE8E;
zFV=qM`x_6?ftY(8in)i;IuY|dG6QpOb1?UOC+7J)gn7?bn12$pejTRXMtuo0|4qj~
z#MJu&bKko$b^gTM-#@q?mb<)Q|3fhAYh#{g15EuUnEhH{-jh~(I;PH<n0q)Ivri|?
zb3M;|7vrwlO}p#)dV%)9)Vm1N$0e9P24Q}VU1vPq@tK(Uv(3-N>_1O$*E{r1y$iGM
zZp^(b#+-Ad<BvQ31m>Kp&9Bk5jz44ktUiairxzUGWd0@0IbJn>&3LQvHcUTn8NZF$
z_dU$d$1gGW@xAdLEjy~9|H_ziAFPMyp_qPZVBUxNn0*>J-cTE1-oq0eZ)yHyZDqc-
z@u}KIPs5z=Oss{Mn!ikk>j)i*x%Y{f_ht&_-?8^$>O6$$a}nnAuoQD2%Q5GCOjls$
zJ#M^8pTN{vW4u<M)@Srt%=&f4>oNDR1#@q2Vfx$d_=ozDeysn~PxMpWp`Yna{anAm
z^tnsF!+ihj!Sq#VbisKGW8$Ki`6Z1@IbIrbKV@}a$M?tF-+`EQhhXmSP_2TgTh+Ll
zRySW0a}RZl>tX6N#MEhIz6s|2I>GoPJsC5vm7apB*Vgz9JrmRaIhcOhnQw30(YTYI
zr=7J6W?eT-e?7DpX1_}uAAotDmtyYaa`U5gtd7$wG5t@#)V)TpGd~5h?=<7-m~+f9
zzR~fUjc?JJI!kBkt(baqjOSwZz0>$Eov(N6J(zX(>iznlF2tN;5vJY}<E5DUS&3P{
z%KQ_Ud21YBXS@+J??uf0ZPqt1^|w0yroOH3I{u!%kNJJ{C*z+n`~9weWBMvQrr>$)
zjj3M>bH8OU-xo(>zPB4=o@-M)47+0Hchl~eelIfaiTOV5Z#+N;>L4AgLv$!sWZfj=
z$$Bkj{~Iv(bEDpj6^QT0e2=d%|2Ss;T1;Q-j5j&{viU8TbG)u^VD9A|<99Lpy^p!S
zkMvXXpJCST)X((`$G^gS5C4MseW1kHf;y$O9A<ra-4}EJ6)^i&GJk*`==i~oA8K6H
z@oJdg>l&Ls#{99EddC|#)n?jUTj&XTqMoEJ^<-^@xtBJ^r(ya(L(evUj-HE|-_f{}
zo`+f2)wrAS1(?1s)SlWKb8mgLFXnw4jCtOdVb+b%%Q5pu8IQs2H%=#D)=e_L4zqqL
zX5BQMj(I<B#++le&c)2X9aHx%Og|4`)<1|@_XuX*QuE97QC+T&=?Y9=PZ~d^t96a8
z)u;6tOdsov*XsuJFJSuEg!$aOgLzMPU_LirJN}LFPsYDseh%$1U;K)K^%XJqaS-Ob
zm5r+yABO3tx^WGyskJcckHqv<SL<s7Ox=dsNE>StO#Ndq_thLz=LE-3#MEzPd<tfM
z8*Qs+>ABhgvu{W3gsI!vxC`d~`eF7RWPUJa-caLVnEgi>kHowWR~V1e@p>huzloUd
z-)WfnH#vT@-eP_Zrv4q6eeW_}fcbaFBba*2%&)-AdmMAVRr-WJiJ8B~c&+iXnD^ra
z%=%53@89j1{yxRj+o7Lf-mC91ef(wqZ~aIA)jhh`xPm%`Fntuo>{r};2`%Y(X~)Z8
zS>_$=_#v44tA(j^1g8H6nCEpg=6ya9(|>DBpXXxsYmZsiNxNWvf4l@Ue+cHhLv@&5
zidjEgM`G%Xc6^NSIGterDxIiT>oqz_Cu8bOF`kNf{x@KLzn*9Qc1)l57%#xgd%*ZX
zeMlGTB7ImF>m#}ZbG~K9k7D{+fw|w+n0=nX%zF-VPwR9&=Kf#A^!F-e-WJTdZJ0Xm
zW9EHqeg~$`XXd{${#tio`q=IGPndeYVe0>(e_`hRYrNNZ-#5(n_THHBeK70FI$q9r
zU*r9Bf31L7cK~MHLFOy#!R8Oe>{HA9;aXdd&?B`Drd~bc`o;~68)_qMjOpVT%;)c9
z%srfnc|ST}?y;kG!rXH=%s%I1o=Y#sdz<g8{V??hU_M{N&5zVknEInJpRcQpuff!v
zirHs6=J%C5%rC&KyVrOjrv7qW?f4qZ`lm5{JcrHjPt3g*zOrDSl9+mXWBS<NxB_N<
zCF2A1Ks`t+WA3erR>yq)8#~?vv;VP}dp*T`YdsaS&uN%>XP7?|Q}<lV@4J05>o3F1
zAA#w6Bxc=c<1soGv;IoV{aj~$it){uIy24B!pxszJXhyo?rQ<2k5!m;PwHyS`}2bF
zCVg2qWBPnmU(+p)zhS&px9OYu7Uq2KV&1<G&3|OP0}mm;$GGT(f_}<lo@ZrDzlUJf
zS2M1mwJ`5nUB??@`fa3*G0(G=@hO;l=!*F{&<C^65Uh$bFzau`>^BS3|81D}_#wv^
zV!pqY89$1tv)Xu#uEqSkdfE6DOuug!Z^b;fw=w&?Xa0TtK)36MnEUt`Q)egckH2I3
z{l~ccRR#ZlkUE(8^)UagI}<BmFU&c6W9szBtRI5u<5JAJ;h6qLVdjs<%o~q~;ylbf
z-fw;pX5PbiAg(vwpwH_@eL-K;O_+UOHhx7n>#JCU@$Zd)z|8*%)Az6Df79Rf56n6K
z!92I36AR8=3bTG6Oy6ZNb;@JTSrL=3g!w(Fj&WVBr}gzHZGic_w=h0IPsG$c#kjSe
zig{1ZGwzIepL!bi!t{SJ=6xE0>1(83uA_7`{`Z_P=bdDJGUgm}G3UM=Q|As$zYC2Q
z>BE>h%XB4X-73sIJ%w4n8k1j(d7su}&a)Mhe-kU?Zrl&|y1HO}5iPEJV<Co1X=%*&
z*Z!FEAAs4nvf~G1zQ1Z4AAvbv3(USJV)|)~nb*#I2P{O~(fB;=g1O&|G5Za`LO9TP
zDCYBaHKy(qEQC{anoig2^#;uRnZ~npHfEo>#`7@eyVLkC%sK8hzDE~e?qv~X{v()q
zOEJ%Dh4JH#KZ%9NKc%a64QAcb#?N4W-n@vZvl&x=i{r0j=5ICLhI!6AF!R5}oO8GF
zPgscfXUw`^^>_Uf3z7fJ_#fS4ey?i^_Ai22Uko$9xR$_tPWHvZxW87!%&UaCrz*yW
zIbO}Uy4KK|T1yYt+IoZ@iFt1IjgQg>+7Jsf-q^SaX8v*Jo0@N?&9#M|peO1{+EP!}
zR(guI)>E~Oo~EZ`p68jyXX)8`j-IRSurTvG7<bf8dY*RHF4|SQVfMQKbN-&@duea&
zqkS>+`eW7&!n}_ou`rIpJlD}W2J`%{F`k6^{N948b1UZg-{tsxy<6|m1$r+QX8!${
z{U5@-&r2}-F2%z5C}w<xuEKoJK8>08j6SQ+={jAHsk_PeB}~1|SQuY3zs2|s<E^^Q
z{9DFv>pS|czNhc&2bg_6(oZn=^A+azn?Eu4U1U<h=eZ<iere3P_R%t!&-;Fu`3GYW
ztfJL0b&tfXtD|+bp5sRwAA^ONcO2%qHOJh~$&R;jytU(}n{SInn0FRt{W<2(HEwU*
z0gEtxo^faGg4yqU;|nl#F2wBL*SMeKmtYa{12N|xX8uyWOoux@+IWnP)hl$Ij@K)7
zf?lN)^=iFFCt>=%7W4kyV19<)s5fEe-C{h`c(&e#MHruFd<W*a-Klr!e8=z6d(Gd6
zxvxdW59?xmM3?ANU8axfa(zr!=t@lgt1$1?Gnl{2y^2Nfb<Fp``<T8z!2DkNIp#U-
z!XmgE55QuR3w~}@#LTaR`Mu-_%szFrp5yiPC~bhLd$jQ}n03b)ACKv?C1&4KjL+1L
znEB^n>U77Vcp;{ri;VkV=3R`b)8Fv{m~}%PztsF?I^6Nmm~~f}ABVZO>6qtrGZw|!
zIv4Z%(<7MvS7Pq<8BCpZnEux527O*P>I?d!Zo-`7Wh{!XVcyrbF!kQX^s^oFeX<j?
z|F@WbuN1kqppP<`KFaI<nDrI3BIfzlGp>*6|2RyYvoZU0bi5O0p9_q8V1Az&Vm#D%
z0_OL=TQKj*eU9I+4`9ys5T?I}b&2CkF~480!OVNw@#io<uU>Qf1I&508-Im|5tqEK
zU|lK9{)b@R@7h=t>tb<igsI=e{Bf9i$7@sbCt&()iN&!E=6Rg0ov;{jH%$E=SRDIc
z>h;C!(+^W;fbk&52OAH?)V&-tZw#jINtim5G52x{W}jQl-==ePuFk{Mxf8She9ZG&
z>i9Cu{jAe%nD^mp%zN-N=KU@?r6Ar9)5rdpzN%pQsP1?@$LnMM{m|67nKsAtaf0!Q
zdXl!p^m_{CJv-C<S(y86Z`=Vh?>tPsF6O(MKi~KQ?Sa*pH^K3%&0m9AKN*YTbmJTJ
zM!gwx?pt&wrk^>+bFl>R-I%`a#nipu_#w>tg_z$Po-$sIIp=!JbKQdJ<1NSE#@z4w
z#vkZ*{ZK#BkM)2038s%7#-Hg<^IsT$iRpWn@i+Rd?$+-x^M2G{%>Rn%`)^FYdrd9)
zoR-DR-xqWK{d9k=fVszmFnu1ZhnlaBSyw}AI(|6j{c3E!38v2RnEhKApKN@pw#C#t
z!}uKSV7{Yv((^EVb-|pYyZQ6=0_}nM9`0v+iT2k4I#37cU>%}EG5cL=e3|hG<B`Ur
zj7MY6J>K|A$FIWN!zA;QG5t?5o@#u(@eMjdZ`7OgX1zsc>MTs%+l=SvT%CvM<1Wnm
zai977^#R8h89%IxG54@UmzjT5m+ND?LRaeJx=NqWC-o^^t!ps*J&oyio%!{c_v|&y
z=k{aFc|XO>{|xiozcv3o=6U>r`T19JT0vX}^LN`LG2g3AFzb)OypP9X=C?I}runn<
zZ1d+Dx6}68K|5+EJx@Dp7wxLuv^$n$|K2*#{2<(${7B5t-D}KG(#e?nnrb`^Q|A`r
znV56V#@yQ+^K;GLj=9&x#!E0ipPt4l_yOkkudf~d22<xd%zYJ}Ua)_0%)VuDZ#=;K
z!I=FI#oTihJxr@=HLZ?$ezh^@s*8DV8yO#ixrb9Q^G?UqKLbl)C(J(QX=m+%nb%Fv
z$DHFr$1l>J+Dm(DAI$oTG3)x9A7FkErq9dF57!ZnUv4}K)Au;z@tAwO$@peW{aMDd
z^;W$N^WOs&WA1q+X8se7KWV%M)9=&fpV4RaIbEmgG57s~@r#(cFJpescnhoIFPM3M
zVD9-(OdmzAFK{1Bz6@sna>nI#U)>K=zar*7Dw{tTOAuGbyiaw^*VTHMbq$OgY9mY^
z&5WCC3(WkJj9co-nEp;NZjJf*-`;#jJ<svZ#$AoOVfyQ3+*|wLfA5KLKfOfz>i`{y
znLk*EV(w#v@#UEN9EEwm#$)bhlH=Dpex31D<7t@rH<`c1{7jvtv-MWJP3P!b%>CV=
z^D+1Q5T?&ZF#RpjrI_<PirIga`6n>**P35%egoznU%{+<Rkt|)y1s!~|F-cvnECHv
zz7M{@?DwtXyY)NFef^3##~+w=f9XG%bwzF{aBnS#*{=fTy{LwH?`vV|9HDhF^BQ9A
zp%JEj6HMKvnE5SmA3Pm%f9)~rI%Dc~$IR=2S$~oC!qn}JImZA@-C>w@BQSNwIDQqT
z-y1Od+={6?7gJ}R-mZ6G>fUR7A7=i8#t-R2OkWRU_J7p;a`P*UR~oM}enOu#zuI^W
zrvGP+pVM`^UN`9TnEhWceo;4J`gsjgZ!6|L-oc#fBh0x!c6<k>&Q8oZzr@V@&ioJh
z6XqWGno+R65T<T%;}V$nwj5^P{V{baWBRFz*{3>YU2XIAG5s8+4KVADH9iiruBmY|
z;})3bd8+v~dYYcDZ880vZG4WNi>ce*xPx}oPL6jr?xI~W_jJDT1=_=WPvc(NTl;8V
zy;%F{CE8yH=s+E$gLQ}w)nR%m=3K)u=N@H#w2slSn0e!j$Lp1t=QYWAvR;e%?|*k=
zP26DqdCYToRo})k#5*w0^)ub6pX(R;rGACAnP2|Kg8eID)*pbSuo~vxYhvEBdYJPy
zG~Wbso?|e7Cpy#kY)qYVjN2Kv$JFbDsngYbH|>tur?+t*OdmtcUv7Mr;}bEzx6H!S
zn}d1J@5J;qAJfO(#tU#C;w9#nntv42*9y$~C$ThcaD1cVo6K*+d|uwiviK#Y@9!~v
z{EXT67yT7e{}25evrnO$3i3rU^NV5D?~OUvA;wiP^^d^JI}%f`o^gH5@6RV1pM<G%
z3g#Y8#mqk))AzZ?9Wd|F`Iz^wm-)V!eJ;k-8DKn62Vv?CF&>Hs6Hhaqj@fSp=I=^(
zWA<5y|NY$qQ-8It!7{{~j9=21G5c)B)Z1eIb<BJ5KIR@j#?1Q!Gw(CZ|Bm8s<2{ZS
zy18KA!kGC*wYd2bS`u>~WsS>edEHm{!|Yd455)9yD5m~lm~&Qjyawi+4KekP)fSj}
zC+LZKlD5RuYo)EV4d#BkV(MOu|2;1(Pdpa0&sCVdCS#ue^_Y1#=`5Xt>0_?W)7vrc
z-@W=Urrt8gAI03m3SEWScdf3+?E4a?{>%Cb<~{$&_+w0+PmOozXPEPUj@fq?X5Mbh
zIex_aySnTx1)tl4F#T4;>{lH#?{MSVnDupx>uNo%uSaPE%z2J6J{D828K%w&nD3Rg
zj-O%vY|L}%j+uWU=HE>tFz2}()88meow1mCS7P>?g!wz{?dIoW*4=|yw*a&5eoWn`
z%&*opx>ldoXE5JyFB!j#2NCZu{tUDK*T%c_8~s*y>vxzwel-3G)8{Y7zv^$8y8mF#
zS!ia#ISOOu7d0-X#m$#A-djs)Y28Q5Xjx3X@|b-p7+1o4Z&by6A027FE@oZ><3^bK
zZEXHnOy4aq>)V(=19R@P&3D19>xy|#dtg}{g!|!@nDGgi{w8APU5j~NXW+g#A2aW6
zy$7?;ea81=)<38V%`Y-uZ2X8WF~7|CQOtQ)nqOtS+ITHy|EG<gHGWRlnctur^+n7*
zy^7^<yYZ)({&!-|`7NgYZcHCPV&2DpF!x(xR>8eh#GLa0%=`l}_jRaN$JDKbd7ejM
z?(Zbb=k6@by0bC+v^Va6S$7_$ei!YA<%tL2emD*@Zvv*yRhV<#Xnd32jF~q}Z!<qf
z=bFC*Q-87fM|26M|5ccKSckc%moWXkg4utwzKWUmuH)}x_Wuy`zU{)y|Hk}x#^37?
z`XlB(f5Y6*U*`YTe=z&+(L%EezQ;>q@})5Ql-7OBmo+Yjsb2x}JgQ;(tm$|yJzQ(+
z5t#aQjO!ZLH$F-mXhUtJjkO8pzE8otmu=0Tfw}LF#+@+td698X%>KPGKeq;$AE<+L
zFy?(7WjtEPV9tFdX5UHXC+oGC{ckkB2{V5-=6>d2_MeC8e-Y-MA9j3+@lwpZM=?Kt
z)|!7B_a}ZGv(I~&_jwoQoWEk~{f7Cxl)SYd-W&6K{9%}RbufK3!0gjVn_$)-t;b;Y
zKN)l0)6AcaS%0?iIhZ=_F!$CS566j^_ir+0{d7$I>oN0Zn4e{SHs-n9q4zqz(D6l>
zejdl1`$hAcF!%ck=JT};Q~xc;-^QH(8%#gnW9I+l_|L|_Vb=YDnfDLodtsm33i>XC
zSyvu2Z$Hd^9_n}%Jq$Cix^WGyskJaa|65?{oQOI1shE2@Q`=+is{>|UC(ONfGv6Ka
z9t|`eggO5R<B^zkqcHb&4QBlm%yYU0)Bl~A^WLNPW9mGDm2f@geR~CSuGcVqZPC{;
z-vb|F1^fZ?eYFR(f1x=A^9ySc%y~;;>XpITcr>QY$(ZNc5z|)}%zj-l^}0Jg7&CvY
z`Ei(e<1y<dn!g5f&KZv1s5j}&dJCrhEaTaFE9N}+VeWAmX5Mn+r!oI7c?)x%cQJju
zhne?*@pk<XGw*+xzII{m<44T=pE2uy!OZ&|Gq1?pg6CES6Ca4V?|PW?Hq@gr>yOc6
zG4E9iZH2kFa~*Go*{7p+cDxU!pNlcie<<d?7=`&<T<iE0OugBdI&;j=!<_3LOn>)d
z`g#EK9zJZm*zskU^^cifp)1Wlfq8G&Vfxx&{sm0k7j=`qq%Z3$x>;Y<*K~`%j=8V5
zG5vpt>GxC2zMo;v^Cf2AuQ2n!bNmPW$?=~t=lTt^?l1FyW9k-~S5UVIre0AkhFMqA
z@lxhXWA34%`2+MIO#MSK>#CZsj;UKiYicdbydyB@s$;&c*3<f!^EEL(8ne&w#!WHn
zo9hYYPsFTiZG5V>(bM#FZL4SKnR=F<t>@^u+D_YJo>y1nZkWE#*B+Sj_H?|L`98*d
zG3V-!nK#J%U>$;~H_Z4_<Ka5e{N*}IN9!0Jt5@ha%zhK}YV+3^Uu%4wPSL5Dex@5=
zZ#={JMoisXjAuGN+xS-FImUB!p5Cr^=$(3(&eyy39$ldK>V0~@KA;ckL%L8G>BG8M
zAJHYc6!W|u!<_SR%=2IC_|sUCcq`^Td>8XQ_6g?a<`0<f(Z4aDr{cF4)Y%(zuca}c
zn|-v5meq1vUiZcHQNg&PR?-77{T^(52xk6a##ObNR>$;F)3_Gq9Cgjt$DH?Q$B!|8
z9M&M;9V_9bj$fw3b%c)8%XJi{&nqzdkH>uPPj>uTOy5&6_czP@Y`qmT|1Qk>yUpK&
z>Hk5@_ugX1m*_Igx+gIA{v_sn&pN&i)7M7FU%=FP#dx#6irHry=I8b&=0DXP`kC&;
zyeEHR-iuOq6zsb%rhX;NzLhcOKGgA=m_BRi;h6Itfw`9^nDxi&30RZ37gok`<|pd4
zn0nV?>P$7BW_&#!Nd7+K2QcSctjjU$AH(#!3iJEQdOR3E#Qgi=d(6BaF#G(3IoGeo
zzhnCT2UEYuodxrXX$j2wl9+X+jrY+qn7ZYR%VX~2VB<qD&$R|-pCd58PaNm?@tC^J
zG3PkRxRv9lXlu+qZ7|Qd6J~x_y#Q0c7v{Yhfa!Z6X5C=pAv#ot>7|%?!;MGiNWC1>
z=NQbqD~zwi-1{`le$z4EJF_tJZpEydi|Ow!%)I%Sc@JUwU51&z-0{aS_x6<WYF(pi
zG4r1>eirk4*=BtY^E^M+otXW<#_YdKzrj4O-!S+27p9*gcNNSlis`?EaY@X+rHo4(
zm%*&x7t==-#}Cu0S`AaLCZ^8edW7RgY8~_SjO%08H8kH?k2ZgtaZ}?Km_FKK&U=QQ
ziP`sTOucrP=g`gZ?s`6EUJv67^&<1VjC&jRHNIH;VeWsh<3r64!|XfUc!cAZ8;^2)
ztod=6d%ep1HOALr`o0NszMC=q&cwVo^UU9_cj%p%br0xL^UE;nRvAB`Ph$FAgQ@?t
z`Dcuub9{sO=XImLfLXUmUpD^=rtWLTTl95(1M|MVZTt?V&U^ZS`R&FZ>Ho}sqMzyx
z{Y-b_f1d|TAK#kat>2md5wq{lnD3p!^9$~~2xeVzOr6r0d1WyFPOFB8;)&*4YAek1
zItBCdrz7S)>4y2eqz`7F%gm3)%pap;G5d@&9*^ncD&vWIHD=x<<H>q0rr+s$6K3Ce
zdY}1)nD_1x%>GL-^_H1mVSXj1|J9i9k&XXn@7=?6AItuK_pEy~MsyD|*{y0U)U;42
zQ7QM$O!mxZCcCv*LWGJ++F>kYwe(0z`-f=}tFf05*_KU8rCO;Z(jK8wo{055U$5&t
zKA*MLq38L1pX2v^j^leA-+!+6=h~guxx3D5^SL)!ep}-0f%w}G0+IKDK*oD0>mN>h
zBoMy-ocQ-ZKL0;(PPsqXBoI1V2C{w(2NI`t4TP`V1L1eiK<51cf$WEl355Um1#-`@
zAaG{jd4cN$ULT15ZVH6{w*zT^XCU;x7YO|yWcl5J%#)u6a_{!XK-&E&>mLn7j=yI9
zi9qN-8Hk>q38Z~vs?ePgNck#($kUhkwF2>@jRT>#Ss;Ax8mPR1(AhH(e%~BO|NSyQ
zCy;jg2SWeg%)c#=_J?Nv?U_G35WO86$a;2CAni^LM9#A^|LMffBz`vWbBUi%{6gXv
z6aPK&>_GTll=*WL&kMvaE(wJ1%K{ny@<8OjD)Uz-UX$h52h#q=EWau7=0NoE?abd9
z$hhB2{9z#WzBds2{4x+a4<tSq$oRhwq}?N#|5M_lf#~&#K<w1>r9x-bK>9sD^DhX5
zkF^4kV`d=!vq2#C+&1fX2xQ(*1k!HbK>Ymef!O1iK=?R55I!dZp>t|rBXB{MFU<UD
zfyi}w=0BczM&c(D&kUs9S%KJpQQ#VZR|YcP^?{AR8v<!}bLMYJ{AS{}vi|nW-;wz3
z#5)7g!w&<I^MOF<|1s+yN_;pFJ3W^8*Tg3S+0XVaDtd1P!q1w4&|NRE5jZpPMS)$B
ze@Wu{Sw1WC8)kl^K>BZ*`Bwzu@7o4qhv7i<yh~tL;BJBV>0X)NC-B9QKQ@r@-=FpK
z1L5zKEdNO2se#b_SmysNuq*N(4@Cc8%>3Db&^sq_D)CE+ivrQp`GN3%Ng(509!MPi
zMj(9N76_f&6YmJ@iu|34-wni%e;7!+9|c19o~-{_;=PIY1=8-9nSUVh!7TrE=6{>{
z-vz?g9|Pg*i9qE4M<Dj^I=7Us7D&5RAo^Y_5WZd%$apUfq~8XC$hB!8d~TlgTLdys
z-WW)~;Xw4VYaruH1k!$=K<Moo*cCV@asR}5S$|;STLWiAyF(HW3w%lBPtE+oK=}H2
zAoM;F2>mkyUmkc}AmiSc<u?V={?<U|_m2bV_mjk*CjLASI`=02BJsY&Unbrkh@8I<
zM9#+pq4$?S^za{<e<~3Dcb`}2t{gZc@~Z^G-)dRDdY1PE!siP!zfRV#8`zBU7iaz@
zfsC_JAoKcFf!JZ&tZxTGXC#nzyJY#USw5EeJp<u?zd-n(n|MItfq~F}SLTlnL_hCN
zd{34illk{09-H|7#1ABXF!4iyjCWk-kI(#M=1)wVpLkN@$%&@~BJU>x(dVZEn}MIr
z^7AskI1s<SG?4Kw4}||K62BUVUtgd38xn5}L~pkSGS1yu{=-1%{5bPV5`U6-PvTD#
zf0p?3K<GV?_`59seIWGynE8hiA5MHE@lT15CO($<cp&}%lK4bmGxGlth@AfjL_hz`
z{4;^HUz*tXa%tBU2!B0+v|lyLpP%JxWPZ&+^to;zele8gFHf8u2>nemziA+JUlj=5
z*JS<Hfy|$FU^8$y5PnAz-;}sRV0V=7nm87SzwR5z_=g2T_lQ9DxgW~>@qy4gA#pOx
zPYPTw%FhnO?u!Gl@0Ee@aZMonUmJ*=HwUu*{Wy^I`FC0Whd}s$JdpnX3}l`^@BDJl
zH8T)8FAjw6tU$)wB(OVh^Fa99BJov;uTI=Dkp5c*(*N~=D+hM6eEUGg-zAX!^udA9
zJ0$UKfsA)lAo_Vv;`;)j_rXBo;70@D|Exgtd2S$bel3vux;q1D|GljLMIhtc7l^+9
z6bSvt6aSj^PbB^=5P6?UT$<%8T~Orf3WVN_#Aaf5;>wB73&ifN%=ZN%|2mmpH*vkd
zm7{$9%+E^PAn<unK0EUp2cq9i1F>&A5IKhf>A!OzeC-m5ocjgR?wwiwuEe7fk4}7d
zAavgwh(DZ=`4a=-_oIR6>tl%@4`iG(5<d}$9nJ}a&X=-$QR2CY=OunQ5WQRy2>(|E
zqVKD-{+h(ECte%K_}6FthCt-|ULbz(^DMtN5PJ7#{(-E2BoH~C2z*}P(^>w!#YJzc
z1=6oC5V_U}><L^q^XmmN{)+?QbA!a8#7z^oNZcxr{rZ~&p*J3gUfvSOIQs^A|DN?n
z2V%ct0~zOof$S?!3&ehB2cqAL0-<+tAoMN`gx*&J@&8-1{<f^YBM`dZ3xw|7i9gQr
zp9j+amx&JsqR&SI8RxOY#}oe?Nc+EK{_lxT2BOzz1L33fmD0a2ka7D1nb$AQ@|OhC
z{-s&}@<8O-IO{h_+%(Hynfc8UHxGo~>jSYzJCJd<Pkd7#{O=G5-JP<0*DT*HareYM
z5=R4Rzh~y(oH(91k@%KC#@i=wmB6<KqL;S?GVY;))E^#*{*MZT?z<D;8@Nj3j|)WK
zAI<!kfjyD`eBz=&<hvjcIu{3`w<`moe{~>qzn*wqmfsKvA2$ZB5_m@-bnXhI{r3Z*
zw<M5uKgs+r0+Hi^EdN#Jf1C9W2d)+MUDJi{)dPvgFA9XO4Fl;v6v%j+CT<bPxUUXm
zye$K1zfB-=Y#X>r;Eq|oN0#pyNWbww`b`89pXO!$z(D#R9M~K9&OqdPZy^2N7sxor
z1=9ccz}~<Gfy9r`2GaiQK=@u1NWb#}Y5(Ow<e1L-OS1gZK<HkT`KtqI_l-dKxiQOc
z3WV;r0-3)*%<>-v((WgjzbEmhi9ZX3k9#x!i^Tg9f0=lH;sc4lN_;So{=domZxer)
z`1`~^1mbUxXa3KD$np0;`1?oVKeN1XVJUA0qMz=>m9u=6K>Dv9$oQ>5?7B`M`Wnpg
zS%J{sC=fa?O?+A6P~yuIXD4nP2;EIH|BA#{1~TreGrwgZ?Y7GN)`_o8d|lRWlleC!
zzA<sztnUOu_f1*8Lm>9tCG)!`?v}WF*6*45Hz$rKPGtQ)ncp{Yzr;C-a})PZJRosi
z;(>{8O*|;^;KV}$;rsB+ACY)u;yV)GnfR{6qY{r!e0Lyvd~fEDNqk@8v5D^wq}>M-
zKOBf(ogCO3czPiB6<^5ma|5yad4bHo>C9i4cv0fTf$014%wLiC)hxd{5c)R<GX6KS
z{##jpM<DU_$63B4koEKrfyA+;f#|7uQCYWM7zkf$1wwC~%&(icULfOb6iC0>fvW~?
z8A$uB0_neX;%fu(^KAmr$8eU9B)%ySI=f_k*Ffm)6^PvXW%-;y=<lET1G4_0%pV-c
z{5m4@M<%`_kbdvV{853>|4<<GPY6Vwj|Q@Ce<2V)&kfW*nZGEIdHl63zbcUScLbux
z`vYGWxX#68y!8U<KQr+~fzaJ3>t7bgy78K<-zv*roA~-H-zM=5iEm8YHqiAV5W1sT
zJ{E|+$1^_>`25Hp7|8qa4+Ns`;{%a@ejt3Flz4LDDS_zoV}Zzdb|Cy-90>i(0@34j
zf#~=8K-QDH0-^W4#P0_(-j6fCB#?FB*ManZG?0B@^O7RZYJv1$J@EyJtw6?IGZ1^d
zD3Ei`7J>NlNZ_o%Lj&RG?SZsEG7veB3PfKY2vq(+#yKI7cJniTQsT*przCzP@zg-*
ze>4!f{}#yn{bbgED)Fp9=zcbkc3;f$e@{F+%NJ$-+{E(|znpk};suF|1F_4cf$(v8
zmS2(h)x;|USC9Hz0%`y4K=`;T5PRMqi2j}kL=S%p#J<f-OTIgC<v`?ECG)+Bs|GUu
zYJt%03uK(NvwodG^gSz({u>3-?&VoOJCOdHWPa1cR|LXeI}khX9JqSm-dVp-AbxXj
z<_`(ve(~hM)dD|}<!1)c|BHdxXHg*Y;F3V*%T<B+!F8GcW*~gtmgV2c{Pz;?&hnoG
zqOW_h{O6g!HxPM$llk8!{w|Pl9?twDf#~V4fsFU}K=^q!>z4*1U+-mQoK*wWbK(oK
zyp{Pi0-?WFAaq|G$a~b+2SWdif$+O+Vmq-D2;J=yM*^X<bJp*YxNFw$p7}izM+1>}
zpFqZ$6Nr81W&MGH&_5{44-JI=k(oa#5V_wUNV^YaetsbQo|O610-=9;;wJ*J=VucC
zJ&^Wi2SR5maZ#3^n|NLz<9{Xd(}@=*UX*xo;w6cfCSI0!dEym;jCW<?Rat&@Ame-^
z@#d`mX5y`hw<mr(@jHokC4N5;fBr=v@;;dP#{%(>rvlMm*X6|ys|KQ{)dLxS%|PR1
zAo_W6AoiIR2;El(V!y2e@tbV}={Fq6{Ci6v{pSTT|K6GS;lzc3&^;}X_j%_8(*7%f
z%(JfrLg)5C+TD@(?Zi71zmxdg#Jdu|m-zj}A0*zL_`}2>CH^>ZN#aiu?@9b=;?EL)
z9tgj`2}JHc2BL?@v;NPCe@Xmn;uDE~OZ<Bve()ce|3~6eiBAV&muCZMx6&0wAB{lr
zGcw-{#6CTNtY_;5VyB_3e|aE!-7@p9N!%(B|9C?n{0|2*-VRy6W8zMUJ16cENW0xL
zzegbajAee$#5ZU8MB-k7^xHS<_X|XS^RoV}S$<IF4+%t`_h<PB0_p$Zz!wCb7Kk0r
z3Pk?T1>%>B0%`Z<#KnQ|HywySU6^=L;>CfCb5kJwz8Q!<?hHgfKL}*J`vV#0*MZRc
zZRY<N$hdzAq~AXRk?Yw&=Gn?$E$w>}dlR1@2>mqzkz-~c{npR=4H7pBq}|4en+L+@
ztF!(!fylL0;?{|;4TRq}1VVSa#2o_RYmY$q-aC-~`vfvy4-G`Vw`cj`iAMxN=cvpd
z9f)2g1L5bSK=0WD*NFUOf$)7*AoQ=!{Pls*zcI^i3RLfjw`Khufvsrw(?IxrAP{+e
z69~QECjKt*_gVjNApZVjAatL~`lmDhY+~ceLZ>SbyRDM>-az`Tmig5K;kT9fzChZq
zo%wYF(c`SlZx9HdFU#`HvV8NzEduHP+RVQ$@%4$@1Tx+mGrw&hdLPdGNFaLMCG)!m
z(th_q#@#dX<5@qE_?E=I688?I-@cjOFY|K)p>ts74+=ydZwqAoI64q}d>{}xCIhj@
ziHY+QPf9#F5Pla1LihA6|9Iv<83_GPXZdF`|M|>+A&_wvW&Yf(KR=N7AU6g=_nTS&
ztw7|uE%Uc0-VsQ<?`Hn4#P0>7-ya3S=g%_#%Ru=0RpM^~^}EF1CH_9~4~c&aWF9@4
z`Tq!n&eMs{X8F>@mA+Q^?FxkcjKpSQcjC%{$kh{w9M8}4)w28rnQvu&jm)o^`GL&8
zFmbIw_*ysXXJ+|}5?`G4>nCoQ<r@XU$IG&Ob|B~1g9G8~u&h5k5Pvx)5V_tL$ao*f
z`VVFP_(1f3QXuV4&iaoeE(oOGX@U5`nSr$XWa6h1&r1As;%5>+oA|lJ&nJE%5V~h)
z{+z_AEI&8%=LI6~`I)~U5I?y%kT~=8K<s^c*546G`#TfAo8@;UelHNc{4Dc7PrNtr
z7m4=;qL&8)8Sf8)==YI8?hR&KRrJsdWWDbTL_cc>5|7qToE->1n*`E-vp~-CJ7)RL
ziMs}(*WD6#PuwGMG;u6(&p_yo2cpNlvV8BveG>Og+%NEj(eCI#`n@O1-<$dO1?nG}
z|4`O{I1oLYnECmMCj}zUse#b@XqGQbJT38KS$}%wKc09-mY<pVPbPjU@vN->Oy)nE
z`OgPpkEtyGQkI{a`SUV=e&#O-gwG4J{Nh0Re>Ll`3`Fj$Gk;AW?Y|L7yITU0|Mo!S
z{B|IAx-;v4komh4e;5e8C4scRC-C)wYg}FO{eko!NPJ-+<ID`C-%A4Nw|>^oO57lE
z!$8`-Eb~K|-#Czdn`ZedGQU~oH&5Im%U>M`ovpHb>%`Y4zAg}Y+hqO?iEqsEcIG>o
z-#+uhi6eo`gWWT~M<DdZGQVdadfh9__etC@>*r*C|HOHTZ_WCH5)V#1BoKKH%lz9D
z4-aJABNN}5cvK*Ees3W5I41FZiN^-g?*p0tU?B95%lz?~KQWMYCncVecxvKD6Hg0d
zoR4Mx^u#j)q5H|K|5TQLI`OlKpHKW^;@N@FJ0}o5o)-w63j*2CUY_+=Bz`sV%EYfF
zUX^%t;x&n1PrNqqy2NiJULT0uH)j5(K>YZ&EWaZV``;Z1Uq23n-z8apPvXzA{O5`H
z24a`{Gyg#1uL2qG*O~uK=6{#@-v=_zLz#a#@sY$oB|e(?SmNV}e@^^M;$IV=Nc>yk
z-xHrq{EtBRdMfizC;l_>nZ#!kmnN=sP0>dqkp43=-wZ^K=ViVp>w7c5YUWqV{OXA>
zNNgqc1)~3TGrwNq%s|F_apqqV$ojcyAmeV9<(nsN5r{or6G*?UGrvtBe%HzJ?E|sz
za3K5BT?6Sin&o4G^nY{a#{(H}uPomu%lA#(FL6%d+{FD84@jJscwiv(56b+(iHBtQ
zp_xA{^M_~th{Pik-w_C3M`ix##CIpYClI-g&HVclKM=_HAIkiP6OT(gK9Kowa^_D7
zWSj+o(EC^*_Bt!;Kb`oQK=gS|;ssg0IPoiq(}@=*UX*xo;w6cfCSI0!c_8C|E%R3;
zUY&SN)_)`O*C*bPcw^#Ci8m+S5{N#&mHAr}Z%e#A@s7lA2h#pKng4F$U0MG9%>N+q
z?!+G^{wVRsiAxfH5{UkO8Hm1rmE{j6{yNKlm-xp(_;@($9|>e%_t(rn5s2UaBM`kk
z6UhF1tFIUOuM4F7je*4B9Rs1ab0GE~4TPV)1DP-TB<>qXzx^|RKp_1N%=}vu4@x{Z
z5P1&G{9%E#KRokC1maKc&HOQm?+Zkp;{xeF8A!kRS$|UE$%&_A{i%WQ{qZb6BM^Q+
zmHD$0Kb`oQK<Ixy^Iu5(V&cCio}GA3;#45~EXw@3ng4Pi`nxd8FG{>P>n{x?j$NPm
z8?*kV#G3=*=UbV-HS>1_GTxn8{+-0{Cf=3!y+GRiAQ1a6354EH1F`dif!OociN8tw
zZQ}0&;s239`aPEUzXhVle+1J1sX)g0XX3M2zBF;AYfJwbfwXTXb|<c!_`E>I>CODA
zfzVwo^Q$MmAhDI$7f8D`GvA;27iNB~#I>`0-N5am{oKqS5XkxJ@Id%GBFo>A`FAG1
zEAgnrqZ8kq_?|%M93P0^eI(0IO<WL&AAdX$InNBl-@lOgvjZ9boIvFHQsTLZUrxLr
zkamj`zmhnecwyp2i5Dkcl6Yz2Wr>$3UJ;1BugUz^6R!=#e{RhDO^G)r-VzAE-^%>0
ziMJ)*o_I&%w*wjH`+@YkJIj9<h+q9A%YT~XKg;~Rng2x~?H<VT2ebUwng4C(e;3Gk
z`!9jqJ2tK>dg}^A@2h71`GL@DWxg+Qjl?wrUm5k=1=4SqEZ;S8x5V8O_edN~91Dd0
zMB+Yy$h}|U+{6O{vFp4*{Ntd^ADsC^1L^nn#3KT+%Ta;!J38^*S^wTZ#(jV0KNyJK
zCj+5>QXu^oWd5{3=>J<F{(Dv+bUz(Pzb^)&&&7fCzc9-$4}{KDfwaFm^WO-B-VKR2
zW&O>Gw<LZu>u=5cZHc!h-Vw-n-wA}T9|baxejW(@Uj#DleSz@#>&*WqkaiDe{*gf1
zKN?88Cjz1SWS0LU5Pdz9<tu%owC_r61~N`}ApKVjr2pz!{({6-Amgl&`85;!69*Ds
z7)ZOdGrvyax>-Im^DhcyoWabmpExUVgTR5P-#YWJO?+J-^tZ|U8xr4`xNTxPu@i_L
z-xSDrI|ZVT-4e$F(dXWQw40md2L_^-w<aEx<%eeeut4Y?5s2Q74usD81EF(VmY<aQ
z1%dFrFc7+@1;WS2Gk-?rKb85j0%`x5%zrlXpU?akGXL+1=VbX*<`-rD+{E*;{QN-1
zold+Y5IHVSyfTn+uL^|Es}rwD{CXhq;+8<<ygkeB2&CVgfolc+D$5@Xq}}f`|A#>A
z`b;3>u5^9D8Hp<=t`dk|Ul0i0zCid{BXP|v9|(lr?96WxNWU!t*9;uV@*M&hZ|6Yh
z?2@=^;%<q%2lBpU|3Jo_7f8Q@0+IX3K=gN1AaWfO2)*|u9-H|7#191G7n6a+nU5uY
zHtRnZ2*2k9LigOn3$lK3Amd+{<(FppWr>$3UJ(etR|Z1onk@f%;<bs_1=8;N#2d5x
zro@{QZ%O=S;<plSO}s7f_QX2^p?hcMzmxdgK=}DV=I>7YVd9Un{+>Yk|18UYo_KGT
z-<SDcCf=X;K;o|w9}Gm_e+-1)!-0(dSmys6h`s)j_}4(%Kb85X1KGc>azkm?o49Hq
z?bgWrnu-011BovTWZbm_vD<oqjQ^6XAI$n$ncpCB!^DlU{$-gT%KYriZ=ASE;--nO
zNPJ}=<8G1pS0%nWam&Ql1j6Umf$+CYmcJnoIfnzGvqP5e7)bk_GrvpXu35f&=J!Y(
zO&kkkp1dXVdu4u3AoTW6oR{?nCcZWCpsYV6^KVN$H1V*+w+BM+h|C|E_>Ms2I4Y3&
z_u(u*F3Tq~e`4bN#FGM{cS<1r7i9TIv;4Hoe=O@io_J=~e=3lD^ErX|+f?G05*H<&
zn|NO0mlMxVydZIL;#U%<6E94>DDmRNOA;?lye#qZK=}Gb=C4n@A@Rn<n-XtMye09Q
ziQh`RHSxB@+XFd=-JN)Emj5CUJ>DOPUVjyc-~Bca|9B|N9|=TXe@c8b%O4M<|6ddT
zp5_0M`KJ?~&GMCQEOZ))U5PUin~B|tD<?iLu_tkrK=@oO^Q#BKe=87v*39z$#DT;Y
zCa#sZcH%mL$ot~N4HAd4{^fzt-#GJ|ByO7cimcx(^P4Abk>#(>{FaHYN!%*yUz_>Y
z1;YOuGXKWJZ4=vxoj~~6KJ&wgBZ+ScWc}JT^ScGIpPfjYn|MedavvUuKHnM0_(umq
z|2>Jv1iGFk9-H|7tp8vjbWX_piGhrNYT(*|=L9<60vYGLK<J;J<rf5UZn`SVuSvW%
z@f(4x|KH90_X45!{lp(6-ktS7%KVQr|C7X@2EylkS^vvG_<1l8xqqAZhphi&AbdWY
z`A0JUXyW5p{^vmYKau&rW&M+h|H$&EGXKxaKa==umalYE&W}LGoss!w=2y=A^AdZq
zyf^XriK{2J0*Nc@1;XEp1ED*ZI4g0(K<qU;kacGBK=ivsAb#_@K;p-6AoaThqQ}ud
z^fw+zyE%c3b3h>d=Vkt&%paWjLj&RC?TJSuz9SI&?+v8?F@dx@Hjr^X6bK)u1k&zf
zfsFg9K=}DwAfNBKFp&Ni1;WpziB|+t{?)`Q1J{ZCU4iK7r-9V}ERb>U4TS!$11bMa
zAn&vP90=Vf0@;^z-CXKt1hVg4JCOSI0^xI3<~K?l3WWabK=^)TU?Xs|#LWY#e^nrK
zUX$fp1tNbZ5WaQ_L@&E#{qBi-1k!GwK*pb!<p%~b&yEUo-Ocjj5|2+jA#pO0{-*}A
z&psoN@jexZzkEIr{{B62DzFjxF9pK?xmkXGmS2##IP0gg{^G>T62BVQi2hdwGX8a0
zenaBTfsH7?C6IBxmHAr(Y5(2C9|aPB9?tql0<rUxfynW6mj5&HnLy|*&HPHY6!~TZ
zqR-U=8-c4Qz97rj34CFczcP?GwQV5!-6fFryC&|IxO*Uea!??8e0v~%@Xo~d1magG
z26hFW6bPM@Gyjpq1%Z80{<%Qpo(hD{mjh{kejxt()y!WRh}_o%((d{|{NcM<e^=Ju
z9oQA+KMI78Uj-u1p8^@@i9qcBWa3kSjQ4b4SKu>=OA{O4EcIQ9GZLGDv|l+8J_Z8e
zYu&_`1VVQ(knuMO#P44n$a!{`tlu>d`@AI(dV6R2K7q(PH}m@^9uNqfLo<I^V1ML4
z97zA;1JT=Qf$(!i;EcdeX8yB*=>02!(78MiJ>3{MBk-0izcUbe-wi~Ly8@y2!>s>#
zmfsu5x%kmQ+CQG<e@^^M;$H)2ME&0bk$0tU6}>z^5Wik45WY7Ir2Z9w&A=@J8UIy*
z(BCp~t3cxAPJz(fHIR0@CGH;BjQpN~^c&Cei9q~dP9Xgb$od1b{;gU6_RJq1h}`cA
z#P3fIM1N-m(*Lu8jQhDj#`!{EGw__ma|5YAFAzFk$^3NUg^3p>UYvMIAoMN^WSp;N
z{%e8F$bUWY`YgXOkokCPAmiPZczfa<iQf*y?ssSYhk?Y$djlEozQp?jo00!@AoP9{
zh@by8kam9yBwlpiTI|;fq+MU)8iCkn!_03Kh~8e4xI>ojn7C8o&WXDu?ivVx6M^Vq
z?=0UZao@!K0^w_J=J!uLAaP#SzcsKs+8vtZhXpnxe`Mz0k@(KQS4a7x%%7WhULfO~
zpZN<C7bkutaXRtB#ETLyPP`=X(!|RWFHgK8@uXiMb>VqV-~@}pW^*GMG3(QQk!|i9
zNzZzbOj@x&Wi;-g$w1whiZ)YrkRDwU*e9M^AC083faJhPraltM#7U9tdm3es`p3s3
zX_4)x$)saQifk8ycby(dLwXEZxT5coSbsa{4bG2blpZUSjM8`%DnnACTSLmH#8VH!
zvPHH5r!C6*C~MGTKCV1S{2Fvzo+hd2VaUB+6v;G6mkDZ`SQyC|-MW}AQ^cGh${Iw4
z2BByXq}mW@L8|m6+f_B`LJ{4x8-dC!s4SpKA4xX>xq+1Gd11wnq-K*Hl>9;Q)N=@p
z4<k90<Y1DwksMC)c9J7VBwF<yM4~|kpU>d)nVUThL|GSQC9B%uw25-M9Jf23riijr
zOsy%5vINCWKwt>UJ(Z|~711GxPC4$8@zi(+$-5#MJc{I<WFbE+0iHIisi3evCRzjj
zrshV{W?gJRbcn&*wJqs8M!OzrHEj!<Pn;UbsD7nK98E0(Hlf##96hxh5a@-#d_-6P
zS6xWnh2&jG-bKYEhANhRcRUsT<civb@?5hE6;+}lCYSdd{v>auKAKEA18Q%_(u0RY
z@?Th5#cJ+%&{&<y)q5fE9+LM{@d1(#MlyK<PbZR0lECv~G&za6Cy$9_@O>o5k|6#h
z;!h&JGi9){(ZbWCOacorK$FJPT7e$Z^q593U5K?d10BXdJq*-?{}tA#=+asSqsnC6
z>qdn$xm%NIEgJvelt@;qf`Xx75sK9)Q<Wcd)uT~XrfwC2CK+;E-9vOr2U*?)w#IY1
zjgts?B>c^<xEgk#nNikO$`8fUV5R4t50gEP<aiPW>LOlE!c`YKaCT3!D5}IE*syj|
zc`H57M$g048ZsI%-hx#7Ic(YEK(IC+dkkWVK}<1-{`Bl2gjq-v%`;uA2{{b^{is;Q
z&Z6;66)y&|k;?G0TjD7UtqhZf(ryM{#&e+3Sb-tRI<RYxWg-Z(qQMv~l8$<z@e_z}
zhT1qJp8mU2<Nfs2P|u;ip$4_sd=PVY;f^i*pmjhbZM>ufFN4V2gVz~L<g|&_x>)Yo
zc&EY5VAQvOc2LpMlgQDaamy#@_3(6}LNjg;jiqe2HXhMlKa#PbNG4uN@^X^dBrhY`
zG?K<EBkA6R>}F(N!PDlEbY4Yv3lerIqgzJOy;UR=uOWFI$?HkBjim8Lp4yR&yq2d9
z$s0(vCfSZ;8<G){H<4^lvO^?Ghe>uMJ2#S1#4_Zzm5A(=!k}Vz9?n`-r=HtbzlkD>
z6*L99Z=u9-vykd1>4RRcq|kT=d$<>9YaTs^+G%*{&_tb%p3D=q)6_0R<BPDuVhBi7
zpB(&XWQQ4JW1gCDFrba-yFfO0>XNc-P}U8p9eu)Ix@j_tA=}uz_y)BND4(v=knLNS
zTKe)Q^((chbxmGmE##+gNB3OCH|X1RaGL0tU2xi%8A%&<-J4B+geTPAqH%Fd$DmzJ
zZ<sQv{GHK;Do-3mj!~4T_)S<_j981{;6)Ifpz#ph+^bv)LvF!(G$@V7nHo~*Lx~!o
z*LGNI7Ef(B7(fT^JbpmdJ>j72?n4uj76VPvZqg_C)8_6cn=mw{sPJs;oV{*5HAqHh
zMAFq2iLU2BCu>zA>jd-$;bjc38?YPX)!Kb1v5AUXGpR+z?rq<~Kn<82K{*?ummLs#
z3ZeVqbT6N%bq8e_c2BL5r(su7T`wmg@KN+qw&$?xPR0%Wg(!R4*aT|>dM6xA==W%R
z0*Ty@Uj`eOKyO?%*^V;T^B&rDDxpWHZBu5bGAs?!S7ke0rX5@YXlR@v-QpPQjBCRP
z(?N-S&?_;FGNV8-o<m|wC3=X)$HCt)Jw_nk#gjpysm*Cp9}qTy4MV911gDj=XOSYH
z@x>^6kVH8=aLlGcA2(@Z#)by1RH85%MGd*@GbvI^z!<iE^!@)cAj|Im;ixR*I$`ys
za!yD5e=RxT%c#<a6^sXsdGX{%#OX3osbpd(Dpp*hHjc7}3l(eBglp6*c-oxgRgtt=
znHsE26aPvF<8P$4P4Zfj4#^uxwkFw*WE+wZ5+&M+C)cRmc-n<zXOi7Xb|o1l86$Z!
z$sQ!*Bzux<Px2O$2@)5%y?Lrux}|$Xb}7EAORZ69vN<ElhPoo@;ASI)2bX*&dpA8M
zsO_$GO1o}n6t&L5DVmIuNZx2>*tBnPWrmzyE0PI=7WQ=uF#+Xr9Hxn}q|8BwwAMMu
zkUfe32L`t$WwXeRNuI_f_)^wk8W=levWvcL$Dm>XvwHzEbQS@0nA(APk+=%=G2h%2
zxz<k6?I02ZveyKSd69MFpc$te&5!}n9*DZow`kYY^&skWo+K)_UGq?c;mi5kRFjA`
z`#A#2kv5(mwO%n8bJ`3vs#qwtiR9jrEFj3Z(4Vhf$nqcYE7@ZV@&nkz^|x1hF!+dy
zfQqNj8AR4G0~rFkO=&hLgXo$x9$7z<i490LjHIy<PfoB|JiV0U<s`F7>RDE82g_7j
zemm%V>r~spvHx_^O}r7xZIb^FlkR_PzWqPFth97*jIu%;(jD<w_YqDFr;sz$`=%-U
z&+WNWcN!-g#qZ002$$ED4OK3mWrdv)6=mP#d~>=m(5Vn`wKs0K>vI>>q~buD^g!N#
z*C9JaMR!dPM|V%?{GHe@vhEostYB)inPBb;?CU<()qWSInl}U!iUsAT8Dlm>x@j{K
zcHnt{CUWKR!HMk+khgCQ80zVYL^eFrx&3h8GzMjZuVKl>utS;q7pHF7b76pSI9SM2
zKO(3Sqo~&o9^viQ_<vsfuf16B)Oc-t&BJ$_DDP3QUyq=oUvsm&b^+29BLv?jx-S;g
zhcpV&wwLU$aR&W#k_rd%OfrcJ7~2i?mbu`1l3Rpdwb&1NOHw=sOS>s@r|)8;rHem9
z&vkl`Aw4~roe%djYEI?PBOd$>BA_lcLN}Lf6P>zTkSG?*P7^9+Wk+bIhWeY0i}oS(
zV8x_scMlW7^ZBG+%(PeN&(m(K8eA$$G@CITd|C~`?lhx0E4-BTg3~ZEi^()dyK)$X
zsOzt_lQ2F>nQ_Cp;1utmk`rObK%o*x;Z$Xp4Thu`d~r2~hT3jslu4!d0YbY&nNP(8
z+`H3q<KWbFn)YH)jqKTAnKKRp9fMwsLlg6Of)``wFi!@&@{&Q7Xa$ndeIrrhh9#ZV
z)7TV@Ptl}!Csfpn(Xl?>JPr41s8nE&C0KewgTnJDTN24PasCDyhXy>CIL?!bm_mHb
zxWF=W--TG^ssws^u}U0-ze$q~;%S*p$1JjPPvUYi0I9`@upOr8MF%?9QaBw`5i}WQ
z47u{&Yyk{){x1uC2tD{ew!1--B^aUDNwb=f#GAnXi}r-5uIGPDbs7psH>#BRVKn={
z%;8j+!u(GhUkx`#26W{atiG`}{JTIpiKl7zF_Ip_+@z}u0mYEkKQEp<=XtMEUJ;)}
zJA^ripfE<?DQq<Aa!22Cn#H|cdqyM=FNP7L$_?~*4^6zLa&2^(FwlC{<nq)@Hv|0B
zbqJzc+c*SeMOX?wkLE`75?-l2nhG~<r3`xRL5z-rG;ttT@jbN>0nZrQvtK-w*Tz(M
zH{3Qvl6AM)o*z#o9C{ArUck#>_t~Sg^WFBS7Y2ynLUj#KEf<Uf<EibqJo(;b0V=-4
z-KNRB$S!k$MshFIye9Sh>s^5x2bT`77aK$x_Z#Jg8S>@$>N0^|+=unShI30b$^C{f
z)T@dWkjC|iz-^CvZ@2ND2)rOE2NTA1$K6JMEAIXoO;Q?40$x4$F_0E4`ymD|_de*#
z+Yb#@Zuq>d#g^UZ%Ij!1N;l7pto%*FM%ns7z*?{FJ+5oANrsf@eB^b5aiIR<RtP_^
zvEDvg3o08<GpQVOE4xExPuV2Uw?n(KlbIJ!I!!;Uwcy#^gIhJ-Y`PI;z9OkV#L;v^
zpa{^DRFASjK;8GbQ$U2WL(!^aow|zU#eNrtUjV6L2)I*lo1)7bS^pbzA070up!Gt&
z2X<Rz-LmUL?xoxW`g8+5R)!5fZlWT(VRwRafcNcA1K-5SeF;_6_G9jG2wYmtUjsmi
zYw86y+-Ga)33m16)lt=B9vb?T8iLemRCstikgRenMh-9SdKkCExMO&j8ZTC$@g$=S
zQQ?V2&+(FNp<d#|nG>}}#}OKLRTaLBu<<k@V*;&tRCNcxNUOqzLK}Tl=N*&{;FIM8
z5jdRJ;Df|n<uD3(Yo!Zx;B*>4T@4NSa^oh1UIH)UyjmFHYRy-4`lK4a7VfvNoU5FA
z(S~QWQHb6M6>ao1QW`g;XB?N5Hdi@=kn%9~u&#`}<?cZPs2q54+Ix#=508hw_|l+5
z4CJg(Hfz1ZEAPCtK4O*2Lew;ldAbm$k7+uD;oVcqt^rD6)b0>X+ORf=i~2DpKV~(C
zo);nXLfF+t3n68#9PFqz)TB(3NJ^_Zq=DAgA%?V%NPyAY&>Np8Bmf&~tt9eHS$TJY
zntY?9M8+`NxxI8Qbjyp9Lw+_-hPhF)^O3C|M{-FWni)?n(duAFbTz?{-83#UnZ~{_
zZxPJf$mSi2apO@4H0%bG3#r`=r*@{4w^|VJI;Pw!!$CPp!|6CC>qe|mOr~}(gw%x)
zZR0sg?x}4It|lSSaxx*?RkXXobs85il(v}=YD4>8JTJ0_0>i@?eH)4eyWa2CE;Y`K
zFSquz(=vvojTy+pnN!VB+SFvQ@o&@)pgI?-woF2P1lEfF*co|$Zy>ve0OfhwY2{hB
zg(A8ksy&R}DxwKx&xp=Owd*J72GNz5{g&OyG#y0*U*=4rVsAP;+ZQ*ZN3Ro+r?NZ5
zz29a9Pp^dWvWQecQ2xB~cG^2#B<cm2?(M={-c&QLSAVTWJh{%C22s89WE3_Zg_T#>
zR6K({+(PJ+wkx|dYQ2up3aiYFtn;nby$eByf!uEtUXY__UL>{9M%I2bIfnT4CeYoh
zvRT%HA_i^3lTsRjw1;ba$6Cf%6;a*X`Dq7ZSU0jsudMb^DKBK`RyK>(lO9`^G3cGK
zyu34Dx6J*z2ju(6YV%n>VJJ5fGOqCq3RBcBv$&eA1k}wY8Pc_)g=J=w-3+mu><*;v
zOO$u^(9XN&VKpQ@=Bj=wyWWzhP9v}a7E(w#iymLXWSVE3totF|)>UYdvhuE%(PVOf
zr&0@((o>x?$ogc^D(a-nxM9VKPDF^#f{GKzYxS<>+pSNwQw=m__@b;B6aq$cL(7y4
zI1}4hp)mTcYtJldhmqHIP6KyAPgCZ$r{37O8}<`w-e?Xvg<!*7xa=CoHA?v?2vqih
zwYf0q(yPYZPS2v^KDmdKDxOwyx|P6V6h$v?K4%d?%RHB7lHM7S3=+*;CL~`rUO1-y
zS5D=|9K#`T+;<axZe)bPpJ5>9N7)}>cz5RW8S*I^;>op4QK!jzN_9i<6|%(zaNzNI
zk$i-WO__9x0F@Kgi)6vfNc{A~>`IBF%$a8}UN3s_q~ym#{BThD{6Q6;>kldR(&H$0
z0G_u-jM*shWGK7Savk(zB*tCGb*JEGgxv}mH_C@w7`z`Uqe!H)+Qj2)8$IhKPa=Y<
z^HO^gxL*n{z6o(}>(teE=NbSI-}^+xa<ncq<Qm{f^K+Q2A5+M|fT6%u7s{TZoe+NR
z+Rp&hJIfBrF{=3Tr3_BD&0uYNDI=RakFt#U+Q|uPptzOBI`9bUoT`+qyk}}CSR0lU
z!aPh!bGKCHGz#m5U5^<<=&GZdc<Of3p`j{mbj^#3F?67d>e=PfCy@8^VGb^-7F0Z#
zbmPh@$5X01tseP9)Do5;Fc1D**_#;DXsZ$p!#b-czK+^uBD#q5+6^Dlc<xIU>kR1c
zXlF9ypRP7B-5QE96yXH4>+W@okULH{cVueA$lzJS5VXj_(Wz9*shl!r*<#pmmX&$Q
z;BK)M%hQCn?0%HXwQLMdeb?cB+)3Pviu?2{7=H|gmOyF*BfC@%U?``Cw%pQqfGwMF
z@RdqK9+ysVEyD^OMpGF5V3~_<hBU0tW=PLb^3uh)o{#)wlC%397;@9w4`tOf0eM53
z2doKn>M=-$&d1Upfh%7qI2R<f9dbDCq)d|ajOc<<l#LU5ab$GtS|_sZ3ClYr8ONJ6
zlrNw>3hF){BRz}z^exCsrJQ!)xg2?nK3LV44EmbuhXLSkFz!^VW0UdTlT!I$B~8kH
zTo<qhZdTsJA^x{18)OVWLea+rYaonXd@ZbbFrGm>mF=f&<;5E%w+kNdhcV+~d9LP>
z0_qluKZ$$$^%AdgeDZ$7C%4_pB+fT)g|L1Pyu5@lT=-l>7LwIuu6YZPsLA5cg9!bN
zNZe{n8ULvDh*F}8Hlj?g)5COX5AJR+b<8P*>4$*f@G2Ccp~^WK#TxpYoZ9V4*j?EG
zMK@!+<n=l^ollebp6Cq}*nCt!fSx>;9pjA1&W4m%O)j|QP5}bHVuH1per|xulHl4n
zC&~s8a2<$h8CNS;A<yi`BF7X|T!gRFpXuf;(<E`y#Y+`0)C#CmA79E!tk*4NG!KB|
zXuO+wsp?$349I%yeHdyf>^cWsH1#W&yQNs+IKrI!b|c6ntW7B(Tp3@6=fzX~$$=dq
z>L+ELR@*^kvf7`HBH&^eZ=ysG={mJ%V$Y%O51d=>+>EiupfJ}rLq?O3;dWo^PeasC
zX1<$&+Dshfn0+Q$6WBGp4j_UypR%mVE+*Szs<kWD%94upuRspZOeH=u<Rbe15`#Wp
zCF)nS^OWj9eQ-KPh5o!4{^YsWY2c8U$4}I1qrQ2O^}_i?rkclxMz!n|c^N}`^|8w1
z!yvO_M2%|&5_coUH4hhkl=UKI36YwDZcQ1-`3)Y$P$~xjUHx=IS58f<GgwiaUsCAm
z5|nc-eb+|7Hf}r2jB+BnQE%&$XszrD3=i};8{JRR!|RFi*#Nax8O4hVU8=v5<C~Co
zVbMYQVdF#09%J23*3OKq>a0)skJ0X3Rk8?r6IIeph3lJBd>jH!wFdlIwl-zzpr5kQ
z8IkDJGO12|Cu}J79&Y=ZQ`By|OW;&>cIsM`qq&#D##1EE(9TV?N5;7_1eM|zG+7My
z9rc2W-2|4+4p~JQm=jNBE>JcK<8H>h0x2soa+H8<RAJn~bs}-0UxOjNl5!5Z@EJTj
zi1pBS3R`y3x0}R@mEBcQu?8F%^tH5uKLODtc;_JA?)8%{I*pWGI(fw~481-Q185hb
z8k)Ok=Y5!WL2kFoZLNx6Tt9a1k~P3Up7T7#lnV`}sgc3AOPk@v)AJ)SgsViurUy39
zBnGWPD7$`4IrA88gt8HBiCAS1hk0D0-JngZ8(9OWG5ayNH^#bHdsJ{ve+8n~*1TwL
zvU;|Wq24^XF&u$|el6(WG#=E$R0I`%@MIJwpJiNM`3=M04U9IEZXOlfrW6OB6J>77
z-HWQGa?R`*s7aS{n^zWLdi)I?47=*WMlYjzg!kC&X+ryUXgm&oF1T%dlB~O%asY%i
zH$}rV-VE+nA-Rx5ZS+tv#%Sua%uB`{pl^pT;v75*o|h2;0uSg&=-@=2yvNir2g!~g
z;Fw&|_!QcC9xT@^ls!z7;<hmEOc|<Ofas`CmNydkTDf$mZ{gHc%@vG+CNNaFtGBUA
ztQhwau#LPPTSjQy3#p}b1ko~ZdMV<&9n?k!56@fm75|-*qOo4=1-rAh7te7w=bZK>
zjbD@~*C*};7)UP}g-ZFVA5TsLW%E^IF&>;c#xM%ALgUpI)Hm#=!VHlvO3LeL!WC!8
zP=L^W2-<H@%&Mk_vgrp>Q5cuwGF?zpc^T&XfL#}*GI1&<4fQ&@>%JbL11p<5w?Xv`
z8!AVSowN)A-RI24px({tw(Xgb-O(ExEK}|dbs7vX$l#@iv(^Oi&*F)m3<qdZCNZR(
zzwT(u{6(TkNO}9=A$Y0^0Odj#{(Rj~_NO(w4CEDchcdTO-uex}=_C><zHAr{`<7?&
zQJJjyL(k6X^6fo{uE?o_ONB|&hJeZ*Kx?BgbOyq>Ku)5ja>1$U=-W;0-(hVO0$SRa
zgyoJA4fR8Qkf)|1prQFR_C#9Z8nsRh51z&0XzZLR3A{|zibcw)dcMJ$Dp}?vqHJ@b
zVlxsQ&hTYm9P=a$0WUthCU9%jD=CwDH25p;PN*n5RfsxWibufT0ko5iVqfBh4%b%2
z8UWP|GOm-@?W$kA7^SwD7TE?dfaWfnUdqa@pyJ@acuxl}ZPi4NHvA1DblIw5X(yfB
zPuZj@Ycsf4LuH5JkPvVID(_-2qcdfcTBFbAD9l5HbKl*j)6`Fp3`3y4ByyKKM48TI
z2=e&Q))U>Zpkl>$NJSW{ZH|B~hI{~CrgTSoyqCmPt$Y?yQtroTTxOJI6#+LGgLKn@
zSAkv+iDe!=oD1$d$KcsWC~F?^)PY%Fa$ShIr`%AHl#dyp)1p}S-e|4vtHO#7ki)%H
znT;$;!~c1rd63OTSZ`8S1CUauWeRCk#j;1m#sFEJp?FD^wW1vL5n2njBp@lu_B3Rm
zy845k<!dOkGiYh)N#=q^@k)N7Q-f|L%Tu{xsMKx{v}}NZw2^~*zq_67NHkPg!N|H2
z7e6K2k2iU8P_|yxM1^z9fNZT#-i&#XQf3}Z%DvfTUJ1c&{aQya@xs)04Z6AFtnlu{
zEoZrZaX&;T=%8!`bcHRiWhra$`v+a`Avq?JUL0xa!;wrMPl5<q)!(C-JSwui^ysBW
zFFh90xTk77Np`;3<tf{4F%`2N_g(SSyFxqqPS)M#M7DpuNT!j)4-I%;Satu%8voqn
z`&Ii6JE#t7*Lc29c1iHKWVeUciCS-5-9IbQLL{0#I+C6h#3BG_-!Zr<+F<+Uw=cB9
zB5dw%(+`3>x2|)Hx-a@$gd9%nat}vY1Bo=A&fvwbm&U$av+**PVS<gSEBX%Nr}Jtx
zQ9BNk-o7cIF>%}qy1Ba^rO6;!Em&->Z2CQveT}-zxYRD7hkGvX85UqX)%jMUyhbTb
z2o>Md`0a1EZ|+b|R9>!VblWzJ_q>Nho+0lh#%vc^_o6+ObCuU7P?4VEPuJqp!)Pm4
zT@an3Y!YjY;ug-WYo!O3W<_~E*%m#DUT~iUuAvCyYYD?}xyz-odxo(}m=>g#ad`-o
zIU^~kIEItmb>BsOgmpnbZO99T8e3kX*$_}H*B&J|JLuq0o@KW=@igjW+CQEeNG=Dy
zeNc72RPij-wQkh5Xs2hpXcnib2ZyMkO8(r3O+a2MWkS@A)gPhRJD&1IK-VL^gv8ZK
zQr7w{@xlQD`gYyJyvB1&cL^MfTFX;e0wFq!KvuRm2wm2F%jQSc72Bv+&NK9FF&jNC
zdI<509B0tDa0Qj;LwUL~cX`1>*+R-(PFg(8Be85D@_K7Opc&!x#F>$3T4Stt^}cp0
zGZ~k1Zn;XT#B$t(Kd;QZhV)9lo5)aK&U>v?U(;Lb6gS+I4-~u0z^cCP@@pAWFj=ls
znE_*J2(jiPbh);MXQPjo5(}~A6tzX;(DS^t5EVOxr0hC)1~-PQK-73;Qgo`Q#B7(!
zJ)ze|L(XYmUAoqGGn(d}MGs9jgul3aDwYJMA>aa};d>BESMjq8C9zC-Ny;}$dD3p5
za_PW$l2W!Z1{JOly%M0wj_@*6XJNdYFet094LvhWSL~|n0rojAt}3GJbL1XvEM6xP
z=b*Pj-7_QWZG}6*`ty2fvK;o%Zh0vH6@_tgGVZj(0Dh*|lTA5kQZed~koVMP5AQ4&
zbWvuvW<2>DGqa!1Q*R`*S0QPUT|bhE4M;X5yAe-AB(q3fO7e1&*(4iB@|<5*M|H2<
zoF=b|#BZ$kZAo@3lIQp>mH)kOtmBg-J4Mp}Cg|1QSl^xOb9`C7e-A3gN%kb!p5#CM
zviihc(PZh{BWd(R(uHqKs(Yr!F#hr<f=nA%o)1Ir;I-gA`VcnvsO7QG3G1@xRJ#O2
z86JGY+v<+&B22LeQ!IjVv$DsS23SG&FN-Gs#gExwg<cHsf94l$5PDbxsLmZCyUfqp
z;J}@OBAKO69uiOg`bXH_O>Mofyf3oTABv>+15|u4lEDv0vhX;P<0F|qA(CZ&pNr9&
zTI&cZYEpDtM7IU-GJP<$hm+XOak1rmhg>zDW-;!N<I>mDPK&;iif~vwm0y&j+oEG5
z@lZHX^_XTfcNASKFbS2eLnA4QFlIw#dR`>0=TU*-yO$?5guK0SU-zv=T|ss1D0-pN
zksiV<tduwjYtxDa(a9C~Lx~LrT2v489J=b23i~V9vNmJ%Q07*s-0nlFEzh;WYTeh|
zBYs;>Yu2R9x>Ys~kQ%KGN(>u+e<TfhEI=5QJ&SIBie~tvcyhxsG$)?Q4Ko9s!Tfbk
z<NKr8Ox=ZYs+9P1V@4!Xwa~IQGs?<r{0L9mL{fe^gC3(hL^3@b$pS`eDl`KXBTGto
zkEBemdsjS7zJo01kA?5#Ni)uir$Iz5J7l+)$lH*O%8WxFjxycMgUMnfZ?9lWWE)hr
zN>^ofy+S*99?`Q8ji<u%3hc^h&7^zQis*_Hk#Q%PM}xAv!f4d?p`k&Yp|)xz+hSz%
zKv}lGs&j>zS36Rr&R#Ux>$tduCryt4jpyLN6H8N~2Sj5RSGUs>L>j&Lxx{j6Oggx>
zto5Qw=-b8Mi;&VW?BT~|nripCy1{c7Q)sq2K>YbonZGa+Z?cA|?X7HHA6B}~j0*Qx
zC90?mEu+8S77gs{>-6%`_H82T{$AFmYsI37!sK%asCVj)%QLy?xneQmKZWac6k4v7
z2obtutquSym`8KVfkPts;>tI4d3T`RXu8QNQ-gMswqx-A(;{hBI`Dl!3-e5^Fq(}u
z<8xU?)fv9@5{0QFL+!h2Tn-RW*PHzu1G#T6yF*!1HuU0wt@-?TGNdhHT+K7PHbp)C
z=uv(q0*MR-i=JaN&$V4Eh_!a~*a?R-Nb904UBH3f78T3<Fe*{1w~A7|E|4sDy>ed}
z#UuAZxGD>?F$YB~s&z%yCX>k4ZbqVKH*Jh|iy2`}dcDx=bsjkyjH{PrTp3rHs=2QN
ztqxajAt2?Pey-)Nyb@QZ^q6AK_&Uk8r?rA&Yn$t*|8i+l?XscW;C?aWdsmKRaBd|1
zsKkp8pL)ouoUwXpR36czUQnHGQDP_N@kVaanW8;vQC;5A-zB)&U`>>=Chca`=3!zN
ztL_s|WejE0E$Vg-RO<lB+DJ1lv#VM`YnlQ9drpg_eAJAAJlOdf)oT{dLY~mdC+ty^
z;j6rF#7nwoL{i?IA%_?HzU1$wad|I}OL^q+^_kml4@v8@c9&x$_V6oc6VNL!JkaiJ
zD*QOF`!HR=%K<}p2M(OS67{=Zp3r)z=*aHA@#HDmOZ<<yc2nVn#x<wJ({YZ$Xr4p8
zgsI<OHeqOL|45u4ll1kI7tYmV==+D0BWcf!#M8T%o_9gn4-ENv29MWnT)k*CP7cu5
z-Hm!_SM2)Upd?y<0i;ff#7|x{kZquctl}e}6)royHb$7DXSx}m2koZ0?Y34_s7^oD
z(^xOEen_n46Ya{+wVLIk!dqQmw%gd-0MB4vuJo)+57g9}7s-F|yH>bLmrSBvZ}Z%^
z_A}YNIrhS(+|IzMx4K`Thm^Zt6i+?fkxccHtU|I{BxS2xotVcqjIzc?k<=$<KN{Be
zQp#RVGMi-MNXoH!Q=VQ)*(M~Lk$nYE)!F%f;@79fc45e!Np>gMm1H!Mu`!Z2lk7p+
zI8S@>v^~jNNG3>jAlaK_m}Eziy(00W_r9QL%!}+Y-?K)$P9`tt`mjt-S0rU|F!bS~
z6Rk)ZwTdh4c4kqlPSq}o9e){RwK~z{0Gb?qbtJ7Vsojd?H6*Vic|FOtku=}PQ=8<q
zBps4BkZet|9SM%q+J>hQl2amS@5IxaNOmLnSDN&9^9Iml8%>U*$$xnj@l2)3<7!Q+
zAzZ$#nwZXC#xduw9Em^Zy#R&Pw+7zg44|;aP$YdStkPBaF<f*tpzGCMSE*_IIn>lb
zO|5n$EBxl|SgoezHPnrUy8o4i#_+X1e69Xv!TPdh<X`#Pag~Pdtu>@_P>EOf<wsia
zoY4&+k0M$qqQ1!*z<rud$;xxa(8d_r@EAD@_vxUL;pIJNOwXyDX7pb=&GOo4Rf=d-
zuF_!A4WNi_6wzVQjpHh{BKlCoKsD+5s!4ZGrHJxl!c4jw*NMb?R-^m`p{i^7d89T*
z(MJFBZZV1?$}b~V9^ud4wU!sZOq@#mRwe$}@^faa79T%oR^pE%ejhQXiTEAFuYX~3
zqzX5q|H=jKuOvS~7nlzL#6N+VGRP$HH|O1MtuQN$&tpoC3wW(LT1OCEV5Ab?xZ$1e
zV5NiMI(VRi;dUeqbTIrJaihD|K_z~xvVI%!`w)K&>yKjnflB;t#BWxH?^K4b#eW9z
zPeAge7~T&wc}YIyT&N7+bQ)muW^HrDuV(;~w~*YA@LXGkf)=4*Y<Zza+muy?Z!T~6
ziCSn3U+xJBk^M+Mg5k%JydTL&kh~wsTb1M;Bp*}qN@#z`Z;U18E!te6we;L58&*`L
zG~hYU#}RP!r6fq%!oB;6dJRPFAZimS`!H=EQg$Qd0JFPMnRWmvM=H~fSFvuH)u1wM
z2LZ<rP|t24pq|}_fZdgV4FnviOxr}t<_?i4<p5G1TT6K|wJS$bW-`%CGb-RX0*)h~
z+lukZj0P>^crg?<>Qvl6mt(f!UZV_Sl<|t)Vb~q5*md0>gWbPZ6Y>6v-7{xI#o!9A
zSIxI3?6xt5oVw$2hrGab8Y?ug!f+L1TkXiUu|jv{X3gy)tDAL?@PwO{@LG9TKOWY^
z0ONWX!su6hl^m_*ovZ$pNMp$}icrrPylEKjyWw6RGEj`-K4UQ6592KuHzqc4ng%?N
zRB9h*Y7EHpo8rkmLlb{d?fr1tfm1_YJ@@5Qjo*mIhjEY+`j`fO&exAayL(+&>Avhm
z;rYL@1KmHmHReU)&dpcY9&h}_Z~5qCU(~i<6v;v+QyU$$5vzsTn+VgwbM$Qg(_(OK
zvXx>RxP=bfLA&nZRIF~{OS5ifs0&cDQgH(ncQZpfxWyQHZdbmsLi8!iPBkxEm0Q%?
z+9piaZqe?E>|o^^&!TYyPrW1CN)a}Q3Po@c8OAM!tLdjx&&M}LVSE(daO2=MN`H2H
z*^Mg?)Dg3C3msTZcF^Q75yTCod&j~qOm>)v#tj{~o``o>Ot$gjQIsvGO*n1Asd^rP
zNu#{`;y$!Hyu4y{;Bh$ZTb|PvVO^cJooX=I#DV+S<r?Nzh#Q_4E=8wJy!gf{Zj|jl
zI_<#e2p-YKcMaSwN<Fw)r()M|ttOpis<Q!;o#khlXB>B%>aQEtT6GjyJ+hixKfMmt
z)Z|?iYxm=}Bg?yOrwX}kxEj$<v2+tlH+9i=JQ)hc@lKZx`5Ug3IF7mH&j8Rsf3Aat
zKkQqt3**aA`-!fo_2y)1ZalRs{@UnIOSe#>`g2D+8`U{`>Hy`$X>UzCRM%gz=GSu!
zssqd(_bO#UtJZ}!6Q`{v;k2>5C;k%s?X_1Vb1P4D7hdl{n$>i1cB{HJ6T6B3xuNQy
zx)%J6z+XMP2b?MRUlaa3xsHy;Q?0}SqmTSmMjn94ODZGR)5Y_u%d9UCiaOC`xrD(J
zJsTL71~GR7b#`O(4mLL!d2Z~3-S+bAx<(lt4f?|beciG4#0E-i!{2np-_ptnCsvM%
zK{oVmYx^smHrWu2z^T*4Q18h`!>eNVIc?Z&w<B@8s$%QyOl|n#Q=&-^+8su*hM-Yw
zzJj?8JQr4+{;lG4Del;)`GYmj-LC8%rbY)N>x2W?*Z9&wVWarp2n*!EPLVV;GHi^Z
zFpmlijO<CNT^*{2G4jY;B5^Nekf~YwN5$I2+R@|+81mQm%G;nBk)7;{#E&O=aB|P@
zFGQ5@2hhY15jA_`shlf@=z&|f&^NKUv%8H5O_bO~4zCFs_^G?YZshRT=VYpdX&_7k
zJvXY@*+HxpV(A-BupXkoQApHL#p}~#1DYV#s55?cJh{noJ!)@G7PT8JomQ<%?E|QN
z#qHqkG$x{ZG&iEVN;Y7~%b$9c)MKx${9s9?>~Wm6ypXOqEpwj*$ZIcm;jNP*yP1=T
zd1Qd{PNuAqaO%d@08n$PVx3WAXjgR(;2TZ!S6@@r)9R^;$+<AOgxK_3Swnx_=+7OV
zVRMwQUY~y(%yU;9`Rl73*t0<$MP16rFuXH<6bEj@pHa%Qfr@pTHw>rsHm_a9`={Zw
z4@+-Qsq>yWl&u_zmmJ0VFzz|tjkVTqV0{k*BZSu`a`>MS#w%lNs~u&d#Mr)SlUkn*
z>KHq&7bAjefM@nGe8Y{k2h{o$qjx@5sd(Fp@p@%9{5Q~ZhlyjTYQtnd@wABxbkMUq
zX^#M7)oLK82CeR`qsfZ@WU)Mx9hj^yGs}|7YEZkyFv|7~{?{s=-&yf|%)U`OxppM;
zh-PIqV5+szxErc*<S5H5{_Mupy?~!E=t3Byo(o!kJF<Of-0MaCrGI%j{BrNGo`W#`
zIE{A&o|s*Kjm7m<iC06l`wSqFr-aihiM%23%OXv@{n*Oe-46CwR`5{clpM!jCa{MK
zMnCp&V>nI(@xxjJ?Rat#QJ8*asJpKfjY0@_13uskb)6>udDrZOZL;I3?>sy&)R$eJ
zH0uqJyYTxe8{J<;m2QmE9E!xfPXn<$Qn(J*2V>uu3=$5V75&)Aov#bH4rk!$ZbyZC
zV0UhAKl+K2u9C$@_41@_h&6!MX`{Ar2-%FCW6RI;e%APU%dU-%sm+6%m5a*_<H-w+
z2EI0cSZxA)d6$6K)fXBQ*lqmPk$7w6nW;@Y^+Y<#JgS{=fN8(N9rt*(^;_mVT}7@I
z0=m;5W?k@f=e5l+>((&qmMg>x*EY`*!=A()o0Z+l$r>pe)nqb!m3V+y9fCp&LwO~o
zFdC{=NmR~J?P%gMF^WWbqShM2T3*o@flDaZKFXXQZVfuHc2mXG#7XoZ;JF^n>Wil~
zA$O#UvY90FDdVk{+wR7T;%Ts(gxbDUctVL~-LJw;i-FFtFGj9+mSy|P1oL=kpm0;P
z%!OJ-&;70|Nx3yZ<DRYD@fdvFHZ~aTtBBQMv=V&v92D!SRv-4<XFHl&GxB>qq1Hy7
zuxQOC---BAZ0QBWqm}B8Aw;%jk>H(firTC~?m*jBEb)e*(WZh(=&7x|JU~jv^;4a0
z^Zb*c`WQ6AoVlWkYjqU34@YwD`>w4Y4xI7jgo_Uum0kVhz#H7MA7WO>kkPD-56M+y
z>v(d%;j9=XDytG#5*^)hkKXi=imOXeiTAPfn%6)t?#%lcO=oo=w_t9-Jt}yz_BzW=
zuNR0TM3C}tOITqX9c-}tXmc_0F>6OMk2&MJS(lx%S0OZ)l^qo~ki(zR^Sza;i*mRQ
z&W5t5@^vfIPC$835m2mh^bu*?RJvlTI)!l$tQ=!+iY5jMH?zLXDLVy3F!VVqZm$&f
z0nF$=)<vlg_i+pB*~R-<4W<5C7|JztbZfe87m4F`5O8d#NL+S&(c=A@t7IR8zZEGR
z+?}fWD+kn}s8GP>D`|o{hYdknk!6~?dFo~zROou^Hvk)MO*LLwwpjywo8jQ&tj_M0
z*N1~P)ATpBo-6U)BRQEGx-wJO_r$)M=|=aiTiv|6Gzhwp>!1>?;7Y7Y+>4H*67Ly&
zvE+JGw()4=7<h3yZwD`b!<+7zN!clp_zt{rQao)&wlhDH(K%FDOZ=PF8_6WxyD4(%
zEsSfyjibWZs0i-$ymXT2EVSEr6@6K^+@zLMD#~{88ui$Ms7A-y1?p{-bJZ#2$*YaL
z9t)IGjW>|ez&H;9FGQmD+L3gz%^O@VlK;KW6;wM9SFiyL<vpfR&-*oBCiZJBq;x~2
z-8@JPu#9<-@Fd*bj%;_eJM^Mkm-2Atr%v5;j4-WS%j#{nf$@F>tS_()L4U(2UIBZu
zare*%rvp`J?#9vU`>SpoeH7i-%j|$}$lJ8T!<^#7%r})?-iE`~C_9A_wgzp2mVWQF
zFCiRXdkHRoA#QdLR(J{gx$bwv(7?+gDcc#$;|+_v=m&;tk1gee3~^1vtD&;cWB5J;
z&-U@;MbGOh!~dLdo$O06e7*B<s*UKZPW#FVWkp2P`eVPY0q#%hQ{~}|;oaQptLePg
zP?Q-<j2mUIOaxY8#wOo_dXDl;;!1Keq>N2hLuwz?bO?*9pI51`R5}|*S!1I}eA8v<
zbDj?q`V2wctlgcR$)b(=vl=fwEwXI}G6bE*K-y#JEXoKs?h5MnTTUk52Dw)B6UbT^
z)Q>*@9s=9Isl2$2nxzu4kpg<pB^ydvf5zdx^!>Dbol!%P)gB(hn>cH$a#jQK3fn;g
zvX`w!LLH>s8sMykL(OP7RH7DMq5-@@cS&7@HVhA5w><_!b7TmW^f6Z@>PE@CQa!@O
zT95EVU*Frh&O8E9r{w8K`89}or|QSW44{V6&GGiuj7TQCcv?G>@xDm<2(N>^k@yQV
z8=_9H;M|9;Z)4`Fn>KeS_4T5%dCYLDJHX6)2sWOn4CUK;FRU6kW?!Y3j{bsu^%4(V
z&e{?B4iNr_@yU80Ti*OsTe=luvHKU7!1{jUA(Yb%S6;yG1?43u+moi+Rk39OXRGUc
zQz9>Kog_~qkxN!NhRXrMc!5~%hnNO!ra|#t#4;$j`&X>HRS9ZegjmgmkqqF8u4SIL
zyi##DQlIi&!TeZ25nnHHwW>Ci^(}V2sjQV#LIDxeHMvA|=9YW;PoSKyA)rCaxn)!?
zv1Du7A*#!r{_IpU+IA``d!lDO`*o1oQc)|<)^j|pv<RC<Z!dM6s|L^`A)pt39|Ai|
zKsB+nDc}f>smaRHi?#G2t@XKzgQp<yK?uyn(&b)?zRplz=}M*Spm0K?#hDN=yj}-^
zPeP#MioJF`d0V^8FD0E4Wp8Fi4dSQm?ntIrA)#y%qIynqemrS6Pa?Gvoyp$9>M!md
zE`ozm?9l}We}RKz>Nr`6;O^Qv=ykcK@J>c5^*e__sNAO}5ut?jp?LCgRIlr<LSOx|
zPK9|k)$80xc%$w<!X0uQ|H_@Tmf4yywj;q`%8Jc0q0N2gU(WU@N1I=XysBwcI6Y2I
znOnn<a^3GIT4*Rmb#}WEd6d3d%TW5PeRVhnAD~-_QjF#`{hcV*#>de35OxLbfnAFJ
z_4L~^+Ijw{UmcaVZp%+UuMIsaxLwfATnN0QEkUM|{Il@#bwxn(`)lr(4{mDGmBjOE
zx7!}h`&#;A5b*N$8<IR1QF_4o2?a{IzVutZ(j++ne&l9>kq~XEU<QvJM-_4%_7C
z`ZAv{pgin(F!5NRGYq$*hnq_8M(WeHubeyB!{d@>^g3EIb_kJvneSBO7(;}&Bf=6~
z-gU5l>v%GFIC08^HIP+S^(w-%<k=ATMXgDwlp`ZPY4q_hT3$C)1Uw_UL)jDpOC_*&
zJgsq3B!kTJ(H@?v1=4r8?n@>yV}mK)u-jJgRBjxWcS7Yg+PX@*9xY<D{p^OJiqkl8
zm@_5NvZj<%B|<yT4OJgRVFt_NpyIBo1n!qbwj6mp*+5VC@wUuKcLzKh;S}{idZ=uD
zSGP63$g{haQ>xMSt~69uXEmh9qGFGw9v{?mStf8+8JE<?kkVRvRHVv}N6m?fTe~9h
z#><yEg)2npyeSf|{^|u|2sv(q%4!(ETj%=KWsji%7jR$f)ubP~b!+9}!uaB5w)QJ`
z$VQ7V!Kqu4ix}fV4CThv%iFR`+d9hXh+eKwaO#DX!hBbGDSeMYz;bm(x3S7H>Z)vo
zSWVq?y{#JDtAjqhWNy^DHE?zIEH0Hk{6!J`*js}=h-y-f#gKAM`F5r85q5ok9k^by
zW>f2>j(>&WC9hJJ(+O*>%IQYHRUvBN9--TS{8g*H3yT+xUi=z{WytNSdwFlw+H@PJ
zy1BJ-6>_~a#+LbuuJk7F!*opVktFqLI8a0Vc%mC{!=^Ld!Rt-x(>TpEJz5OjsiLhO
zVYmAHp{Pz5_kViW5~h^{)uG-iV58J9ep+6)5?|bx)SvTkuD*hCeR-fQ4XQ=YIy&=Q
zUHR6?knwLQ+bDmnHiCfB!gmA)3hy!1i(zR|C1sfvOam7$&ne~d0;eepWW8}^@cIK`
zb-Z$!{U?K$+eUOS!VXk=a^R<4jO}h`ig{q%$?o>oaMSOBg9$sqmT8EfI<yLj3q8?R
z5xNjz0iu>U<4kr)Oy(q32MXhx5VfYp7ggTrYCi!LH+*g;$_*8w_7j`zW^8(}Vpj*T
zn+{U0+u&~i`_><(P(!_Nx-rV>W*}cH+RgKuI{@zr#w*hrhFx|%8=ObqQTmpTTA*DO
z>+)F>sN<<T*Bd@b=^!2e-3oaV(@lKtL17&>Xx`VAoiD<;EgQy94HOR{$JaG7yc{x%
zih6$Fou2)gc#eI2Jy5oEga-{TDHrwoAvGWsj90!302{SlJT7TGJ*Rwz4~9Af8CP}}
z=1){y-3`5R?a#X9A$`P^j#H7Rg!0nX$%K^Z-hETOf%K#82HG1co6lnmUDPizxX&^4
zIm_I>mG4r(+9l}f@rtXZYe&{EgZsfnf5pSk#!t+PiViN{J}{DnYeq7?UnIjbA}L=A
zqrz`zKD1UmH6hiZov(zv6@T7GBRjkx62EliuZ+y4te?JqiOZiaUAk{%r#Z1q?-R-R
zIwYq?;txjnS0es6PWf$?c~Rj%=t`<FlPqI&Ao>uC&FDh1_|R+^dW!p;22A<`BW*<R
zzjZoY@iY$Og$mu8bo0B1^XHJ|U6$_yCSk1ur(IO|!)yce_1Ct_e>>544xzwBv;3$V
z{Q0%M2Ks9-co*aPaa6zJrS4DNKg!hF02(U4O-kSLyFyU*H_7)xnAZGA)T!V7dK|{R
z0P}myO74GI`Qu>CM%4QCoJpVH-htX$70TzM>=Co4Q;V(|G#)`f|1ncPn8iSTRZWi=
zB<n7yhj#9LCMa|Ms(WR#ujfn4rqMz1AuK%sLrc-KpDvQMXK}p^5#KKxF2S^2ACE-y
z=sC(Zf+GCkkU@BsUA<`$)@tKYe$`JO(tXM|7@@ouM5pl=dG=S}TOWyR109T@m&Z`a
zjTrRDsJ)-j<j<d~pJqsZD%WeOZgkLwvPW)jvgSfS<vfIxes|N++B9BBncvRzr&)&?
zcNh;-81K3K*@RJ<MEueO*8G<92n6n-R+aeU@qS{frECm7sT^{OO0=rKA}!Afvvgim
zH1WD7^Qf`BUfPW7C;SE=<<Fb@*I?xrg;7bn5sB<N#Rs|~>kM^Td7GzWy1!o=b*h&I
zsuL#t!I$nC@ia(ZchaXJ*4=oMKaDh@W5R)d{C8h9U7R1z1^+%!8@U~PfX0_Weir<>
z@mJpRwPxt~neADa!r(DY<MN$WbTEi^eRDe@r}H8^iDT-7r7@Is;JsjI08&#kBbkdM
zEz=HO{9*065Iq(n>lS`-)Zf-D(U!gj6ep}d>+j(HM&lseCbcC4T?;9ldH~gxZwStd
zC;e{;svE(0>Rx)<?Ux8MU?79au`uow`WPD5??<swH|>UD$Xb7K;z25!5cSK)<!4is
z4b^FYr{oICgY@;@wgfzfWJ*p_`;2nXcw}AL!A5IdB)(E>o=mp#$tI+RF^Zq;nv3!L
z1mZ1>dm)^<R-6Fkr9?BoIjJU(g+F8GQmFW`bN|;!#g1x0x@`<EO?YXmP8sSV>-3{Z
z`S(c%_cs>QePdo^^>M%RxGub?Y=fo0Znu!W{y(RGP3Lc5`xh4rbvP8^)b$@9=hIjp
zm%KlCb%KEwIl3^wzVJLoyG8IkfmkCn@rOeFaMg`aeu%`$DJj>5mrypFZaW~L<b5Nd
zef>WSS0?{@W<HuM!L71UH!2xr@Lin?{qf{HFHx^@23=(?+K_-2bm=I|1nmrw12ie>
zRL=-FQu+7|1g&lal*#+3^$$9xVaRVemvQw;g0zwK0t8%w8Ou6TYY6e5g1iFGM!+&S
z^o-9PWNXcf#K}HD-|}03&S|VatSKO+%kQPl>2^3SI$cdV<B<ES9jTq7hkv5tLgmV|
z7me#5&YXnt5|uHJgFF3PsBC9cIZ0A#!@=oRenA;_^$3&li)e)PaotSvP<GohO=Eux
zQ;%H)y|XG|{Qn9!EJcZ$2gw)HZYwx&Ci{cgE=9#25LH9|M@u(r{4nA_1_7P$X$bfa
zjU$v9z1=CO`!33yrX|L*Zz{hJ0+nu5ynr!m+=h*A7kbL{k{2;QT<J{o2p4N3wZYfT
ztos8U$@$@?*L6$Px%2S%|NJdI|10WQ7<cA9!-i!XZTQ>Z9klDe1}uij@+UO-uD`SB
z|K5zkpR2u0J_D)y;NIUVE;E_F_cN{fXgpVqBd`A%){p-=yPu-lqY&^j`O9pM>HB=T
z4b#Mb0aCW|huE?<Cn|IczaHV-GU&S|`x6N+<>jB-7<?83wVkFYRwcSbcjiU5Y^aQh
zw3|UYYhTDfdpS*+h=X`tqbm}7l%J@in?b?1eQktxp)Wt}2)kQBWdZ_|^ew;MifpqP
z$f@gk?xwB$Uh}?D;o|D%X@V*4#MX1lh7Y^>NjBrNLOTNn8S*iugvunY=Wf_&G3z;^
zg-X<O8tA@Jty>NEQb;t865owcT$6{Z8LuDc<1SAhGJGi(Y%D=?cVA7^<aWyq%XED}
z!0Vg_*D?c!)D?OzTL{J-Vl)XDH||As?r7DsDp^EX8{_FTiy6%$&!g~SFxnRKU6m<j
zGe#48jLnP0@4rnVYCW#MyH#)6+{rA_C##)wS@$t*7xX-hwOGcwF^}s}J!?IxE@egy
zyC@M=`~krgrmpLZ6X!|9pN5{Vg!~tgr;zd)#X^_?lrx6B6L968#${2ljJt!_t^6Sg
z+&=^Xk6-SyjS;05Qsv~J8I88cF@#<m!#(<Z31HvyUjc*>-P>Ot@n2K=>9MOZo54q_
zt>iNQZ=m53VTtCR7f=3H;>+plDWQ+SHMdcqoRpZ2Wv7L5+<y3varO515-2-=?}xQf
zj9lg_;t!F`hI=<z18~rp7s-5_f5*zl`)RT_wZ}lEtgF--S&LIc%H^}@9>ymcBvjml
z62~B*Jq*M1sqlwZdugoh-Ko0E7&qF|qy8E65_4c}JG3zffitkaTM`dROEeVhx_m16
z%1ki-SJ40YaqwAm^R4O-jeFprkEiQ&GsgAr)?93yO#h3uw~w*&IuAQ>ukMhe5hct`
z=Eft31}=yC7_q7)f0D7N_<hNHWqsK49ec*2Ei$4)$+Toea&6O+EK<6>l<d(cmDM0Y
zo9-5!&Ke6Y4LC*7MFTmlv<n1i`cKnE2?M(*Y-B%*ZLm#&u3;c)qJ4h92Yt`w1s%KT
z{&R5Nuk-PIKhJs2d7X#a-g!tH+D(7elqyEAv!c+K?$=MoJw2cw1275GWgWvY-+{6R
zz|>>@E;7jXNG}ZBGkQ|V!6Y7d{|>WM;4L!2#0L;k=RIwE5OA7IJkYsNyJ!J;TKB(T
zq>exvc�mddChZ|At5NX%5o)I)#Evr%U(xzhk)1Qpo!@rE6(ZxrK@Wc?`Pvu)VfT
z*=Vr}7|_3`sWa4mXpQ0r0Aj++MnAN@r?9WQ=an?e`1W`CO!D!UuWjUg{#mZ689|e0
z(sEM@@a_GA9mSHtF1T?<d~HB_sXF*f+ws&jQCr~hUi6XLS=DhpsE`MLB$qJ)5vtv(
zsHz$6o%EXk0~Hd(Huv&}TYNi&dnEoFc;G8zx||nQux<@)7$LCp6`oG`)&W(#_c7+C
znn+8~-WapPc!-xpvW{JbxM*23DvxdB<pD5IJ4@}7?QvjmU~d!xR$|pjdLP4(qr{QL
z8uzvtkTddc1zlj}&#PP+U16m9(;9buULN=bMe-4VSP<d>$LZ%0S%`X(oR^?c7z~^*
zpKUX(5diZQLE_BwI7<3hXF!@Mf^*>)<8th0H1IU9D9E;WutOh)2}UQQW~`vz8#OWQ
z5z%9yYXHKqYyO~!_EJ63=-5fqo>=v_dm^fHGVW=X$wE%w8Tn729<VPifN_Bl?TMKw
zX-ms(9^61j<7VKB|HYGWuR<1A4x9-}{2jr>XF9>Eca>`f0+@o<;h+Jh2XZaYigHL5
zm~xkw*I+eGI$fSFOf0ri)&cgeP4S*&=6{DZkHkmfjREzU$Gp=50k%?~j9Lz@8Mwh^
ztrkbBqTfCh@AV&#qIH_$F^bPqJW25c#Zwetr1&+8rzyTn@f^k1qsVg`U*hsS#h8MD
zw{7KlE|KV(IQo~;n(1-XzIgCgJs$Z3wETVw5|2mZk~qPe4o1!Nc@xpf8Bu0bY*g`d
z=I|p|Z>IJ=G5W0`t=vO#l;UGiOdXF5?Eh=DWE-6)DtfSPD#M20bV5c@VlBVH&NZ4u
zMtqX-X)=w$Zx^=G{g*&~5Au-_wTrJ+T+>PqnWHoMRy%4V+EXLGkIBeUlDA{|W5$vv
z@*%VRV^aPO38GeOY|*G;S3O}}Zk-iJr8?ebXuiDqXw>Aq9c8EAGLN!+;cs-spog<N
z3?nfEAf)3YdCS<^-Y$69K<k-!#ovKk?FT3M{||aIG?{-EVbb5D<-v_n=)LEkMIhF0
z)85bUdy&bD@r5Ck5+(T#@H0}rY4h}U^s7;{ze2(E*Um<<<F^d0uX!Zcl}&u{UA~z4
zTT4y`iV%Yddm4C)FLEqkTCZ*C$}TH<JHS&d)%P(=LG1rYwwedGJ7xq*n&PDuUh#;)
zCB9Ypr$DCjL_Aa2!{te4*+RI<`{$XK!BKU0A2=)U6o;xs|1yyU&~U+M5jyp#IU^H^
zDCk~d)r^0OE;ksnhdi!b6Ez(`?YiN~xV#Bw+IUYCThMc-6NNEB^I8tf#pOkYyRnA{
z3tV1HaeWl&p_xp9yxQ1LaZ?nno2ji*tWexSaT~<}ioX^`>sD$_R@m%0fq?VP_wmXH
zqWJ&wus|r{A0DP<1sOuYErBW0KEdS!6!%j+NWmg{*_XQUX)ZrWu}<+RilY>tp&*|o
zW#TbvgD5g10L-|)uM02`$%q?0ksqIon!b2PRP2gN4G|AH>kDF`3ryFpgsJBvbp_|`
z@l@yWcitN{w`Gj3CiH(fo(?Ns{w3b4qcFAPYVv-&0n_q*@t&C%o(X8?k6?1=<P4bH
zk7L@xRlzhLO_|enRaiX~PYoEjGT`dcCa{v}G(?twkf#I6rkNuG5e@;9nzMH$n9M~i
zJAsv_Zwe;J%x$GzVAcCD4SXaD4~ub+_MqajftANP%)&}S5>|l8Jz|2%gAoLio6Bdw
zG=!Q=$-OF=ej+tB5Hj$gTGZ-$6n3tlq5j?|4C*X49J&N>O#dET37j?fas&;n-@z*o
zlId9M07oNVYO1fKCKGb%kH#zRP*|&?W|EjBHM%=43z&LWVY)(1h3Rs0AWY}^5xGqe
zFquj236t)<DNN@kQ8RRZJi~N?36ZPW-2yHvC!#1I`OAfto1bQd77%g=lzRoWJl@}k
zs<PS3BRSpVqq*hzNFK>Kfc@sGD9m*cR#)Awd?iVlZYN0%5m0WNH}A`1EN1s4A&iEQ
zk7-e+>l9&R&WVfdx+uC7XRe8t$8s3}`znOgZ3szyG(`NX&-0ioX$UQ(ef2>Y$F#B*
zh#wq^qNHZ)#Qn#HKS~;ytYLm^51}e*=Z;2AQQG@h)ZV-jt42D^GW$2^w`wkmOezGm
zWoWKf9AH!?&k5oYqpF}pSFp-i7G@W@Cpfc<pg%K&))DZTX9PG+g1#C!&uE-5AR`^R
zuR^VCC2;{@1r{<v)NNmqKn=h;49%3jqM7EAC3(mwIQM)N6N?ssl}AvS=(^F!Q?t>^
zK^5wr0<d>L@<b$;9LNiC?@k1sV=)a(<Q2nz040tXOTk<2I{{4cWp3VS@MfCYAA`50
zMg>ZBF4A;C*;F1Lc<Yt{X)Yj)<Sh@z(lIG#<OwEIhI<Tck)h4zhcPtIK{LlIrNxyP
zG$(piJO6xqHa;1}Djc8^7MB&M&DaPvV+sZ(^xjJ<gTWAu&u_Qlz4RQYnYyvAM@j=5
zpdx=ww;Z*1PDY_IR=*sz)~iwIZD)cM5Hgr4`)z!KSKvG$^cRfT2=l*sEo!|<6r!wa
zM-RuP%Him#sLlKvp?5m&{S*%uTO|#Decdm`<<!qTuz{yc@vp_@rDrKn(VM%YXgwN5
z|GFr4UWlUnx8>5=xV&_Z;zf$BDC%!TF`RLmnX0z3%V#dR_zSEKQ%w(zsU~`Wm2SP(
zgGo+h<<{lBb^!#A*v4ag@g&6)6i-onk%F18Jrl*&)n<M&9^7>0^v4iQtH3~9?u~mt
z-i)FGBx9g)NrVE8h3BI%VBlf-Q_uhqa|ICMe9X)HqtLL{$<&fDw@0}ARzm`wPu+#t
z&EgvS+0A^56<MuNyM-Fce+hnDx;=_HjG_(-&Cr2r%&38T1?#)w!3G*^ql!Yw+Nz_L
z2jm{%!C{JF6h>yZVNSP_c+86yPj#i~-1|(t;s!d+!IQ`hwN>lhsF^lB08tXs0p_8F
z=M>y+qMr6Vln$fUkn_~j7IMw>`MQdl>S>ALYQt5CT=(saU?IbcN;r@HOOpgKq9NkZ
zYwjRZmUI}imWRp14)+AZH8Z{L@EMq%?2Do}KbdR}oYeuE2F72iihJ0Y731sP9fih|
z*|?_syT+$Vn2~iO2B}XXV9nJXF+O#uDexU&>Sj5ekT;=h$2ZUT79j?Awr95ghq(u)
zhhSRYKrrbf<>ub|<3URPqrCF5C@#~7$=4$Qk-5q6rC#dDE~R^jutk?5&whoAXPmob
zyZ1ppYgDDA=XUuK18M=L4q(#M@V}*k$z7|ubE46cc(u4aj7q#Y(u$F~nP7vFnh9Nw
zk`@}H+i1Sw56|!H-VyiGRfWuS8rB)4`_+cfnp+44AEm}f3ql3rA_S*i9l<0fH!r~-
z9l%-6n)Iwc_nLbQlIBMXgpSZ)65QcI)EvFT^+-vxlr>k~Pb?D)P8q^24th3*{>lbU
zPnH=nG&h{5`EgHNYF=t_t3x$42GSMi_TYi4;m|i9jeG4?9yF*OAXU0h`vwAK5vY0S
zk}N4M6(hwE7NMF#o=_r~%%Lxv`Fc`2ROfDOO^roKkFhN6iNX<h)`olR?6;i2P^>RG
zZ?TFm)+8yI&d$3>s3F6&Ej-0i9O<2Cu6hiTN11ybXvypsQ8_Ab<c>(0&NVqBi=%7e
zfvI%{zu#=sgu;CCa6HwqbD6yJ6qi@iLg=2UUuvC=%luhQ(HdjEy%2>Vsj9`L1>88L
z92n4iJBsQ(QA~Dmd3_X{`=aRVk7DC0szax`s1995j}@6w9X(j%YN|u$CshaZC)kVr
za2>Yj4+ZiPoMK>R2ptsByoitJP8#ts6PPL5<v&3xifeaD{rI7{JoEJ^`Y^jEHMF49
z3X1iN=?Zq1lihHsBOxcduY6tpg<aGdBgQv)Fbb(;aTK-UuMVJy33Tf2z1$uPyLgzP
zheFw<!ncSFg`0foAxYf_;tR>qUkt3T#iiok(*YI#Y9?jfm*`SO>W-yrw6kGT<%Ucm
z-sI5Yw05d^MO9R$>OKtpQ1`Cu0w^PtQudp~kCbw=gKEbO6}8MszY5hJqSPSZ)VV1N
z(28$i#1<4$!-zS3K<xD0Ze}!Pk(sOW^gk3(I=t6|WcyVW4M4KTaVQGbKNOc!iy&7@
zYI{>O_kgqKOymX}td_DLtZpQsduo(~gGRNvt==6dp15N$Nxh|mrR7mEw>cDn`y~^w
z5VhW&1ndSYjiNh2<GTeSlQ|FnR&ad(90hEuVJLJxo67C{?!jtR@l+Q|n95*ok9DU?
z4~lg=mw_m~%5->{pvQ9;Q9nQw7($YKirP-Y3&3h~DbZ7ag0}&E<jJ5%qo(mBOG+jC
zz?M6{<ce!0(HR*g#iCB+%iLN9fv6!gk2Fp^S<vGs#hWU%$OxHa4eAu8Y7Iq!$Wut`
z$+^a|su*65mpedLi#xLt5un=CZlmy*WD~G3VUPpZSxxL2A4><~QZNmXDm|o1>tGbx
zrxKxIG;Qi$6%RC06^t6G#TS<_2Ph>4V;{g85Gt0ce;i0l^^@_`L*YxH%z%*QV2NP-
zIVAN6Bz1ymxkA~WkD7;?`)jMkB_5~)m|%|?X{vdGsQc6%uA0-`yaR^@b9Ru*YDt+>
zbpI{TUBGTK&-^WMuPdx>kJ=g#(qL&}h6<8m@X0u7s3T}SUfRPJ7vWF|SYcF7bCI}?
zfSDd0sj@uRUSd@<X$y3!c&gmh9<rm<M~DK2xsoPj9fTzB&jCdDmc9W??m!GXz$%HR
z0ho#<4=hI388ywQOU)%xZylm`Zn>)#{o#xh1=@d#^<Myp8rycR*U^83m){eGR{aQ+
zd2&@Q8DP<-lLUy(KUfjE6_*%5Ivk4JA!135qpzVZ)7&2cVsYuIC3E5u@@X+&1MHfd
z6&Osbin*0N&^>5FRjCruJeOjNm76jL8*_xzlQ|_bDZ&V|KBtIEo0{P&Ze$3RlEWR)
z$}R?}x$i-Z3K>t9R))1;Nr$U^UJ?O_W}|~jWv8!n)Xa1G4pIqX%SN}_09Ni+KrI1X
z%}cEZ33!g?GqeWnN=T}#_LYN_n(kbVSCm5;u?C^yGQHkM<8uDSD5}#@w01=?hW^^n
zUwdB^Ly*#e?mZoA4&om*63LS&OGZGA9$c%UC~ix!%^Y(JIJ<#e5u`J!Z?6dQ4Bb&<
z`zURP(2H^?b(u=Dk-$X`8Hc+JA^q8fC|1=03_SfU6pNeCm6H-{>M{^zKv#Bg!>UJj
zmZ+q3rO%|>C&RA8u*&<7ccRt;R@w)eBzblv;@XZ%NYsvu$9g1~y-61Kp;jwC$sQ1@
zm|Lbd5t=JlC*m@X+y|i&p{sbgSSKSQ3ex>Y-n&kc{s;DfbJNO|5<MGSZK`D0q+eZq
z;}IJZngXyx=>Q$7XDB#ZRa`p3)Gjb*hgHz@w~j_ltzpY!xbYz-Q`24>2cgYmaz{Or
zQQ#R?()^IO6ts%Db*8m;C<;wy?buY&OlyhWJQ2Luj!HP)T&K{&zH*^Tmpj8J0BjB+
zBe5zETzJr`Qe0=m2C8%0^^FZ?iaf~Q#7l56iT=JeO%Fk)not$VBr`HwYfc=w<62`e
z9e1S9A2#?%6A{q$Vcj+W6Z;tlf#9_MJ^4!)Ky513jlhgtmW1ALnTjuI9Nu!HvEJu=
z({GvO5`=F2PYXpHX%c%7oxux5vtCw`HwwrlCQrL8fdCI;k}Q;e$E65}VZhwXzc<5W
z+b#jD_K>@~GV;_2Euk_0LcFrT%Uxdfv{jEvEAhz@;#H(9DYFp@5|^PNCBvZMo*ra2
zIKmeQE$!W0<VTg~1OcM6tg%((<TciNWGn5&8Vo9>dJzAQ8@X{=0fkCcK>05<sYJXf
z^o?bt*aUehrBotNq7umi@0S4G9K@iVs6!+wx>ej$)9G_+jkxNO4j?>Jq>q};K`t^#
z4}H+$(Yq^gA9eBokf`5u0d%W8*waBe{R(Ol$#SE=K~$=YX<m=&5}2L{2$9SbioIc6
zPf?Hz3m?g_9@$fbYL~I7yOVn$I9W)Ap<HuSOD9A})`;+!&ik?uSXe`@^<beA-cjMo
zoJj{q<7x3abPr_>>D2gmtpNCNyeALN7@yi3svY&9@G}%4(}_^ExI*jq0aNRmDEe(`
zt5Kw{i|J>}8qA&^B8h5grDqKc^a4}BH}rD;N}hH}DAPkgfC2Gza~n%KO-|TFr9MCc
zYh(J$8|RVz-cJLNluorKdyVck<Si;hfW&1a;bqrE0GP@vkFvysP-!NyLie5nq_H?)
z^n(3ffdk!l9;T3nXsX7qHDL9RFl^G8opA-3s;6kR`~vJ7#?XC`4l<CejDR9CnvAqC
zz9E5>kg_Qrtty^s|E<^508_DeG9-Fm+%r$Gtt>`MDv;+8BigaXN_E`DTge9U=rE7=
z`93p$mYM&`t|-R(2GDNCCn3Eei}gvWD(;o|<OsP|90Vo5%xR=US<(kC8P30jm+3>R
zRFz-iof;Tv<uVx!j1hk02q7rS)Fj$X@_xI)+rJZK3|B8EEk@yDa7x((zv*auQi$45
zD=3Ww;H+Dqeyf3Qnm9^jhEdL<Rs^YM28_U2x+B+g<AGyY<qPKPYLKhz+Vbj9F=$-C
zs8vyI={M4wmV&1E5`9#VnWl4^Gjf!Of~3+ng_G-sqY5V*(-%i+^rA%Wq*0BSi{gW(
zln3hHjMJdJ(tg3}GZR6U^zV8)w_4IFrE(yWENMc7E1tR9f+jY(B@GO^bZw_(9Vme%
z0d0Bhzl<@0!Tcu!aMAmep68H5QpB(8mqEqFF%hfkc%_OG-DCwL<+@8^Uyjeuu;yA2
ztE(+;$ZI{vy>n5x9-7Njpw{;c=X(~qUQAh)$pfU}c-JponvvN(?dYmYGZl>j@gbC`
zY)@z$iA%#uvPMtBl(O%S2mZQDq?i-vB25o=siAgPGS>l<bpI9tPqKXp?0eefnMT&R
zzLl0LDsT=SRG;Tha%eAkW)BbiY(F)aP64&lXikmKjB_=NBhP#sM4geI!PYxKG7$y+
zO_3f_B^_v{D)t?Vrn92J_7<3ti4z*-DuOv0)m}61m_i1?)&SP<N%5jJ^7V>rKwmtU
z-k<}cWiDQ&-a1Ijc+n{d;R=5@RO|z-G>5j22`&hxJ9zK*DC#PTRI|WpKm)~V%P0=a
zsf8yC<)ZTIaj&e0x>P2C$kPga*IdQEV3N-}H^gUS+7ssrOKla6B8_}?Pq8hWR;S_%
z5Kxvr(wk5WO9!8cr^fV+sEYtiq4bI*3!~?N-H|HmqzLT@nOI#R|Ee&wePqctO1LB{
zjeuYNG5FD_jbDx;f6|FeY=J0Ow99&<7M^%Z%N!9|79C5deXij*7aird9{lFab=b7g
zUEA^JmILV7<VZs=8WWjn3!LwQ9HpiYLgPZ?ElHqZlC;;P9GMu+l-O#dnI_2*8t}KH
zDiMtsod!jpzJ1Y%Z@d~W=Qb#yp`Ri%4d^#X1%#HP;RbNGPPN{<O=&UqA=@#MP-UY4
zRk%5DSyDPxs+5TJn9ov*!pTLE_L!<#Ty*Q9sWjzP^=0|2tV5&)Dt{;d)Z&m{iPhVb
z2Ck;Y@D`qe&>p;H%4<e?F&phXCkT$T9)6#Zy8fFB9y(&9xF?8RIw~0zW$EY_%2W?F
z4am-nRD;<Qy<7;=W-y45_BitI<Dk}^j&ip!sGw}=zVjrle34=~iutccu?k1I%+V6R
zx5cIWTUNA3(74pDvXQaZ;XyU5{}S4$;?h`w@p1Fl6h^LXuQLT>X+^52@^gV(xIp|u
zQj%APdK%~NHgxaN#XkZ<nFP(iC8Siss4OEaN<BqEE|oPp>JNdC4X9joLDQecu(1`>
zDx_V<GJx}j4OZvrd>LX$!xeVUh@4Ev%Z4J<Wy;uZGsqOQ6*W52N|P*1EZ5GF?p34*
zn)xdA6k2X?QaMQcqlR{MWSwsrZoe6>GC~NYo>5TXwrIktj-796V!$LJIR(YKVl&Up
zg9K#p3L`;3%6wM+)pGz=Q&ML+SG5qJf><5kx>X8kSj~=p4q}y8^FUExr3)`z6`^2n
zo`Gn#h$77-=!{9zO6Nipn~FoISo_a~W-S1HrW(rDe{)nbxf=eV)~KYbRR$6CT#bS)
zRq?IlJ*cB579{V?o`AfyCp|6H6p94SgYiHMs)oiD88ayqQhN>Tr8Up9)O+$&%rv8z
zQA^9{Y}Dqp7#W5T(wI$kqS5x5cp!9tel=>YC1^n<IH6b#K111RPQhucKz-RSeF#o$
z3#mPc=1^RwE3NQFEE)V(u8EYJAqEX~la0oX9>YexU)V(fkg;nhLgG(5;<>oY`NC?N
zM<W==P>zn1LNM2;_%>AxbT9oKY3M+|))>X7pb|~6k2g;Ze#?#NzyLp?q5JP=yZOxY
zE-e5Tee^F!FjM9P0@EaKp`q3V-5MgcmkTIGJTxQF@JgdiEyTB`5kF49^8TI8%S+2~
zZ|gV@DAp-1Bil1Rgkj@R0D494iOb!4u!cv;cqoe6z{{F%54`@LKQqFzNh>`bq@mQ*
zJQy&WCvGudEC$6)Wo@w>*Wun~Q(NWoz|TjqP0eq+)cnc4o47{2ym(C%CgiwP$UTp(
zy>ZWDpVk?{MW*m4uSB8$)}BPoS-Kn0jNi+AvBrDu;Se~UhqC$ExHq~niq7Lvw7x)b
zn&L5v&r_%tpP=>>g$j~UO_*x+OcYaDvF*5Ad^!qs=sRDg_SGmdJ*msn=eW0<;zSf{
z&r>@aMalhL7ppVT9!76wYi@@FR>x?-^-I&gYwoqTqOkKF05QiscV{m8z%QVU4Ia3o
z-rd#0#bc>9dC>nag2u1K7gJk*X<)fbl`dP(@+vb*8w5I7aa6p#&gIU#q8ROpV*C0i
zJhr3HE8iD%k{;CAIu*r#)o`Dq6^xy$?o_6a&U_T@8z>|YiNQ(x?OZ(Yi1Plm)Xqm?
z<ZfOd0u2?AYhFes+<n_R$;*JreAXQ%J<rQ-%pAWGwIg8A(=R+NpyIODMfqIR#z4bk
zkcRK(0pETSxQxX#^x^>uqXyl}ug1Lr4?Kg!2JVz1XxX8SU<i4pq7_f;Y4Mp`sH8PF
zoV#t^FDlryqVpe#X(bJ<$nmf6bbW6W1D-z0_%>;|60nTLjjira&P~cRV8*_~@YD!*
z^Y&?UL`Qv^s+|YTD{kBYUo%xV9sGvV;;FfVYg~3WxnweP)aC%9dwwLiRbC#_{1P>V
zmNL<EvXq!&P;70}(b}0POf*~odATpuZyO=l@;&Z(=!4?5|6*MF#U7s43~jBs*ZPgP
zYze=ndFLypjnHq2{Tum=$q1$-5QZ@ZE_uFy6QrqxP<g<;b*6f+K%^-TK^X6i+P-f@
zQ86m>>g7<+^Kc7>AQ21Seows8<_nMOn)-ba4P?adm!jsHO-7Usb2+4dyxmD7g=HXF
zC3k;3&}QxF@E+x53?MClcV3H{QCNN5vS&rz?Vg&<TWZvznP=L1vSc5{T+)7BC><T*
z-Vus2zI(W2K*qBTvy|U)Z6FHzWmLy_55vX*J){#NjsWrP$D>$Wh{DYdCAwfZ)%bzV
zl*?yJ?~V6dY1KV6V6c8y+|!R5R8i9**N-ZpgR72Sa{(YuZT!K?D7gmaJpD-Ccg&E*
z;#?HDjZ7e}#${$I-bU@#C|rKj@Y2{Z1e~iLjI{j2ZFU)<fpbZ!kq(UU2xpDO%+tDZ
zr2cf#{aczn3Zo=A#-{~8V0^~S%xcnt7o1B6<AHX&Cri&UNPq3D-JT1jHQ(MHMXwp8
zLB%9ii2-mrzR^#o$=p~7;?=(=5deh*It@(qG=JichuREW=t>9rJq5Z(9Hav+_$~v_
zgfOIlR=Ci03L5meH`38x30?o%liSju*q<vTurDs%V`7j)5Sv#&gbvpJIvw2+g;D3>
zom@h|Ys8<G&;1W@@BI`XjKXy2Ii~d!|7fyN#Zz-Mo!bzkr0{@4{p*4=GJPtEh8Y<R
z@cfOxe~d5gjbe&kK!rkZK}Ng~mwlPN!KzeeD;`WPJ11P=kCZR)N2Os?AVgE&VvtH;
zBS8MaFN4OwS+C8wkB8g8M0+~likr-$VG5>Xcoh6aNB<D-K{+bdAA*@a#TLciq4-xX
zN1^z?#4vWa=W)E9HV=5JjhWVn_XLDBUe+#Ipr*yxITf|ErHc9c1Wz@3HD;gZ@+AtI
za=%cXFvO*{fYyz`*7aC95f8L6YgE45*E|bE$-kpS2PB&8d3poG$o+Xt|0mny@8+{&
zl%sWv?cFN2H;Q!OYf)@IOYvM3tq#R*3dT3OE{drQd8p$p%#>V_b-w50vNfh?=J7kQ
z+V@_GB1b<TwLW9^%!@pf2St$k?l5fXUyPy+tTe!1DpM^Ge=?rt4m`nr4PP*~HSktB
zt&>sf;8)~vA<$&a1arrsh~FERx!aBjZa7t}FE=2ryb!gr<xaPL>s;K+eG}h`+OSL-
zeLgdDkC*u^rk^LYgTcA4N6}x2LZM|GMApsB243E7M5*Bf1I}m8#)Ao+Px#iglgu+3
za>-|_u&GMeNS1#G*%`#WJK+>pL6>NyO)G6$F^ZF^R~O=P6#-uzibAF{BH|ME!hO7K
z^XPLIP%a&dnqih}(J2`QN3OY*N&Ty|H6}e^MrL$p#Pw)g8euWgW~5_dSKJ#HZ7PJ&
zLJDptIH48ucMX*oH_N~;6Es-ZmTdR%nN2Y?SI#m*aF_?QlF^S(amiS^pNYZ%juzBE
zVs3`-6p^;vf0b7@DMY|6Q8V^nEH~5R94`1U7?Q$`@V0RfBY}!Y|LB>Y@r3|$mWI^}
z28STey;_+h!RQt1MzPoKk4phDYLGH^;K;-e9gkucT&BzOeFx4x?RF*798Yu85Dz@{
zMG0qG_u{+ap04E>*Rl_V?Z_XexO_Z{Dfj;gE{pt=EBL}#hHjACV>^v3F(}c8g;IFz
zrvU()2H%0GC5=N;X7L+1S0j!-!c#!E_)!Y%=d#&GOSPy#-4;_%wV+JJbnCuJU9)>{
zumlY{75nNdz_bS{isAa{GT-HRFzN7_RChLN8D4xPYKuksG;<TlEx8s-6x|Y`(M#iI
z_V$MmwZXWu0%IJwJ`@j(xs_!Yc}rq2<edR$x>Twd{mtz!OwhSy>M3*ci#Z0qgJjsb
zH45|QG?a3<FiF`ZB*V%T89h1?PtDR(M5dIYmCWrHQH)u0YskzyBMQc%Jn;WV%DM?~
zws|9lmGOgvanJ34Y1KE=my>g0{bTWpvSb9*dT>CC8I74yN)-{{+!%o(vgR`*;Bsi2
z&&Etf(QJ@+fYhrc3t-@?)(08nC+J9amS_HL!8C$GS8$Fld$&j7YF~1^fH=av!#t(Y
z0sL#rX84V1d7ZIT$D<Iinc;jPE;nJTl-`ISw}`Btidz4%C^8sVs8c;CH`0{G#Mnkr
zl~dgKLR?-v8$~JyWhZ9s;{GTUc&=Jz!t)*6b6S|NCZWobHJJ9mlIrfLDgIrrm4=F!
zu=WX_J`jbG=-z`-%QZR3qHItz&M!CCfXO(&acVgwSK&d$%>eBC0PG=W2{bMV4UL2%
zmX_nepA-uKM2$A1&>c2-o6#*mtj4Z@OTvDY_duwuk&E&Oo(hC!?3&NE!KF&%z_F-F
zSNj;>T0@g59t81+d6@<ZCyS$1=Ih>@RlZes&-n#l&&<)LUxoQX!%Rv6ZFJ4)Z_{tC
z_A!N2Ua&#QyfC*%fg*=Gf(dkQW|!@`uIse4Xcu?I(=pJ<y-Ofo=5$3&%ANuVDo_ch
z;UJCAm$3kYO3MSHQRdiyT4)8u>}O*z{Y@sbbYm3Sz!@T3j?1l+Q5YvQbR|~Tu8Vss
zLXDA{>XYd(LL6*eL=>2m=@OF?PN>ZT_FVj<`HPTBy&1)@SchA2=?>6jY(`(h*aFe{
zDql>$_<7p%2q5>4I6)69a&e>PsXn{YQlLy6xP#n7qo8a9%Kj;Qegm%*78g;7G>!L6
zcv+20Nm+lj?B~?yG)20DUhPy-h}9bOECMyz*#)S~XT2}(>72P@=XO@JxVme63r(8f
zC^Y0>v%)k@J$FU}(p72K={|!W(0)9MsYQn3Or7V!m;$(LH7rz?M>9JW+ATKl1j;!t
zh=Jc2F_$N2*uaIjoQDXF#5ht-(?SQTA`j!slQ=-d08C5c1`I@_b1Nw~L%NiP9y%Ab
z3H?qEN1^h!qv{y1@SY2O2BFhcg}AEyybSGX9U2fShJ!|oO0y4FY~IEf2ct;tQOUtQ
zPUi2@(cIUf*nXA*V$I~?qfsjirjhDcHW+T7fg6f#qioV5L+VN6ItXQpmss_ehqjXT
zL1~4_s23z81x1P*M%q=wqK^Aut7b+SQ-yX7Vx^r3;`@MP4a3kF%GxRJDes?%+N>G+
zaNKj{EVb#Cif_dDQXqeeZ>0$MqeSzZ^xzKeDSYYI6hSv^%~f$}lu0S7ZDzto36ts!
zsPo*6@e}tV8^P5K_yO2*7oz5jH1vNm%l7Fg((GQ2n&dbqe=v_7u;miTc9{p*8XS&7
zbn87ydgA<LI?~b6=CY$D%>8+2y!sGNX&^NOt*8x>(7~1t#{iTW%Fg8Biue4CFLVIq
z59cO+OJ=b9VOJS=s#&QWnv`q}w~d@M6P&T;^i8+vBmFI8PHHPSDs3~{Lp`M%d4!jp
z2UIt@hk|)%1XhwB-^%e_x=cYK&iPh7t<fg@<X@>>(0Ksp!rk$neRS!=0l9kZ7xL=h
zcs$VJP6lI;84bX*aW{u?0U1m)S!k3br&ai)rWJL?@4rq1P<C}P3Z3P7Xa1M72A`?V
zbMd*&rThVLC0&6TC8q0T8JE|d+!qgqaEey0UYnev&2I>^B=Wi0Mv73;OHE1v{|Ivu
zd=%@j!Z6H(-X4sadrYcP)C7b!p-zKXSVO{Tm;6gexdbFHPxuxvtyu$Bi$KF;ajUCI
zixcs*#^5U<E@yrb7+icY3Sp4mm@v3fAH6Qmi2Wn+vSQfStHeLVFjQR2*z%hWhpSb6
zzLmHX@aY=C1|w+FydZN!_neK(2?LrakO7@KPXU^T8}OO*a*f=5CN391RPI_m#M1`i
z`oyWcU;|w!ZC0HSR}flsIi)XG;<5{OE4|Zw<y+lI{nhSiE;YN0SENM3j*vAxd&1l@
z^|T^r0Ov_1MowfLnbV_!M6G6-U=l)UR?xX}$X9+2oXZve7e?FSW)vekXW$vJ0X?=-
z%b^flCTRn@(vfDjT;@L`^b+b<;uSU1;_j%mAR7O3k*skcE?3Y|dT8>(lyi19^#rT6
zAB|!{Q}!kgw$MjOYalOI3Y)Nq`Y2P<4#v|F_DkV66NS@Rh8&Ah^85(JQu@vqN<DbX
z++-8}W=@hQxr6^3=(!AZFCUL0*LoSf2L}&NMeV9i7g$&W3$AU?0ikUmwDMXM>6m~S
zb*M;jMN!(2Fy-z&utCKLG%GF@3KO*uby^fi^jwW(S`w1dLepLeyl^ZYNO0yNY6!b4
zB}afEBI)Us%cj~xE;Xj1$t!8TL3(Z4lW5*xd@6!9;}#9oWjK{dj~QRd8B-ppgtazR
zi|U@MM%4Gmx9M*&U-?L}sR8b;ydpQuhqL(=K-XhvdKk+Hk335cDbRxhdWzhmQPT!d
zcqj^rX&~57ID*x9FP&i7845G4(OJsQ)eUj4-|(eCv;lJ=3^^LuUx4IYgIuJk7Soah
zMvTgUv$@G>urQAzWLglR7Qp0Y9?0GBhugVYr=s?gZ9-77s=lO^<hNaXaW;ySb+BLZ
z^gdppd6%K*kh`N>qcE~#c3|$uL3g|S*r}p6bApP9%7cFIkEb=9r_3&Ac9pDM@%E?C
zl_6mLqkk>uEyul8Mw($Nw3P>m`yYgg(Qkv_ZJ~jX)<qeC+8*!|6&k|+-O*@32Qxsr
z>RyRU!*cHIY|Y0jQiO!Aa!?=1GbIC7#_6HywDn{S)rr^_h{8(Qq!uz4R`YFxK~_v!
zd-5a{YafXPXLKR%^$$lO+1gujGA|opNyd3^T+U!dZRJRnF9Uj0v|I$Rf+X|d<XZI`
z<S2z1rjoM7j6TkUF6W+#B9poRyX4ci0i?1HWzBGtfSFq%VAta{vCAHv;uT3RPSE&)
zLcy$(gvNfF0wimvqWH;#ew_Cp^jyU&1VRI*tY=J1dcGs>$=F>Ce@A6p&#Yay$7b#{
zGct_3eoxfaq31lU2cmZM-#tkpRHAleWq>>=X%+tu@Kad&tJJhW;me8`z?gv>?zr@Y
zsI^X0JVsG;D7;_>PO3_&m<E<6QMhs;sR&8!9CdsW4Pz05QrJyT#$_5TXscZ10ih`;
z8-(h<R*;3cBxy#`l+t^S8EG@LkVx*$P@0t$9SQCl7*Ri93_jBPv=2V3kuSM}8#v2v
zzt2>+3t=K1T~bq_?Lb!}Car)UfZFuNXv&4JqF0^1QbkcPg!nJn^6k7#mlC@ALvC~g
zs^02WUV%0;DulE!0c3zioU-N+0CGy(sR9=Tz<FHD{)s4Z>sA#tN#1^YKyu7vvNwkI
zLFgb!p3bKNV1*9G@j!!b7cJL(kt7e)zFjO(p7xNKQ(#BRn`t5&^Z=OK1PyPG2ye>}
zE+M*G$Eayq3QWHc!&D~Bees}%HF^ZP)%G<^QVy@spYa?U*`*vKpwS_flyj_2Kr+02
zu1p4Z#Du@h*mo?Dg*rvMX#gY@`hs(=p=>>2#i2%^I|}MXV~a9rL+`0`Hd;v%rAtFE
zQm1%RyLiyF=w1cg*Mc`~Wj%-Va1X`3H2eWXS!vzJ7w7q6X;&0BuNhE$`x@h9WIPdP
zPj%8i9kpaw#y37kAr<55oBktnYTujIBi%bl*~OS!MhN^&p23J#X1i!6a9sXreVRs(
z@)QzP56baxL)n?CyoaY?L4Hf?y7|IIy%AF#GgUqHRyS(vi11w0W8j80$A@_ZTxz}A
z@02#BOZi-tRTV!JYX3h9f$XLM=SCkDPo<YO{L$TtB2&@1y!H_;jlF)B8ioJaTcVbt
znOCeWJ^Y_*Cc19pyNngSH03$MLwYj`v5oHOaB13kigoo7lDf~gX11!@)_Kt5<<$#O
z?35a`TM{ev={DZo+~Z|apXWEDHgbG?>(4V(>Py$er3vz$D6$0XJA0!r+tokgt}_MG
zvGTW>f`kDHI)5qcdtG{4V)Wbe;a~1Hze(*z3Si*xuKeshC%9ymD?ofIj@k5PI{BEU
z{0Y}0qslbcrX&9~bMGthU;`<i_@F(rsdU8b*Qw?YT^Po?Fkl2G7A?`9zu0R7jY&HS
z+0?A|4cZ&=#SR_$J0u6*W~5U&PhW`3DRumLE}x`OAZrhOkxM<Gr>VIH`8>6+M=>rG
zJLcZ0DCTXzm*;q8f#O;U#yoG!0{gkRG*7FfiAygs(;_34>LyHIs`KZE8VM$3o9j0Q
zh+g(vQS&E`1MbODOYe%BTQ%~JXK;Yd$|q>loQXN$cR=__n4s)lNY8kQzeenTBQ7&>
z3~+90(=dfiTKTNRWh*qGV{GR#PLF#_wATSC#>hN8-ZbpL!Do`*pV=i4IzS&aeV7~n
zM~qpDFwIJF(`Ar`n*2l0%Y5c<;x`$UpLsHJ3$MIGAO6tPUwu~1Wcs{p@?6)?n2b0|
z(sO8n#G&!@PUz3MjmiV2FHDyhmAs$%<IT)Xa_`!nO<4otlBn^Q_kk!kZ47wDjC~RB
zr2X-|zqaxp!vnhPDU4v<)w5CTyck7a%<y2qgY%3zxq?^beT4@t9>~+%JZMu)eDE7Q
zwI;LEQ3oJ)X?Yb?h&*fJH`B>!PjIgBMGZKemc*(JY<l{<4P1z)ZQ5&dsoY(lfsMUU
z<Zdi+beMZXihH6M9gU(!Wf^k(?!8fa2?bQvCX9gV=eZrd&&%5AdL91*FS`+SM9ZH3
z<2H|c4~4dmMm6r%7*Me-(s`u4m19vDTOaXqk48tbIMdQ9&@t9Ya?x82s~5P*7aFNb
zvp)jFaz*d%sA<IbZ;lelw>P!3y{*%mcxx$Iu8m76Ry8YxbWHqAUvd{+s$n$)^l>z(
zGAz;z$XMD?&SQdpyT{&u)f%w3<u+idn?_~lOQIX@UZVCx=+CW38yKXn0OtM=p0+9a
zzRY`ao?FIxOL5uafy}PJ^F3GaSHy=hQSzph+-YWe(mk)7l&%1*zp9kb$EV`5|5y}L
z{uVUeN4W~lKjK^eh?+ZJHsi9ZEg%~(szt49MxZ3@(|{u=%W*?Gn)=Jc>9|Zk@AK52
zpeQ<pd9<F1VnyxpC2D^oig{IsdWO0Ejw9F|wU!RY3sJlDJeOaOVth6Vm&y$13-RhD
zo}Q-|Q$Uzn6qyBra&U8u%kv#@Ud>0L=_0^tx}4*^kvgRTlRqyVIZOC_YHm+*IWs}#
zHnmT#OzUJkxC!HK>I83$n5n-<AUmtQQ5bA(2?Ow9D(iMLeMMh}xL%B>{&Bt*Q8O?A
zZ$@kVV~8opJtmmm*v!jE;(^@gYG1~4j&Tp-FG-Y(0H-C{f`#-A_H*yHC=3GTQsbex
zG{&RZ*vIg%5URTCTFp9Bdi1*2a;WC{n11yVDyM%J-!i^*Zii7zzvpPw(#>U%x;z8;
zcX1!ZN)$SP`nr1lF2I-k)2UF{FU89WvNZfZ9+#eir+@wr8Ia<ktP~~?5AYdNHOa}*
z8=Y2iDFs}Ib!U!#fYNb{EHTb7wR)|k&oBnU3c0HkRrSiSZZ#c479&Ll_?6JLcrsoB
zyUatZv%D|A@og8%-jF5X#!S$YPS3>UR>O1)Fx?cbf#i+VD0%?aNNR?%fx+AzQMjOQ
zbiv4-s~J-Z8Fjp3_^Ae9#$N`9qo#}3x-V*)eg@cG0GM0ji%&;U&Y2j1^RiRw&%lgm
z{NI78iyK-axwxTHKOPUN=b~sqvF&pdFwXpiC=_Yi%*df-fC<>UFW-1c9ziOS8Lo=T
zQ)x0L2)0E07DOn4v}jLg7<MR8L`lF!VAE!NS=BqZJHDuC-Z+<MKj|`7Oi-TrAw;cj
zDjR4Y<f<wlF)I1S3{Q)w+j;sC3WzH`e<mYG$?<top#;Yi)N~nzy)`cL$OV|L5O8LI
zNwExE#_{wxBZg-9Ir_I5fsq)OE=$U*=(auIljB9Bl17r(hSRXlB__yh?Nk)Alr8N_
z*^kH5)@h0-DfD8baj`FJK)`0Y_mo%CuCoM8CQ{|`9tY#HCY+E|j0mcKIf}(93I)m>
z^sH$$0G!gZ7*ybG12A!EmXESd!BY+(^Ypls8x2uwp}4<C%9cPB6qg^1_F6@yIydzt
zRO|}Eiaaf!7j}l^Q&%`&vBhvb=E{^!BfVvDtq&h%T;({m0#ku_OWaE~g;>+>gmP*q
zM<Udc(M-;KkgL$-;Jxv3=B7xPG-Yy&={1st9Ho)94eVW1PH6$G1e1c+F17XAGac=Z
z_e{i)?&TR7*2GFfIekFl3U3Q+<sux-+xh&lsHszGbc#u`h7DUFK7~G?bpe=6{0jg(
zBW=<>H01FaYD5D<xyE;YJV?dYi266^O<mijJ-fVoE^2KWZOdxRsAbE|z^7WiFP^Hl
zs|Tpr2a_=<Yj%vVQkEzpXBiA(c~!IVUGX6Q489ySe_~ecqQ=lNa)<d*F|-nZ^*|~2
z$t9*?CgqU7MpQ3!Dn5fIhliq&g^FjBdH?ss18Kw4rah?4-1D><J{3<TLRCTyC4L7=
zG&Rxrnw5C@c-$LOxGyX1SlC{n-=g!mBT?i^1Mo{qq`iL*W{kqhjU$Mm90yY}d0kH_
zK}yLdsq<=t2_H#AGLsAXDJVW34^pwv`Kxqpms$XlvNGFaI?H<sP|2sP6StP*+sqez
zJ!&2WZxU)QSyge*;iiwnv{KLl7k9i$?M7iuV=_yn{Ov|M?e5{_hB`eJu2sc7=}-Bh
zU1w(Dx0#G?WC|HAS^!}hgv(RT&5m62SKD`+&+Ian=b2l(k!D4g$z`cS;Q%2t0+MRy
zax!;UVxX12qnE9o;0q<|qfzrr<BcbqdukF$Ue{v>P2#~innLEyxeI#{Q^-WaJZ{g`
z7Kpz_*5#%bG|5I4MNgfKK#`NpBzH1;u98#_zHGw=<<A`+0h9{`a6lfihb)l|RH<el
zs#13&P>dQpdn#(FKFhR#!KD&)J`tBgysV3&=f$Xb)<#b-K}d#9Kghr{X`qo|cY>%T
z*sqPuPC0p)noRu3sFgk18rDmE;i{WDv>4!X^5BE<wCsH~1yAy+n2|cl<ijFzJ(k(s
zScS1Asmz$6H~orxp>_jNn;_m)QMb#R(`FoLn}Pc)svc6*^m)guO7L(n|DUI+o7=(x
zNrq1jQKNy(|6}@|^rK=a#*TrC{1YbBB-1I3a?71CpmQ19$b>5mYBh~$r)BO(?!JJ~
zouGMX`#O9pDesS(N$yL~S}t=S@PxQA6rrnL)zP^L2&Emi9JTF+Nfrx}9OsosqA+;o
zR>|S#;!+o-nnX>lQN1T>^Sh$xV}fm8N9|k`{@k*)FKYSg3Q%DhVlKQMh)ef3*4So-
z%vRtdGc@WdY9qM^DX;{@Yi3mRM;;&p>_-SyGi%IsE($}j^cRqYobo1gWvnU{By)2a
zOPi*9DlV@Y=_t(fOuVx2B?_R{X3{E1Ri&}W+32~V85VyN63DemCS&eQp*q!xbgxe$
zsR)DQGGHL>x~L~v4Dg<ust%<VfDL%qSpU8B;b+jPVlx+{KS5KFUYZbqE)zT8a|w9T
z*m`11>79A&jfI>xmzV)5X?<xDOVlG8WsTWUNpMD!uY*|IC!)~99bi_h0U;9@QcoX^
zOBpda8I<`;nFA0~x*1XlW3gZ7rr=3c#5_vBWuHjq7%JC5z<}YZAoF-ZhO0MbziCFD
zjeFbYDC7??C{3njM|`<wFSQU2B>W&aQK2q7q8Z;XSVn&VQ<4Dll`ka$>GUQht<jq{
z4YYRhLKN*+q8Ko$%m~=eC8P2$B2%pUD{;>=wTCdBe*Bz9CCXouSWzlC-A|&+qwvsG
zRb2N#t#Red%7Kde@LQ(JGX<r0{_bTxF2#b-FssBJjPjtVQqsW>nA^vYK9_0F*j)8E
zH6;z2X)LT}tSVk!X68j3<YtG1aZfQ^Gc@;%cEL;z1BuxOADU_%0H(jHpe@Q;de%}E
zwQI~=*=fuA;#!O@_?84rqdgwJs@}Shxv96_up`LKkW_{f?vKl|Vav_YW4@3|Jixr1
zy`3T)5*4~y0P9e;sRtTLJw-SmNrQzhGd9QQ{n4TF#kG8c5)QI<kjLYdk|3J9W@aD`
zy(phOLeA{>bkmrYPOy4EjX*;=06XnsB#7p;e01+Ee9^S08{UJ7-K8<Wm~c~Kr;3^a
zZ{`q_BJ_2BGH<C-hQ<`kE&ByJGE$j_Fp9-<Q#5s@XYJh#NolaqimU&c8mln-Dm;_t
z<{?Gb4@cn_N=zMm&18R;Buo`DX~qj!3HVAyy7X$aV%|~>J5O$mB9oT6T(~ZZ0}?--
zn_^Vo8ntv^p>YwQ5wldq<&>+67#@ifMw-9#xi4zE9`kTTo<$AAYNnLPtV(nJD6c3s
zkzqqX@;k?(P#7I<Nc173O6z14xsMN)+&&e>_^~K*6`+JCnAZH6D7KiEnGH*;QA=A2
z=`GWl(ySQPyVDfPv|p_o5h{yAFU1a%O^r9D3C0xsRu|xmtO4@UpaKS3L7L9X%*}i~
z6N~g~Tkqp#I|7J0CTbzQlQm#q3}A(6t-xxshDg!Z%`7PymfliYpj-`i41nn$z!h4<
zb9<xKmLZs+Dt~k#YJIgDI;wLh3YplZj2NbUN-N3@32ua1)E%j-s427z6DT*-h@PF`
zx0}pdF|3s<b9T<gy_6sV8?#L5lR_19L8+%a^>7!XgU+Lp&>2Ar3OHHY)`iGq7q#Sw
z2k@KFP~DY8L(R|(NJ9K>1D(=kTXC=VFb_^gQKEjC-VfufJkJAIcixw2C8Yq9QBC+S
zPSsszO)}EVDX*vjakZZ&leV{R+8V@Q^eE%8KqGTanU)IN-Gv@zEg!kxMUm?){|ON9
zf-=u0xC~)_|5Oz1r=pn3!H`p&2XwFvZ<$)BT2yb9V_nlHspA#d-jQm@x=Fw^Ke1JE
z)&35{(6H$<f<595;7EG8P2odvsYa}-s40eB!&PF6Rbfz0DbR(Nbv1T0SVk|zx2`Ho
zz{dpI(E4w|`z<h&X|)i)GwR;K10=P2Vf}+q%bm~Q$WsLrc<yNN5DBSMS6ZesuP=>2
zI4@KBK+gcG%fE+xnoC^i`e+oMy*+{@U3M?0r>ndFfCoFEOf{>ZSeNt|y;gXhh)ZvJ
z6Z??BTCcgxQ3Nfm^c&%k>;hvuT#ZHb#~ZvN{G{RuEIAQJ(rF_PH2jTzWDu3+g}t(A
z3D_^td=IiUuDo_UYPo@lM(1yeqIGi=E}zv;aQQ`w-4y#Nn58FrX($=tbD7P+xh&*P
zi<!USd1kQTaz*n@tZ#@unvGt9BiAN!B@WDJ-F&3s8I8rSLa}GGXq}*Nfk`%Dl1q$P
zcRj~^Yh3D3=7|nUbW}$nDcN2c*XS*4nxfX2ha|7UP*~~~Xh2FJZuWBbZQ7zxwrdWW
zDSqaADbhp$j!yMD!+>jD{qHx(OEK(w*uaa<^2MkvFqxz)rYcV>RmOZf1}-fa%us|B
zp>bEid2P^bSV_sRiBmV9)jZ901IZCm7WS8DbltHmN3Bn-`x{Xxr2kuObPCg~JSs91
zpFSJ+hCY2XYQ~UFmdQLVTJga4!NXB2X%H1JZ_&})2yQRid!yF<21PT1CGO3gh$1uI
zcxsx)n|=l{eD{?o*7)L*1syH^VicxN{VC)#*X7=36rQ@PSGmK>o3w0l)hewFZ;Ybz
zO$wg=xal`DJ{tNvz|%tv%>c=SS&o>InLmb~9${#^dC%k#S949Us2I4fc&L^XA)C6>
ztU47>`;SMF%UzFA`#i;y6mqTlL)I-em!76dL{su5UOrDTrnn)BzD><Vt-U~TEd_&|
zFMV+7;X;N*Ji@`HZqJO9iXHBRX&DtdIf+^xH`<DtpN$}f%-n-^S9rzCRy%5ouSQ`8
zow<#sPTEHsAeu3w^=h))%*)Tv?{5QkF{sKHu-ay8uf+ooSvJgPDqKru;(=-BKG5-W
zi6r*~)2})-Pc8-Lel}07qFADV+*l~UAbHa-78uYH4GbGTH@=&GBTV-g0)7AtWcJYt
zwZl>9%IG3156=4@Jh<_>DB8R-ggNDIW3ReZ>Iz*;6`H!NO-CAr0~#=IsZA>~vFAk3
zzdJ64!Q#oNbzle+^s2p48^anEWMOioFztgp!z@OR)b`rhM;PfGGtvz8<yD5O{L{{M
zM*DcrNxNp~xtx8du767uM*YToruVM7I$|Ds_$*uIsWDm`o$z$0X;fNE8;SNL%rt+G
z#-#xJRbKYg)85CTwn77Cp+FwAFvYShE-A(oEo8R9G1s&Y;`_3noF`ep$;Ql0GOkYZ
znNL+&M&6<J(Pk9uyP{~roGa&|*s(o8oZG_yr-a!(#XSYU7q|yAb!_=DE`J6ofIT^;
zw|V8k*{G#O1}|KikqivO^P2zSm8f-j*$8g`m!hWGsIsX_E595M3@J+tnm9T0L-9a8
zn0^2dKLEr%xpc(SG}UkhB#ZLPYhx4|+vYYJ;}*ZUc5)BjO4&+aEJl@j*}S6_R2&I6
zvZYcV#><*po?R)3I|Yp(s)N>DU8PZ&Gk^82RjOT#36a}(nYo68W~kb^7&`{adW_UP
zY`SjlQ1Hy8^nfr!WoOCpUmW9QAf$t!=X#>R8O&_n9fi`uFpwsvsAxk&xpu5~d^9cx
zG$kPN2P|A}!2t;zi2A%lEjamN$Wu{~2N=DV%Lk&+AW+<F0QNjQKtO<1ah@2Y(Y{Ez
zC2HwXfNAMKggRecX{kw@b)80Q=LULQ71X-1WzFuT!b~UPl{p4JwZDfx@{cPFsLfRS
zG`jD26lH5g#)1mEQPHMJW{&!9UcNqxF(l9j_9gqSzb`I3`=ijtv?)VFnwK@`nLo%=
z;F62Q@QnL3by0+YF<%plW_~n000AXdDv1`y_<LZ+6E==BBdw90Q&AfuP(1y+1zgq;
zK|O$@QPe*iHJj3U)o?ZXrkg9p*1VFT7eKcI=rZgo21Oo75&Igm`4?AbY4f7&SiB<b
zCc)hrm!eiSm34FdNZLTmNyWKb=*;QY;8`AIx;HM{wgOpHbfmYV<!joJ!6^wun%p=L
zuPjy+e9>}fwCtoE_}lPrx*i{onhICA7}Rm3t_Epzq=$w8sFbmd94{xaZAc|E0(5_T
zq1^BgHW!QCfjodVT--1*ON!8WRCkx)vRlydXCVyI9|U@kh6;CPi)M+<JpzF!G9D`A
zeT1>V-F|C`y4@g3kkn2=qzE<8t!$Dx6_GW3R4qqw)z~m!NMRyACj%i(=tu=VVQAt=
zG3=UV2JOI+L$iT0v|`xQ1mkkIBCYuETdfxDFjvh-bd<UWrD;cVGf0tjp=Z|{WnH<}
z#Wfi=<B500EAB{ZRZ+{OfSOtZoQ$wA@bt1ETOp~hHeQX(E*)hcg{S&aX`DbC?)F?N
zB=vCIb4E#5XXA3~Toe;X&wrlI{?i+&;!)BPx>~J>7E_y+2u>LvHzZ${D&!tdHz{W`
zr;urI^w6pJHV=om(uYtIP8gqiDgLoU18^M1sA<3jw;nTh9lNBA$n4s66;IoIA=3%E
zRfttOSmCLRZS;2?5?vK}$c_G56d9-CJ-J)b(~5K~OVT|!nmZar5+<#T0bK>XD5_F;
zsKvDkcv*Si?h6U2tnrzTq|C8Po#32X!Q~dsGf2JO45Xk=OslBCex{Kynbv_QY9$=D
zg;8gJ=(2&7GBjz3zpCO=alH-^rX~?qOwcfpAy#vWa=8n+p?#pV(CL5cwYaBM<;<T2
z_U23do8wU{OZ!R1&&NF-+20dtihqEk6>iK!m@2ALY)TBwLq3wsQh_5pa<2@AtBoq_
z3dbvH%G#`n7)1c2$Y82|;d_R&d>~({XA)bo={-%C%Te1t!MCqOQ8QIt%S?TC%qu)C
z7{vZ-osFWbEtX)CC&QQUv9YgA8f{q8|1Zdr1|(H-;z*TWPOEi|#h**di2qGPlV#)2
zOS@ti=25tp%s@qMB7}StU%Ejaw=x0Fxr)N-t3Hmd;5jfY-ZJ-BhTvX{nX3#%K$-?J
z7{Ja4j7o;k&>YdPIgq#0TOZME{8`M11r2pm!zxgbD;Kn9D`P%$6-tTjApLd%C)bQh
zG|F44e>-u{AmwNVN4t6OXcX;&EnIF5w6xn9uEvh8n>MRS7ygYx21{bSE8a8XccY5h
zOeC;A6!-Lc^<qThIwtYp{wNf$100xgG>9_dM}V$oS|UKp?abHWa{F0|JyE3an2*~0
z4HR%$bzc;+l}ih<uD)~19N?E$3$>gP<5S100=3{}&UEHC<Eb`?Vnn3Kmu-gDqm?Ob
zs^9@{D!F7+=q2$xiJItfMb@X8TLIqo($pLh&X|4~kT}mph9mJb%`!S4@R^!3a}iO|
z#$%I;Q7l@0=_B&89>(Z64;t2;`o$x3kc+l7Rp>x#y?0yOQ}3IsDjjSbq$%v!!AGJP
zK?g2Ax2mW)jGAFwP5G~ZEs3Fccby00p2@y0412(rX%NM<m*Z>9qe~EoTLWHVZkK`I
zLqr3%o!gB40$(n@7)7Srf%7@o%5>S`C~E$-Q63cgs*|)2AXWvkT6dd{sym{PCG%zO
z<%%pkqf;YDw750U%o<nJS2IHcmnzal9{@?UuGsRguq|<^XlCZpvrTo>Gho4lHP^At
zIY|ptQN<ThcH+E90W}ji-#JGC@4Fdj10t+eqtFiX#G>wdxyR5nK81^lv~Flp=TUdv
zRU?x}JRQ{|7hIOmjGAkbTZD^0DOL;@Urq1*lnA{LU5=T8oFW3mk<#1jMKwhAX@&+x
zbTk*hg7l}yoF}&c4H@yPyk{$Cn5s#r`IBtmoWTKQi42By<9rSK3-<P}#l6-F1#rnl
z5J)QX9kr2^0(2?IzX3Si;9~By-eQuJ=y5=_mt6KyUIwr(4F4?vVait<*6l7w;g7F!
z$>8C*bc<mw5AgK6#v)&jn!&g+_tthtp@%!S6}8Ny<dsE0ys(!V&3k#xH>0MDeBe~n
zeoFLQ$4-!F@4|yu{cqai@xXnl^9xa%zm|f|=fA*%rzmXdX=*%hEb8t7Gt%~P31aJ&
zxSXGGkJ^A*{Vocj;XHV-LQPl=5ddk#ycRW$m^?a%FESv>q=i8XfZ4fw&42YoT&hy@
zr~(?jb1$#(o;#o28<~;vvvF_rb5YFS7=>vm)tRU*@W9O{EgHydTxudE!{xj;rZ@93
zjhT;ag?lcCTy*rJjLrp9MJ=P#zZkWjB{2OBzJ*B)8u#y^b~FlYTjRw_M|}<Ls|ix7
zI2>9s@>b?8@vTDUBs`<B<Eob0%2wte^c7k;4?Vlar*zbKdWFxF@+#K_-aEs44$?kq
z`WVp4CQskZ7jk1-6g>6Y-{t9DJk2e4hoe>+a7T;z?PFe1daG+S_Z1IM(!h=b0<hOo
z$U-uNk?pY<ETR<B^Qk}G5y?Bx#J%}1Q9Kt#>$)iVFGNwc3GTp%y$ewoFTO;h!&6*-
zAqwNvi>IkQMp1f`%NW&6D!S2><UDxL<D4#@i<-xI9+*)(h#Ser|IYnW`3V0*Pst!#
zu2KMvJed(mC|RhY)t9D=Dd806W`AnFJ#;KycFAC$kQA3r=0H5id>_nTt%p>zbo5(%
zrm3c3voteiY!D+<%3=vlH13B#G*VUD{+rkAlet6jAY;f7h}-^EC%G$+p=o%fZp7@-
zto5&8MSo~#tD{u;N_kzV?Sd2_o}0M#M@>0yzL|^E3Vo?v<I|?b%vX+5(RZJtb6JvE
zx&uWpsekil+9(_MjOJF!CQMrXJKU*w1qD=2Fw=+7nJQMey2luRGcxvYVmUr@iLQn)
zJt%K#Nr<Oor07&MZ+Bd_AZjDMWd(l@;t~e_!p{6y0h>|XyW**6tQl#E#As1C(h}_!
zYUbPxABdONKOTj4gbdsJOk9=>A^+1Q)qNk-{s5}mfWgL4U3z@j=>AwGGgcuG%?cgk
z0bjJ4(M5<s38K@QM;#(dOjJ@>>K?v}juv-CF+NBQ-CYuVTu+gFlz&pJ<Sov3NzY(a
zf^$GptjTy9?=Dy>*Q7f0(Nljs5}#EJa=1zZVCE*qm;16Hu8TZS_$p(iwei`wmmzrt
z03(>YmBP8fBvXD5uOuPyAYD-Ao4M@FS5aW9)bLZh!g~s78O*6F@8>nj3%lsQF_v5#
zV;(vj(!GA!0AZBW{*kDOfNm8vlb1$y)QkfUjzmp8<IMe|w$8Vvy<VV$lxR#DmIsaD
z=^p-%$7l+Gy~W&Kgap2V6)92gVCkm=1B66;Vvtwnp)18ru8lRXn5O2+XsQ^<%muA<
z2I$gO3?aQOaKM6G;YKIZe=-Dub{372fYLTyUnsjjjKaY53RCbvU>!C`B_PU}E2Pv|
z_G6@uMZ%Oe3<wnyTL~<LoX21WgBT*BtOb=MMF0&=Vt<Ej^lhz*d#O`oHLxYEi7iF*
z@DscOcYCm6?qd36T&76^i+gf`boH$L0zZAfe+SAggR*zfbUTo|!HX1lAB)R*UY2(I
zyu1kB#`~i%Sye8RVU^0-FE-%J+k7iXgwT|{Vlo?GM&|St(IaEm03yN82ok!0Dg8#I
zUOok7C33fmOUc4)!_u#fw&~40X-O(AcpEKjrJ=c-R^Y7^$nchj5xB9ThU3N72!d-N
z4vj$>hciV`{ZUf6hD_CkmcnJu+<<9{!%@64v&&S+Zi(sZ*trb-pFqGe*!q8wv6@2p
zHwvgY%|X6Uspt76QXTh{oiFpP<een-Xk50DIa+RG;3uqR^if5>8jE_?0(L4wicWEX
z)(0!RyaxPIHaP<N$Y6~l5QFk1T^iEUJRgeyzi23I%5pm%FPky5J)wrxva~yjDQw|}
z8wTp$#DkkDR-?$wYLK^fOBBlN9;!n>rHw71h&1!o#UTZwDnHZM(GXhtdgZ2`Dhb{4
zWOzatya0n4;umFF<_|IHGmL)mR1^muiy|F1eYG!h3HxdJZbOrDg>?V+;kdNDq-TM{
zJ>%30C|T_m`q(%fg~o|7o<8?30efFjjlC)^ZS+1~MsC#iQ)_rtj*>~tg6kK208S^Z
z;hIvErZU_{%gJgDb&i?V*ajTma$Keh;62$7d`#_qRX~A8*DGpXk>94aK!Po^>HUe6
zQpF<@3@`TJH^tY8p(!Vm-<;)xaWA*3ZA48Q<bPr2YA_=w#!5FdNN4P4NWcRk*QIA@
zH}^c+(b>41Fy9F?GK3_``n)%0`Wf17@ZrMT9G@kp@SZsD3l}a8QqIG_n?N^l&cKCp
z{(`cB<P@hs@}{1;z^K`%m8)zT0U_6@B<~)=LubXUu%0&UkJ8G1h}=NOF#hibR{x#>
z2`kryQjqb=fPt$kH1d_yX@UWR7ISkL($F`UejBK@QCZTi!sxf@sJbSKewzmuqFAv3
zzL+w>7{)Fhq?hIRxJ-Ah8Lp6QGc?uHI=nwDlD}X9(ld~!$k5SVLT)S`h{7KsyR>RZ
zFa3&xJcTBgu(XWut>NsAswl+yI_8#c+WMiWnWSS{p0HPo%ZM(!Yi@alsb|r6*ok}a
zHDW}C?EEZqQ!)E5XAPSlJr(z+Quztl`ag%Xn?TPP7%06bhoh#}P4f~+rqzOImPu0b
z>t^to{NXF}P_ZgsA(I;#LG6H1sf0DHR7&=dhMdIOa6r2t;Me#J*q499WN@Vctedlp
zSqfAC4x(kb1Wa8rsF~5Ip=W>A<)#RyuSzu>^L++cGozQ7w4lpX03jqGI`N>zjB@#m
zmy<aQsX(z(w&0Y-mw=UQ<;)X8^rrAtvhLF!XQWcQmap6+E?=he1Q=u4<XcTOJ5Tg5
zDyhllRSr{ouX)<p%PSVgqt>MupzSll@^I8L-pK<4V7)DBTrL(R4AZsua*vwHrCX<>
zmJ!XzqSpF6g=l<&+7~HQXwOsAV4jhpesP+o;@qJjSvQ-pJR%Ri9{0?=H6hU-0eGlt
zHHim?i|$~|ZvXS>wG6=G*cpMG-=u*PQ4E~ND^ct4ttLdD-ro1QD8^`<JXX_|e<dz!
zU@)dOSdQX4T3KUMHKVHeHj{#>*<1T;)OMJMqN>ejE3~W=tb!aIjZ6K7OHh+U(?e8A
zFG&pG{IBu!A3z7@rK$atqWUQlo}QOP6>~@8vT(OHu%ERYq;4+y$mnkpCc{wi>12BP
zlp)6l`NG35GlK*Xsb^iBYC)&uvVl7p%a<93p%;Gy?~+UR5Z^Mibc_)wxi!9l!EZv@
zD#$ySjGHFkTA{{p^<mu(Y?7U8W3s(L4w>DS4NZF5kuVTK572Utq3v&4{tq<jY28Dn
zW!y{$OLy2+^8sUayRBlx1xD?wt_Cj5?y|9BSj+qqbfn?%Lpl<dZ!iV5h7N*yL3b`U
zsURMPcSj*i*G1mpxO6>FGo|=%>!0F109J(le@~an0}TQt*U5I=llb4BL`_Whi)3xZ
zz0PtJ{z1d^U}L4-Q*kf<pC^?(#<w#}{FS&j|FtM~o~76o#l_uG7?0h=6>K+r?!g18
zm{B|KEf~QwvnzHm$?H-l#RkKe$}d(jIF`@HgDKIXYE!<zm)&Qg$ny`L<~?N&Bh4Rn
zGg6lVhS;LzCry*Hyn?-Pi9wDT-`b3Tg2fGcZ8Oq;jNEWlOD5K^9)ksi^tL<*j#LEw
z2cjk?r~eBPn#6WlS*P{`-O6kQKw2%_8{HFy)KoD(!KvxDLM!@t8nf!;bl93<Xbxt=
z6%yn&9_aCDhs{F*?xb<ar45zTP()i?7E%Epx%ET50SLG58<13xnT%rUkZH-<m<DS7
z4McS?z9{KzYHTwb>M9<XeCXyZ)%Mz<xTlv@40V-e>wa2+>B>56W)flfI&*#E5pXdv
z)kx)6!9q`v2=+a^46EsPyV_E8a$2Rn3=i698ar^ZOQ990o!XMR5|?Wrs(3pY_okJa
z&uYFfpgU$P*&g>2$py|(XTtt$)V4tP7R|d;RR7$Z3G)TYk2Z7PSQYQBAw~@R>hdUU
zm0XHuvl7i^H+-il=?Ojx6sc2iX3{+LQZoff@1Hb+?42@P3<pC3>CRo?k}ovr^1KY^
z>pft<$)x`$shvv!p>#C8HA7+8b&fs*9}L6yiXE1V3^b*|Y$Aq^w!Ebt)Q8ae9{@k2
zQSz3~cRES5ywadYtMAuEvg0eW1QqGOiHcj|GMC92sf*MGO&qgEo4pwv7|6Px;5`Ju
zpdn!Y*iinoCfH3Lmf+q3U{djC8IZwDA*4;ynebFn0f@$~l|w4cYxu$ms?@a|jpVDD
z^f`|>OX0_iHabzr3%)E!G~UyXXW;Uia$}WmwKfYrZ1heZXzF@Qgl6rSd&w>elo`OD
zrm3P>R(aV03F6ziJQ#)T>7}}heX)vrE+Un+7`-QBX(i#bb6>nCV-FO*z}`$4&u$X|
z?hcmYizDm5%3SNX%;;65?`S*1TY6(DBOsq;5ReB+idv}MnJ4@Z3Yy|2-l@<ROdFch
zydq%dsb#<v>}xlvIz);g0r5KpQ%2CI&;>}G&w%)NJk^YEZ$+(#^j=hY!>etcYS(G0
zUbj0gRp9LtQCsH=kH4wGjP95AUylZcw?&bT*j-W6y%`>gS{`DDf_#G+>2w)Cs6+tL
z+su`62xW~qGZJ)6MiDL${b$K#>GW(oNbT$d8Bkf&Q|-8=qb1S2m#0up(KyENN&>ZU
zhT<r;82D5IHG?cFMhuDtHM_aLY9_^v$omHlve81KV$hI}7I&ty3U+{_6<OG-&&z(6
zOeZvGwCo(xK^6Osz({IFqIp@C%mn~1A<W;T0o}P-J%iw$ciY!jqSoFQg>q7tLU6i9
z>Y9T*=o*Gcp{TayJO!SdXHt#gMya<1=hcWlGX^t?2u$-#S<tP^lgb91zaNs<dVLx|
zoMGU9E~bH%e<4-5YGTU<$|3bkuESml``m924ANt!Jt;9YQ^OT=FhtLeAnN+HD73y(
z!r_oBX_NvRr@8zhg<l9~MsIRy33SW*Dxf9d?90ft#V4BvuH!R0Zvp$o{ZUjmN8y4(
zDtZ`9RG5pcK{bN1a~1wz)I^Gon6V{8AgziAh^lTKg*cMDhezU4HDTN$Z53EjgUa0;
zo=he;*37Ng0&>6)z>&6qrqz3ZX{zlDQEL-ivC#n_xrwJniqe7t6=T_or)BBr2vm|D
zE??-Z_$SQVA_r28Kilv1LXuYU$XZB6UNCa!+$0b&sM_&?s1QflD8tR4g@{YB3O+KE
zNE3U1Tx#Hs4n$4SqAXDhY1rr)>T?#2qf?l&?m-$@i^AnmhpQi|o)ILkH{djQCds=M
z+hl+xFzsa20Rl`(qX1|F&MzPnw;Ap>uvbtRUvuUH%+RUYDn%6_7MF_Vs*yQLTQAKD
z^FJITbU^BOs7VK3W>QLd;C)IJD7z|@t-rM#_x_0D+q<LC@^w)v?L>7v<gJfg-yJm}
zS;R;Q&Zpf3&P(VyEmb()$*eYz{HM&wh2{+~=$?j-)G6iAkyIiACPL#V_h>*dBIS%3
zpN4}%U*(z{5AAwyi;g5I$0CUe7i~-tkS+jd6mO}bZTSez({BwL-*6a?4}i5P#uUcf
zy3{Tz1c}Y*rujDj_v)+4%SxAr8D_su8ZqY==0R=IR4qPEMeSz})y+7;vI{Ls5nGJP
z5ZvOsqvmeKB%jx~#BP$Odr(vDsF=*y2NbHs@^=**=oZZcjr)(yE;Z+o@hjS^xF^LL
z+GufaqkPK<JU?vI45Op_x<4M&ph8PVo5uZ2hLzIVgycuiOLZs;!!ha<<MM86=|Pjd
zPe$QcYq|ObO{xgKcZ?eNeF#!fe|(XC7m@OrTEYlaxPx1xRu1!W?YMO??wL=M8#yqt
z()?v;YsmJ(I7PN6<23qL&wm{$@9K?bw&qcIR}S1f029pqn1Qb|aK(}XKLO>OL{TMM
z-4wOfDwoX^HehFWItt1Bed_Jpg9A25NgI4H3J+*mXKovKPtMJ7n<w0FRNP~N`mkj`
zV%fc1CYV7nd@D~M^<-KD`v%I)J@;UpjZeob1LwOQHGSR0)@SIbL78@^C@ZmlfBq#6
zWmFfZD|fy?QijZ?Gznahmh`JpTic963)$t{_XEU(yfS<yit(2y<VK=p;}@dHklDAQ
z)_Ez4sbeu<9An7eg{Td5FHU%c_KeqMpo~#<9^-)m`3Y)Yq);k<EovDBqqhZO@a8d&
z;iK_Yx?F)*J#{?I)sMyH9tcF8qQX!Js$t8Hz7#>cyozv3t&5^pT#vbTlL8M+drvK{
zRCXoghkiy!DS^4nslF05ljkxV4Tw7ovI|_6oT>pefe3Zxt`8Wj7_(iNNiypliJB@w
zy3%GfK$3nQjFUUpARkYuSCuQh<@4JaWEXb%WB7c)NZ(@SeZElTs~l7;f5=m9q?|Ng
z%mV{eN2(ZPjw)X;rG`Be_tFaD3r*AYrg=5{Sfub&12;d@DmWVVJmb<XeOiO=^c-&C
z3x?Zf3Vj+d;-44<ev87CFZ$;wkYRZ`CH$+f%!I#Rj!SK2#injSiJ2$YLINAW)E!^`
zI70Ji1HLrREddKlx*YNn(;6`}H!Ham;GYSkIt{c;ct-lT4y)VZI;=Zsu9nbQf(Wh~
zxCz9h){@X^FvI}6>jo0xH6X9r6GiHCX;LZ%eq%s)FZcXIUSE{)K*S4nM{3JLTwse!
zJ!>Dh%SwwU&p-rub2K_U9Yu2TVQQc@BbxN9khXzjQ$F(`1MRgw4J1W?yljOS_X12J
z5S8NtiQ!i6X?%iTVWl@AaE5eVa9JaK4a$xlq=rUJgyP~Ad)LU5_O8*lqF8hvOvsjr
z4dl6SGXiw_dB#gBrjX$*A4u)YQo~i#N?RbKbiiI}cQx%tFxUVZl4wi{vAx_W08wY?
zh4zw3cv-xCmyX1~0?Oc&DnHFP#-e3zxXASW@|Og6`Uu!$q1R~&@m1Xz#hem&m3z{z
zU<cx+ub3`oP;rQ&R}VUU5${gw0l-$jnGBF7@n?7$?nwie(W}r3#J_($?xk3A^h{Oh
zZOpRArKBv;^aZgLCfKERE|picmwfSiyqAtP?+IsP%{oNtT5*}cL8@rm8kf=oHz&H*
zFhq`b8WFBzoKBqZJ3vRkYEBY8$R)yj4(TI1yZqfztkYf}iggWb?Y^jK)t61^uC7b5
zDR_XNu~IqIGYkw$e3D7;h6o=4ep3d)*Wz;fS&Fk!>_AQG>HIqb(2#swmQ8U3Qe1}}
z0W}4HQ%C_oN0$XV9i=CX;U$}TuGLQodwnQgHt*2jfYfxg;DBj}B{+}(16COq5TT5I
z41Sk+kWM<6+W<n!mPt}~OAMe^J2rQi&sY=?x<asbN7P)y@Z}o1cMYRe@d^wp$!mLS
z`UxBbk2x{9h5-i*@v#k~nF*s^qA@E6XwoTjT)r?V=^aN(A1N^#7T*MespOboF=93w
zu8{5(<~A>IG+0_QQ-%KY#a{!BNiWM$yXrv|(5X7b=7)|?*fTU+k)y6{IPW1}*&k2!
zt+M&kysWZ55w#5{@e(tdJx2)~X{{^oU98hQS6`ML$j16yr*r3Nr;5*1??z4CX`9gx
zrkW=$LnTEyxcyX9+%)znE<-?R;)ovk;|I*`B^j*vZw(+^?@1}oD=xw*ZseY~@1{NM
zp$?o<uTr4PT#_6hGNiTYuTz8c+>}_~$t7aMqde*laQS`;{FD)V<O-WUSY|sYX+Fv;
zh?_yhD-e=N^R6h?hqwnwYM@c0r&VZ4m|#^^<OW2wkY;FMI?-KAe;~?mg8#LwfRL<~
zs+{JXzI*2spdOw#J{}LI1eC2vD!@gYcVR?>iW+hmYC6bgFrxcw%W8{=R~7Y8MT(3i
zW;x@M%^_Z$hnvvTet|<>Xeo!Tm#kQ2>>ohm>ZtE9K@(z6+z>T&k0!QUX29iT;G#P5
z+ZDc51j#sApeCf#tL;h3z^b3ol+(&ID+Hd6s{Inq&hj4KLjYv#%Wt{No#RYU8)R8x
zU5-nWDO??1g$GxGy~`59d7Y5*%<6-t)c}O5s9ZCw9^f-<Wph|`+)F&s7;SEUL1oQd
z$x%|+eSlMe_t5oGQ=jWG8}1c|^5BY-;pM(l01$#*iPVRhG#3WG0s(9BsL!Rp+z`NL
zS`V7Fmta3_6>SS8rYhe^kv}-jG<(|9I1y!qr(KLFex7?C{dPDXUyLoVr93(@!#j_}
zrF8GF#r3<}yW^g@gXvp+flH+A&Z#K+k8}AL#pfxWq<DftDXPc(JeM=FozDBd0&t9i
znW2{d1Ai^<IgHM$QOm>B>2lTa{R;P}jUbf?wG#WPc-lG}#paDsxW&~3JwvhUstLNB
zlgzwC)Eo91*{==oVC+1ck$w2!WIX71qflILH=}R!h5LUOXnD0rV62;xHq#fJP*eK?
z1)rHPRQ>^P8hdWDJRMIHx&^RLaZk``;XTbIU0ysB#TJwCeBgN^6Kw!tc;cdji6GNU
z*gJ`ui&8r7Qr-5waqm;`bg$9u9T1f#SU_ev68-{a%zGo~UY#NpE4dT^N%yvb^j2`I
z?fZO@OmaACT8w(-dT8kx!Ui^AA8X9bSnfPn5b;JJ)0$&OY8TV6b8(V>b)ZcHlZA@m
zs!BC=L&MdWR`IeN<T(cU0?_!^3QIx9^o?<A3dxf9Fa>x1WqZ6LG&D6dywc%7tmiAt
zP5)?x=C%K{33ZVb3bx#%-!!dgRP#uIq6Rb6w2ffgc4&y#%yDf%m!b;nXN-c8DjD2^
zWk|WR758!@FM!Phs=SBbaVw5Cut|1#fEIu?V5+<~yU2wb7sE{3pUJ`a569vQHPhc^
zzGs-$8Az%R@k<N>?2Rku>nv48a`Kp|j-3J%bRL6F)C8ThtI9t>EB#w}K=b*NA>cO#
zq1!yw`_#c-6IKm$r)*IlFkI=+kXeSjQHzpF>M}?#Z2`uY8D-p4L246f4h}E~@^}j1
zSB!F3YDDs&F>(Ckih4_X+{HhgQJs0WkUC$f<eSXsGINta#Q74V7k>YYr*1?jVJ}}v
z%}JMeD20?ru?J|RJ=DnEisnZ=%>_J~H?m#Pe1?2PGOuLwz(C#I1}^Q@C<MEwv$+<i
zu{cFs6|VrCJbdO*)MT853*3`Vu$BTwYRTJF|CR%me#|hI7}|e49>r8xq|Atp(h+06
z(x0}9(o90i^DQ7ar!RChD_o_p%T?fVFMSw9kSr8^rVeGSidU|YIVCf|q@AO=^(M`G
z=?F3l#4a5b-zr90H}jriPQ}-bOXa;bl1E3hCvk7%nkf1{g^AsLTLb%=Z?9&{Z~W!9
z{Dt^z<pWXt7a8=!D!#ayL2s0}Ec1GZL>u6g%!ZPtp{)B8U{H2n80UH$h|eTV38st{
zK%L4?JzSBew74FM7&Xw%9U0I;r2wU)R7%t`O-Q_Pc|8PI=w)>|E>|r4c7I%IWM!N|
z(|K3i%N35ZQA_6$ESQ0)_ip;~2oU$*8$}5=l-)$?GI_Od#pnnxGmjqpy8&zH8r2U(
zZE$}St{~KmIe#SrNR-OPWYj%xLWxS{zX`iM0&q;pZ5z{P(qn{{a?+VAZj=m$xD)_=
z;FoSBU;H|)7((~Gj1WM|3g3)x?2XHeUkN5a>~J@3;5}YeR8_}$Aaw7Jnx?(|%6Wap
z{GD&{vajH;w!{J;R`|M`t**FakUd&a9j^mAT|rm6G~@ICDGk8PLU+>TcPf_}fGJ<h
z+Bjk7bZI<HRc;?r-56-}`Aqs#9_XbCupt1;2{M@!xxilZXb7vKlR7aG6%>#2nKCxx
z8;Wr0l+lYLY1e?pYy&`O>Srsv<1&+JnL<ff(#!W(N}h?Jcb6(3(o<WxRoqn`|AOl(
z&(etI8Aj3z9+laoqRizk2~IyH3GQ?8io2NfsLb0GI5nVKM0NDfQb53xg-spdsUdVl
zt^iY>CJ?0@KHH&z9dQI*Ni>U^&QHf>QV!rW?4PMGKvGN9k&gH9-k~VgXh006Mb=Q=
zQUf}5qvrv%4#vx#9q0mCrV~MgQ^CRpk|h|Y6`AM4KFG^xx(b+T2Dr9U{=(7&x<aEY
zx1NBI{sQ2nbYyNaT?R~<g3Os#l>?G;AkkFiPV3gV6z2j$J7-MGH7>X2ujH+k^s@?J
zr;TX?hvQO-l!gey&>GQamx-kp&8=0%7eY%XUCQavn>iTCA0Tv~v`|Jo8kfai9X%YE
zg-I@+ip#&MN2;s(QZ%5!H-hO7(A%bc;PL5Xrdx`^JcqaFu}Y>@xSvVRt4_p&b>Pyc
z72DI@%wG(6@5Ok~<~@IWplfc9&EI?_?kNuY@(0tOGGe%Vn+C>=%9b^Cw|B*58?SP#
z1@Ao0^U9dw0uT6Xn?`k}l(97coAorGh<hzLr8t?-@Y$Cr<Q`$AyZJmXe?1D_kuj4V
zGq*8ay7K+cCQ)-SOiC<<(mcFn3`J*8inykcm~vjx&eBkZKpKguxY1HDwyB8pJ!w3%
z>XxIAHc;#Cjv^tcx5G%&K4rKI`=jtDVPyeS<6XsU)>^JF)bZZPfy2bobBWffgoi3&
zm^fWBnAiY&S~s7H%VO0#`u~+ls}hus#+%MD$aIHgEBIGCM;Ih)z`ue{TuxER(RiSE
zFbPYGBEbZe^l54vzEt_k=gQ93Epac?b()riWTqYfp>+ga0b<sQq`obldPa)kaCAE_
zleA*KimS+G5N}jGKm@sLtae^&U|JFh)C(@;X5I~}6aukGLbGlDE$~xjo9lcWA^2I~
z=bjoZsDuWSR+>%akYeW;g;3UMiaY59@nN0{`&Xl8AWB!p-a5=Ymj?iyttb=IJ>;`o
z*8u0X(q{xOGd^h}?}^{tajykUrdz)+YNJ)|0r9dosQgIFlb6g3DnHV46P(MQOI3Vk
zVxS4EhSgnZFwm$KRAW6D7SXrreIhQ+G&aTD^N?r%tA%<mQPbj<nqe?tZf9UllOO*4
zg{ZAI@cW<DIJ|O+2mOXbU0xo;ObUSC<`vxnX;Sr`E*Vf$v53o6E-L^w0Sgl^g(P&^
zx;YAcrX?;#nTDY}J*>FLfOKC6ke+ZUq~|#VU2JN-6YWHg=Sk}Z)Vi!K9sEf%>5ZbO
zoTW5=Hyzmrb2I(IXwR6-KHnx!1Ks3C9{g(az*(k-m85t&ZUCDE=cNw-gkkU^Xzi$K
zp1#b>ewLO3u&N>84fIDJn5OZx8Z|*D{dwe3rk}wx<-AQ9%&3Nf(R>xRU>JkXa2w`~
zKFI*Y=K(TI<~t4RD*D`ob8bTO&PX}Ld*5U9GE?dRG0iYssn#qLbQz!%bih9Car$uj
z_o~{|Y7Nd6j9NzOj|52(3Jli={nJrXDM=*~rtv!Mdm~Tjwk*ec+m25is=kB>CC8D1
zLcvHMB^S~ENZ7m!U%3YX4xRncTrshm^$IuJD05^#H#+^P762;WO(iMZn2dnPGcV*Q
zTG6^zldRktmv)|cD8Sx$=e+F|ivsl2Je6zJzXC$K(sK|W#XY>_7lKpZAfGjRMi5+I
zCGxGPX^N{qrV@}aK8@wNUM6j1n#+BB0hO2$<tmT;Dx1Vnv5-CA#|Y&0DhG!lg+ZOt
zl`~R9=+ikFm9_AjKs44eLLf+(krF96*+zL!Ctb?f1WYRpJ7=H*v7R}{d!pv~G!p<Z
zLGqh_r8kjb*Sq4LM!AVB6#}J4AMTelN5pKFT;EcRpm7957yOzA_F-7)sbf)i#JfK1
z-=oX4f@CH}+U7kcBmB<uh5r6;GKI9r0J}lT)D`MC03k8-rE#pF6q^j!ruww3G)r_t
z5y?k9ScL=<1`dOM6SZ89n^7f)o``$7_v^W+wVtQg&%Fatq_ugFnw$*$S``ICVZeYC
z$cCg!G$nh37}iI;0wh)HS{Z-40mO3vLcj6M@u=x&>n~*TCc`y4IN`l_6sio@1TGDX
z3gxiC&}>=m@gSjuZbMRABA!-qT}{aa#TvV=7*J9eoSdOpMxTZz0CR_a`nrrttg2V1
zd@Fb%R}>qT^wgYrxlwvXj7*)i`&eB5EUSHA=DjgRksAO^SS{dDRj)-+cF$)DJW<XT
z&cy=Md5O!#vYG}YUC-0Ysu)^}Mm=M&#nb<fy|)dL>^cwgs(ZV!jb0F#k*6hMF>7B8
zmvnPT(NLs_6irgy%WjgANV!Ms(Fz1cXauDA0Ra?94HpEl*oy`Tjfa(pcF~SYCCTj(
zO^yZ?&`K({1K3!~2`Xi|92-@fLLz0kbgERAmu13C6#@&Z<>WliTe_z)1&EO1_=l4}
zr|!qO=e*xv=iGbOXd<nW><&PexgtJ~6%TU_k8Kfk3%4`3X<oiPl&)>ldGD6gEZmWr
z;v=c4NrR7ZBvV8+11W#WE9$RFP3-bv6hG=4;WX?q<}YO)Ja+@!F`vaesPW3FO0vQe
zijP{RmC~Jr3w%qut8KT$svp!;70M8DZ<VA8AwpthEfcw#8?p-+5tSl!BP3!#<EmRM
zy}^`-M!?OeD*r>F|HgDp^)uX8{o=)H!FUHx^k@LciinQSz`>%s&NWKy2)c`An)ST(
z#lNF#a`XEEzoV)_NvmMeKocH&CJ_D!^eaJd#A#ja;W5K~-HG(mw(I>Q#Ei$bfMtYk
zId7CO@y_8UxM62)XAV<p*Q6U!Goo`|9xWVH20+~*jZCpoA!ZGp?WV@S!96x6qd8_U
zsyAkE=h^6Osk6!G2ykOv63~3-r~iJ25`A4hG_b6x)ctt~p>J$nn~sVq+tFeg0TfaV
zz~?sWgI0$@jUFsO6e^w9cp@?&Kyw||@=o|#%&S4F&)&#T#`kiet_Glkbrk}s(?fJC
zl326!6Rzgx?zK{9axA5RH(VGoya{FOovtP~{xe8$Uf;@l4ng_?MAT**=7rfrrweYV
zk~38oEZYha#5R_4s8aqK=0S~+Sm+nd)n~IZp%8~2iwKI5=TZPEiR^}4RdkMw)Xqn_
z>*UAMk(<qps+xZ(o%(G)Lo2^0Bf<P?giE^8%B2NmXoMG!^(_cyY21p9@s*@|P346?
z$K>~b){+)j({gJ%4Fh$GBhc4|xjq<SfRMgD-3Y@YZjjj16;9(Z`w?m=FWxo3v~nyz
z^oSve!15TxQQ*Q3aq456fKft8<xkE`omcS&h4@1rHhXq&YD~=oT+ng}N0l5An#NA!
zJ~zN)_?Fa2Np(;nYF-yc+^~!REU~_THVT2OG6NvE4D{~@jCWs^ngSv=v?afsj@-a1
z;L1zE6UN?=F-vyoC%zn+bj=AD5_^d`e8NT6P(*I5NCWN~;@tjFI<i!!@i5+ay+X+F
ziolkli~ze(5Zj`-7D6aMALdk7d4D<z|G^EH$)%+AH7Th#$g!D)3KMcSLJFz#?!YqT
z{{I48>t-9$N_(ojoJ%+6!Vef4AI9=7FozyNCWlfi^ahH`7>#cWDP2quq(PV^M}ZLf
zWX-=jOntd6SLp<|gs>&(muG3JL6u#_)-2jjF6b+kej@dYO@Jt(^-eo?omavVM$P>u
z<!94rjU2*gIOOz=bkPnWth!!byfr)4>ng0Vbv)ZiCeNGaYWNQgfN~&4T^L{x^DioS
z`P!$_wb<?id{NqfLISR^ihgXDcwM?7+0FbViMgK++$Z)hs<E6Yoh2e#7SJnZQqnmp
zFh)`c;w)eU+fcj??dx5xF|U?T(R0nlSLsZa^{Y{_IhFe9I!wcLPx;*&%VWkR7R98C
zU%^wI^(k(A{|%rL!F81w^C11Sga-;CSdLJ?0%U|QDlG8BPq+%HrX{j0F^8*`?;=^@
zT9`D*q@OXOP|Koj!9+S1M{z%wpS6kgHy~KJH#PcwqZ|8~$1C>}kcmOjvdG5dv;jRN
zj!#T*&9AJx?YB?-SNqY_=y7xja?rOztvEzRuOcFPhJMCjzMCP;Qti^`xQpl+mplhR
z>iz-Gs<j5kav^Hw3IoEQLoju<fM5|{y^Xtg7X!$b%2E#*+WbPEP^~c3qUxtg!en1}
z41)q_nQvnSQ>sC63G4b%jT=_kP4L~SSMsdLxSkjD@~p9ZBU?E5F|s+Dj@+^tCg|1a
z^o!B}`)5AWou;G4XHzraNZmK?TxEbBPn;Hy4NiH?AAr|E(7W|XI$Ge_-ig%g(YTqh
z&a=L(>5)yzHILfc>B!fq%UjctLlw)_HTun}&Qs>~?jn!XxM7iP{d+lGqM=LcuXCds
zc~(!Se(^Z=i!xE(UHR}WoHAu?D>aQrI0DN){Y)(%=Ewn7=^WWH*ZwIG{2@+Q*48ce
z(gdSEkdBHQQd9pWy6{B2%gtlP#lCAU+3QHL@1Xi|$pT6?9e~Hyc;WyCtS@r|_=XRq
z#%JAn>R(;@JSGiX-fJC6r!CNzWvlW{3RO)To{#BQ&@Vr`eVkMI%iWT@ffmme4uEVd
z0F}jo5ty6I*LgNjrJu{UHHqUXO>A`KNJxDNePxnPr;OJF^OB|o{aPR$u<(SF`-D4X
z#}B7#J-YZcwJyYIGMh;6ad%MN^{?^b;SGOF<ZSg@Q$Ls4`f?xY`k9voLxsE?BE;?K
z#+1%3vijn(qnH@?xf{uk1JF-48oqgV1+LcVw?)%cVnMJAEEY7ZQ3!zrjk<F06LzMV
z`^KQ=#%l(KQkQvMhj52gAPqo6sQYJ-PMMNvC7q=eg7gLMN`pSKAtil3@3q1#eDqtS
z5y1fx>nB}&>}p8zn>^c~v-;5&(wn))-S~zE;}xK)1~eRnL2n1&CE$CQ$Ku5S?rI5R
zg_17LtKV9qb4`jf@6vK=7MN`$GIZ{7+5y!824KvoCZmDLSioe&Yh=KuggHP1(ep^P
zs{0s#hm<gU`F*L;>iJgu%~D8~!90syO4FE;4;Vm$C+YSXO3icfH3_RT1m6L}w`jI_
zOdHX1Hb?d@H)>U*E(EyT6<jeF`d!H+XBeQzPzyjXa}Hra4z@VeX8$rzba-M7wANAV
zA=Fh(r`#2D_iyCCRV|676IGyDzH_TZgc?9l&}9Jzj6qsh{0ydlgjs}J0^Yqg9hD7E
zui{8&C|8ZXj-_im7Weds$}{t7uKm|m3)=s@qyxd3(j@xa_!4-y?L)7js>S9_Zdl4u
zlzSMDdObA@lhm}JhIUA6ruPdW<gpQrQrvVR=2d8xJmu~>6z_7w0d|1pU6a(5+}PDT
za7{M4v#{i}3G~X#BJ`8{3bl(by-B*(SWeAnp-_{dq^UB#1aX>7xg~pK0H-TDIYr7r
zTj^Sz(-k27HMl=u^2ewn>1MTmyH#1q!f$Yp&I*Xas>WZ#9CG1N#v9UQO*|M>iFNoH
z%4sn1IQ=$Jh<DL%2t${Sr>4&n7ipxfA;ebzLqXEpuYrQ<qS8&7yvqOs1|2iXIuLYt
zY#sSqXQHn%(TIKu^*VF3Y1YoAW-jOCnjeOC&_2&jcvcjeK0&j@jrVXvm>qAC8`>~;
zvxJ{P7eXrlT**0fV@Zr?#x${I31klSv$SjHQPt0&@=!kqu(R5&<ftx(`sFT$Ooj%A
zU_Gq`5UBQ{GoWgvXiXy0$jjfR-x|`SWKSzLKCOyFlCL7|bH4(DBnDJ|KSyq@(?zcC
z@x-O&)ZECmvDRB^z}3Y`YEGh@Dwv^5_P++bM!8;VAR7^g0H5rBmZ9{1OPK#LQqp8L
zMa56{xVQF6_`4~8p~kk+E1;DisIykP73w9mVv=su0ZqRu)%;7K0;j2W8`uzSG|Ct<
zl-6gdo{rWhsWGLf0hNjvOJ#Z-$SG(g^!q08tplI5-v+I9m=fdhMB_85Deg;6tLnU7
zOQ*&bEwoXw)e3X~+<A4Hs`#E}m{?`4np|r(W!`mcoeFVqJ*RZ;L-EVZrrIk=tAJ*-
z>AvgJK%m`t9;%6SpUHKaiojzGu&>u(q9KgxnL`k$4<OD6#STdXjCE<sUF}A{B5Z$>
zt{Job7);iN{Q^K5;QkO3y^}eXVC`uj4L1Ra{nWbSMPSX{e55fNA{)BOcZ&?Cmw0v;
z>J9)+uc1v`IFe4g_0(uGehGj=ilJ_g$1d?iL_7@Tw!9s0FrMyiyVAUEIyb;v4ys4J
zNjeqZTNN5meZc`hH9j7Xn{8REr_10(YU&b)er@L6`;%#^sMxQf<V~59z-Q3=f7H^E
z@sQl_vEWvGl7`OGZwy)*OQF}^my>kuDFXsgFbv>{4**}FKo?)@E17MCM8s|{x`b(g
zp?z2uNgPM}G%BaTVgm^?P*NOCV1!Co{KdCRjXM9xNf^|uW5kg$Vdsu?RD6gVAEEg$
z%?D|Ij^<-DAEiMi*MFX)J86`cyEwX=<`Xm;nR__`qa|cqllyjvUH>#UP-pWQQ;*s?
zu02Ro(x|4A3EYNIS8#pJvIh}+NW1Bh(U-#2qn|$qXwqe!Q_YADO~IVO%i0H0vwK5o
zrl^`eMStY*s?h6^CiB4&FdOF#&353W5lRQ4(NdoQ(ym+pDreI$>MOzy851h3LdBp0
z<Y&B9&T~={e(Wijcnzy5hEaAlj9&oy;3cqxhd?6YyMB;vf!X3skn7S>_eg3YCgcf;
zGh-rw9H7Zfrj&FwK&yl5fT7mJvXWTI*|4+MDpn{+CD&w+7Tv<aj-c^`lBi+)FL-PW
ztu(%aiVKb$Z^hGPz~nm>1X~>K0$-H47&&TM$_px_X^C_l1WRq~aymMzp#}Z^BdIZT
z{>_Zm5jsN%RmfD|&r*$e)_0u3;4z*toV&97*cYRR1DLXnNbOyp8X3BNoFllp#_%~P
ze+p{!wG0fe!`Ogjd)f%3WDktgS|#jxX$8LfOk~2=VbC@(>vq=UIS9I;uB9A6upKCQ
zPz~_BrWslp2_DpVB0~==LZ=yikKxUz6lLoh9VV(X(FQba9!-rxvkKtlvD6d~&>+CR
zourl0<sVc+^E~PjA^<~Y-T=WX#DrQdS)n^t)$~VaKt{9P6mXb)0NGdNKG63VWm9F+
z`aoiT8?pr8C4igK?g6faK}}EuXi&}B9s{6fne9p|HN~|w5V7Nw%rRo#f{FUsCBWJd
z+enH}(VU@arlzc@Hg0ozH4P9%Oan4&S_W9rV)zc>g1Rt98?`yUNUnM4z($krE&yGw
zR7leG?@8C<eQ{{eQP$v=<c#$I*kf_eFy)E@0@6y3`Jlm52}%6rgAORNC*YIkvZ4m`
z8r3ND0FZh7fl%F%PQ$Ul+TKU#{9&4x@-Er2Ab%Eq*Ac?rT|99&jV=Z{Uvn>~;s$jM
z^EQyCtVgp?)8)R@=xqFmc4SJAq-(tgxm(hJr6PJaG<X8WpGHJAWPLcJb1={h`^OVe
z6f>lQBpa|yO^JkUIh|^i7g0pHdIr8Z%C{iS2=Q42nMhIS*JbihSdpQNMNjQe0W}&-
zWCg>jl{M~*GDYp-uIcqt+5=EEhxp$a+34C>-V$bq$DDkm2rz&qO%*riY%6Q@(~Dba
z+oYfQe*AKsyNAOgpr0{ic*I&d8YAAiF}3^44vK)ZK9rj85_#N77tlAk7#U^2cwcb2
z4P~lBzg4E(hmEBesj#3h1Ojbc7uDUtWHhhm8elvRMrQ9iIf<>!?kRASbhihi)}=ja
zWs&@fY6Tcu+C*6<EGU?Dep9Gy|4hjavi$?n?{L~sHlUEnquL{T{D!^mq_15&lDbSG
zgx{^W9$40vQ=`<m5lsvCxufY?{AC+hLJ|>%ekJXXGWnwrOn{Vj^(~Aq{T7f@ixP(W
zpJleONUI-^uEy|YN(Lv}P<$I?jOvSO0Bw_*(yvQNhKdYGIb}9A<r<*!_fH);=ulv8
zH8gA(bheZ|6i66v5UA&jxRWkMhmuoKZo>FFw9<k5ic+DdFo5+EJI6n6WD3Y^G5Mir
zZU)R9I4u|*x-4o6gdV`7pbLdAAZt-wMF<yVGO!pfjG>U_j?VFPl*y#)-GEFr`OB2j
zz7KAfAiJrm#eo-K<2o?cq#9FpB&_`P8j~yS>Tmc85H$2#l8rbU&Eb%tjHw4GNfZa@
z{4FLoAl;#hO9mzI-H32`!w~GStpm57vJ0)&>Zvh&)2c)U2^a3p-~#;u*DeFNwl0dx
z&~$@Q3|u=W(vi`+hHQ9$IxWu9JU|0fj`Bt+1bcR&MlQSlea7p7!bzswuM}qolIS&z
zhE-xigDIFVDNRVH+3^Or88VqCt1it+x@IE#9-RZji)xFJ4c-OqJW%~J;L{9`6nUW1
zLo7fmg+FRCFra`g26x}81Wwi(Qx9R3kx@2t#&yF=^ThW8nj9sItJ2Z5!rWbTerD1|
zt~F-6AZM7(63BwmUV(8BFzecmAumm+v8z_dX{1=EWY}nIVN4@UBYUa_cu+Y-NnDuQ
zf)J;%&uW>bd6z-gCaJj&jP8PY2himJjK(4Zu{&^I3+&fDql^X~*TTUDa-cJO0r-q=
z%)tFdcTT@lD{1=VEj$L}bvq_g`;1~Otpx;AC?w1w)IjgrL}QYU>VRuSNm%OKRnH^y
zf?yp+QHAtY1`g_~bs8U=@>rM*JIe~tDq+0EbdgpY4T*>FBa#s4JgTL}1lol6?=jvI
z6rYJ37HI)CTJsH$I7z2MstZ(n6vss%cq{U(l9;u<2^ttF{Eb#RDzBx1OhYCYASK%*
zBuXmiN4D+8oepq?q2gI^>%!XEa%vVazEhAMOAScpI<fJ~RKLJ%LLVs^1N50_uB^^b
zGTy9Lm5iTB0B)@CGE|Otji8JPWslL9sx||R3(9G><C*M}bT>P`2oYtTMgjW8)-`5R
zE4~A6o1kyG$R)`J*P@D}y1>Q;GHC!Ffdd@eg?YIQ)IsV@DdgKG^KLt@>rp6)bp}Ja
z?=a=#Ok_YH%Ur_IzHDe(5S2Y9+5=yMlki*M8%A)#4b2S{Z=hf<d)}soR!AF{G=acG
zQ8hJD)S*Qn462HXiS{1&cEDW5p92id+!&<wrY;3om)CC@(=1HV-Dd#kHf0Tf4#jZh
z5a(nH!ew1PTd6lxbeW5mkUC=;6<gs_^Ez}shaA)(iHV}o>w*gLP`Y*!5^EXO;J&uA
zjtt4I8Widwgi*<2ygFpE`k{F^jV`PJ>6Qa9-mY;moxj3RXMk@FmK7&bvx2l=VHD#b
zYqS=&#z<X9mk+TmiZv=JHXQtbT^QNPJ21Xy7Y4NqX_d}M@NndzBGwrhSdIdzM}3$l
z(xU%Q7%D17H>RWY*U+3yP3^TDb!k>;UPtqKnpK)Nq{fd(<j>2vNqhY!ZoG*uKS1+?
zsp<b`T>GKato`R)do#@s)4Y}DFVp-e%{yp%G(SSqr+GWgTWC(vyp3i^^J6scqIn<9
zPtg20%}>(2mu5t>M)Q7}zd|#n`6-%r()=_Hn{(aXm0u&v?`W$5Q;7J)t*PlKTo6`y
zi3kPip8G&*+E7>jq4U~>x?y9HKWj_Msg|q5plX2G;tHHz;hK@h*e9x{%~1A>2ox|%
z!0_57p8bZMIdWynIOimeMOz$8F34o7f@&WrHuhG9L`FOGM~u=2mMKVEV1L5l(PXPk
zMmI}FugGK{fDAuIK;_zO=jL>z^=|Umh-SdZdvp%pcuP9c%sG^Kw>Hr_&0w1Dy72E_
zl5L)K#rT}I1iQA5N?Lm@fz-OZt5q(4k3iuJR>~F6h-B1IfE-OyV~k|r@WU8@Ghn0_
zk{W_xWWEF}hAvTQL&2m$6CzryiOCsWA0IgcRCA+T7x?s-E&^-njXdh0We}rYI@^fO
z8hD4=oBZ*!*EG^m&Q7AXghh(Nmed7OLF)rcY%v0i5po)GfXUs$z8Ezha3rm6s|ah-
z>f{;<ixS~+ps56l{LLx3<~B7cq`ZW=fxjB$DCkNnU}=M(nMe?u0j^{60bC6r8QDN&
zL1I_@jS4-Z>~KTdX)(kp4=NM{{7XKD=ndhoBA?$8iw{)M#YJ=prni3$ofQEyT*Y=O
z7?7@AaW$ll0kqyO9Z7>WV7M3&fhHtYr;T5lFGf{Wi<R$5>SvZE12i%<lvp`6b5OC5
zz)S0iu|y0&E9jengGGs0uf_ky$~@#0G<}5`#sQ<8hW7u2IYR1|=_mT2s!z^==6iW!
zqn4UbFff<Srzhzss%kv8tsdduLUDMaQ*~b8uA!fqjZmFuN;6^Ym4a#Bt@d03$R|Ni
z<EGWy2No;pLq<N$pe{Mf)gE{#5zjKRW;529D=@U}y9{dRk^_fD%!9h>DDKQu7_Kno
z1qRSX%j8%>WR86ahKy?>m27$BE-vl8RVL{i3s5|EhR0lOQA1CFs`g6gt-J>wV5IO5
z07K<`w6Z<|bRZGACUx!7Mb=^!g^?{0p29{L;XTHnHN<BZv|@J$qGA>CDkgfE&Lf^x
z<4dfH37czAALOnZB3yYK^Tah=Gf5vv5<(YaNB5FgBvNZ-eEeBXV}!hzsuq$MhKjFs
zuDaq47@rqQ9BxUo%d-n$DYtIKg7k>F%*J9}7x@c?U}Vj;3@t#_;$H1UI;umBC5Lhp
z?#2K-_6!iXJRgNOy!{S}qvW_uWaO(toJaFq8jRJOqx8d4$C?eeWv{Cu#<CMMSZ7|N
zzM!8>#?x6QFu+h#1jEEwnwAVIZilzXWJ)?RhAdX0Ag3~E?qZF{=-N`}1|)G&&wA%j
zhYRiZq3$9=bJ_9WxiNYN#*4nvBvT^uqMt4>d<t~7-#4-}(M1=iC=Vcno-XQKKxzqY
z%+bHkLjAq;gF?A}pmDR<YFTp#RLYtQj2{Az4o?_#C@RjDtJBW_mGJc(8B`wvzQ=)T
zz<2^_G*U=J1jI#)#YsAP$<0QL67@O=w%{lTri+QeT8FxxYm65T7m6PMhN)W!4Y2I+
ztU#I>mP**Mbj{4#ey$bithbq5p^0^Sh95A1tExRtm7y6LHRbdoG%$|+H;7*3J-}ir
z^Sf2v)2ee#z=3KT3iSkyp>%C8peag3U^Gdmn%9lxbQBx?9r^Zjx`%Kn#RFhAH+9i$
zy_HVmGsTc@jX5GrW?ngM@~BT)<l3VQP#jN9#0LgXOcWno0!vwDJyz!xSs*toV|cnW
zNvBT%nr=~x)PRE)bvj`F4|u{ohQ1xLj&ydwP3NG)Rp4BpZ~*<0qgd=Rcrtqz=`X#Q
zer_50leeGkS1PVuo7;#cG62~YAke=Niz%7hcc^z!iudRe%EV*d=<-BBe<B@eB27^)
zsG(dlrEn*p_2LNMkYbacIoAsq&S8qY!W^M$P}eYY5qmm>elAsvB?SF6OCyY`MFh3h
zjdJb>v0`J~xp*U8UPE&-HMQ4r)TLRWc^%E`X;x|8kedIGB(l)H`Tr9q{+jIjh;vy!
zVrDgE9kKO;@(TZ<y^X();{2yj)j${yFzfD$h{b1&s^0k|cYSH&6#5oRzgSUoap|(d
zscD9Rx-p?q%O<+2^U7J4p`?z$WGX5mP;!iE8FCbhQXJ`V%!#!~kriRgyWCa9Eyvib
zGJwh1j4mZj+s0fCSmdC##EqY>Lb-7LTQ~<J1t*dQM(fsOtU|bx(v_bTCNd9@Lgpkb
z1`r^}`zG{@q=u}wQUBx9CjB0RrUf@N|2?in0?iHc83R78yhS1*m{$GFnCIeWZp@gn
zrzbWFf+=SHy(8%;DlDYFu8bP6L_rs6cSEu3p*n(Rcs6gJDusTMuI+1b8D$Zxysi&j
zPDf`A2azEO=2no^yrl{+E~@$?Hy-c98U6GcfV*;LmJpl&9a))#*txV(RAH?WR^oOV
zpfCo@LP#Ov<HSe{)x5E5jVX6OkeU|ZOyu#1g@syo8^&vliX)9#s$sX$gF%t$h=Tbo
zhO!{;YUCbN%P~!q-{l7siY)pw>8SX0YTOv(E78SSu024bP|IW>{T0HB3yKeo3q~eH
zyLJJE#w0aS{FY2S5#gI5;_c~l)ondcGB)gjuK|!dDuimlW7ePskS>Y{H>4w@rC7p&
zuzKCYyUQzZ?_`vLykuUrwgf6CGOGGM5HvcBSc=Y3Cb%md#TFwVh$T8k*{11cnBCsx
zMCf#0MZQk-L0ScC(`BKS8n*{qL@>CkeJWimDq6XTsD!!}grq?P+km04x)JSJg;m_Z
z*X(mqt3p3X@;?CS|H#OOOosMtp2(GskQWGhSX6#7U5kCqD%crLUo1I;&v5N!B?tIi
zv0JOBBkfR><)FAEap}^~`2vvc-Aq5UN6wgmE7V`UBVBvNTY?lqAqDdd;e+=3V7>-2
zhP9>{nwD6MrL$7!%2^42t&AxzJ|QbCd_*V-f`*7ze@}r{ti>>!X2kl?3|=}>xFS)d
zbElG1Eo*21G|-IvS4tTDEcoqmDx@m0O7gTr0fu*-!+N$t*oF{`8ZB|t^vSaO(hcje
z)?Rm{3qzF<XW>X{q8eJ+wFMwOkHSWl1uWmgpcal?Z(Qe^%WO-NbQD9;#b{VUSgO+j
znW4$)$SaUoGi#kgwjg3@6~{d+ZWq%|)|lf9f(zi5803-tl$;Le>k&<=2O!flBdDb#
zUy-xqBa?R^;toT(p?DWP(veySjXm3Rxdb5!j;7bACio$B8GaFNnb_?yl)FW=j)oOV
ziC)spMpom)wE=15$@iwCyoIqX{p1_tnSoF5Pe);&fyF%PHULtwby*r2qoA8$itMox
z^RjFC<4Nil&H+c-TTV@%$q%jN8wR|Q*;MQflH+o4q}Tc$Y{?eHQ6g>=w=7bVbi)mk
zedgHI2Z$w=CnFj2>0MOFijA)h8uU8eK}D;DN$T>Xwuu4up~ewFGu4u?hhJi_%=h8c
z<?vop{ShM%@8oW_{GZ^IIot@OAi3$HjtZ5U7Uk+3{T`$d0>sQ60qS&!036(_r6wa|
z1;dJ*>88a|%OaMpw2rRHU4YJpN56xFS>e+#UV_d;GSVeU3<^v2)JWZZ=rY0qDg2h6
z^&ejsNJwmMZ>3y<J&z(@fsMW%>vF@G@no4|5q?mXF|vi^L*>f<4)iUnyN6ksRCS{e
zg@CjWOt<iiWvFaCk*=*Gf9?&?Zs;%`tpYj2g;?K$5F)b*GS(+InO7t2lJmGy;s`34
zS3?_u&TJ(@=n|b)-mUV+%8qvtvMY7^3O$%C(<}{mHg@<yh<F_YA=IeKx34k48eq6r
zEzk$^3(!<BjQ&OSDY2BWh?J!;rAb}b1vgj!2RC!XC>zLgXdcK+0qC>?0MHVFriF81
zaiPl2AA`SbbphI2I@LBACg0|>;IWN>Y(U)tJgj6H4jRJBbA#hUGVV@K_X?R-MdnZm
zoJ+8V-P*|#fpfEr5)suNG^O86O&yYqD;u%U=GxUXS_{ZznMGKPZv3gb0EIUN1#gIg
z-noDuy-8}U@P%grUv0KZ_o&H=Fr%<8@AZIm8+{u=`^J&fL;?fZ4OlF3$+uyCmdCz<
zuo^SG8tP{Bx8SP#d`GvWBiSP>R(V3wl@8^UHi7|@T`5$tol;#e@YhgS>^C7YwbTkY
zuNj6|?x=YTKT|{*{sQ<$^dM*KGD+9^XsA{tK3Jld!Ke~3)2SOIXuTD<E0x4*04(Rn
z4k1Tow$oUy5j2f`Bh_r7p`>#iM4+=w7pH_#?zmJ#H+Z(gP;&)wF!DQ^5f3q_vbMsF
zXBo<!t)UuRdmMmt(N@`Iw;_bJQXT2Ptjvwh-^DOx_X$wwA)<2@gw9U6qelcjgNd?o
zu4h4}j{qOa2le8nH;qZU8+lX}%xc`Uz*~+C-+W1Q4-~$`L``XWoEzVroOv&k67h#>
zJQULKxTblDL0vmGS+O?aW>tmt8_W@FZAv!m@ilm<dppktomDWJ*8_MRTP*$Nir<rT
zO+TagkC=-m%-S4f2A11m3H=tK-vD9{IJJx1Dmb-*@yQeEXa$S}cZ+9rW3gnK2cS!b
z+4?k!@+T@1D-03IXpFQz%%Re`#1sLG4nyOC@LMIBBE?bO0s6T;1c;uv6`;`PbPwj~
zSX9$(jDmRlRiKhbZlCwBd1eW15Uc~e?p?kWoN5zYl(%>hMvytW2+bI7xgO;cvf|wg
zwF-h;N4X1@E5}k3^^I0K+P;<sJbpnd3ONngjYfnt>=lBvaARuLUqf?}ey`=IOS3}r
zI-1whtkS$8HLs-54IzpjOwIp0+qMAQZ{Ph#*}EkXA*WiVd9`L4YiPh?ek2ELlXU88
zpKh!U?F}0vsaBehI>VIzMlC~pv}W1}SAt{j0_Gvt(p(_xP0}^hZ!YCQkfOYUv@bz>
zL+rN$TD)Nn7;bOS-1I=^=kN<VcoM@D3phWgcymo<vOMN~W%q;F&rgFi8Q_b|YlcyP
zhnvpy&J%AiyhX9&Onw4noLtAJuMp-g;MOs@K`<V(N)j6<v|K>tA3bxLX6H7~dc#+M
zlyPx;2YxWN2Lq`eRB^G%f<|lsK;z?kBMe`HLZOobT!W%lGfE9)%(BdJ;;jn0IGeSC
z-*h%ZVxU*IRCd9A93Ul=#-r3)U|G92r^e)VUmpOZ7a7W(+RnSic$eU+2H+A5U4xQZ
zd9A2<&#NRh6YYYGmeoYt1$arKKPu>=fwGFDwS6^U$o*GyS6&vt@+4+X+Z2T!T?b^c
z2Cs{;?!60q`n*9s9p&8wYK3o_8LN~aJq6VvG<7Xd6Fy^59di%U+;9{I?H#)D;#1#L
zHe$4#phm?LuqVLai5XKWyLxO1AsAL{N;-Q(a&F52QI(PqqJX+nNzAn#c+_jDiP9fO
z#-~bq!Kr(HExQh(sZp~46>vX5GcI;=<qz-Dz;K5yaO0VZ@y192F5_8@llCqiIByB0
z@x3{RGKdjonkhp`;!rv#mv3~|DfN*9Co-XwoKM<C__(X<P{r$$bXq{G@FbYX#vQ47
z#q~eHJ<0&FE`($&LyM23qcDfiN)a{n*o7n)j&n_QIg*Ydw4bD-!&~&1)3pwmn=Zva
z@IcjAz>iY6MOB?1aU%?#bm7z@ktX9Lv8`#^fbrY`@gQ6{MPq$;#tlnSD;$NYRWhXH
zw1Gap(*Zf{EZ_FHG3Okh%akr!5L0t237Q*4v?*EuX}B<f5K#$~3lK-sw*^fVLW>t8
zM!pO~b2Y$kty$;tC*E!nQv~F`0;j|_u1}A-w<kyNf51=!Xb=X4*`AVgV6?#v?epG?
zkF@t*izAq<%Q7pH72$RO_t5sosI)l;s9JB=L2ACF^NiVcpr3JZNk6~IHn#;pxxzB+
zGKa!tNau?P@vW<>)U=+fOq0~Pk?ZY;-WNCeN9p=F7GIh0HLVV0a#brf-yq%*H~^`M
z*m+22S)_sevo>sb+dx<i@J;lnt1zCstz(gpQNG9kdY2t!LlYDmE46=0*pYN2z6^kX
z=!8#CI6?<|06rwB)O9G1&Q_ujc@@Bv@l(VvQ8;394Zsd_{5GA9Xq8j9o9N5*6`DvR
zX^ZYICPumm7xo1M(#1wdWn2{kUtctd#JHlGFPgZ`+1Vn+0}Qu|MLx~59p!~Np24B{
zd7il)WtB2n%@e*UqgWhRMSNtym1T}V(A2|SVQmDdNz<X9LD7GWrYM-9DWK){y^vj^
zb>DGPw<PC3lIM(~Y0fV%Mr{eeE#SCnWEH}?nNp%BG1A`yg2UhAfuXS|&%7n1YauGy
z4GXYta#yiM!AiMSFXx;p(;i?vj$@V@7q9!{p)`F41m?Q#WcwJj%CD8`nJn%V5cb=;
z<4HQ2LL6U|UBJdlrWVhxnVi9yystnZd>4xCO2gNonmls%_My7Px0t8{yGPk!Ow#E~
zmMkEXzHzRy$V5@RkUi*;cwA=Wb(CpUFC<AO>2&EtYVy*X<w_Sk&O!S&Y99=R3#)J;
zO6U&HY-&ZgC+_O*RMSP^izQg(*$C8nXST(%EY?FA>Tw2*Y7b&z+V|uQ>1YQp?!It~
znHR(y_$;U9H$|m};Ofh=ql#h;V1&y8(jl0);aj+JCeKBz8InB)s>ficS%Si_13;?U
zB0Mq#>sZO3q*IrEt&Eu}Y$6~|x#Q%JQ(AowBVP*`S|sTr<~3%sFmp9sqGF?x=pd2j
zb?poM_}g^R6Nk^!DA<{P;c&U;o1#&Hba<MsvW7&=5C*Nl=b(lgnMZ*8G!w-eG63Gk
zBITll3f6;6f3l;uB-&U0fT|Dk+AyyR`Wla@JplO2f0PWRvk0TDyNPEbM+7QG<yZ6&
zlXMziEmn`vfI`((7d(<okgo2Lwhj|%upL<p)U5+<EF06s%E@9g9fenGb96P0l2|e2
z3xK9qvrMWkNZqTL+>*QtU%oeMijqBNyl{?$<@nwKTG_?Gne13P7;eSq0>f+O^jj_z
z7%VDb`mK2Prm8>h!0zXvi+U9K42&ue3-^Va(JS|@PjUmg<bI|tby@2uJXRu2CQd5J
zX(<jR|BW#S+>d2m6#`oQ9$;t*3oze8*+&f8<FU8Pc)GZSLg6>?a-V{awO;&~)zH7d
zV`hOpCbA^uV_HWaGqKxZj>~jjWRBp6SOVHV=SJu>@{-dpS!)*P+vFD0mEs*}5MeEX
zX<gohxmbh)t#~UQsI0Qb+K#%#ly2y>!fbr-_kiU*s^I|Vc+4QxxADUrW4Wvd>r(T_
zyEV;5$U#nMAHf*NTmoN3MZ@D)JIyQHTC5=b3gEgV)Igb{QUpoX5XTV%46tzHW@-u)
zqCg=G#>V$^gjN`A|2|WeAm}?Q9mswY!^Z-dD#TqksO)f~e<C$o%n|u2ciXkpjCod>
z)Ds8Gpr6H+hz}UR5ZjDEdm4MUgcKsi4yz0^boTx3O(5-J*Ho*KTI&;M)3t)T+rV-f
zvdfgnh3H}wDbKr;bo!J+=(E6Q+9E3?yW1Ny@Dkf^p(|1+8Qzg^Ll-QSXnlSZ=u2P@
z!-G{G0eILPkhWr@-zrf_RpYQN0U#x36=Yf_4baq=K=#ZC8k{OGM>*n&*fkELqHZfg
zZH}jF1N9tk>CeqMw*bvoVyq_1nE^mXyvDPBU&^@dVUS+nuFl#n>aVmK&{;w>=;sq%
zV1C)XTg^$jdm{{RKYDx_6o5*Gx`St5#i>L2Wju|7ZZT)-QN5(|<w<JHI$W*a)O8@<
zzRqO=XlbJWK0hO%`i+qi1Fx{B$W>?*gO;e3NM9p~8BZtYXL91ba{7G}eRD0<kTbTP
z!hJitJP@X!%5gLv77-#1VwC?@^dYfYp{yyps*qI@H9!V*=&Y|*e7w8jHHMnWC}1|q
za7D3qB%KzRe`VJw(oIBWW3fR1^vx0L%jqb>G9<?Z83QGgZ@-BZeydV!r?kc+xW*-h
z`+*>2e@bzL;sykMTDi&ankuWIuTl}|r<F1%(Zl+Y+kF<UT`qJYKiptN)7N-R1-C#t
z7XjnDexRk#Gm)qRfrI`}V#yOjNM{!FPr2*&+{|q1hCR|0Cd*%Sx(-H`K%%OFC|}Qb
z>eg@Ym`PmtG6wka4XM%V#=ZwwA?6bHQNY+B&M`tB1CWcI1_|eooCQEr_!r@&!apR(
z)&RI)w95b?Il!mMb^CzNDs@VtL4rw~zR^(J0(M8JnB25}1k&cteNd=NNm%AkUuAMp
zur$;K1qCTgJ=AaqgeoMNl)4^nV#rYXiigp%-mR(eBhZ$lI*jaMgu{pYG5q@~#nv&8
z&@$5kHK2{;9G5c{!4$_ZkqqGQe!t0NUK{2HZ;Rh1CdQpJ@C|tZUn}J$lUo~((lg_Y
zXu?Jy0-uLRPC4YrFBum+Rx*mQ%GKO3F^NHS1jLDo#fW+RdX30L!(C%NmrbO-a_=f$
zHZ47vr!hBfnhCB-N29eU7+%ejB(LH|gt};+cHE5gJ?;@vyLsYiN9NSwSKyX)N7oY6
zRTtLvu$@Zu^eGZYPMc#}kJYsJ^$@`@tPr$Zb>0KC8I7~)XN-bElp%Y>jtHxD5DBpb
zXbQvZu5=Wec<oX(Gmg59TyQkyNVj9%+Z&!5c3V%(UA$pa@YpKEnQ>&7EqrZkLE)Mx
zv=H=#BdIANqHi8VkxlnRzxV(Y{oJb|5k;n6iQWzqeM#KFJik%3<^a`fg#p2_)MW>5
z^<Mng*lWzYSWAtufE=`Dw2xq_m$7SuE-p(H%<G1YxsT#8^8O?>|FfCHC)1Io`wfE=
zfYh$N;1<Tpb1H;?gn;}Wvso!J+WZI1p-Wu^=|AgRYS`UD<+fb{GvR1Y(hVaN-=G!q
zCc&|<X({*HQrF4B&0xZkUVe#MOB2x&T`YG*NlHyIbptK)Zf!&z)$K6o65Q7aehZ_j
zNi)5k`_cg77_7j2MYzvIyMU%#8THMU)9KtNk3f2E*Ojm+5!}$z#Ya};0^`jI64#VT
zaZ{S6H>7KIrgV$ZfbmRVLOErvp)+hWbG0nu09t;(h^9*@5$YUAXa)8Z5TaM@hEkG0
z)2R-sI1(ZPa17ZVc2XPh&j9TVG#wj2GE~%12!9<kKHLBejJ`Huiggg(@u96q7X=cb
zaX-?b4;baupj_sQ*M{Lj-u|<1k_0&|=<3BS$5@dwuXS70K)HuA5^QuL9T{#(`vPRN
zFY;WcWN_j&Q&kstEm;`xxCqw)RcnWR*rPnVol+C2pJ~Ib8af^2EZN9o6N@V_xp8f3
z>UQDA4nCkG?P+4~CA1HgaqVzj8IowsAJ0O_Q4KYS4U?SeA=IO(vq25Z+d0)zx8BrD
zHzY}`&C%5~GTAr>=Wj5!1Gtw!@C=G$XsX2O4d%Wp1a(bX)x4QBa6Exz0FB@xH#RtZ
z3U0Z_;X?>q9|Zftj2^k&DgL7FP}to9&?20MR^cV-Tq3Mm+f_PS^DrNj5Zg@TPC(bT
z|Ew!Dt{*XP-UB%>WrRY01!`2SLiO)T*YaB~TiK2uT7{C+P($A>j~>KENJ)h&HdZpP
zvN88fMNk!)lDh`}LH5cCtLGn;riMZYgKHB`bQzaw(ANKWI?~0cbVg}gFt3#M_j6Zk
z1}_zh^C0c;8UTaY$TaPImM4HpEi(`qFqC16_Rr8oUoimv?{y*=wWO(Mw!B9qwsA7K
z3^jQSdB)y{$>51lNzgaUYu%iV4zru^H;VmAC-c7i>KrKO#@xqPgMON-_)#AdxvJ&c
zD~3_AQV4OjLCaN-_|FeqHRn8s#2zfC<}5msHKhv*imkuUiPtjd97|E$HREf;OL?CE
zBcW;--9%ynq;u{GJS;3oztwZ;H1^e(9LX6PkLHx~Q|VfmO=BMkh9YjO(|~57%#_h*
zMPGs$8e9ZRtLKXH{mdbJB{vMVw-qEBEzRSeDzUhZ5rA)EEKSbLlul35X#>&~jOQAV
ziMFqyMDEOETB$m;a;+`o1unD^i(r{f4p`QkshKfoOH|vOViRW|UDx_J8z#uT20EZC
zFgZS+2e|QRM<y3ss{yBc4CZOQ)tOHr=;m)mX?l$V2FO6Vz#Z>2fWCSZYqNsOpz$G2
zMn2>W^Vj?gQqKaAM0DBMMBDXl<A^==jq%)YYW!^B@=f4oL2U(o7+`EezfuZSB$4)#
zM1~qzyASksXk~~a=6d4T=KM&yVHCcrF2Ii<1T-+Q+j1fT2kn*R32ACYDT-5*bk{KD
zC^{WW#1QASYfoUbk9mCp3Uz?RUF{+m1e19R+~?9=3s#*Y=_r=n88iy8=((!fWB5TO
zdRBm}E63K|N|cSwj1(f=;>L;)*hT#XG^0M7RWR3tcjYg0D0;>qg6mK#!2Cyw4}M?$
zP_g~|h$g(O7-|&ca){YtgwB;R4VWlo5~x%}L%NZ}3uy(FG14m60%lmI3BRCC1S1n;
zH)m*{{j!!nEfT&Cs`f5%19J;>S_`WrUtwfT=?1JA0D)DPd!W6=0kcIvabgPEWt39!
zK~!`$qdPeIP-?!*H_j_Ry>=&^@1nVzrdrOFlIw?@^}Tdyrly2FdP(sZV=baEjkRcf
zIyJTXxTfeGvbLaVl{3^76#TT5m9eNv8xLiuUN(9-k)B<Vrii?O*N(b{l=u>oJMF@%
zXbGW}D|`7Pp5_pTzo$B309ZEnU2NtsF200rnNRl->To+MiEM;}om*3rxB3{jD~=ty
zSZob3qu+Ca1|hzVTv@>JjZzE011KJ*9y&*)3Sr|jgv|RC`1)R(8xmbs>?k?Rw!<9y
z?ubSv>D0}t8o@UqNM8dZOFq6hFf!C&fELK;NZsV5g^PVM@MwLKngzJv=aT$rxO*jy
z1<oi8KpIQQRs0e=2axfIC^Bc1XTa4P_zGJ1faB>}*uM#P^;-s?20+W))#@RbyC7rE
zG`%hzX`7^db`yRP&73I&1PG?gy9`?KRR%~GZ`J|T`N|es*UQ1qVr7R6?PZ=`!)13<
zEm=`#_#VXe@3xMj6ZQ>&xndW*G)ORR*T`HslCH%v0=PBkVzE#eQh*zDiRDKK>lYIg
z!r*y5U2{-3#+0&5ssYt+Lr#|###x@3!=OCsG<4P#y_*~QRaan4c(p#y!6VVP9-(Yl
z7IEb}f@7eU3))lPqYo<p7_KtvW-WAE&oNxkPp{I&{X=eY8tEYzZ-|(6fy$rh$WV9C
zxdF+th+w?@YwM6spXV3zeM{GnB!U<wa@#_7Mb7s7&{;X?AU^(|I5K)^rA5*hp+_s|
zks;#vOgbtMt`-=%%Bb_|$pxhjWa1@9MDHUQEz`{iVF9;<p;1S2HVC1&H&9<>$}U6c
zQ>+(Tcr!qoGU(rAayR7YnXbp%*N{$wZ3Cdp0H2L-QS<C$090xKfLXb^#0|rlC%GXq
z8srkS4>wB}cRINfI!YxkR%}hK`FRiMH8(%QW3dD#lfl>E$zV((Mn)<jBs<h_2dtrZ
zycexj0NRL=-Dl~SZ`^gIrs%A5;QG6mmbFG|lwH48Rw}^O9if$f_c8Uzw%zY^C_c+}
zVYXf^HSSt-c(vz3_G<LoQIxe;5G*gW{1%eq6S;7%LwgJk_GCOZcCOL}FHM%+Y}CVY
zX=!56TheJH!BC?Eb=^cB(J+#u(iu0cKs6VeWW`lX85uLg(clf3Jc<qa3Z4*EO{r@b
z!m`J_O6;i$zTbd2isKPfsDMmvMRzxltnDBmPB}vERj}S99XV8d1O`|PO-s11jdiuE
zhWc48bG6iJ*(x&BQW(Je1&PQ2oyvdAWU^KT+`^di*jX5*9a15by7;XQq8A^VfO%$K
z5prgG3aSPDECdV~?{<v>V8q%xN4o%|6;-Wvp_*~AUUw7Co54`)*|FL;NvE-${6sp^
z6Xz<TseVM8V6^+0)V%!1HFVL4fN!5&0?4q>cktMUQu9h8%RA|J7tP%?pcRp&@ZHO4
zGd1-#&DAtHVtk2UaTlc9jPiG&-|z=hbNDqaU@qUBn*ZmY*oIMRK>x=W?_D(SqxlJ%
zAE)_An)lNDhxynxOpd)^aNis(JPAlYq0k_De_mBXq@v6M(qGKrDoyB-`G-||_51>(
z{A)&0HC)FYGL+Tdw^RdMMk@>hTpkG3t->BtP%9l8BNqvMso3}`3_Q^L8JeielWNKR
zpJno}FU+e-n)mpv`^V5XHT3tuXqAA<Ief`c#nu8lKx{1H39XsGxpXIQ`$)Q0)>Gri
zYEPk6VR9{I6wd(C(>8X<Mc_rKn?J*E4OzAKBd9%T3K9KesF{N$<^9S9HjwO9%!o0t
z9!yHQ(aS*HGLW&4j#&x&C@gD0_FS0_tH;Qau3*r)E#_eERycQ`NJ*>vFEG@CL*bLn
z5e>^kdd7Gr<oe`oxZtX<)^?!XC6Ud3!TQy~Bwf>$yJ(hItK`1Y<O;98$23Th5;v`F
zY(xVpmj^~rGFPK6lkK?X5dWT<q*Fh2=t5dpF0C>g6!2`woL4Me|F}&lmYm?>m)DgX
zs}*hd>*m8Ln6Kj(3^1%RDXe}D+Nz}9$|%U;efK7cL`oCGD?X9xD@~JhO@Cw&U%<Oq
z+Zpm~-NL%w7S9e4q&AP~)l8R^KkG;m(JE(&@l^=jXc(#s9=>V2gtSK)NZAGb9>X`#
z0GDvA&dBLd90eFtT4(Y|(|lHv0IA==39fSbB?g^>be+da22iB@JFDJrVUn(ic>_7k
zE04Y~V*vRmT`VV*4C=KV4Bm+G=H7w;Ba3GvP{{izjP&cuz2HJvgG$}3U#fW(5@u*+
zelEq0MZ@MG{Lbd&uB@EkTCQ$IsLSw%wOWu<iWJv&INGA=0(cJtWrUYEv<KCE0$3C&
z=jdm^LEll#!SbH+J@{c1?oQN`h`g57L{?!Ph#>t;0Y(x6XRVoYyx8ovoUW;+#y(ah
zMaFz9-l2q{TeZ|gq*aMUTePtES_W0pKqNLs!(WBQKP#E{JOda{RvML^(jk@9EXf{C
zkOsgIF<#n$YO&)8#t)GLv$6Oi3`mE*S+(MByvbmI@$qz8`~r=k1_P{N){WK`nzPEw
zXXx_j)L55lz1WAIN7b~G*<NKgwuz`%0x{T*g(@spd{a!G)2LRVesCsoHK<#gq~`Bq
zmB+Bh<aUKF)&=f%0Q$Ys*L?&sVtJpvDm`lzRtRolD0i^0aHKo&Tay~RoTYkdDlKl-
zx%6B6nq0VQtxDl40bJW|$nI9X*uREQ??Le%G&S*b+36K0-_XT^Mh{NAYrF&CzB&_0
z12?p+?_mxTJFD13(N~n=$B=$j&vzK$D@d$G-VQqGo-8e^=HWaV6+5u${J(*qwFhHr
zjkJ2HQ5CdEB$&g`-uamb!H9wouxQpJ&tuS#7t~67pb5InL?%u>NIZakKgF}wb1Zhr
z55Fj4P-GHkHsMmYcOPPuSaD(i&6hl%GL*_*aMW^S(AUknUbX;ef5;8@vImpx%ni+E
z%hDRQPA5_@Kv)|3tuT4y_Hy5$K*lAA$fNE^r!fj#h;-zG>8NH7@pBw~gyzFEAEWsw
z&7CxmSdE_%KpJwoP`be!4Q6{3ni_&-3v2@IYK!rZF^F-jt57R9q$j#)a?jKQ6gnzS
zI~~n^{YSlI4tc53GW~ji&TbHpy8m9O1AVpZB9B>Y&|}9^z=?FN&P4BnezMlPYSfp-
z(i_Zn2@{m}-<#~+#FV?3FCEY-UG~7-z)+o$DJ}T+EQZ_P)0+UUsl-ek!6I`QSkDEn
zJf>=_Gx7zxC}#O})+<o01X5q#k;EMUa-Afvti(&ZI2O}aDgG>q9U{u`uS4Gkb|Fkr
z#SN!(rA-eOq5|ft5{KCg+N`jsE)iKWX#7tYJT@gKQ@TTP1Ab^CC6l&2-q)a?E+!m3
z4x{nu)Wm*A9_usS15jg|BR2<m^j3K8a(V$>>QLPBtbecTAzbQKfm<{yQfv+BbixHN
ztE-lPdypjdI53d`)kt-w-vCG#mQ!QW&^n%u#*Exm^myX)QiyA=#q4rJ2wFT=1GfQ<
z;iaE-&>r;YCj-u-;?`Qam6Zy=rA22H;}@&bt|JOdP8q`)0)e}vRydl`2#_X7L$Lxm
zmD7^Dk~5CPQV+`w!y{?B0cdKTjM6hjnx++iOfv?3w3eDE$G{Y8BRYbx44^RQDh=Lw
zB1$Sep@TdC0*j9)dBW%>%Bf5==P(#x1k4we4IkqP<IWZN!4rz8VTC`9QO3{ES<5dG
z^)3gDXRaF%FqA^0*6OXgVCgGQ8_0{VygOd(qd*9QWjs3u0{<-MW;f%oP#2zcqHUnp
zu7z?!P9s6XEmim1c7Y#}Z`lR3&I60~Ku;3La4nXED)fEbtW)9kccJqtgYH9v9*|1n
zkZcBc9FY%cqF<N?I(O_WEI{xOog=>lBN?wpJkDdHrKsfLW!?_McX(n1R8}<o?S)!q
zY|*bc{S~?d%Me0ifq3a2CFxW=&vW?Jhs4%+yjfU)bhVPs9Be-;HQ-xh;3#&)*;-6A
zPqmkqQ97+aQ>1+V$W1i@=~9{Vo@ZoJs7!+_#l8aW&mLXrj|~T)syq1^U_K*MhY*DO
zOBIjY>L|sZV74xg88Y?YkqLB^Khcl8QYbRdGg~-02&)dRFn~W`vBV!VWy<WfLqnI-
zwM8WnSe{_`DG=nl>Wrq|zn)9ob9RQNK}jBSPfW?PMMe9CNjkOC;c}OE_1$6CVZg)C
zMMjCf8gGQ+GODbwh@j!I$h)p%!KVn7UqMk09Ayz$dcZeRCKVN_hL>ys-|Ee&X~2{o
zWZIN7NQv38^Ddh$;x9|oFW8OKY(pyxGvi0nk?vD{^VJUx%9U2SQCv%-GBLS{n3kcT
zN;0o%6~9ka#o@H&iRXa6<k_JSBd1?sl)3MbfxZhZej`P1(57F(jfkItz})2}HxTA^
zl0(}OUTiXQBzR$x#*2bDknUWUnijM98lGNb3MKUmzF6qXHj19{Zmir^UFH}MHS$F3
z)yP&1!|@?1ZE=Gs`aze8WRF6#h49PJFreV^w<HOq8*pmOU?O1!!*YjGryXh|7Wu9C
z*t>N!U8}WI6Wg|qr6c2Zw;tpjTl);J$q3tt2KWkr#h-<GI#rW3n^%-Spz<StE{|FZ
z+JZQi>KZa0<*d2y2C3^n149kP%s9u-1-oy(U_3ud;DWE~#LBZ9vkc$%8D;MEScszu
ze;cyq#z0qTC1PGPJgIaZT0szB2I%r^2hK#)%r%z*%{5Gs75Rm%6)GY;*LT~`4OhKN
z4Q33lb{jhTaoB(g(UVNESs$b|hx!j|6rG$JK|e7!#~uM%PGGc?H!<F?DmlPsF|MRv
zAs#+Ka|v+Q6b41Wk|@_=qf#v$xd^-}K#XT%;#LdeSe4z4>ir|>?i7g-=@Tc?k*}j`
zsh<O&_=Yaj*hktG^u7DjQE`^$0UCIz5RNlvut<Y4H73=XB}FBY5Ma)+z#2)vNul&R
z0Pq5g44JGBx*5t?<yDxz8BeUi>A3)dK^5SVXQk;(HX^JBSyKF?h<pt)<vjEfa^Omw
zA1rZs#V>suvaeg0f=rFOc<gSPNoqnc@jceFjWQ-s?Jr`pqxc9RgjA>bkkTZi({y9}
zQWoUn>4rSFhG#75H$$u@hX(^e^JEha_acVjQJz6Me&Gb>f1Az*`Km^P$$Ox0wPJ@O
zBO@KX8n6mO{jN)&XH6drI*cPOD+kNmkh<_=_x{ur%%<^`&c6sOel98R4CyN%V6KBS
zPV{aR8=#dMYFh7Z*8$|zT<6AOzv;-K#$u9Q$ZzV1jJ2DlbOzi$;Og3phQs8_eZ;~g
zHIoZJx->qXu35Kr5y`FNT6wqRif;tqU}QLR@%nUR1Yiw$fGCF<#>e~h5EFNkbe%)M
zhnhw>h=6PXuC{#*a9a%Oa*YwiKju`7O72<%b$P0UDaIu|Mln&mifhV{-0}@AzXGa>
zEVMKdM}y*Q#~Ac%YTWlAg}$nWVl8wcCZ3u|qw7d-&_+W)1MtSJ>9j+6M~QWZP^>Ni
zknDEnS<O=!ui9(txF&oh(&?9JBk!)BHOE#Wq~E{Rl{0|pgYGW2N;xQOjMWVKX`eL@
z+v);$&3`R$8b+;gqXAbH`LC%EAn1pFjXRr+d<>DA`<99pX_Bs$#Bh?%u*Y!gu+L+7
zbx`MTZK((PGj1#~@+X;BV(Ty_lXU9Ha|j|=p-k{xr>-B=>-rUi_b3ctWZkTk8bB}K
zRC_&`*}J~`8t(#uD;(l+8AU8<?DDth;tHkli_gwrkMWiN7nefP&s5sS{!0c`Iu+Xi
zsOlE;(IVG0ZYI!q6?6m&3&tR~rX#asziZH9UUwkMJU47y1JVMn6|fX5O$Xd)5|1mQ
zM*l9~8mYQ`;bX;#)MbT{<2UC@82r%rOS&dnp;7HCFOcMEKnn%qZU9tIM^eKz{|0&#
ztN$7|c)m-s3TNU?GwgwDsn&cgqgga@7pvaDs_8mI>A;r|xa)~GBFn-7twC(JwD>pG
zMYPOCYK!=0i(2Y`4)jZqJ>n7oH=a}nqpBuuh_~e|U(%Dcb*79}L6rnN+KeZKz63Qi
z6bk>`4kr33O*2MTY@=j$M>>5a`%s`&O~-sEM|aV{qu$*dxy|`rj^JPf{-5UbzSNX*
zCdVozH^9xXR#>EqFXhCV10$<Zes4r?`m}sQnv7(O<+Uoxn%nVXL4kg*==493j`B+D
z6@*X^dl6*jLJhbY3uL18@pL-31U5;hkHGHt5_$N&mL^z!5T17`p6`7oozAU@!F?C-
z)*ebn){Np)6fk~VWp^$ah=;!|jK~)(rOOP#oFfU$>rGN~m@XJ@{h&)JZqV}uVoxJ+
z2mK}cK^OM`?J=HFU*9D^(DEIkSSUqZw(wD_XH#R`X&z-w^*-qgq!*DtH_loxurhlJ
zNmOUF#huEcn<MH@#7zpVOATf-5i?kfmp_5eS4dqvZNez4mkm^0SG0sT)pzGiy=1mQ
zrB=qQlJik$Z+zg_DmnleRX<3wcSCBX82AX;Cmg}}T>LqM*qTyAAOgYuEu0?vnB>e!
zh~^6csVM?<HU_ci-h^~t10eq=XBoT&yG`A~cSGXXhr-Ce!{h^%jya+WqnHo60X^(E
zo&6k)7HQ1nQ7U0Jzw#QQ0{Sj1YU|u~u5DMwBYA`=1`d5D8c08m?qQBXBhAq_MD^x$
zWEm}fWyA2=2$uniTr<+A@PzhHgJ=AlzuvXQpzbS>6^%(cH5`nWsi5ilvDCPJWZV>j
z1gc*IzCNV$k4f&W=pi|C2|2_2HBL>l(SO(>9$Nt|Wn4GmD+xvlQ_we=cc|VZorcS=
z49Ws>&|!GDtqw$0;o@4njR0Mqa}I<{|1oH$Bc*t~&C%5~Q0V1fNnpIo;OpykuFurO
zvYxI9ZjYlVKyYKmsfyz@8|z3z?_wU-7gD3J##@L`!$HI6F{L@|3PTwj{~fNKq|uf<
zhG6>HyB<ct?@UC(VHfXmBYxG(wH2iVRP}G>k_H6vmVul`?)8m3bdEYJG__K>2_@Z+
zuH$p*O%E})b2WfB;`K?c8JC!mT8f>bd8&)r{Scr%sIi0wMn=-qIA@oUT}+H!0>I}e
zb*F@@23e*kd&}vx&v<J0%hr}w;blobZLBsTkO9GL@Ki+A%FsZ9t;t$+)hdMBz?#6~
zOD~!)gRy-ebx<YeOU!njei~`r=MrF;z0a7}0Bs5lN}7VEMPq^*K|b@A(6oW(N&6<F
ze2G)nG+W+<$p(RaGAdVPdNLjeZjzxi4&@FPvo;4c9XY_h=+w2;GR2g~EzO5*HdC%S
zJh0pf%p(Bu-5_^V7+KE!oCikb|MGM&>*~T+cuY$@x1|))8Q4U{l{vmhKZ_FY;n^rt
zk&;C{8-Nrp!7$|>FlD3a9O4Zxjb8l}{32a8c-DPOvPb2V8os_^GOz^^qe1|xt5Z$^
z@;gv%9WabuBhFC~9ho2|GGS=t%St+Mw}HKlp|%-HlXj6&M#`E1F{R|3a<tF22AyTu
ze=kG1CNIOq&~L;w-JP6P98d9>Lg?2Z)E?s(L%+C&S}8t}n}VDA=Ff<9d<DbHQ1#74
zGi7&$8$M?Iw<)-i1iq~*8yt1HG4KZ0j4}O@s$rRFz3%~eDn@<=$y0-@xn~(g3EL+S
zQKw_^8R1{oO)y(7et#aO%;oHo7~S$4oZ3GZ^^9$Y(isgkT*w)LnrEi&<Yr1v?!Fsg
zHhooWC5a0yzFcX%re4M<(BL(gYnRDgRB-^aRQ23B)V$gx^?Rz38rc}%1Y`h<KfepO
zu~Gxlvd479ZH?i2u}z(2?$YJlRjHXt10v9_(usog=n_6f#bJ(JGYkfBYjK+azRIbE
z1u?hKs7Z_0>lnoqtSNI?8kbB41d>?XzRJk%s*C-*got&X)pA*d4c0Q7h3*nhB2B*r
zdz6iBub~ii@KEDT*feBHv3p!+LM6UpPUj9I8xmV0lx52E5)!5(SUZxA%y4}l+v1NE
zG(E9it;taNeMB*`F5pX@YUl`=ROCa!5WQcOu!w+6alOaY0$0;Y=p537HqL!nRgnh;
z#mrKy@lb^Jbhh+7K;%PqkEWyAvDElRZbWmi*7v;J%q=DLJ#Jso;kFs-U&$1X?gLAs
z&7bB-JfgEkkS>-_bRBAx>2T~q2bsP`=0rMrC40|-ZyVwmuvkYgVTz%+6ZJK(27w5x
zd6~qvN~h##Ec09Yk~4lSsR&$?9@Ejg$<r7q2+>M6ifd^wD7wmh1+J2uT*`@keL!le
zpde|@Mw4_+aBGC;3blqTb|foT3~EtKa=K02t$ISQgN*7R^z-y#jxO-n7z(vG)kZ{&
zB=?au1GM<!#3Y>>N#FXXq5U67dxnn<8!)d2`cEUfIiNR^wn!v`JxqPWgctZ`wbUry
z=2jBBU{I-6V7aoJjr@(f7Jj9b|M8TAy7&8tq{GlIrz7{PbP&;yI?#_lOw-RGVa9l2
zMI{c8U61{XG(2bE3LHl8B2X=vHY3^bF7hND$xG{wk3t+-_U|f+*O*9`YJJG?+|=A7
zZA2XLgq+q&x$+%_1*ogU8owxYTK<rs>h4h>Z3AQt_{7pZuJMsa`Y8-ky)L3+E`CV(
z2MRy16i_3AZA?p_$Mo!a2s5D1f}0DV3!h0x#eJ!XPnt4-BzCv;o0!~K$~f$!6}Q|B
zI-v78NSs3rL+1v_C_dhJNl39~T9v|NilIW9I2xc}x%#17lsyeK4~!zZrnBLU-oOFc
zNJPz80MMd{4~h3MHx}#zTuwDcrtq=!!N?Oj@1k_E@+18`<;bF^sYijm(#W)!QmycG
zyJFcka~N5g@#*RA5pzIW!O2Zw#SejpYwCuuIjp#keivv|2<I5lSqOY7HY#3>@-VWo
z0EGsee!pOF)Z>O8Op_KG%8d>;M)#zqz#i!1zek>5mrmWbAWbg;!5_j^gY8`;%p!R#
zSs}Ku!2*o7flp_x>s42c=6=7DCUrk-4ueL9tL5j}m)wdj9&+_~>QXzB8cQl+Z7Umg
ziKCYAVG3FfsyH(80UYcp42-7+tWDBU1Y<m_O^k?)U_$+k6}CsxsUd_uB^(Q*JTDR9
zYV1B{@&Z(wLco-#m{RYoHY%O@U1<q1(*y}AM0Ay+$MDZ6r_52imxkfnQgS&R^_XLN
ze`<=eG`Mn0*(N4N^+LMJ@bS5UO5fsJR}-Fq`(|UCQW9E)cjh!4Gw6p;W`Gc>3D<bm
zHL7?|2~;iKhOE`nsew|5$0ExI870Ed%97)bOo8GC*7B$Tj5U0B8{ge_Uas|SOwIah
zXilc4_F9g*G%GZ(qj^2eD$N^GQ~Qe?eLqKkiK92t+{EddIQjvaA56{RH#^_VjUT3Y
zE6rb~`B9p8(DZ12gr-mPcAB@)oT7Of&5-8DXx>HhKANAP`Ei<`q<JsRh-QuE{WO1t
zW=!)_H1DMOX_|GKcc<pyXE+*g^d6eiG`FP26`0<UbTn7P!zgs%<u-IS%7`$ao=(jm
zpGHotW$b$G`gE$jioKzbePNQChhcXpNj09kPn01`z=lUI`glzNG1yjUvhZuTOSq~T
zKF@eAZfHxiG`^>#KiX&B1%;n}mfB56BCkL?UC=U&(q8#VlBH?7p<R<2gGoAVGn7_z
z56fje%<95oGo6;q;b-~Et2sr%%tQ=@zJMj{L&Vsf%24Z*)EI1Qq|L<*D;!?AR~xr@
zY)3^z#SKz*4s-je(D=DJIsnZC=x92c+hHYC$I>+|jWo#HtmZz9#YE*x@bb4N>Bf^n
zj~THTvX2NH6F0E@D5Ps(*T#4{!vN(fvL_^>F3aQ~LL)MBf4kv^GNkVw(z!&%En|f9
zN4q03XSP01nDBm+Ah0eqKz~;tLAtBy13n{jU9>L$d4zv(JT*~i0}q)p5lHU7lEopW
zJO>zg`N#ZC2mYs8{xKO{gQka;f5w$e#;Z)=77HVaBP{bDjM6?AIU<~9JnMuOw!Vfm
znIuJcDXN(UM;HpuXiqI*Ym4oCkHHD}ZqhtV^ZhiYn+9^aj>iQEc8A-+#aQz&q1^hV
zbgCn@xZ;aW>fj7yjI=roP=^8WatG8^BJKl%!&x~p8T<M$S}&QO>(l|GLx5`?E|PSJ
zBhTH>mDBa{a=LLNlGD^sAU*<_Ke6*_Fh7lFGTj_8`3T_;1~?J2YgFnojK0ht1#R){
zZ9Y3mM^7=EetJb=MQpd2@&oWg?NLNsY>%xVlXOE--enX`WL`FTDg8C0l0BM{MYs^=
zl_~uqhW{RoVkYqI!@<k)oH=405iG}GnYTZ_9T~C^U>sqg+lkJAf=d-@^3?Gd`Ao%H
zmkXon=NyxCcK}0m38oZ(m*Hhar(&bYcP?Z69uvttD|d=YE;|^r`nHZ08wn9Vi-Sio
z3R<zQ*g_YJ9GhU7UraKwQ;JIr#pij#T~~`B<Cj%;LC_a)wBu&39q_dvImD=1PW6XO
zM!6P^ceI3)boaF86%xlQ<%osV#v%69Lh@OpSnKU8vu?)gAjOw}>S9HRh9-l~rlrbh
z<P}`AW~A?l3IWf$Mb#};K|iSBdaad&a}|5O4<K(tXwK+o5Y_?+{wkxG-Tf@Y5rPqq
zxe@J9XdoB%c_Lx~nA}3o{lJZJtlR8$m7CIqDWB#EzpCfX8vl*ef=s<yYFz6zm2t;g
zxJvFui66|B9R0pjv*!D+7}R_^7CabAaOGAm^te1QLm@Ov(FLCO(etS6aH>dIz_DVz
z#uJvZb@bYU9mGPpw_tBwz-pdOE@X(IT!)YWYnX!2ef1;jAP6d*LlDgSko6?*LL%-5
z!9s~uETFS$I)HCx6B1%qz2t6?4j^OiAZ%Qmq$ajLGo`P-842!JdZ}PajdeygsyAqp
z$ri=jRX4X?(J608o!e-hTH!t@_u{y?7H@Dj(_J%2Yq{N{>D1^ZUWPoDPF=2^K?A`U
z!ITkTd<XPhXCFaR^;hA4PEJ=yo92ew1{Ob+7C|qxFDo^*mgXDCCf^2&m(b7-3bCpQ
z7e48Nery9@)LwBXI{Kgxp!t@I!Kda+n`p&LXBdTj0Jq>`Y~jm%e%ZrP$@)F%ZY1Ow
zj~?s^dyiVZNsWgZnlHCX{R_n60>aQyage46+IT`DD(%v$MHj<>uolWVfLrr$rCQ-~
zEnm6UTS=E+r=Q%9QdkwRL`4LsLO{47(yG-ttWU5)aYR7$YQA2gUb^W!(rY;5V(bOb
zj~6p55shU#(E55M8<_!a9g6KHe?4{4Te<k6H!z{}W6|G(66^S4zls4a(5phLkaH7q
zYC9v%V77sx$KA6~&A+5t@-a^-Kr3Qa1a9ud%t<<Z7^ci=fYuh_+5j2rbt}xF*H!5>
zX<EnUnRkZ9n^emRJ&DaW18mWFx})Rc30I9Q{q1=7#il@*_fQvwW(@Rty4atCf0HTM
zwYeUe|2p^#mg~U6YK7YsCQ^G;Y?Gt|bY}0m)L5SA;is2S9M>upO!?;TcT}8U8TcB>
zd<$0STrK%D5D*QX#VMH_<)?Fhe8YUpG&c$rz!!lW6xU(sG)ruv5ruw1|7)<L2E-Z=
zVquB8=kO1v(^^S`83_~q;^^=^0^-XUN?k0#BQO79eza+J=j)hxk<bd&$y%BtVnRe9
zUQ%J+0xTn|gb=Zag#48nH`r4LX1Ne=s_@v1YoWwQi3xdx$(kju8I4#6R+^^B&<>KL
z`|KU(2xqf4NsWJ?v&z(C@<^bS4M^-JV^bMX2<HXhu0if$04^Y0J(R?)K4F_wNmYYp
zU|<73WU|J{NY$h!kWoK?RJ-ek&Xp2_lOPjxc^=Rrtw;Wv3OWwxFDMbvAW~3(w1RPz
zyDg}u-3YP+(0IZ%293{>GM*)~ZHTCpxQoj|fMUCZaIGTfs^9oTI$D7Ss+D_0Zi5<i
znTIl|5PG3iNMd>zpHG9A55Sa2jIjdoO?Phi11je*=*YfSE?7wU`iqnFC5G3kXzSFT
z4zLumk-OX2KZ9NupRJd~;vYA(qH3O-J&cj`=L1G*Gr$To6{@_v7b_gx{Z@^;kZuSH
z<^~;}ko$fJBHrX@yl?8ekENrz)lwK$XQ=NWH2Nr=;Ua!ZTax{jTSo3iUVQh)`kIt%
zPUvhft86F{DA=Hp8W$cuj$_exZ@4~#?D`?>F%Y<A<o`roE&xU!u^3g-+0_Dr)<uOH
zVQ3l}fOM8qy}>FXpvb$dcnaK%*Bn`UYLc$5x?1J(vYjJ>V$hL_11zx>NiHCXC6l{{
zG<Nbb+dwq{k4T{aT>I>5?o|~V{cq$7r(3FSC4VZ3TdH)6CWRtXRLEE<icB5i#44U{
z3=z=CY#;}!-%rA$!?6_Xi4St!n~r2ky!?c0Tnj*ppkNqf)j-Q?RIdv8T>~Sxg-Pn?
zqPN9N^~2B5^du7z#e%e|xYghZ1*Cy&D3ckPSoVbMx+341FAxT!+y+O+78e=be})Z|
zrmNEF3|q3VWMcrji1&777?p!HDGo19IP|*aB=G_X=o^vg17cMW61)6pLE{++y97;F
z0n%N?zPl7=M3-;e<WK;#4g@+<J-fz8Ei)Z?9k2w_P$(;Q-v6Rt`kuAxt1hF)o1IWs
zCZB_$R+$u)E?64<pN35D#=giclW$GS4$up;`GT8$H<sxK;KJuxhyNgp7#+W*^@+rZ
zDQh5T#BFLWNxsM^=J5k$Qf~SBh7%2R1le{v0N3^vV4#XF=#leUpOjlp-s9O^{K>nY
zbb+d_uM|BFVFxk~8d!*mFd4}f!Gp>p_8?fvl)){P80{Vj6$5Bg-wWg8KL~Kk&(khV
z(vjt{aE74&zk-&Ls)pNdTi*sOqR@CC9TkV-CdTv2c(F*$0J?;Da{~xEbdlXw1N1m9
z-^xavx}V`v#(gUwu0hk|H0Qv44^8pIdsg2pw?*w*&M?uRWAgaeF~Z;%6D*FJCyQ?h
z_x0TdS!PAPs2Jv&$0Vl$qM?mdts0ME!i|ykfl7h1U?18InXL6~PtuXEDaf9PauNC1
zGN((xGp+H2d4;Ag$OIPzNOe)qozg~vg&u)wtYjh#@dP|KaL{u~MBhobBBF;d`&2ce
zln7M04&t>C25tyi6T;4wkGavW8i%rm63fugZwK5)G<tEtFk{)pZ0b&SbKr^qUxF!a
z)*8c0$sS+IL~>j~*alw%v?XY-LD3#)uKyjf5%LEO!h|cBbT-;lKk|pR+~H!uz5s$L
zjb>eZRnAzO#UMw@cz-%7&e9-`u^6jrfU4{X`*M9c&7V<;{dzz@)W9oY7<l6sx7vms
z2LerB2Zl!Of+>iRK_)(o0O@4ot1;I-MvjOKrpQ70VPn|Shl72Lxf1Kk?;`+LH7saY
zs8|FvfAFcF9*P$9`y6vf_7$Ylt;IdPGk4ou`!&ek<eH^}Sn%M6g#~5Octh?_nP?Au
zV<RM^JjrZx-ywj+`v^mPN0Tm3z~m|XaLKPnmx7^IIof4BQFU2c#fg*xFsubx45?RS
z(jsY9<}4Se>OP;6wdiVjYibO)Mn-~k2`2}9pOBKE|5E9p8&r%jHK21j32f}Z!4B}r
zMm?-_`8FeO7=kgmi|7XB7TJdPr)wdJ^vfSIFxPNlX^k6y4bc3^k7=62yZL4bxia-I
z;V@=!D?^*dbn4k=$B*zb(h&O{^gAbofKOv-Zr|qgZ{Eu__+i;_P@&L+jebin%2^J|
zP&%6#PsGQzKb4N$__R|6MZ4&XWe2+~oJ*%sH9$2?H?^W`N789*(XXc?qteHTBSMdy
z5-&MqUZ=E<)aHf~p^R&|t&GLbeYM$iv2NQ2j|V*k<~@eed1*_OmjOy_OuYyg5r9{|
zN8*&wufZtd;gt1dPPt|rD_yKWo}{yX6*3s>BlmT5M)&@8FcLw>GZ2aKc&tgIdF?Z@
zuR&$ktMmGxU0{^<PpTObmjEOUVm!KN3bvVC1Ml8|D2+3UA?FVLOlm|X2mY&capY}D
zX9GD+%~w1HNPU0?N>imw8U?xBsz1^K$GkkIaJf8cX=CmPSPba~$U(%cutHjizB!#F
zi5=6(_$q@&!GNOxjiZvLV9=mAw0ClU0vCE-`8QeSnu9zyM(=7>Zb^>~v6#j2)M$a1
z>gh-u;Vy=k|JjAyQYHaIvmW(lPS2nax}q^yhG{`wq5=f;O17b?D1009r69Qgssqoq
z_yTr!%gO|Fxq!4E)Ka5&+0>pgrG8rK&h05@WZ!U8q<%qGfVttq4D?$v<$A6$l!O>h
z(oy8ZrdrGCRN2)>y%k@kZdn+7BN%CYehs>~d@%q%w`)uByxw?tsf^+Eraywzoq2>)
z#&fA`neki~(uxiM++-liParL%YQVD20a_IfWrYB;N6d;n@Wn@k-);7>n*~UR$dKeT
zd33SPNMBiNqiU*^>n^5C+MxnS?dM+zl!G>6VH}~Fs=wmauW)OFmhM=W;B2ltk5R;%
zPNZfPJmQT*8ISQTKtUhs@Fw3{6~)`3On8l?m29^h%IQ_)z}zbG6d)J?=?sZ59=8_m
z-#lEo&}(cOwIFb=;f7h~G1bqszNKt1it_g)wBHBPNSnFwpb7!4oJ|g1M(Re~FjDBt
z08aG*k%!_XJ$0c^S8h&c)xd=e1MnyWG*jnUo91d7z>qH5@rYzpSVX=ADh1u);~g=J
zPLdsp2W#oKuuNw(@BN_g6*Z5c93{*RJ}+w5aBy#unv>{${@ilp*cEZyT{L9S^9-s7
z^QWX*TVq1A9c{!WbWwlbBU(Hb)=Unfx+5UHhJJ1k2(h4_2Go$#Kq<al>Uhp8xEyWK
z%&Ma)M+*Op;rps7Se887tEa}iLa}YoMdz;x*Zf=jF%oQ|J+V!k+3G+l)nWjc+>@qg
zw~2d`ITZ9B`WuyP1f(O73}AWDny<?460s&of3%HwOR&oow_HE3!F|gyd4X*1hoNxV
ztfS7nn%q)~-=9uhZ824_qLlfz$!s!eZqV;#sQm0%1_1hPl<7M>A;IQ$7#L7Jm~PZc
z8ajv1gUMk<p`<SViZ&Z_Z6(3@RCB!s3clzmT{gkvGFnlf6{28GIx+$T<XI-Nw&_|?
zUJ-MPkw7YcYKo=0xjY4?L>34}u{NuMDKs$37x$W#<ha-rIz35u-O<>`S?dGT<luRx
zpK}1I|1pVGEoNIonvO_fu-v7eBK5d;nOwnCq}<}6NFDeD<N2v(J+ZbkNG!22mU&;s
zJwjh+JRmi0?~*zAZJ@|ancQ-gfB(A#1ZKEpEgf}$^iTDDR^QSPs(%Q39zbY}$Rx2$
z@uTLhz!*5V!!LHEKzj==F2i(C-N-+{+~v_{G3yTR8kH+-O7R+)$FdIei*L&Ty-O*6
zu-NT7Vf7f<TC;)Sclm`UJlaw^A!is%$QfIuumXLiyb9g7Fw;;3xTcK9Vm8kfuwr47
zn!L~al3dm0S90LO^$7aa8QygWT~>G(P*p(m`v=Tx(B{aomlODMrnHaH#D-4BbNjay
z5rfaOKx#0il89C;$N-vQsK&!3@%%qhje`DsAD#AP$aS!EwMrTPAYi=gUaRMUWr&yw
zy(}|UaW6#d8xb_g4wg0O7nS!D>F6c*lHdndkJ1_A5!*;4F}~4MFx!&3y(G#h6psxh
zsDU3874uMg6l_~?F%)@q^Yrm_>gwirl8%H=x1*0A0g&tQX5ebx9uvL59Ez=Rj=AKU
z!PlDfjl%p$IyD+G=gdn6elVgpNq1vafuS5_3(!`P`+vv)@r46$`!Zm-mBv)xHS4zI
z<gT&zzDXvHPI#`M22@J1e!<s}WxUE16+T9ZRYoSN%K#`|9!pKcf4T`U?;<az4-TNa
z)94%a7+V|x1qo(7Vw1=Gl7x`@Ukabl*-U}2gOsRX0@(u~GwWTj)G_xUiQD(|d6E3U
z&<LW@^EN|8)F%X-M!3aYeUGkOz3g#!PVJQq9(y&SBAv~sOv%)(kz55dV|l&3T>TAn
z+A#E&^+Js7>qvv;bkxLVH>IiMWR%jF8k4bz9>q5SdF+?a6n7j+$r-0E6!aj(x($!{
z<A@$q+vA#Xn9H66Q1F}3@sgf$4^&--GWAtN7amBb1w*-rsnL1{MulhMD8JM7P6jAo
zygtD7BMIvUAGd{e^ikle3DPv*4%y3xQnSN&uEf1_nvOaU?9cc?n!~$98D8l1DZYpx
zE8NPk1K{C5psr=o_~steFy(gti&4GYa#yF{zBB&YKn?t@lWx|4c^_na1HhC=JFb!W
zw@MBMWucQAGbQD8hAA+XKP8EkwL8+a@A4h0pJ$XiY3`!Ao8}WV_ob%%G)D+hBgb$s
z)M#WCSn3J`m|I1hDZq5m8~jT|`3wM=Mh-agiA%WhA;P85EFfXJ@_hyv;l-_U<&8Fm
z=k5$S3D4^crDDh4BZexZI72Onf_V?)Iq#)Pt=#9aUFp&3N$S!#q&+||26G`WcaDt-
z%D5QaobDO`X|7F2X58o^qEVceDG=uh5v|rtHY&Rv<jQB=F|Ae{R#Un}%*ByOrcN$$
zU1&d*rhu!R)T+pyQ{0Ws;pl?7i|J<GspD@*bO|ot8Kyi}L8`O1*wve)0WA3(`ib{#
z28&qq5<2h0nJ18E-){BWa${&<NhOD6rcej1bfdVIMuk;-j-*q~jTAR<EkV$uU!jZE
zdmnt=+Y`As5X9e4MOBh0iSS%EA^LR`Ct{FFRP+IIVWpbf<Th6SnMgZnT}9@_R(_eR
zgfZJMGq38W9N1*l@TbvAXa5&dzv7cLIMUIfk3~1rHQy@|=5d?Tt7)LA8-`@Oj^)oy
z(zU0+d<9h9Yj4qgOM-#9<+0fOg|50m#a(75_{Ptg>v1Lgb6Z~=1+<1*W#sRFVNjJR
zQ=_;gT~qYl3ip3ieS^C8+lFP?yO=R|)KIj?oT<l=LOt|`-pV7?FBV$3X7*GvN7pBq
z*P@ALM4^!`*<~5~m{sOd`!?`3z@v%O8Bv(^TI(~1-C}sFx`s>|8C_8gjp|taup*~~
z8JR;IJ;Z6L#>k3_!fJ(ng&R|e!zjwL=Koe;d8&HM)u6@$>6-D&fV+_kDPc@*uI2)d
z!Z1Wc^$?rbU0LyDjcYvNVrLgy*M$|X9`^1@M-FfG-2VcsNDaCeY_}0_i)T?_=dQb0
zjZiv3)!^D96Dc6(A384+?Y<B&o>Ws!^JnGajaQL|8sDbbfrx&HDXN!TbN$WP%n_CG
ztYE&r8!K$6ZoDufovV=x<i+BiC2!M~sH!Ok*e1=e6_(gO09t(pnDMMxjLt?o<am3N
zbVIMZ1;yoW9S9=#b8_7T1Gprqk_<TA154|oJ8nGYnuaHyRkE?bO>VRqDxw?5gCwW8
zu}agWpFA?F^a&SkMvT&-F&bF_BbU?Na^vd<mM3C`gP}xgtFrv=HE=E3F0Jbs3SQ=1
z(sd1kRQ;B#Mr{Oc5lEGLX==n65jS^h*b*V=!2qSnHGe%~04W$WeY%)8dc*3>Hfrw+
zYE~~l3kQuDBiHy$I*qRsfYu(J%{2Xzdk4_seI`Q5$j_PM?WoY)TTGZ66Hil_U9`sn
zL?jo0)?+Bk;C|J6S2w{>?&;7Gs0+qEz6GXMxJy7?=*e2FvbNZ~b_){k!{oPAtk6EM
zKaj2|iAD-%P|ltRB7e17YJ44Oz_W9E^Pspj6pI|O_yRA@t=ykuk;7$}sXjmyj-@Vo
z61})|acOW3*^NaSjWpm&1Gi&^DFrQ8y8K3kLbCx>v56c?78P!O52c3UXuXY<g=&%d
zi)K!RHOhz*KS=wMt+l?U`U?$mY+;yZ!Eb=vN9h1ab$79q->8M5w&7d;9H3!<h2PK=
z9<#pd0{pgkAi%y9k>68tk!IsWx=|umRw@nh73zI}T!eI%`4kJ`>j+;x-B6EakghFP
zX?8f(ZUm<Vm!?jmRavP{V`~%enT9K;mM3J7G}T@iDZEOb!te%*77+DTuY&9`8+54w
zv)1-;q+RE#F*m4mr?w-<A_LCaY}>TO@%{k%orc}QB3;6?C}zwdouh_QL1k>Ax`>A|
z<a%wC7!-M+-<3ijeRerDKX6rQW|D+xX$sj@HSJKmgJt1O%&`pCPd@~1eH!;Vt3oAr
zLvdiY5G)((!QUthR*~<nvw=~>X27R2G+UJ2e*~vRLF$_Is<l}<HD7Cn72px!w6O)l
zTar={Bi^lORaT04aL516-rom%nw|%JXPq<Mqcbfdb+dgOH15eBwT0<4#?cxF6Bd$l
zhSRnH+nk1O6x$l+2)}@2TeiTnda<PWP0JSTh9oYRD}j^)C!G!3ZrKun4GD6UWRj>V
zVasfsqDxt}2C>=Ns?BI4+gm7FNhQrb-_KS2e#c!STUMMuQprDeJumlj-`9P;U)TNe
zRPTG1;?zIuzPsD+t$1_An<`!^;tPp4#Y|Y>?bQpXv-&Qa&QIju(TjI3h3vxhXS;o_
zVpTy9!!8O=PC85;rRMG?`?5k8l6W@xDCQrDZ_u3Ho9o^zC=oY86PtV`mYV7GI$CWl
z1>yYSdukfni#5Mj%iM((v5#cjOX`|>MXkx*L^2F#93QOm+ek$mfuDI=Z(dsxXCYxT
z3uiIX$E&=V`C;=uT`SZVjcNbt7%!04(H{zva<6|ku{!F7^T+>t<n9(n0PTJd%obPh
zRfW&50T|!yg_MN_tfx-Vdf)QGT-Ky=YI(DBL&^uT5#Vh(pR;H_-rz>fi7Yiwi<<o7
zP{AaUfq=cViSX3!8ZilefzvvF$&Bz?qUh(@f{ROvAcqB_?|6ccQ=_)!lB02tLhPBS
z%A_I%{Y;!k>&0B7C=#$aXSQdubVqA1$T3_{LUEgwVbAY}7cHBQ7Y^Ua2%F)xf$7_{
zjEqM9?aNzU#!iwlE_;%)sy8a*V~LAZta5YI(pN<D4JJosq9ygfA)RyrfR2|Jdl7F{
zm;N_#2ksjiO+Bj)dL}#^hwt&_mIRDuI9_gN;RH<>#!tfObWXFTs;)#WnXEA#jd!!X
zMDw>Ao8<Q{GG=GY({21o8D>c2l;5t(V}Z+~W$mwKSnwK0*s4U%;x!hhmidVW_^A~9
zM6TiaR61)_y;|m`NHpTpVejxaT#_KN;xjB7oxfXqn|<Kp=R{Q!wF~Ch`^(%1{^7>)
zrS1b4t5GNz9gBWu9YtS@>!5Qls2)UaYZh5QvAD1R4;d<*_|+sTX4vdnkgq$831^R3
z3P93{Q%;*r3`-+zEpNxgso$eGLe_<7!2QL@!%@egX7$ZsFuiAc%k#L^w@<~zS0lhI
zMGw1Fl__V*bRtgPBu#w5Wumw>#b>>Mm1}<X@-$SN?@0V+t-%$tTIau7zq02-qkE0$
z)Kqs(IoOjbuvecG<3w;K;fpUAVq#)+9QJIzU7{!qwT_JF$GaMh6zj=RIL(yuHEnkg
zX(u__i2l2^cHASdwfv_k)rw6H`5(q>oXWeGmaEq&ASZzp*wN!=BsBlA-eNOaZv<=&
z-*s|cfsErY2f+jAY*?l#N6oF9nviExC2+;CH1L4Y+r8kk>_hV}sg+X&los`BO2aRC
zCe*K$9d1uo@^x414YQsLpb2C{lT_2@*KgN<pto<SI9`e~eFiV|E`C1W3wl?3d&XNw
zROD`yS=9$F78qlYQzo#=0$x<zt@4HNhxIPkCBDL8H1&<+<(a9i3-u<WKFSStdn<nh
z#i(GsCVcnfCaD~rJ|Q*%%FFnBQB@N4mx6%ay%z-E7?7*4S_)YbGJUh#*#ze^$|DFg
z@GX*95f9UhnAXa1?b%j!^u%%{y)!@SdS|zrd~v9|Lv(AN`l6AXX@|5wSySx(=$Bv?
zo$yO&M_8k@PSWF?tnUs;I*dK1Jme0g5}!;yW^s7n{zNF2Vx8zDsI2xfA}V7$__i3T
z-ZG08mp<tBUShhc%Hi=+nCX)9U{bl*6B?xy&NZSRsb3t%+zq6?eO+&vMVx3@2*CQe
zHGzzuRT*EPA<)I!wX$6+&V@Y;DnDu=-Av$(s@x6mXG5o<@@;V^FI<NC_n`iU=E&U_
z8u*!p@K5S!)$ngsk88>glSh&lm304L^$=9dNw@#inj!(`f?o2+D1RbLH^%FDx!N4l
zzJF=CB{)tQv7=>laf9KFjLE9r5Fj8m&w#T`mNmkkneP$j8lXToxl(H66VY716CH=T
z+sK`@;!}?milvN~?f#l%5XSu}n^l{fTX7i(#vAD~2~&%lF!l4;G0%hiN|}+jKYhGB
z8~?A*=G5l^`YRDx6tmq6kjjPIy)w&wYY(J1LJo_Fz5V-w-WbVCaA+0%?lx4u1qc8n
z9C754A;{)Q_x?+dES0&EvqUhcG)3v_9D@iw#@`6LBfl{N)JYhOR78}4zN?i2|Ais&
zVF8RjGyPSOezG`2R+9UsU#fu6$<m45_gS?*1Eg(GF!{IRc3g7Gd#&GD<P=_RSA~-P
zcrPrCrAp5qFSk(gxG%PrTMIz+dn>%a#?NGrsFCkTdi1M3eT?6f48GKK`d~5$&PGs)
z3X1!+iP}t^Ll_iyb-zppmj91I)iaBr@#%jTR0qdPK}E^|0L}k@=U0Y(@waMzPKsNj
zj@F~JD4tIxboc7xT4*}gX95!<`Je_E2MC+-!cg`0j+bHr*&499($ohBjazFY$3!CA
zRZ;XUHGDOM-3gthNUj|(S4<@ZZ~^Y))2oz>TP$A#+kwjj-mL+1MQACtK?~r{DUv8V
zpzxpAZ`CsiPIh&tTS*gKJa6?87c*!$LbCs2m6^7kpt9OV#_O4099D2$P=?wZCy?Oy
zwjr=M`)(_wd)*S3Bg~)>a(6W+mPaz4G!rwSel1Ib>kV^ViFjj_y5gKnZ^29*LfCjq
zDI=l&-DoLsDR*-<NCjAXea6e!izwC*97NFH)cmU7!mJ)!K3mo0N#X0xE0;oYg#n^j
zO_0&|bC64<n0ymu4wmog+4n33Yrp^Jy85#fzkUiX9=f@4CHbO_a_{YZOKAn#&O5q&
z=Tb;r?LOP>a}}$K^uZ>HC6s=BkpqSk(&7@VtWlXtxmVv>J$7o5Y!Lkaitc8LL=d+&
zt`=N<vgU<B?7r#DkHk6j=>@Dp6H*?RmKTIr!k*4?K-G=td(C)r!6@3jSc*pmw9R&l
zRaF>zO5sKr-+}Sp46<>_skJUYytFd&@uuQ~`RM=9v$s|7F@t`VbBLoNiX%1d#xKs_
z<)s<c)!3V3Ei=nNicV-1kqD>vm)1OfuP!J1_NwJd<&R^F@G@(9RgTCcO1=rX2?<Ml
zuP<ntfwV7H?^}v90VE-GHDoi#JL$TeCzj%JW1Evg8VtUtUl?n}EE$RA<!<f=q=SFB
zeq6-e$7==5$6Yld=VbAWBvSZwFVGBe6DRuTqqG%U&%{kk8#mN!n$9PAftwo9<wgY0
z!t2+>T-N&C>BV8~iSkY1Fi)RPI7MTYUP-$iymxu#90v-;cz+eLH!P3Ra#}En(gDfB
zj`6SMP5jFh-$brMqc|3Of6>iP7Iql)%&UbV0LkF8bPw=-*oEgabj0y;D^0Z<1)mAX
z&0mgZ9wL-5uD?CJ{2TEB=uN}O7avtcj`nVY86UWMFu5JSa2L!h)AAG7hCdQNKNE}G
zGba-C&qv?QX>=ek6LP1PpQ%v+$LeLvm7h~k=7_>V+=&z~uN4Yvvj+Ett>xJUFQ04V
z#=-aA*KZ>d0&FI)Zr2_IxoVWWrDruCVp$=crKv_aRsN|Y`JCsg(FQGMGAB3oce|Dw
zw7`z7cwm6Fd{n<I;)uqXDnGguV}I9kJ3GMwW@!Sg2t77`ZWH`Sw@uDBRd4~f$CDgk
zXJ#^$S9@zGzs<OKXy!Tl!j{jYuFR~6T%lx`2K5m>T#1>+SN27eHVbu7AG<yKEWt^k
zY^NEnXA$YJ2lPP_brhjy(Z$*2YI}o&bKPct%{DV__ywNF;|`O%-?<d~0mQA4Bp*Xi
z^H}des=yF-0}Uh1Y;N$~3IfN4P?7U3B?#OnmbaU=;@_&NHKa~>-T*noL$WyHv*Cc!
zcdpvGARYJ;Nf3NK2xdgRr8aRG&F~9hW3wE3=jGjIYw7Gg*rnSWmO~V6Q<&hVw_GNR
zTZT6ps)*TC&*bl`r%QtU;fTB<OdG`=Bkjk}N5@MszYVEN6C)=L=3G!sKB7T8HH`7U
z49GCaO`fblElbSYd}O)0Rm-NyU+VX+4{H7|ZS+Cd-L1C^NgiIq7IZfG;H!)_%QBtB
z8-_|6`P3BD-|xk5<!>$<<!<yK-76Mi?HLW%g~`K2Nc;he-|Al!UehlMHhhl%GeIRm
zVn@MFe8A_jqhNk_703E}`{#r2Mnn;;aM~uNoDa9V)^!~4gODh5A`^(=$TUv|W%&0$
z*T@fQ`D~0nW-O@0QCyeH;cAxj^}+-wX1$f1Tjw_NK~mzY|H1&jYPov&z^3?W_12}>
zGY(eB>~$kQOe+g&HpgIc=<(1mCIRv#yY=z$_|I<^d^U4;VR!S-PG^)6FgY3S%;l;a
z#1WE7KbA6@JvAX>lzEaO0<x0rXE#;iajwhB8rf3Rlmb2O|67BrI)eK_cJw$k=g{r!
zLc{{c%dJ?PLHoMK_7B6x98#D>F{zrenML{zKi@$V$}QQSA!wd-hpVA}Fk96$`MSPf
zFU>7lJwZVohG$8g>*-id+w%=(tT_7{X;0!h5PT>y@}&+_!;(Z6t;~|}pl2$%3%6^Y
zo;(TTY%**A`-9m=G1_OA)`j~~k$7Xni48{n<r^Em#KL}bJBgJ|uW*=o56|R_ngl^}
zBf1`nCx-_S>z3s^G2GoCH8Dp;PB0KUXTmLjerc^#Cp#D`n@PIci&d2$O{wn3_tyJd
zi|=hKVA?JwGg$s|Bf@p#e$@P6GBB<oaxg!9NNdloTw01rd6T^Qy&XDDyv`!501{1%
zrHeNhAkiYtK!d!l-X_^v6(Z#AzZ+>Sk{F+>F8^MgTY2HASI*9ndVvesd-2e!%0_u7
zFbr@;biCaDHCiCv<sMPKZ;r!GMLTtLuWxV1G?qz<G{s<Cp^q3ZG#xKhc!vc0+S%on
z2k^FYT|HJ&Y`}**itw|hBLdv2k9$->jKm?G3~ms-KOo0o7*Tk>6{LGxOCi9L%9+J!
zwL&wP6Vc@=X1LIJC)X0P(T(@@?poP|=636BW@Y3kHd}gc#z;rUOR=iG2T95}^jsn0
z63h<LWtcfxGzfpvD#!ajxm4L-kC%w^uC2KfmX5ZlJ-S%r`V2Dm*_qI^AK+WZOYu{I
zK?{Hdk59T%<+yX(C?gaH{7FrP$@{%H+m+Dc1Kl1}@O(d1y$@8pzi$PekJj66<DDPh
z_x6Q~4<%MxZ(Irtx2N--3HE4<?+O+RHW*UusL}tB&yT*4xLzx6hL<PvCLPuMt(fMy
zZn5p_h+mwzo6a$DeLbZB8%zh2g$mZ5U-NB9!~!e?4}SD`!=D8awY*o${;v0t{PvM<
zEFUvmKVFK>PSJ;$;lV4Hw>vTLVK86SZ~XVEI%m%<(h+nS47AU>zWRNlj_48&DIvbm
z*cd+2NP~QDEq@|2F)n+6OF+;OMk(Vz0oL*I_Hqn7KZ3`_7<fZ~@NE8LKUv*Ou2GiU
z_``<4;{oJ&P-~+6^O149Ye+m^Zudfjqd`XA5@Lg%MK!6umpZ2D`8C7s2A`8|Z{rfl
zhAaso!N5+-le^aW{Qv?p78pDOD3<@t9bAp(yf5NXE2f+|d{F$<<IA)2k1U1hYT9D|
zc)5LO-77Acb+6%>MSRdW{1L{25-VUG^0@KfaVHm6#^$qd&}Y1dcQ@4A2#}LN!&zFa
zaN6ZCO<#=Ns@|ZaD=KI|u4A^Pl?2PXXc@Uh4qh07tmFiWea4p(JzWermwV>+JTF{|
zFx0J*06IsONtf!)VKIHCOS4sH>wMII{Bo`ae(uszjN=-25(8Gl&U@No(kC@F>8yfB
z%ib6zs+oJl`tfOQN8jD>r;m@+?{8xu1DCbpnV&#=VEkW?m*Pj)Rc5NEE*a|?LSirU
zy`LuJM31Yn258oDR&V%JZz0<$&D#xi)KV{WF0U+oV(I-w?3iv65r`q=6X&N6efCjo
zQKQpTL%n#s6v7}`A$)K(=rPwG#k0<!{Z!gwcb(*YF6hDH^kp;n&@R&4N`YFd!Bc%Q
zX0Qo;6X={dUY`Al8ol|%QZUvh4b_a|jRgBjC~*wjW*HOEUfp<K7Zn&0!{;)|oM4)J
zjRS`<sk@L(!%-Q1Yxa=w7X&{vOW_zKV!+Qasls((%fuQpjSzOxjk%j?1cDflOxr40
zFt8j2{Y((4zT*^tjEJPMDe)(3z(lig*j{I5YLgM+q?!x#LP#;rC+@7#Jv0xDM@f8F
z7H&_#(XAA)NW?k$OyzBrA6ec`Fc9>#g`nY1&7WDbD7O|-wiC{moJNhvn~DE72HM?7
zdpGzt+mMmbNwTKSMFiX)EX5`9FDu%D9<tWJ@c^^md2PXnsic_MIWF<J<E1y}X$gpc
zJIg*AAVS@%ifL`m=YB(dDr`_K|Gq(|CsBbCyb@>gMJ>`kd%WBpHD1ny$z9++8?sHu
z-Kzlx<A^z_G6hQiS@3-0*E+|uo%+&JkLPv|>as$^m;pJbl=cFL-LLYi@xl9+D{Agq
zHguwd(9Q*8vNm~nX<<Ws_y}C`+Ymk*k@4ilkD&CQT5E<`=B)DoV#bDyJIn3PD=N}M
zh{xGJo8|(`?i0(^xuAc(20kC|$3moIxo^xn`f|YO7y1;_Oh^%=mL8xgS_<6Zd3Mr-
zdldX!W4qh1##@RRxcl<=N5{+Usns#55ypCUqBlTVk1jR@p~oj{bmo8_U#>Q|GT$@X
zsv0E@JN%L5YVRFOVb*fzUERLB;yo2n3#G*_qekK(l6NuGNlb4%U($#Z|E-v7^l81B
zX^;c>Z)4@7+;cgoiOQ49`^Z?AFGM34dS#p=xR@=eb$$p<zNj+OHWBXDa=Te*`0+=U
z+s%S(pglBC$9v_a!K73Z0ka)$zF5=fR{e_PF2pK34a)%O2u|fr@J)^<s*duF-1P3L
ze}u<jCRrsPwQMpG1+4?UoUIr~Glc!4n2;BKV$c=a-?<d~1|8S!fG;kgy{*Vw)e$ac
zH7?YY<wSJYWDKjqhjXBhd5HOQOzoX*sIy%v4QEoyZj-xO4oF)iH=ghHL~Rvv&ZGdw
zc?#SKrn(};G`fXo-yffjScxI(m-XXBYv9BvV|nI@ruq9&Qe}g=aolII#a#|x9bJi$
z2;y_yl2!Z+<8;n6zwTGERh5^8D`};P67YR2k3pU;*c;heqgb3mPqN8$=i7qM-yZIV
zlIH{CZmNZ^Cy{rs=KC>*wPziVa9z&L#6KFzc;VO|r`8KwI3mx)W;YVLGm-3}U$|57
zoP3Fj$<;$iqdX^A4$^;G1DlCvHh!G8aDSyPx2tF5<Iulsxt;mJ=)t4?`P##*2=zi(
zyOuOs0AeB9LD+cEP+#g0iCc)by}Z@=3z28Gw>L`B>^}%_eTY|)aZV&Ptm-|yl{ntu
z?_x}{Zv_rEgZrI0arTf`8E0cmR)e_68cbMEcfNC}Cq0WYW?+V25>AVQD0mDv$^*Y7
z8vM1*Syz+T4==b_$SB21Q%|;g^S0)3;)96t`LxtaZW7R23@4PP1=upW{clI8i!34m
ztEs!#F>-$`98}yz+I5#V?M(~C`#-Q0r%odYzOA~%tcZ86<|h`9jFjp>_^Rc~R$XcO
zvlKw>@sZ|lk%pQ!WAiSqH4Qa#9QB_{)9k&XqCPf0|98J(xt%%icxV%0Slu?ujYHV&
zM#NG7f5)O`?BI0Pavm?w-kLx>A1tp55lBIwZjusjUdxflf49z`ulZYzNcNXPdHQ&{
z-LE|u5-Lna4u+Bs>uCV=)8GSD?cqrg{nqhvE6OtKAyK<SHKTNN+X*?FmS>|0+}G%E
zX9U>$_NDNfG9$lOM>gf0e|))uKg)2=9xu1!J=D>SOG^Q#59*Ds+vEzYvqa?kdaEOP
z_o3e^UOir3Ory0&7PzbIg5}Gi9Xw>VK$lu3AN&Kl-JTE&^BtKt5k|OYb^mZK#MuC!
zn3ycAN%pWCx_+@6Q2?YpsqCvI)CkT=D7MlGEO(NRsmGGkIU4zFPE?PTCkN2CmeP=Y
ze=G2Jsr_(8-`hdKwR-Sf%d_8LyC`Y=zNH>Jcxss%3n7Y*>*$i1uE8qt^W}Fg#p)e>
z8(t2d?TT)CuB(P6VwucNfd3-h9&-GQavZOkVw66=wOkoA*X3}NzU$ku)9cDRi64)L
zB{?;b7GM2r#g))`)Hr6&%eN>PXk(dl8SlQ-+AZ3Gzrh5~^varTd3{c+xKb;8pF2^(
zk;J=^@$iYIxEv4heV!+3fibh(VT0?((n;xk<vI^+N#B{iy}8LS_pK~DfHr@or<Je9
zf1ki0p?D_i8XLZUdv8~X)#aWE;)uaUjx<W6k6XbNs=c%80Xm?MmYPIpD7I8;l*~Ho
zGRI*x{GD}x^&l?37yC%PT*ddHle1$1rqCB#5D0lBzOqw{*x!wJL!WNuP7Fr#IO3${
zS^HHZ=UtN&;reXhTkDs>LEx@ZD4dvae={A{ZUNF_Zn4@Nt`YQ;J%Z=P203#O;#!S-
z;&^#Mgq$)rP+{soyF*(bPt=zxJ`=0>nepg|2!B$Qy*e_XMRQH$kSy%R=s(biBnQyf
z(Ua$omuF(#Kh@aYQd3_5X#%E?&1CVF`tTR3r{c~tfp+xVQf#d%vME%{CivbcTEyGz
zED@aDUTZyQS_R~7e1$yW;INLUaH$gB;eKE^^OhV*N|+b=@xCy>O8KI>Lt0I!!qq{j
zn~8_UYffp_s->H>B9;Wusefjt8D>>UrcLvQQBWdTkS=dF<nDzqLsOIy+ry0_3)t-K
zA^S?mc7l&3kw=Z~YU4N?cI9}Ui>f2>q@pT&;p(Tt__gDum_Ncafnr3KXwk8J%#9b$
zs{O0!>?czm6aNOmoH88EC)rRR&T*g~RzW)Hm|mPZ>ZUIokY?SN=J#svCg;7+?!SB~
z_Nr%lqBU(qjZXZMMXjbR60;(Yw+Ue*c|mo5XDL>%s8}=2NI<xn14d}f2>;ReU}lj6
zSC)9iMm^W<s-n+C;E4Co^d9T-MW^@90Vp!QDOhCqMkmrLwQQHEZQ(C9Ib>F!u;W+`
zhS*@%^u`C&I@|cGb>x>Go5c?k6>Zl!)FDb#;9g!&H_eg<kOm}i_R}H+ojI1iWh#Xz
z&j<KqI%>*>3KQJi8Ls%8osi~#=>OP5>7-}YI>*$!!!L7Rv=dW*nK++an!<JO&WLQE
z>-MpVS5_pt9Bb}}v29R?eICR)lfzphVmK!tZ7UT%NaV3N^xCy|)%?3F-cu1yzcx|~
z(}aUBgXWD0;m3kS06%;~=})0_P?_9SWqg$=bG1TrcWMF!p3_J-kC%d~m$qhbML-O$
z)x9FQjlO?Pya7Wzea4?uSQ3urPi2&53CgAA>h;(R!-e*XDOI`GQShlWy7NX=X8(RL
zQ;bZYVEJ}1TcY61{z+<A&+jZK^Yh2cEi=mtVi9RzLBmY`0j<ImvErZ+UCknz(3f_=
zm{&Gvdh7DmBB$4Itu`Yvd7tD-2}to<MrT>Ym2|PO&D#AWPrNnG7&(eTo(zIH>mY2H
z=AegIKgTr9@MCH#Wp{$$d`x|65o+Qm15OE=;*=d6@-Xy`h_Ts!V!65%d2ZII-xA%U
zpKJ7fl0ukPb@YobUkWM0TlK*uAujk}KS|jH#{4iULmKy5V>^M#hWbC$<yZEM-$dug
zmLaH7%1JZoH>-RQ1SSd?FUcwFJ+n}ucw!g)TxHg3EOzLe8IE<A8nr?L$?l39gO5KX
z1Aac0F$afeuR$A(X^z<~R&O#aGvQl3gJy+x>kWJFraUeL#$mGWB+rxBc2xu1)o~zW
z2Isx<ue5R$!X|)*zjcKBKk6io8WpEZ6dh=@=;x8;YA>j^wwGd74Pnvr%b|J=>jO3L
z&H%dp_)?HO-tbI}!1LDff|Ed(pa&P$af&O02?yp_Wa&1CiBLxJ3|2bbsJ$10;NnIh
z*MMl-svIWfZrH_oM|nJ$U%<<wu;GKS&x)B#wHL`wZDMFxQ1VP&N(szaO`-i<;9jj?
zbM#Lq@reAsK;!w+gp#8VM~1b>u5L8u{3*8fu!X$*PJJ97F9jSt);MSZ6P6Q01SF+z
z5A+R0!B@Dc2JR>4w~~q@Xlkgtn5*$J^t-ikCBVtitiY+M>#1fMgxY*>4P1yBCS~@>
zay6$W_Dq&Z&XRh)TSt_FqdbTo&Zfd|r1|aKkOM_kQP!>Wu5~h&g8aUq+O1y_;AjTH
z+1-#x7MPB|?zWLzYI`w&aKvm_NMNUdhWdZkXo4u|^v%c1)f}nI9@W&H3Vn!~6!5IL
z)!X@PO;@}Jfy8!@O1((3(JG@JBE?MC(8@u8n@$3Q8_x|mR|BkpDD|nRAXUd*;9t>%
zoOddS{lI3Pp%&E5@`0L{!59-07@vt*t?}ZY?nNlst#65GC_$=9E{!TBN5YK5pg*hu
zlQ~{!8S`(yV|lUF5DwN&?Q~1!p6zPC;;G(_s}U1&x99_jt(S*W;mDTZ$7XvGluPou
zcv6~lM7I&$4xJ<H-QJ761wVJZs1*Yo5wD5>fugVhf^PagFkh;2pAB#H%pPc?6#G$@
zlKr|o$a_GA|4X0kHNXe;F_qVEsf?fa_T>fWe^0}j9S>E$Q@?*X?%e4MYcSB<u3A%^
zp3P*ZRx0+Zym<>CYIwiqEdZ88rS**W#3<pQ-gc`us~vjAizJA&&!DM#kA*~@0`I{>
z$XRUJXQysK9}doJTJBqr%I4h#eT~u<7DTwZ-wUIpZsu7{>2gBVns>Co#H+ElwMXMX
zcIzPfxy|g*0;9v+)_^MZfj|6hP@PK!S&=+dKw8UgDNCKt+x3gh?d*jDT7}KghxL1-
zTPMp%Sy+4S#kF<i%`;0K4`Yi{sT`sk#w{b>*L>h&;7>)`!@}2bbrXZD2@|EYU%jL6
zT8iHwq5Gz#%1fUACpY)@-oB&aolEg>IIOqe+uMMV^R(H)cq@X+$<Ubhf4Itjp(0hl
z09h^kRWXf3p0Qab`L?fGp51BE@#vAIV0*4_cYC}PJbk&AQ}gT*?=8vYs9~hdOh~=p
ztG}7j-?+&p9iPj$SiPbG-#4QBIC1TB-Ot$>uk^U9@+Hijb$Gv0Wm9U`>)iPa(?>jh
zRB^pxatcwE=fTTZJ+a&}m8|{d^kD0>?gh)g7+X-eNW0gt1S_{Az~zd&s9!NAn!&B3
z^Kbxvp>DI%PP%PX^LOGanpz(7cCDNsPsz-^M)V-OOtPk-?#sw^^?bvcv`u(E72HNQ
z^F4B#SYTZH<LZUKryn}#JRYL-pACX9oLvg>hsACO@s&7!^;j<g%`&xdyE79<jTcd%
zNi@bcV{nhQmbVfRyA2wLJr$5<q5Pe}l)+)3ZKk_S=3K0EsbU+atQO{|n}FBblc9-+
z$B@b_&LIfUnXHlk(_J^>Y$ofkL|Wd1WkQelf}gV;>#e%I8|%@<=Z}{wlY0AoroA^m
zyIh^x_8%1(dNJ(1$YY{vaNpQ?*K?44;_~?bw?UW7#HM^c^@ZEVtBM(DLPMQD6GZ)Q
z?pp!i3VRc^<K-C__Iix@{+P?k2HgIS1DtW?Z6deXX2R*LCaE=ggC`@+a-q*;SYZ5}
z^=qN}L1@|z7M0&nJ!4}MI0P3lYYYrw1p8yL3au=Ox|tUT#zeS5?@aB<Q`ZNSZ2moe
zDEzP9Cu6CPSH<$P!!kBju8^Y#BI`Kz%FCBxHr}pXD`+ySe*>gplD6;y0l-YQR*CaN
z|M9$$VDqm;q)(6|iHg9$TyFzuoG6ZG_kT3NP5<ybMsyp(ry9(w#ZfgiYfb5cV7~AW
z!@*>X^l(%eGw^+$iYNjYL{U#p&<eGi=_Y}vDQB}#E84afn<Z7IE=3(zgXQp5OYs{_
zEfBg!$<>gFarg`Yn==e*1&}wfd@V~hh?Fp<x$WDbgnmXmp8jDh#Ydmno2q=Gp~`tU
z^X#E~=d6~CzVe*{o!_E-XMj(t^Oeihmz&^h2$TCt@!ShJ59hDmTJx9-RnIQBlRXN8
z=P~-4*Y2O|ZR%Raf0IJO3^es-C&T2;Qczgna_w2UEV4d>?_;S8aXT$A(PC7%9{=q{
z>FtJa(Cx;p_4n8ze8a*FHd9G63#NYkBP&eCju<3DFA&q<vz+K4+XJ9;LGalWo}}XN
zcnXSl&9Wb+xelv$d(A#ye9dw7Xx80&6B&+UlPyD;QPQD=Xna2WNpYFyq|v3!W`#ze
zIrYW2zxn-O(qlHM%=(B}U^gXW9>P{IGiT!|M1?Oj1o7pIKe=3SVMpD(VYxN)L-Ck#
z=SO_QarII>4AnboerD6c{e8H{<Dq>#{W@Zqbi`L7tFzXg-i>o)CM4r5mD)+0ZLKl+
z%-ZH1#6#>L-MN_un+#+~<WKH5Hj|Fy>!EbRf}AtaR~5=_Ke4wWz)GF{0OAzc!)tTG
z-)(^VQCg@ajXU#~b!67p7yE0N>CX8zPP*<Q=1(DuX8+#tay8lgxbBevGKFJo4+2+|
zdfdo`@H8;T+MmZD@vr4d64(oCDSLlSao#zuSinDhBtz&<LG=tjs6Ni^#thvOPNr=l
zo1~kA#`}L;NPPYAQXFQ5?6o`!IRYIRGJ9jqyCe}EahTQEdSoeJeDatzHPi~R1JI@A
z$`r9I1TFs$8xa>whJXs1iD3*EuZj4^xkLEIA(M3v%@wuGr@_hu@zd3tjAl6CW0F-#
zEKcBGti5~T;IPV{ik3{<X6@UxA6TBv+P8Of^`50LgF2H}v`UOAhl2XEP${S+B=>)?
zFKYg^IkV<XdDkKqX^e3-s`6O4I#Z+L6~wzJBVlFP<+vnq@tDtG-uj`X*sJ-S5OF=p
zvj#)<$O<OTojm`nZLGJmKe7~i?_3J|2MHZnWk@YSxkr8th9)HO!FPfaL+mEyGpU2H
zfzcuRlZHUVW01|%uMJG!l9I6m;98WHk8mQ!r6>b%aTM?H8v}#n&Cg{TIlc$Mjl}O-
z@QKU(LPsB#;Yiwv97afxtoG#5<xp1J%k7!BFNJ6ko=2h3S2cfRY$3wgV-q%K+GZt3
z?LEB8F@QK;|4mr?ZDH-`ZA-DoloJhRyTM!tLwkX-b-WZ4Yv2g-TLUCY>JOo2*=jHN
zL*9C}9aB>%hCrqHLP&<;KKZJp3XZ){=W>TwK;rkf@s8rDc^q-I;cI|Tcz`1m=?DyW
ztpSR`sAkwT!FUBhVI$VtbK6i=L0`|fh`*z^m#ar_;6&lIv7OYL-0<tdy%QY05T8>3
zKZIi&mXPN`Uu<So@u>-O4#E9~WnGITk<9FeBE>5B?CXFgKw5ah*?C_k|5fQpDS>k#
z;xMd9NE!_3Mw!X98FUhLK)pglL%3I=@=-7o9Inf~G^-KWTq(Sf!Qk=0)pXPV#d+s4
z;M6s6UyE()SE6DSUM9U5xa<~mUI~`Rdutyl7BDRaSHt&J`MG3aCK$uoVPDX!jKi#o
zpcJY%-e#Rkzso&5Su19iS6ywQ?Y)NjaI{^OllmA-((PLe;X9TqYxo4|R<Rx(FVF0)
zI=Hl4i8*nOe2)D1W@pZsDl^eI2gc!Gua<YB$94dXyd5uBgo)swFZJ+40pxbV=GfZ{
z#|-g?-TG`i)u4Y&kyu^m>aFX~{;S%f)`f!8QTP*RKhu?Q9BbfGl~bUj*J=e1{dBEt
zCQE*S2S7q&>PbRJ9u`cjyTWy8YSOr0^JgfiYzk|&PfKN3^s(U~_LAHEaFTM}hgmlp
zZ|?-!#~H6se7TMoYtwt&p8FXBxU#3!)Zo3e3T`?RQCP(92pghu%!>bb6x<^m=71Z6
znIjkK{F7y#NZH1c$n2bHs8)daclG$YjrU|d4uR8K>K9aYO>=8c<xgXu%k{z2-yFX*
z$6`c)nT%L3!2HLl5eM`^#4{0eqqM&0K}?%S?jr$Y|J(D4Bqv|sgtJ!>e!TUF%>8v5
z#BM<DhLStSOYzabxR_e9g2aEIdH;xV`TX*NDv*tm;gfUIlsIZ4rC>3|xtWf8e;rYv
zyAA&&rHb6Y6k|SgPRHls4fOrpnWwXTaWB9{pGTBieE(EvLU>WaCIfjT9M~klN{&Tr
zl&3bC1?1j)mjdq|h9T>Gxd9^|z5Pfpgg4gWY_fEhdQ16!(|eXH8rXs8t5+<yycS$X
zG142m3A-KZl+o4rUi|TMp`GK3=4a{7D)T2D7&~dX#D#))L?yQt###2Cp-U1HnuaB2
zal9Z9=eCx&Oc7mYWzs%f*I=;Ebyg_sPP(W@p_tUl+hPXIllVzs*RsKUr1q!>`7p!c
zBoOC0WHj7M_VXzA>g}-4n84eqprfeuVtA2!gqKNK1kfyd>kBGZ7(P49pIDyVuDxrN
z*4hWY`L5;J-n%Or>T7BRN=B=E=W<Ks^477yPAs}#Qy)$CXa2WuQF?`ape{(?|AwUy
z@_^KK{=;6_Pv?vy_K4rPUvHx=!Wu^ZG_=>s>q$y{e<6q2UJ8m<j`6*lDV(*+*bPX1
zU<Q89XJ58FvsdeG+_c*e_EV~~08BpG5KRAkEO9xYUP)fVvXlXPqxthe^~KjkCWr$D
zwC7elfcrY%tRET*)~~$xB%R=(kxSmrc~FgrA7RbYc#(bf@2DJuaKUbe;yLoNes>JI
z&TqbaDICfpOxx(m|6_#5w)U*7^jj-4zwpN8${Z<ARYK!UsFeqWP}_Ym?rI&qF<fzC
zu0%36WjHo~PK2)^;#LiifK95zKgdQGzXZmAgO#Woj0Xl>-F@Y9HF*`_lIgH!$L8lF
zGL1yve+$6ZTxWs;o6*mkzyA>{+v|i-yj+1Izl0dbVhha=!_^#55hGbF2sxWytdP|k
z$H5W<JC)&n7Kd7&!`edKxmbWO;b&W1Z^cb=51Yj@SPD+5uV)*jr8#yH3!raf8f1*p
zmh_#ATARI44kR2Y3l`%wyUD}=<74s0he7bZzFni(I=rXDvls#*u*yV`0Yux1yB#WX
zf<<GCE6F9Lz)uvk(#n=FZa=%cJvF-&^sh@POn#%=2eIQJ&n?h4hSXtR!WJQb?J7?_
zvQ$A@j>y6Ba_d~l*EML)lZ~UB#Vyz4A-;vloTGHaQC+Ic>VjT;xplm}nD)YzX^wm0
zg=%4x;NC(9E6!nDw&%A)Nxj|eix;9kTuGJegz>Q?Rr%!Oy=^c%M!wx`4VW1)-F0el
zcxd_$*W83(cG3cGibWYi)H21n*|jx!l{M!$|6aCn#!<gFC0ve|E35rk9BV^y>}~q*
z-OJm(Smh|d--%1={NrJgcldfu1*A2fqaM<5lO?7ZdTTEaf|@UzHB@P3>vCrmp{Byd
zmzgS-b$K{d9JrqZZq4uG$wnj*wc21PbkZ-BAgcay^#m7xFfkDDA^`i#@xlU|Nxj)5
z1x-mP-W?9BdCsS08eB<qvw?F=M?`*Ait#XJIA9Ip#MR4}Vmnc@5np>&37D$=NJ`K;
z8IzqH@cI906co{01oM1|crnmcb>u%klc1{srt(YOLSQz7vDPx0|7y)Y^@gQ5GW-fz
zh}XG|(r3H6TCr1cE(sV<wc9xz+Es<9mw|NlqI3+WjRGAU)aBRsBJyDQ2{#+yYl4N3
zz8mk}tMWHi8Mu~im=~fQ&5msM*8XYycfM98>Zk!Hz@0j?Va@lpH*v;ht~Zihj^*z}
zGBlq>M2S87AvzgPA+^rQ_hK@E`lV2U`R9U4CXhW66hg#}2FP-X&yC}JU2=)fMb&eG
z@v~1X#deLF+n&XOHF_(lut#<uUTW`)PbVeG+WQSoH(5`<ygOFD&V?|Tlps(4*FWs!
z8xlI4vlsLn7D32Ch;<M<N*_dkvBbUD9HsEJ4GT`$nOiz%_=t0(l*PH3)esv@P#6xl
z8YY&^?F?lC<AsJV%YHG$%9UFC!I!%6{mV1{^O+y$>Ma$=OR?$5$xEE*<G}r3$QgQs
z7ikO{Y0l4ypGD<Px}cUGS{XcUMKI%Rs=#dDp#f%QPt8NTa=G2Bqw~)##n!3<QzLwA
zwyMin?~`NmWXQQ0q_0*bjZT}<<6GfC7Kf{G;p}qz#rGD3`(exofIByH>byC=)R(ey
zo?(d|yn1HaJdQw7LgN|W#sU{G9ByJNQ$ZKEmqM199=E!8R#pe3$cC1>S09!{9$}vl
z!LhM0^-{O?;SxHtrDl@G8}8eer~bc$$p7yIA6NwaU(}uck^>mvt{dH`=Ff)%f@75I
z)?ni$$n7Xg>37<eeBhT4Vw$jvCuv*%-?D3!|91s)*4pBC$Bt~p(|ya^bWj;ZOG4#O
zC&8JDB<x4W%hgOKrcea3t7L{DxDsgrB%0mcT5j1gQ$0W%$%Bp9o8h|>;H<$X>=TYq
zW}G%l4lPF*L$I^FolRV9cjMTPeK5#L-_E>HFYFJX!Y@3s+;UP4;gw<cYa7Btb46r}
zNaEcOW7{D8=V7-I?bfm=dN*B1oKa@4MlrwX`Mv6KqbMVGYm-x@a?jN~$~?HVTzzF<
zyuPa+>x$3teUax}#N+Tuyv?HEhH6#aBgy&Rxr)as`r=kCvqu+06QAa}s4xF~J8X=N
zKVGg7^dM}AwkJ>c=yJOkq0Z*8%<pp6*jNSM*@n+?MSV1vgTL3eCw;pc;GFGQUzY?N
z*7+)u%?`ofGpPs2cQt%#7Fmc1ZX<xD&-XLTX!K^Eg#$}Luo-6EP8sc|Rfv_}l1t9Z
z*GYH0HbOnO4#`B;Umv6&ij^skk2i$*Ct)hcg2bcPTdIy`z#E%vt?6q1#&JOE+<fg(
zz1@J4`p_sz!2&Xxk~--2HySFt&mDZ;D8sX~7ypVq;?s;oX%uNA%9p#1F%M<ZkP&%L
zqZOYC#q{p4aAKn10l#id0BE-Y;>_{#c2Z_BA;m*?dNFKPd*=dom9^ST&Fmg8x8_(*
z<ppYG6+jlx`P-KeWxH0)v2Z{BX1r$OGUS**#*;kS%OGtqmY_JjS~xeWHfrkK$4fzU
zXCX_^_};SihYi}C`Wr*BB?afY5`o{TGHw@JG4$UY%qP(eE$3viz9253x}uL-mabf_
zndX^|SRF5KMS(LP8UduE`SMboqBe7FpRIYG>g?bSyC=YXV!6Ui#A<?3!dd6TvBC0@
zDpQt(WRBRbo{giEoryaiG&Y?>gkRnWeGOj&?8ndd?Kk|$Qk*&;P9OC)fBdOE5S)vp
zWJ9R}UgTB{9KLHQCdIl{<-V2ok)GhP%UC~1bT4kCHXt8_)5Cb^HQip;n@mUQ!35be
zPb^o`sWUBcX}LYD6}r>P;8hLqKutW%zc~WqPJ_8lu!D3rJ$a=ruk~W$)$08#Y1YX>
zac6>M$6%^w925ShD9+U0(C9tO6{UY+UD-T4gs}k#PZjXjT$#hgVGOvrZOxyH+~k8U
z-in(fllH!=j=rbcKUeW*E55tpdn?{t@umvy^!IhOai<&LmKi6Qe0#TNm*O(KysX<S
z)8#DFWf*#SSMTV>JC|Y=7}tdV&-H9o5yn}2k)~T(U?=i039D#%JxuOpn6bbpMgYb9
zcYg0u9DMy!Y<<I0T>qDr;^sG2{Jx4mQ1QR7_*X0bm5Tj}f4SnI;ty5){)#6n{$Rzm
zia%0O=c{k(>RT)RSj8W$__m5~sW_^*UhyX@{&>ZWia$~Dhb#V6#m$OuUWyn0wXP1k
zdR@iqD^g!(F77vc(a9?FNfsVu?&2`3pJtXPei3?b^O(RJk1Wq-aZ84e=Vmk2Xba2`
zgHUs$v_dS~sAb#DxFqC!|F~AFVzq<SK$9qoscnxifhSg*?c-5KQ(|3o+Yi#Sy|~xd
z#yv`ycvU?3S-lfzQl}b_<55K-Mb`8}O!Kn=bdZ#Bl%&??_kl^g^v=QKU8Q5C6E?i|
zH)AP3#w1pL6m;s?r+~Z-b-_nf2|^4|4EASwVNL2zmA6C0pg5?|O+th2L_DGN2OGKn
zwt$70I;)NgL9lPvG7ov&g_O+htCm6)DwhlKx!~i`@{xE@bIg6nk%t@Q5kO1_ZEN?L
zLI1Tnf<L&Qg=l?CcXGjQrK!!6FoFAq+7Qn6?f(1z*;~Uu=<OfxEnG?b-VZO6<LQc)
ze-QsMvzP{lJxEh?+OV=^*!mFOd?srB&SY?=T)K7W5}VApQruUP9x0l=5QbVeg6g?s
zhEq;8AH8w8HC=P9@tTJIFKhm8xRRZI0zgI?I|AI2C1X?A`^D*{pE>@#w21?1U=qoV
z{Kn2wKofM%Lb<9Ch!4lHL*Jh5#<5(h*~=KXn14TRAI=HfTaE4S_x7aQt91kn;q`9M
z<nXprk*g~2p+ZtIamw*>#Z`N$BNyJ=7blmN0-9tYpm_7s3I49$=mQ&I;m<R2$v3-N
zQxZ5cD;NYD_d}rgypDqTA4di9JN`j1tMZtkh6Bl|JJn<EUMhS&f85*5EKhL1<|ll`
zwh?}>-f*^n*9<u~>YM8l&CQ=veSF))2nDLaS`2a~cC-)NS_oY1TfEC#dR36(4Jcvm
zt^~o4ledb^K2lpqxmzr|_-(blQ~}@Wc)6M>lAynwdcZjr7YG_gV83H|F@`j5;&<-#
z3=07A!truDUo-^MP7nX6B}9-i-WQ!MUuOt+QxELNl>|&qdaMRQg=cJW*zl<bn-gxu
zj(3yLi}ArruDW`3d2v7Na?4DMX@wggrE;r2u7rcpr;jXGC(snH*^bFcaF`8~N+HiY
zL^4f2_QD^HE@5CYar4_aj-~*Rh+}0rC@rsoV5VGRu1~*pDfXV|*_oxl0<+u+5~IRl
z^~9Wqh!;Ibt;`}YtebmHEss|kWoDK;*26>3)D)upS8Kfn_cmDF0987c-P)~zf4$y#
zjF3$h##)7|yTNy34WF5dRk)vjXywS6a5Je#aWhXtg_{3p%5Coz6}7@l&`2mqR)>~V
zo*BUUogF+|%N0Z5$hX^6RYRq{*@aOQ(RU~M?jJA3jRgB;nLw+{aoy{0FSm@LaKu>x
zo9P;4l>3s18c#Zd0jD;9YIFjpka(q$Z>HXS`wVbH*k->0ll7SZ0>;;cdu!>0ZZ9<G
zw<RgNjSW?8_Wx*7VcKct#TztF;l+(GrR(-11*JdB&(;vg1J4(=d|v~J)lJ4LQb@3m
zNY=`W4eP}k9c;uFgOXqwCjn&|;6^{sIVV9ie@t3)@!M)olEYTc&D=51!=5-XHuY+)
zJ%@l<yj?4^s5;R6PsCR2QMn+9JPeg}L2_=J^Io02j@%zgoQdQMjRXIEeJGJ1Hx6;i
z<j2n~FSZ}6SkqVjPY&QJ{METr53a>^SL3?t$4dc%Ra|$Jf*J{J95#kPgcxTtwJ%v{
z6^PyB))i2o@+BspIb)v*2ZNyv0iIZ%t!f@m5}`THJqmKrekJDOA<x3V<K_0@|7FE`
zhY^Zq*ssx@8a;|ZxJR#j?{Wpw$&y6rQT26n7C(n#hVK`;Vs)AF?QjLY6Oh)?Pls69
z6f9+-5`8s`I6^U)AEf@zhhmYd-76A37X$KYcxJmNp04@pp~(t^-Jsg|tn3jyn@c#1
z4^VYn;uP&FNTG?JeR90q>W!uzUpaJsU*FQjdv*E$%8U{57ef<sX9{fpc)8U8qxAlm
zRlR?^-cIW6K}x}-ima{){aX!T7$otd9$w@fra(=>&g@sM%qj4J@%fY+Rd5(UH-beP
zocDU_Si`^=rGS-F3Z^MH#}t1$sGbgtPk-f7$P3T{PsA!`8vf8NPW-~RFU4Vl*-fkP
z!>psQy!C~|V9$u6`6_-f&vYR!;WmD#QC<zPC$$XsrjDd>uOz1axWX5Z3%dEp@{Bp=
zq3CkvYeU5NaG>94pHM<Y?$w)FRTPu~-+5wrcDZjku9jJeIvzgO>+Q8-FtyA<d$RWC
zY%eOjA@HTBmXCesa(gknh&0Ei4r})}Djbk&yqGGZd~xiDYIGbgrl@bJQVI_`8C;J4
z9MT#oekHU^95Kw}XGbVT^OR`Ryr5tFD+#?<AEb7-rg-aSm&~g<gU-<cs5*P+;<f*6
z6x0#7+ZAO#t55#?@^<TZDMmksL@o~YCX$>~*)l!|U*+&UOTE2Xo+^|~7$-ejDFF3e
zatj~Cu2}^^iQg}WNsDB^Xsq#@|FH!$nM0#WX&PenPt&5Zh(nKX#VfrRPtIgwcm~=Z
z-`MFsb6o4i<LV`nXU&Mkr~gg(<7AmNl8}6QEtjK8YxI<f-dN|e%L~Mt{~3lR=FVcq
zhwXIWT7ElrI^55xTs6O6A&%$6t#ld2C%2SXZC|QN?0CD51l3%2RDBT4SoxrqMS**@
zvdi`yFIVRq>L%;V_|Mvp+Pf2PFp#vs`EIS&#D7BChi3pc91%($n(~_jQ-eOWJuHE+
ziso{l4U2Y#V`YvfJUm{W?KfT?{DlmV6o{#mYYmn7v5k|Nji2|^5fZ<WSWd@hM0l37
z8nP-r<Id0wbI#7z@`5t|T%2<|s3wQZ+l<mL>C>z&_L&fNYVUCEnIuG7x|mNxqmQKv
zW(sP6+*qdQF^12uda-&%1wyTdAjCU~zDLJPG1@y;xg6>^ZOG{F7Iewk2(b6V71g^H
zH_dc#OmnY}Cb&qjp_rm2Gw90|5@<`<^Zz~*u2_=I+$Uo_nTfdWsrcY`gdxV?f}zc+
zQsxqz(>8CnmwN24jHbLE#?+g?K}j&NAFMMSA=cO<x{Z?P+CqiMa)%#?j1Nue{<~H7
zVs&o0nj=#J_Yd^qKEn56{8;1Y#hqUGx%tjxU3n4BCuI=<41qYG3q!9DrwFi_TD{G2
zVC^F#B(54hbG(^OCkvLh+Ismkql8QH1z09)7tW*!H!iWYN09Sz<_U75v>1D$#n@Z6
zN05n295jvk6ZhmEVMp>Yh91n1{IjO62eXL&ZlKAi6Wz%vtQnoUz7-$Xin9wK<H)4W
z0==+1!EN;<gdSQfWBUP%IJ@FE_&Z*{z#EewiS>4WWGVLExfB+r(y>so37qsndCWrp
zs5J|a8Wu(puaW?ni5F^_b1WCMWwTAVXWq%Qy-<@I4)^<M6<YMgc;ja5v-h&4pq8%0
zVYAHO<;&IOnx840P<J8b`gnBXC^`OXq8+jy!Z^$%z$CDhA*eO}A076@Qg1Iq&%>KQ
zCs$X{dQg`SQ42?KdS`D?5Rjwy#I}y-89%J%8S6Xr6<!NwcpmMOb4&?ZdL@T<HdZ+o
z1K-MN6PAw89|1uBOL*fams@Ex9*S54Bh4Iml@G#!^D%9R8q-npCz?-5OVRKrtvwf^
z`Qm(?Yib|#8*LkQ&*SV3M>JG|J)lK%2J!>ZS6nM`jywHQn&+|c9w=$&LICo11Yp{@
z@$#6)_K#wICVDscn5++n(+9oSY+qxO`IGW0^zJ4LArJStr0g0bVwHxabCdk%B0P5$
z``lI~7CoxB|Djv_Y<`gOVmT?iz3Kt&DEhMEoYW(&gsYLp$hcicT6vNRN@}r>1nfEd
z;zi%CwA`~R;eM=;D&6W5cV4X?8?f>%PeG`q;O3>iKnj*w99|z*JM47@2Y3So|C1Ue
z5WL}8@mA+|!aaVLCS{MLkW6S<p2QW`%FoAt5*2hOHG(b9A1^Ovt40iW7z9^Cjw{2$
zLwn(r&x)#oz0EoWho^9bO!~+!$U0j9vZ5J|mj-spe$5*`4%3GvX^uVS&}-nuzL0N!
z>WSrQ{D=@M{|xPXem*)4GTuXYIlu89@=(OJ(`Y2alV2H1IPW|%{XWF3r)DIuRO7Ia
z^sphEiIt)K->Fd^05yz8<X47(mfAb6GJy@nbQAjv5oAVF)H=FZSUv_^N}$h}BGh^^
z7T60L9}0}4FvNpG<Xa8@$}1}Rj1aE^(s^kYj3ZrgHeAV|-Rs$zt)-ZWS8x0FFB2U6
z+*_-6IC92za;Yyk6E(lz?JV)=*^LY-@<^2ZsW?pX;%aaA8<s?X)w8E8#M++A5A6oa
z5zfhASk4P;yXh<WY+B%U%~KTr6u6D;^{M}ZFf`V=5q+}ecOO{_$p+hpHYWO7>Or`K
zJO56tTx{gXbEi>Kpx2I<tG^ypUmG(#=(C?q;x{Rfoi!Lwv$}!dJQ=Zuy!YJzx>ExH
z=kW*`p9_;bs579iUy6s_qCq5Z07RT04LJ-rwpi7`>oN%p6=a_S{o~cEU)hS$79j1}
zNw9oHU0Q@C^w8x_?TOmNGaLfT0%nx8EOmMY7`1YM-0|rLYR?{tRnOQ}LE%;X)-seF
z#e@d)nOZS9$s*4A8#Tp$Hn%f-wL<$U!1<j}!s*z_E5LfwXnn{cI_T(3U~oAl-fWa5
zYP;1V$%hf1TejUZl`UpGALmSPQNJGu(n*U%WQj&{G<(e(5zhX96qZ_-c2s+Z8B!c(
zx_>VYV@Oe76!Q!*t@3W8w9)A>4g>u`*szP83vr!E4FddPlA0p|e?)S86;%>H(~5gZ
zC{yX*Sn$VRJCl%L`TsJ&DMqPm!H<QcqbzIhVz{D^PskQsg3<vg7)A5T-I~3|z`R$>
zmu1?F$l+Utp!5AVEMGv$>tZQZ&Hg+uOepL{h1=<8@?l;G1&>k>+=`zK76SIOF&vG_
z%h=jpZh25T=jzglt9ldO=yERt_?PY+GejZB)R@&_xch5_!2x+=)GZKIz|h}nL>J?s
z$)==1>@0zM#_$~cRb|6w_;1y-y<jHi_q*LOgy6HqYrkjbdS<-;DFMS=HR`ibZoN1c
zbD5T4e>u^z@n@@yZH*j!M}1)(0L~@`8~qFsXOEXE+rctQA?AA6g7x;+dzV6Ve8f8U
z>~gSBEx%Al!u5?gw(>%9ZzuenIbMn@q3$H=kE`3&nX^mrP*D9lROvH}^r<?xVSiS1
zds~-aMluCT!@=Ek%MV56o0ipkUqj$KT9yJTO?}b8{eDnE-PHhpIE1g?je@@ukdr^?
z%I#|X>~2?k6`rx@wEUyV?;HZ%TccEu@_nd1<mL~d8wuse$?Lr_YEkg#1(~&nBk<Zu
zTE%A!<o@w;yAzFYm}v2GU)-$~r1<#pa`mHC;mhmIIXg3jUFEAunRxXs29CX%Z5aQ`
z<K>n^YC*A$U9(6hmb$mr@|<33&f00;elWb4h@>L~vRt(pLRNAjrKm6pFNNaUvz}GB
z`ftB#xmwjFoLcQqW-L~45YEQpfW>DoW~2C>{9?k%_oj?wf6Wo70!TYmxXA-#T54L3
zk?3qNZ-u7OF9CV|%u<Y1a}G<8T7xEoc0ab2W;sZ&-RcXG`po>KOJ+S1hq*2-5V*3=
z_Aseixf)WUBD61L5<a!IM~3ip46?Pg6p|~JzU<O8zm}@h$iEp|e1FDuvyNwzbmH)B
z*QEs0#rG~(ABkT?`F}d5ISM7Pv3tB+S!3<9iHXC#Og@7CYQ0TnCu~f*C^T`WH#seg
zX@HNddozo-s>_+lY(%2KovsYfuM)tbx6MrdhV`U2ILG&~JjEDZHa1}Yc)8ssWosHa
zde>@#jSG7--(mx)_RfbPVt)(Z-Tn=v2uLgAC?h~_cGb>Nxgp}D&pye}_btuOp#75?
zpt#RTUR3XeieHYKwuwgI+6FKt`gpmWg;dGlOgU?`&$fCiJ58eQ_l$5px2|Fcsyx=S
zzg`uJf|ZmL6m!z79}N1x6x;IVen;39<vSrl6)8t40n5)G#2ZYY*x)d+7x7V0b1reJ
zMSFKpD_-N99ri**C8(i+e?I2~7jsD<o71tou1v6J4<OQVFakrYc6}`d)4;~aHMW`M
zF+c>UQTzu@SChfrm<DOZQB*it*rCTm;_zRz6v9u4l279^>vAvF+fE(rhw&GJ^j`GE
z=vIrc;#z*RdIyLXhS-36p~*x6C1_f9PalsgFXVwY$=g-xhHg5QybV$gv;5FrW4loE
z#Oe!VCV;quyA(jp->7*UOG1U=NP9hD-)&g$kH{vp@PXu`V1=-oCbxI)a$Vlk<<@dF
z{tN6zGA94uU-x_SxGJd@Vg0L(sNOz|S}9QHRZXto_b#{p)2wArYBB5mKho7VReWp3
zAFKGI72j6zWiDtBC1?J%8aP~vQ>)se2Uhthd=O*)2Jvt8J`#5M2Y;w5xsh3zP<vkd
zOvv7g(pRFd<pUH7KVmoRQl;VwVau7Ck`|tgI(6h|i<$ZE_Fj~k1%KW0`pze6CofwH
zTO3Xv>-ObKakar5#*UK%O%^`)?Mne%DVmM<`k=~_{;cImsKq6gVf*5>-SP@{8r$uL
zW%_aJc)6l2sCALB#J(iVpKn;S*{rI=gyDrUpEdc9FSlcRNZIysd+oSqF{bo8?tD54
z5c25AY61xeT1xOp=uYL}>y3Z<sW2`I{Eq@?X5sO8toP}j>5U6U<z8;Qa6d~t>U>6t
zOQ0#S5~UI=CxG1TvuD#%*PmDlNsect+pT)Lmi{yMbmM)?6|o|}Gsm(K-2fjmk!m*A
zdxq+Zj25+WB`v>zaZV*q%Pxo;!DsS-V<@b5&aB@<id=sp3Z6f%2+tB=pMYY?;NUxT
zGHt`!ikp5k2sVe_42F)EXV(6{p+R5GXuj0iD0xR&7sCDmxeO(TQqSv_&88iMFzL!&
zR)bryX2LIojd24mEmvRn=u*)9PjMi*ex)J~8VB#6sQnnk*2-=T9KCfZ*y&N>uUu|t
z2|WD88=r|cZ2F_ZpNn@fKYHvRFSi66o_B-gnUE;myVnpHo6T{1PBB6#PV@xTb+jKg
zWCG{E3IRE5qrwY9eV9PNOYrfVNWdf;`U2`E<yaMK5}t0TXny~Ax%z0hl1X4hZik`$
zAo$Z7tvBhNx24TwisXId4@OLnmlub@VkHdI`ya&PuMRZSl78p;VsAgW+U%&8h<BXC
zsLhB%gdY0!G-EjPZy|Q$K%%NVCJiU3AwtiDL^G6|EyqHIG3t$IcWWv9D8oqun_tH}
zXa-y8clXaP^|p?e!at;6#G-LFwfXtR`@)*i_Z#`M>}5Q(hhyWvi(%K|Dj@&`_46Uy
zl<wGRY9!g(N&k&I-8dXKCFqiuS|LwFhBd(yca;^F_ny<W3Guq}6H$LQ{?x~|7sQ|!
zhml)ItE1JLhlC!7ZmY*}JqdMI@$c2t0Q7jz0(^4++sl<G%NHN2-jfZ6qiG>7Mn4-9
z&dQ`vcNkRK6PD7VW1hmZDP4Z{mFiu|+oa(z4eXxH7Zt>PEO5!H4BEKo39$%rq_IZt
zG#LDR;40oAM+`VKdlAiJJ+bM{A6c$ua(r!E=huU{PAkZBywu}WF{!i6B?#bvei7{e
zZQo}kAnR-h)=6(fjegz_jEw}4vaOiQ9KGxZrFwfU0cL4umB3dm^~Od8pL6(*LfC%9
z+jR7L`X%fVy4@IwN*lxX#^v^YBj1gxl2hY%#zT%wreuiPIf%eH6h+WW&Oen@+*r3_
z#W}}S954dGC05jca0KD~Bl7mn@@xfb0d(u7_^+|?u4g4>@XZh~KW6CLlQ1q(!HUm3
znBn(!g6f7knZWgkL{FCkO?sBQAo*xrAG6K7F;Ru3h7>g*{f;|ddA!`tn&`BOEf}wX
z@dV;yq3Qa1rG%&r+T|_ASB}!7uNojL1jWLjlN#78u-G$lyb+^|o@RyQ@p3!+yQ2{_
z&20U%%PkJm3Ss)*TCq&S<k~McB0E|{Pm{o*`kw3CRmGapP6+!%s&8ymU@$E3H>-SV
z@Ab(7*ZI^b3Ngc%dVs=bLn3?r<sP6ATQK?N67cigMCexVU5KBr#~{BFp>8%_rf5Ip
z*r_*GEux$|yA-o><y=>fRlIyDF87)3&)32b4R@<PR(&R_gw9n6JB(uwV(K%wgZ@V_
ziHa)!?byehjYP$b2tN~ZwahPG!D&d`k8OAR?3W`qg@S1uEWxK?PH+UB^X(05&a$Wh
z-+ne6$bj?5#|_n67@Ady!Hn<u)Sj8(ezJb|GZ<1JXSbItRp9h?y$QqRH?Xqg%Gj;Y
z&dyK@Csm$v&+FGoEATRUC?L(lVg`#TXqvlgwLYvDPDzhrif6u&*oUUq*D@Q*!j2)R
zUwIX}Q*Ozbqkmv)c|mthzBo=KLKtL70cP}28M*6Y6N`^88ajD5ABY7`8IeGfYaWKH
zfs+KABC~eza(&$Hg~Q_o2or1cVr+bG_ee@OwN;IqOwm$|OvhMBvf3BF!3mQv{zUDi
zMWwuXhw`Gc8G~}G%Ma%*T}ceAW!gB4Yzjxox{=8+6SY3$>kTtB7)cXuid6AVnDqA?
z{@*N;4U-$&NN3}^{a6oM%x1Zozw%NhfvRt8PhyMu{fXH7DCs^m<&ymFX*68zKz69J
zEDBHF>#aD7yDA4j;w?}X76_XSMh=So!0mgDgW^64po1S?iuvW?L;t&rU%1^|Fn0gZ
z@WL|^rwH~4&SqkmbE3<UioW1^9ObG>I^Bq5*0Zu;NXB#95uCv@4EgqvrLvh2V*lAF
zIBD?SUR%GF9C+vY?OzRbAFg<`rfd>mTuoG2LcxX}Rpmd8C>B?}5VFtp>`&A@eTp|`
z5j0XL$bR)c6t&<$Ci+^vt$Jp9S#sWiIo9kjpLZIi7tZ+DgN+(x7m&;ql-v%CU;oOm
zI7*wV*N2zQ#E2C$j;y%7HW*YcR^bp3m(K^ry@s_1yOC@aPIm)x#HF{NXb1;Yp>95U
zX}Nl$dQbU6<uw^CG6tGR)5xiRVW}Ttlj&1#JR}AS_7;fYCQz}Qe>KejdNC&*b~|CR
zhDCatQl0p6?Zg1%g*e+@JdJ)fNQsLK)rAK5{|rs%QM$^M;H>IQW;o>lKWfm=)Btbp
zLp#gWoaoiJ+hOQW;`>m-2u~1PAD@H2tOjd3>iS*F?cTd9f&e$c!6-5V>|2iEZY}Qx
z!K87+n##{WU6olsQuf=Ams^5p<$whHCiuZ1cWcUDaqczfnatT*t~5YrUkO|kgx$&i
zMGA*&92jb?NYNmVxZ+#@A(VL448A1b=S0VSb}k-*0}xoR{GiWF*;_z364mfocFS=5
z33K9^TDcvbp+vtd7?k{fU?aA5fWk?wyaxfE=!=E`o%aLdpdoD4)P%Ircc!eu%Xy!D
ze^p|Jn?KOi4=%;t4^_OS+aKxb9bLV1DQ4X!oHnB2)3^6FfF6pic*9@H+18EsO5k#~
zF9z<pm}V2N{%T!5l`_JKXHp7#+e<-7->rdF4N%Ci#<%!g(6s)Hj@{`qw&z2Aw%2FY
z8DTvG{7Ak1O5e`9pwP7co!#PC?eUR#prLLzwC~rHX(wLVD;fg5Ywg0f!dk>5jXO0u
z_9#p`7-1({yIbK88Ui*)mvC2Q^LBHTtZR{Vo+N6|+-hKEn}gJJ^$>ZZMdJwJrk^8o
zCMX$Z2jDM;7b`w4H)zp`3=mh+%(rkIPhPI`fv>Au4Tkq%!K8S4Jo7BmVQ$$Li?bH-
z=lkML9Z5yJIlMdt_YGkjf<#yFD0&(VTyJ|-`9Jl=OPweuk2xpGHR$JaXYNImIXEs^
zAY1$rvSoAI#b`uP{MWT?DgT8~BB%BGTDh~9s7+Iw3<jO^A2h6UYn@|8kyme9kZCMQ
zXfu?f`rfyE!8%`$RcIP^=6t-y&fxLcid|)z|Js@{KbLV}ySWvcOu~I&#I~yt0nc`r
zUu`g`b<$8LQ^W>%5w2KqD_fz2SRwrFw=ZvHYp({M8ST-<bV&u_w3<cc<V@{hM;tNC
zA7ic(Nvc%#ZBmmpD(CjB_Kug^e=Z(+ny3}xiMRgcTy_E5+Zw*@{9Jox?Ab7RDT3a(
zv=j^h2-rMRThe#@wGnY>3LDSVFNuF$z0Ldr{Dpgc>_+5c`~t=CB>%<8d6sizK%X7R
zAB0^Th8b=)d_JLRV?O%qE{x&M#b&p{+U#yfW-#jqlOrhxq2P~ON*K(8*fmutedH!!
z=7}wAn^1<12+zU-R)`nmXbVfYSB)IAUK1KMjt32&UfXOCI<+5%bO*uZ@WNYvK0V3I
z;vu2Ohoa1(Dha!@m9XtU@PZmK%7EN^d@0Ujm56e2dnruM&q{=#I{Tib;A)(K=jWE&
ztyM)${goP>?X1Vk)ueaOGU;(1tutZp-=_@Pc%!NVH|X|}JlG|E(Q@3a#xZ{(hzeBc
zr$80S43#HyD+Wflt1;__PlY_U`}S??Q64o)p(>?tqds`5!t2X5H34cvK;+Be>gw60
zIJKZIaBm03tp?-3uwm+w7siIZlUN|}RlXC}_M!|Ik$-PDjJf>7cPuX;b~~8GAH&ZL
zdX#Dyr^8kOE(0_wJRlE<K^+Ms_5uxWF!`KgA~fq%gT>O4?Szw8f9g-|wReZC);R?w
zW+%?$Y@=m}Sk<rGjg2+j9=^bFG?z1y?wDDMEV2CayVu6%g*qAsH|TBCx@UlaVF=@0
z|E9dEa3wUN?<Lvb<yL&}w=mR~^n}IX;M>Ft1RGVk(+7_%pN*fh_DeR&1`!a=U{Y#=
zy&xDTI+|llW|l|sTb}rcP#=pPU-Ty%52rhV>LK^?3s^7p^`-o*rY;RtBH(^3^{EE^
z6rD|Qs`9jFDIQ81N^Zpmqdc$=c0{rWPZOGAzZXX!fNgz(1}UG>QZ>4fc#HM~PH&5)
zu2Y*ap+wo&#j#u208R(#wy@3W6O?%&2>klvY+z8df3I=8_IN48M`o~oI;fmrJO8B(
z?SF=;G#D^bw4s<5xKb;7Pb|gdOH1+jbRFEwI1o-w(Zsq{kpz5@wUReudO1|4w&)y!
zXsskhzY`cg9VVRt!Kt)B?_7f!*C4br)~6;T0+(ofpn4ENZJOw#822LRWI7P40?ZFm
zcn1_kx)UiR47B&vy<i6!svFzOEs3J}1tw=hgg<4oU}k=3J<BQi)VqlMQh+Cj$oTLq
z3If#m{FXPSK9%{5$fr`BM$|J|b~xSd+0Iuk1yANy4S4Z0@v{lH?}(tUj=q!W=vIU)
zQL$HhFuuLBTuspb)yvh**DS@>@9gSwML<&etGYaiecbx}-PYT?eXb(xf?(w;fP~1S
zJGnE#BDX%y7|PV><*lVKbH*LCT|lI?8$Hg3MEXxof5f@ZWa-TRPNVVUs|`Pru_<@e
zt9c~5c)VPlnj-ECk;pJ`Lp|RYhI%ElLtsy;x7*02Npa5IpdvU|qa_E;i=FWN_86U)
z_L}g_HQ;emK~sdGk*K=7TEVW4%XGZl{+%#699AE1F_>2^S4Vxz{%%qrpJS~;@%aFU
zKT{;yyZ*@XY=ym#mn(Vx!xWF{<^P<iOz})i0{R5o9)!ctxtYN-N^<`0_<oqHFSfDh
zt>wzJm{H#87Ebr7ayh`QSKbe&LKzcU3^PxGSHD?4wfQOO$BZq_Kc~5Xer@B)C9S-T
zg7wSBU%(6zPyRqs(TnXM_;iSUZ{wI%J`wMvV(fW1HIunrzZb*3-4~PbI$myPbyi&W
zX$V83Iv@SFmd4o)-R)s$`o*fuw$k**t`|MK5t_I?C+jQLWvl@a+Fq&<wU^Zt{fFG9
zj_BAs;d#F3ioBUrriPK?ZlnAsSU%w-F2k5^L(Xf0iZe##UhWI#X;Pit?gxu4V^7x6
zb+^6Xv~72F>(xtf^Q$X1s^v<zmn*)e;$Nt^TJgJ=V(WKx^?SPd7rXk}ir04gbzS}M
zDt_-${7<tA^N-a0H&uLV#UHEqqZQv)@hufc71t~NWW^t^xKZ&ZD*kZApQ`xv4-*by
z&p=|xcAD;J+n!l`Nc+71@#R(+beS#=#Zx(^K8%4$o=s+V<A}@AZ8N<h9Y_YzW4kX-
zs<&H5R{{h73B<mp7cygfYH}_Yq`jRW<p@jdvbtE0(Y)(z&Ht^$z_vq<jc{qdj?@E-
zoYCuofd9PeGj5NCER$F9Z2f}fA3p8b72FvPMwe_YS8`gWDK+Zc4|6Am@E~!vq-FP!
z<?39RWDU+gx?GLwBg8cilOwgXK+wM-0^s)DCzdPNASBn`wcPH#yW%|+HSh!B;7sTw
zc}6aC=h?Urps$_`B$^YPRd|{BmxLZKw{*6>mg8C02VG}u+GEerLr#Gq80ArW4v~0w
zaifgLg>W$Hu%><=j%cXo8tN}xS_%Q%9C=Rp_7)G(jyE#kf+^Nzn#wG4f5k2r>E{S_
z_GP_z`BHF&%=yc2ux}ieO&*TAxf9G5r|~VU47qT2dA5pL_F%0Z>o$^U6mR@g<2VSX
z`~!Soae+Rt-rmmgcJCDx4VBkyo%v*Y)^0K;6jhp8wtpJ3Cn*v8penbFXL+rD?_^FU
zIIH=Yhpo}0$bGT5a!`|#sfwIF$Kfs6?p4M{cq6lO_ISAk<ou8+!*_UQDah6jhT_9e
zJZnB_@AoaYH=^nv450sSD0%+YQg9shduHSpT}4{ksqgm13q3oj;J?rUEA^rWE#US_
zl{xQwDp%aDsfPl#FZGvc2aRL&5X09H1XDNbh-I?X>Zypgig>GfgLbNAoXWu1j&~&=
zZ`6vlTL?c&h6E6+I`RuUvCB7fYl6{&jmhTK`EL(@Hr^s0nsZ%TNmqXJ@ls{PF9?Y9
z2fO-0#aT43DZYhi4!(+Azw(LHur7II2r5#a0fzfe$3y%u4*1=keGYQE`tK|LN_@ck
zx)aRz`T`QIn2}<5c?@!W&5n{uwET@HmKUSogZX|)d=QG~PlPr9PkUy(M*i`7^M7Zd
zFVJRfd@MS%T2&zxC-sZrXb}HS-#WWIBhIIm4<yce^)}p@l$i!RbqmEwil%M_)&8rO
z;(zrr791Qdh&Z*m;|<HLoH@R-s`GZ(^+#iexH~HCFx|vEw0wXCqXkwqfH7HYvJdSh
znn~2%=-~_Mfj;11`1inWEJ`4@A6c&Cy=6JiN80_KovX@8)@3D%4nFN!x5TL1f3)v~
z&*G-d=67AnT47A!PM&|=-iI59!yq5ZA)BC>ltnGm?UP39+pXZclC}WzL6GY0g|7Yv
zN$HA6o^*Ss+wr8k5*hx@$CoQX13h$S{n>UeCL!4~%Gb9ieS5EG?|Nh@R)Ka#;oF{F
z=@vj&JzE!-0iJc9fIO_RTd%EA%Zd1UGY7PJmSWsZ3EuDOY**V|StoP1D?4-UbtUvw
z&xu6`bZ0O2yE;)NjJwqeizqp@TG!j_N&usZD{m*MQv)9}n1=A}v`U1>W)77(=mk^s
zld;daDr12!RM}jcg(7~&!GUDd9dE?i3z9ZFfw$L#2KX>xjT$8lCs5o?F+$>%0CIu~
zWdCOy)fZwXFA$J;Tx#RW8pyY-fwWg)P7X>C_xn~vKa;yPfOB9RXwU@uosj5^ZpzpC
zhznU$eu?$oXRAUB2#0Mm_a7(b{Rit!(6Di_UxzXkNG50)STyBD1v5sv_qzr728nh?
zjC&q;a#ykU3bW=cf{*1~MZrxU=X=YrOdnCkDlV|x2XlB?b;V!+!wXJ61>i5P^Z_sO
z@7BtHmQfNG><5PAqkni{bmtYE$$Wads*jJyiD#qMA$RN1<?5tX9&l?jSy%FH?=W&1
zS7w_f-pTmicw{MNK~r7c3RlDJsg}d~nEVj-?%56Xh|t=K&p)3@MOFB=l9UuTPk*Kn
z;Ho}MWXPs)K99ncrE9)8xwYIL)SgAqyMggd^?Rp+<~M_NxAs2M5cbkJP)94z{1yqV
zn3IvKe5c!M6-2|;*-Ojq9Ced!njdsj<uItW>8ox>H}!02=_(*6XC7$6g2@>6g~X9b
zc?`_o@b3l;XB^V0+pk(G^MRkqi*yS^LdMx0qWSQGC*gEfA$buPleFXe^z2>`a6V6E
ztRoMGdot+vs`nE$dejx>|8Sq}*WTuMlvRxqAyuyS1uhod$@DV4_rjHW^ox1}Zq2jP
zy7YE%(2Iwj{kF(`5>c3Fa{0TqmRl(C$8@;*T=ibLX8yA^WzZI_Fno9%OYJ?fyj{g5
zXBrHLUZ$MwCQJeye{rCZs7-y0U!1s@oW$OWH`nDA;D(Z$krB;j?Lsf6sk)pref1au
zap-R1tSMqeTWmIW%!`fUF73Ke04i@iu{^t+yLyuBn`u3HtlMOtLuE0oq^zT9sB255
zj@BrlpYMfn|A)OC;J04A6gR)R;x)baon2k2xLoly75_rT)r#M}6#olKk1_8PlL#$C
zn5b7!h>@fs#<hxq=Go0mO;n!l<l7EgB$qhxDh-_YS(-xwe=PRl4WCU6#-LT@;CLzU
zvm}BwRAvgvuNOD^f|xSp9Hs5Py|C_x3&v3r_KEFTCSv~=s=Oa(Gr0J9a>%{784uaN
z&kZ#MqHDZXK#U3sLpJlB?TeG`r66Fl=Pci1ei>Q4f3I81Ab7c_JoBs&Z}WVua9vKQ
zxu_tmM1C!ekY40Rye26#A}Vd=i#jsd#5!*rdZ_a+CWF}nseUQ9KI6@4Z%msP>cK`=
zB6V3)hVN%CEzgEGQW@NZt2MQvkdK!uy;(Q6`l{ttlFj_l#oF7d3dJpnwOPkwfV8r?
zKoi4MpADS*xh^aion!F>v~Pt2?&=xHdGB(&alrSwRS5r$W1prD=KmXw6i-SanR|M%
z7gsoUeGAVUr}yjng42r$zqr%ew>!1M<s472mOr6O%|{BBn8i8wYrCY2w*zg?e*d*T
z>vn^NXU?{OQsQzzt7U2!PCp+k4>_NIW{nrV-U^>42G(i}%lAw)d-TTTYOlA%e)1*p
zDL2_hV_Y&c0-B=F%x}TC)s-@ahp`2iXAUUMKPss4-h6e%S6BR2tLp~+g|A<V-=d!+
z#z>$1gFzbWy?CvwKT`2c72jI%$1474#kW=b7EW%gIdMP5C_3RxygoL2{qa)Zh*Rt9
z($o@V8_i1gvB14GN9@(WDNmn(C4PuU3EJsB>l7rD(5(h6U9(Vg68l&x&F~xSA_<e1
zt4yZv#2~A6K6|g`S3z%klSHjaGim?bHE6u54OFCus<OE1Q@54|W+PyXDIdY1TGcOc
zIcW&v<~_1pO^%|sym0z?HupkF1r=@)uNS;<KKl6M?^~)s*m$(LJ)((U3Igt+-L1E}
zWy?6iq6t#A8(vmvWuD+{#jfA+Cj<AC2VZ5H!EE@W&>oivx@KikRjB%p<pk25*B|ey
z@$OOKDQS7pNo#^&je~6qBD&=e7|iI?jIcdesPN3CTIt(!U2)5{Bh+4`-KhbG=Sj?*
zIbLpAgZ(N~oAYCVNH*pPk^2_(ridwXYdTC3Z;o&NXsT2L?807P{G*zZTt1??j+a}E
z#1S^zcNjYP-i~nihkd~ZroDLz;+|Wfj+vbZlF@AVyk-;}%M|WsCMo*<we;!D-@6q5
zGoMvWJ>W@s5HYr?g&TMS%fnFk<AZ!W3`uH#yl=<P!NpOP_4}bj{6b$G_H0$XZ|T_~
zDs=nmZVzJ7{o|#8Vj{#zy%GbTRFA91HgZnz%Y{gBu_~+WrQkwLEEFeVB$kc~Ybpw*
z@5DLxYV=NwNqO9~S2|X)IBj%JRScEOx&On<Eu}@j+{!q-HQ9edL%7i`R`KogjYz+9
z8YllRs788f`Cgs#asm1Cz1@FwDg408{)NLDb=(EO!7QhG@pyTGS!W+c=p<2_Gxuh3
z^O;`M(QYlTg7oHB^ehE$`WujUy$CP&Yf6q{Hz1wGZ)u5Oz*>6){NH$6qqD!PZ!g0^
zl)lwlZh&tYQk&@yswdCIg_uk<7N|IcA(9vVWIDD2XuQL!Sf~GE)jO53$%x1?9z{Wc
zyU2QT;+Q{99i>68Q_@K-QdqO?1@~y2w)l#L<s<pGiOK*!mr|grpm<`EAlPWuNlB&G
zIMHnPoKtC(Ida<L(F%TMxn0Ql`n7(Mx40c9e}A7r;@33Ai88CC@i6A!i}?jfG?(Nz
zznFWzIqYr-kI1!Wua+ogpDC(f)sD~z6~gn|x+3RKcY73(M*>13k+jB3D7Y86yjS4L
z5WKc$;;60fS+2g!KXjk5W$f)W@Q#XSE1s)ZRRjyW0I?D{++R@Li7i%v@e38WN&0R!
z>h$abjcxz&rLc&@=2Wte(eGbcUZA<Oh_~lsOs0+hjI%A>fCIsWjfL0K5hjs%H&W=R
zHM*{rkI<JTNl|z^#OnoX#U|UnyC1kGeWpuRS4T(5%*9^Zj7#W)y>5LAW@^N2^FggR
z-)A#k(63NGF!-G)r~&bt1E45?AJ$QbWr_FJVrrQ#odYiq46EKQzhNmR<DmJTz1Uk?
zg*G#CT|Qa!W^*6(88w12M;G@@7^&bzj`#%-7Ui6*x~2IcTVXqc)VRadFoVrc2ucDd
zo{J&kV7*!7ATc=GGk7s01|SK8nNy0=M-9s{G~*d{`@vT&1sd7-Gdgi?an9A~-B3cM
z-KiBpD}4$eDV3SDsGeJU-7cqO=m-a-IhKV&nL}V6S>DR&%YIPilLkr?S~6$p&{Jt7
z3~~^{><MNUSOfiT1NR}J3pwMA5$r$P?RQsvZ^fG{-c<4Z72j9!mWoiaMVvFKJAYv*
zw%de$?(MxG-S1hh?z~i&6s;FwZTn??vA!kZtM%>PyOy_e1Z0&-%6ZNAV(J-@rq-X$
zi-0uuPNAUX?Jp78H&V#NUN&kop%0Vy*2up8639Nav6WH))kA3Sv(*<>r4M2J2bap*
zKd=;AKeQB=-?|h#sDJ<)Y&_J5&S5u1?70n1P>j8IywE^AIjO4+hF-Ni!->?A#a|aL
zEze}OAwuke>uxk6`^A5;KJMcoBgaxWCsuJdEP);0B1w_0;PvgMOLzd5*PNp7#!Q5b
zoe%-~9Sj#U3?mtD9A^5Aw)n^!mRpj-VX^9J(mO%AhYG-bDQ<`Mhmc+KbG-ECTO>0|
zL<zCme_<(h*5tYW5P52a9DOc#255%A0d4sHq0burs^PD+Tq|eRMBdd@q}Vnh$Wad|
zf(rEqSL@2Ry?v-7L+~PQpQ!S9xq{P~Ug?EfjqdpZPU8||6S&jfJRbk=VI1z)6bf!v
z*}}?M^iwM~fZ6CNeWIyzz>PsvCV8@}Cf9BpFE93btFnXCPGb2RwMWbQ4KaRp96l!G
zN-{BjCcv*$`CgiJlb(ICR%nnrK_IfZ|MYTY920tEwjtt$AMdIW5s1lm*7BxvJ9ov{
z6DQZ@i;025XHKb`db5G#S_FMTD|IBSzZX+mPR^Qw<=OhhJ{uqzIl;h%VAi~4AHFd1
znQN;(UL^dzx897%t>gf_SPWq8j7G<l2cPMASp%mR!n2GG|JgEwlZNV-$eb9|7YaW5
z7nc{4+pNleoeJ6xAP~?wlC5&mM^*WJZ`na>93F%#F5;)4sQ|{m2Uh9v8AoJf>0Xn}
z><9DC@ls4EQv)Y8I;-0rS*~vNEqnfn$YbTeew_<ABy@FddRfaX@hl{$x6N|iN!s--
z9QdsXH_wU+p1F+hxvI?DzqDLkt6#3h7|3A0{OjB4jS3?656SeOTN)TszR_!M?^zhX
z2koH_xaM632k2XYpu!fow;O?<BlUt+_Agbh%G}VY+^}A9;^BCC@$j6(`rGfi*jvd9
zSDRw<47n@zJlhnT%BlpxU*0_X9|O>)l`nc#^WzQUzwcab$C*tA?KVCNO%pUUUWX;{
z*RJM~?LV><=Dg%FaozX##U>xdv*h5CUn<S?)NT6e$Ck=ctY^BS>tqNRSF^Mh#t@C)
z6I9Ygd%=PUucrj*2#J7u5LU}z+35>jfdGp_wvdzprB&$Nr-OOa^&6I}Z`KiISuyIN
zE>)&7zF8k};w)t8i!-mPr~y%tuxb+!iLF376MY4Mc+#Jh$R*tK`7h`fdFt2j<!R^x
zxDt|4T90#1(&+HKok)&#>BTIrkGU@Qf_pUj&u6`Wx*11Z3R|pY_gfB|qrYO!oiyPT
zf%ZBSyj9D6doH|)-#!;kM;$^=criAL0zJmQWel(NEl-QAieb^?0YlnbLsf-6!jmin
zN9iC~W^?MgDfLXV(TNY&<4Ue8Z+yp1dS}d2^)RMzlgY=O>;k7vJUISP?R_qoH@8yl
zvOTu~@^)3+4iZ^n*bPIwEN4|VR&v$tXj=y8k>z${F#gZsnQq^Y9apvT<z_$DKH<^Z
zYb9DfSi5ig&-U!Oid6;5G$PV-F{ozw)Y@)2mi`{!tN~ux>a!F#J8i?3m=hmNxZY6r
zGIEo&3j!F&@;G}xUL!7(4G4lOHLn3P{q`t6d_)YZ0e{3aJYV?HJ_~=sf2!pbwfC25
zkM@xg9r4SHG!rsA8G=-thfi-XS>c`x-ioSrtUI0&-yg|~mJJ2?x%#->7gnT@0Pfq#
zTJ|Yg1jWh{5PJ!+&}}UHcP+QT*z`LK;3)lgSHE)x82Sd9rT3QCm|xiCg>RteNq^5$
zPnJXq?zTX?Ds%g1f9+etXVf=U-dvS8{j+*}+jG60tC!+~Mh@Acz?UR^X#eZ!oNtN`
zu66YbUCG28cXima*L8J}L8FGx94}Y&+CeM@u|mAjgf;pXBhRT5!XnRJ;7UXtbY(k@
z83g*^M=veUI8R3O{zgQmkE{F)GS-`%2UkP7LuO~v9o4fs!Me0NRmSMxvoeWyGbx{l
zg759sdh15s5=5MHr*UvdoycM%;N;9}c|W|&nF#fJYIAg^NC0z~=+Y%eHN{H)#Xfs-
zjo2SuBm29%G8yp|UFpq~=~MCeDsWK{xHEYgPHD4$g2U=#jA28VJ@e@Z(~loYOc_n`
zG1j2Z97}I@6PoVUilb?6K_~hq#7;c6Z{i>Q|I0g@(73870Jq}|PJ`40G8;``Ow%%^
zAR=7~&cI|@OmwmcrS!Q_5vf1WV)~Ohgi=BTS4FU!4q3(Q+=(u->QY27bfsOn$xg(D
zi?-hHJ96K27*Q&&4D|NBdGDQj&bjCR-gh5B%>!JG)+o|xysZOmL$7fQDywPC|3cgJ
zYHA!K1z2qiB#U(64jQ#oO^wl}dq^$|MO^YEh-sg}&KLBn&}?W$IaRGfERY<F4cav)
zX!l^9tHyiibc|M}eAhq(@|_o2#NoykBKUZ};M||#C}VXN8(V&oCo1&WqU%Tx!0I06
zMw4sk@)8|sZVuPdQAM4B%E<{%kI-o0^Z~%L(CxFvbsdx>Oq$Pna~?8ZA9CW~Qn5Q`
z`K*3F<e*3Z%H4>yv5UGG#@s12vo^Ocn1UtcY`E`3+?7q%3FY(obW0Qq@dk1z7Mp|G
z020(KA$Ji5Q)yky$a_=~oZw=X)F{BO6f};4cGpoLW_6a#kj~xMCo=C1Iy4l%4<`Al
zy?KgDC^SZ17`I!%0L5`mq@mC-6ftSRY3w-#4~#J<i*ytp7U1dt0yR~0a6>a^@uDr)
zG-*hkzP7<{9ZvnE`R5R*@2SzH`K2WRrFcs{LJ>v$c81rzkbcwNcQ2(QX9g|;^jrxK
z=IH%4z`$HrjV~7ibY{|pc?8Kf2^}&8_+|R-A$5waR@w20$Q&7W6-W<X(biC?RvEtt
zm=V_4=z~7GS5wnmr@2N0y>{u!6;pG~O=Fyx0pK+1SK+Z`2VOK#HK(v8P~-bGxOk*1
zGAMs)mS>^&Gyt@OQvit60}h$*pSqHchPDW#qXzfsfCfc6ij)?FG)u8Qhc2D;)<ha-
z$CR^t0gB&&(L?xhTWn~?Rk!zWp!x`*;uBxn<Z2XLlul&LDWP~1m@$g+&})FCk<kwj
zqm|!cJVKrM&GhD>CRgmWv5GhMIVIlX)G^mwM{2}xO)%(!kS}Zc;US6n9W~V-`O)Lp
zaDzc{JU#)kpq5Y#`En``-bl!)eU^OXw2s*{-i;hlvWoQEb!`q~X$0!#rk1>qVtwY|
zhO1pWbToy9bI(G}!+p4BQQHVb65QI+lDENxSLL6&@KZfK(L73{He#w=*<cp!s}Kip
z$h|f!nN_cGGf2A3Mb~S#<NV<34h2`WW?KF}<1wTkN;bXe$~&?S4`#gkD?I%GwpTMF
z_E9x=*%XaXd+i2~#lSlht2_n<o%d6t4HL<3jZ>gha7s;_lHsqYtz6}Fod)h|>wZ>p
zz*OH8_66oR{pdncW93LovEJb(^bpY71C-2(;7L~mQ1@SR6NzRvecu%6g1Y5P?H1z<
znPMdtk2zX?>?f8Az(02rveq1*+dzQ#uxpV_A=JJAJDO3+2+WPKtscj$TstZG^H6RS
zWOAXmceRpA(gJ{M=zVKpmz!Nmt@+oOPeQML%izv!Kg&p3^J*2xn|+18k3vKkzz8lC
zscGmXRc91Jpmln4{}&@84S=pK-rLc_T~4>C=f($qS@?02Oz1E<lgjNJ`<Mpl>2~2p
zQzRLE=NB~k6O6~kY-)DF{zoK2m#}RxX3)khevzn)02`ngA6RL#tgZlCK6Y)qs%vpR
zL=OPB0lyEXW~p=h_Azo`oyOvd9M=T3k-8qY?5OPYTLVdJ!=d)T>q3?RnyY-2#Gzme
znKZd*0pKD}7?1ltj(PGv1R4^5ha{@(`wXoF239f*LndEj@&2P0$z2Z`8NkT9BQMDI
z8wr5*7r)gb6rHBwxo$_-t9|*>C+reIdWX|0M>4hvUvBZ6<~8;MIFn8dx3)Qwe_l|o
zhHmjHjryjkf8ZcLn|`Ys1j=kpq!v!D!RN3;aKnYODvor-l2@?O*_cCU9^4wP1^7v>
z#?mVgcRhpkNr{OF)Go27Q`OX%mkne@&O}-S4%+c})c{Nljx7RKeXM*UGt9Y@DAgfw
zW*xp}6j=vQbdcQ-(@|3>5Z$P8nk6hYL3q?OM;e4se}=3TbXjtvqwNHI1KN5;YT8;0
zz-uTLc;W)j>e|d4jEpSrm@_$0;urD<CuOJc)k?07#0SA7A9$J|(`G*)Vokvmpyohn
zfir5;MS8-hYHBte^&xr!BqM#zW_Sbh;MA{?xe=M4Pq?R3he}s_DSwf0(1+MNyB@<0
z_#s2Po=!{iB`7~2x5=~Nz$FV_ogli7^#V|7-Qqs|`tG&n#$~X&Kogx!MNhZLeWBP&
zb_ojbA7!jsQi=LG3mWBF2#f&*o`I(ExXw^N)5=JO!=OuJTi5m3?@jt`tfj_fzd;B6
zYMQ@Cw<2AF)UP1_#f+5&5;-~Tm|G~L%PyEY6l$ZlL;&Ok)K4@TAnx2^7w|p6M?6U%
zn}AYbO`KHD1&CI`v`y}9xC6XcJ0m**S}jteAFKjO-4BC&C7m|)bnrCdI@D#s!2^v5
z2eXve?-tVOOS`5I5#;*>8s$Rf7~g_Zc<P3t_Uk6;rEPL3_8fis=**^Ofr0<a@8_w0
z?igb!Rw-`ttYKptdJl@!+`&{WLhqad&){uB(K&80elDvnW<Zu%^eL9sBDsYd<<`n_
zTo<UllKhBhi*#R90`&_#rb}qc7kL00&K8tul@;8(+%oq>YWfu(%VudM=XqoIYKM-u
z9YG*3UrNnk#}W*jC{R>h1$<QfV3Xz{jPx2$fSNXJs0_&y$3$1`foDb2WMB=Rst`3@
zFQm@HSpD?FA{}W(9hA}|svklzE>SO|d`utm$_QB~=4IWPbitQ<RiOrw9fA5PM-UTh
z3?ZT17w45)I@LrPmBts6@eBPUG&)Ev$c=PrU~Ra1Qxd>sOBF)KqW}h<|H)=H)?dK6
z4eQF42)_XNEEg<XI#bvOs8OE?MX)VJT-e}d<EzoP?YOFuzC~|NqQ~3>qi~$7a>x&e
z_{vzF-?k90Cc@>_0SHAj=U6(71Q;<!83TvHIR|4$Yutt;%9W6xL06qJa7tHB9tXTg
z+S>!*+4!o}h8^)+gcXpg8`F&VaV-%yptp1owggOvoD%f5=;$|$p!Mu{FQ!C@KD0U7
zQ|VP}>nPac#`1JIcEU(I4rGnF3mn})XCyx#`{@9PF#~tvAtr~AzYq(oqZ+EEF}6_s
zlqMBt2%$I9&92_Ak3$HSjH?=q!|GF%q$~~$p++B-f5SxRRfb&YK9nB*CqO~hjLKwc
zpjgmU-b~HkEarZfTXf~x;`5xoM`IWYlvoFf#<vZGpX4_PjJ0`MWedsP?A4JOPcBRz
zOU=-Wg>#(BCM?qR=QwTAoS=E0W`X8pYW^p$g=04+XhJLCnzp2m*>sZCwM@~#H6u&a
r#YRuOgD^Nv{u)QS>7ReK)IR;^F@5lN2L7kN%iqTTZ=J+~9y;^~>?H9r
diff --git a/src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/read.lock b/src/azer_robocup_project/.vs/RoboFest2023 disk image/FileContentIndex/read.lock
deleted file mode 100755
index e69de29..0000000
diff --git a/src/azer_robocup_project/.vs/RoboFest2023 disk image/v16/.suo b/src/azer_robocup_project/.vs/RoboFest2023 disk image/v16/.suo
deleted file mode 100755
index 5d7d8b3c9f29ea7979e645515b344c0c05e2c74d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 49152
zcmeHQ349}0d6zeLLP8F@gd`=-W;Y3VwbqPoCv2?Kl4Z*}Ecu9o(S2)VjpS>+gi@LS
zEu<VJErm1$8lc>irlmkj1GGR1;S5mPa+CrE8fc+~mWK5I&1megtYc(r<0E?d{WWji
zy!XC$e(#%ieDk?SF8k)2-gnivRCVU0>MYf-p144DuFSm;_CwPBr7G1$wfkQ^@x&A4
zd=>y}&+MZT2&g#xDyn{!Lsi7JtXiR}RBR5b&RMT>G>Mkx1;r1Jh33E3cNo52bgSsq
z2a233t8#;S5;liwxn&&n4-cb7%8R@Hd87Ns;V7k5qEB>7sytFqRwaNS$|3?J8bu&;
zJE}U=*D3we2><O`922T4sN_`{<e!j|QMSXSETh%PO7zdhgL42%M<YM=ZeN4L<<wbW
z&I2BoXP{=MKK*n&y8s~j`MB15)4+`Zo&j723;-7b#50}=TnRi2xC*!$xCZ!j;Mu^n
zKtFIDa0Iv>I0_sCZUAlsZUUYITmca6<G88;2S8~eb2;oRZ~_orcK$!wATAHwyfSxc
zKRc(i`!RI0|5H7-GMc1;iJ|`+;Awal`+o`ayV(CXwS!jqm!8>S>1O|@R&)q)(p93-
z>UtX<4im$Gp)h3VmiPemtBYms^h`3uXSBd2z|(4esQ*jPssBs%om?CBe;wkk@BjK5
zJ*odc7x#kz<-v%n2`~dgz|Fud!1I8Uz!)$LSO6<v1E^k{0QLV7zy-JgicjkQv$&rC
zsQvf=KM(+dz$6d?!oU<T4a@)(p2~a<xE**tKzwc|WpvQOz8U557T}KoDzCTV`ZnP0
zwfnuE-!CGaUy`Mj@~{&_;R*kSIDY{A5cm=B@4#cgkAa^6{{j3bAo>3<*#EnB{|j9I
z2l(IG{r}aj@^o1t@o=h~ssL%HjH^<)2T=*;8pt1guu1%&=G^i^eTc?(GLxXogYFn)
z9UhWUP?Z*Bu!<@sNJhNqt?57HA3^4z{z&<1>QVN9`sDewdy@M|$}2<mia-u?A{}|$
z^XT28WST%qD&P+se2`E2-$>OE@=vnMRe)0Z#CwL$5))uYyp+>IE#@gf3XH>b2`S(}
zDFb>Lf%*njF)aOSK%Zo)D*&bR$Km%%62f7mcJrICzod45C$4t^F9lu(yd3y#;CFym
z0Ivk@27VWK74T}{HNb0u-veF;+yneR@CU%_fj<P^0K5^n7x*LKKHyEjp8)Op|2yF4
zoxr<*cLVPM-V3}Bct7x`zz2Xo10Dqa9QX_1gTP+`9|ArMd<6I_;IDy?0)GQM1bhtm
zIPeMJZ-Ku9J_&pZ_%!hMz-Is|ug~K82=F=J^T3yZF9K5km&#PS{sW$U9r#D!pMZY`
zz5#p__!jVO;9r360RIX|;ploM`ai`hnQry}3y^M#6PLE;tMJjq{@-^9@m&HarT-Q9
z?PmYK2J!WGK>uO*b@t&8rW^gA@_i-ZQkwtYg5QUqPSi<miQy;mx<&ArxWEffz?}mh
zq<2%1_nZKS=D;E20w=5rT#&+_1pg)8_bfmu{qMrBRQ{dNr;(xpX|0TO6p`vQ;u!{g
zqAM9VwKd;^k8^9j#v#3hP<!?H?U#)Nts=Z(8}z@A2;WGJ7Unj*&Bg^DPcsj<BPC<P
zh$zm;h)>9kMhTja5NksWOaG>A(ElmI9jQgpYW^XNw|j%iD7;_Syl=VOgz-lzd%AA%
z(7@qma3fw!MhSgJ^xE(r^i>z*$x%Ql{h!0{7izI7p>L}BZFC{}o#y|Sh-j%%WZQJ>
zl)juy7%kt9J|9J2PvLDSj#gGeJ0pq6g;poZWC^%X+oSm@ULG9n<51rfaJdLjTK<p1
zZ>{-5{>y-#PoPJ#4$&hSD~5kc*CN`WMwnUJM0Wk=xuAao?k)$E(x>rHia<&I>m<(&
zuG^{q`w<Vx^kkIK*Na{o{!{;ZHl7eaRhs_?;Fs!vj9k{Jw^N+tb|J1*zfS|GuhCdb
z!w1RSmjP7v)CQgb{2Fi=cqTw&5{(NqK3omZI7zZ9rT1E(AGi)6UUWS`efSta^?4(3
z6Yw11xxfH$98d!%fI&b55N}`sEkN}|Wn}=20QFllFa+ET+yXogkmu8ad#lXN0lO0z
zk-3e+?y236<LU(_03YB70zeR$1VTU<m;$B&IjuR|-wr$<m<L_}M1Uv|1L8meNCGJ!
z4P<~UkOT5S0Vo0-AjM7fzFo7?_fw!<7Dl}yQbOZl75CH+TN#XhebgSP9Z(ut`IVlL
ztayYP5=?XYE68gUsi0gpH`^J1sobO(>*<dpzXedGbrAWx7%v5=yB^;L|343LP+K8W
zPoJcl1X@K7T#*wvF1>BaQzC|w{&gsS>St2@OYW_%=i(vtZ8G(8kNBRO1m%=a|BYl0
zF{Jp9{P#(`S84sb5%hstBonCPtZE$8%3I#C!$J?1Q`>4k0{Umexs&|6;Qvr9DjV7t
z$(1~MZ3ZR3v?Uc=Y06z_{e{}|S%A{~Z*G4i*~#m7>sx=I)+T91DvI>VdnDvPVLgrb
zu2TB9!tXb!(qQV_-xO%j+)El%nzeZ`r1;;zjrMOtP#P=9)YEU={zboy{<ky!4kMyF
zYmwE{r}-e_c_afhpLN+t$#(b;mA};g8qpUepg2+y7uG{tr3dntFqTqzDy{#MGWuQU
z|H7C~JWVP67sGEi`#;GgB%>&${~Pez#r`kQCE8>Vbz9Tr{x8T|o%DY@QmCdu8%6`^
zNvrBdOC#Ak2U`Z?Uj+O(f%OWqk<@<z{b2>ZsnqTVhaAGQ5d4$Gz{4H^>Sr>@eFUK^
zc#o)pZ_PnY;)D@BC%A6h)0``<ev|Ca3Mpcnrij`r%~%lSIUxki22hVlA%#*p&p|p%
zwRAebG3m{MW^*Y`F<ix+4kTL;Hx=_Br7j5<O36INXb0Yi#t}vtRT=|Gf|IAD4=yil
zQDQ<6{Plfg>4SGWW|x(gn3zUZRfzDFMEUx(6uy1^Z7!F3{v(b=lz(w2gazeTLK#q*
zQC*U&+z|f}TfX=&f7Z-SwL@bznIOuGRvu`?Au2?f#yhHEswF?#4%HBi&HVZ*ff$zl
zY5XI~*TYWNjVH}*^8P0udB>3wS_Kd#;S-SWsa+O9IS#p)+8NE#Zyput{g>!Ura9G(
zCzN;MS!7z#4+}Z3pdHe<FRhSJh&W0nEu>@^^shraozSOt+HCw4`(kJEKgCDyca#QP
z+L|uQ|3dj60ac~+<@I0R{`aH&e=YJ(@8e4IFPHx{-su(AAK@2snJ^QmOqP>T<D_SC
zD(#)(Msg9a%dV^Dr$?5(O9fvh8k}%xLm9Jv$*nIhBqC`?aKSWIG{%=Ai({qibi!bE
z<*m$Mh+WYKro&9a8;@!oI#bwYuP&BcPG*W5of=I$R<d!|lsD}4ERKGlulu8k#2Z_W
zZEo~z;%M{WZ^VJ9chmR~#69sF;y#<7oxqz4FL1(lUKU%`PS`*BiS%7Rdg*1iJ@(3%
zJ$Ti7@BHyt_(S(Rew4fXozw-WHsmIht1gH1+0<&>uFY~>sN^V>rnvjgK>j-!O<VDw
zDd8=kh`EjuIFZN)DQ9xSZiD}m6me56l2-I>;QiuARDdKw<98J)@owV7Sbq@uKg~B$
zY0J~r%uZvn68<ZvKMCq2FVI^-N#ML<?6m$XujOVm>)j>#O6LEx^?_ygH2-%D_tdAA
z=Kl-u`{r64#0#5&-cjfo$<|NB`9GQ`pt%ZZ{%;KSPUrt-5M~y*4e0j#UkPz803?x-
zR8j$|z#^~&ECVF9uL7rlHGt~#g}B}Uya=FnNc~~EW~bqa4-kFg2}kZo9~*XDaq(*}
zxz&33=dUq-d3o%v*TYU}libNJHzdbU_>DZ2hUL%O*>{@$cJa4+zLD;BKAS(Mva2^c
zajTC@$vv$FJOqfNxwxK8b9ywY%GXl+!TFaVwHq+AKZo9wfut|KnQ@r+rd3axhvU(X
zX*Uk7%T{2gw^I)O>Ftogoc<i>aEQ5pXW}Tn0=F<+=O8VMoSMe+afBCL8LSSRKzK9$
zwL&afT;~uktu@g~H_f+D5B>B3$VCi1pd#evvj-qIB$rVg(HsB4s2?7DncC<6>@(D3
zsde$V#`bNU)aPOuJV|$8azk%41tB+|+lL&WzHY=hJIPhtiMQWI8Y%TsugPHip>dG7
zz?Tj{9f@a{au|JS?L_2m`*AF-uP1r~%^#RrS_H?Y-c&}OGQzC9xEEx<_a@3$S&#<l
z4V~s`sbB3!-bbq$dE_7|jI9#4knRssp2VCUjC$Jo9)aek!1T%9ToriV;ze3V+44Z4
z1HFIJ{J?I_!YWK=Q*V_1ofujN{UsJKjwYg}L?mW3s}1qEUaezvNwt|ZGiqZ@lZ+-3
zNsW%x98$fc&pxEV@05jgI*d98qqZ7#S~bJiHEN54v8Xi~i^gWwIGs+L_Vn5<;~75Y
z%;XdN$u*wKS1TE=SWe_4mCRydD9FWPiSis@DMu=a)XIs{%G%l~lVEH*wbtM?t93fP
zR&CZ;ENa&5wCL=tLvPd=POo$?5rs&m*a2k}w{YBtiaayCH%jANm31&UImECgsu+eb
z#97wNsuNmc9Q7EBt0NktQJsiJVv(3GW{Smhr)vSs9R|<}Ro?*xEu)x%efKK$yjmmZ
z+hX~}1)7>c!`5i({-kFoM9W$xY@G-!X*?q-&I02NhSH8u3O1{s?n5@q3@$crCZO?>
zgJs$q3~1bzGOvpn1TTT2Kg3w!XF0pHXqxed$k%Ahk)Dk$SuA#b9JfZhB^F;?7@+{;
z)nF)K4cpVca(L1=x$F%%d<NE-Fb4evjgu{S#zKy;+vd}|$E^#YwAJ7p)eVk}jQEx~
zcXGfMFou_{R$sbkS+*p?HeJq99km+#8O@A|vyFI4oX3x4nkh?q+7L4?Mzzz-L|z|?
zR979b!O^kNf|gm-mg0;0s%CsJ!_Um<S4$&Y+TrF+Bi7Vx)ns2@G*<kXoT;!BwMUIJ
zBc%mH-m#F6EkxMC%2Hv1AF!rkE{8wmSoN4AtK+NVyenl&^6_fL=qni#MVDuVFGrF-
zx8K5~t${!;kq`MTv9a{HBV*1^jxEQFv#ho>n`aaGr3v@Ulyf<uU7fU+mkO4ph^rKi
zIaino&Yd#(wC2GW<4AH^&*(yRVW4DnIBlh(H{;8UnQhLI#Y&vfm4hp>{HWbx^06#p
zW-76<d}7h#E)D4P*3`nZwG@dJH46*&kbk6@nf98U=263-#hvh`m`V3aWFpI%2fYr<
z1lA9=acyLFIuO=RnSu*!c82Axj(l**>8%FL%j~k=HC4$)qUG5#mkLaK>>&pq9SANL
zg87uO!jCXxZrilZXPF+zm!ip}anR2c3a*gXV;Iy|*eQ>vqN|qD)@X3I7cgDcXv*&Q
z<tJv1x-s53>nSWP7#7EPo2R@qFk98;ZP`2%TBuqKKF!jIFQZFL8y%}N>A2Tw$m+dZ
zW^B+h8f1d{8PAMyWQLCzDnY$9AIeRXDw83D!_By3-jX)rk8)m1T9a^x;_1<qXv$fR
zxy@nLmo$f1tIp&1*xVU@Eb8z(i&LwH)Uq)#o*UGKiz6!&Tr@Ob&l$}@zf<pC@Q&za
z#*KL{=;6}YsboIuFip)AwH|kTF*k0Q@XT1NIk#hcfz9QzMr+1B8gQDY;+l-jQOsuw
zMaNPh8*aRo$xkx5$^58eFvzgRki}$9g_%*K!JE~Yd{co@Htq_}Xw$ldl*SdxEv@P;
zg_u8*4a5wK757NM6)Bh_iA82*&^%aXmeEXdv(Ah?;hJH+*#WmNHR&~&lauMCu%C5s
zMP0`4%GkB8h1ImrI}uw<<h0QNqszkv3Wjl~WoCf$YfM$&l&uh$S<LH;vErz8f;Eqh
z@sVsOYELjO%b3AdoN+KS<Ghn6Mq_d0os+p>)nBmL>Sc;6RdmeEA#;8`>^B1Ty)g=2
zB$FfhF5fC!G;-^)_a1xivG<nyQ+n)ueXGjBXYaSKubSTU(R|HL3&ga+jZA|Pn)Y20
zYAevRv`(+21cS7r)QQ<Bnne`%wl>>sdjf0rJmyY!5R?udQE-<AE1dG5di}1oO&*5q
zWLl5V82@%OLesgSam?F^J8NjQmsT!l&6iFE(#miaKAwjvph~z|IOBB+yK-6V$aLV8
zmk$37PK8+oyB0PEKReQ{hA(<%fsOKF0c=?R)gUDOY?yyPjn&{d%6&6k+V4brDTpeW
zn_JW$ouj@2eh#VLv;C<7+t6HyPn7w&$ef*vRSSt?g`XSAR9w~QT!4#mxlBd4Ium?_
z)v((Bc!tmQX9|&2Vs7h5uXGmSExVK89nCoGyFxx7lN0-+tw{41ww-jSTKnuq96kBE
zCtvSz`MTBq+cUZUcbEoNm{#=V`)^z9|J}6HT8y>zKZ!^r){#+4U)ojHVE=E&^og!S
zzgzo%`w{1M_W!m&O{M$zo8JB4V^#IXm<#6%b3W@Uj@=h{<y&uj+K@GW|F<u_G(ou`
z%5p<)bpD^_Oh_b_%=(<k`Z(Tuzj_M2Up1UQTW<=TFRAThnBTtKET3ufb~g!Kj;<$v
z^yH5=@(1aDq*GAx)3{1zE0mpyz3<2m-~Ey7!<P+T_sEZ(bFcZ(hp)Nl@Y$bQeBe8e
z>Tk3>WTovnTgHwK-hJK0jMX6=Mt0Y>fp0#rb><Fpt-p>3zjEJOEYIFJe=EJ+o{|<v
z>()$#Tj|;9HO+C&nlqB;6Qqye7)=8B%3Kl~29C9v3CI*P!dw8`W-g#zIy%w6)<pkX
z-!(}4b+wwrrLFlce9+k<GM&(GqW`V$S%{~T^IzYCe>yKkrVIMt`p!W-ot%IDKKyr6
z{~MjnlB6$K+nS%k$B~+^`tz^*Q2$4tq<e_Upl#;iVHlwPMy5Xh+tB~f@%g`7L04-3
zjp%o%|3jcp=b)74U#|ay+9AzfpRGDam}!MP<H+-wGRM!wxMHQu<?}Q#Nwbhi%s$Rf
zIxORj<{aDc8G+@UUeoD}634q6R6F5#4dy<<|10yQNI6pA=YpkjrYPN=$nsoqO<|hY
z<18yOVcH0{!z6Xc{~5jRxYoS8LqM%+46OFJNleL0c3ut*#A}7sVPMw>MaU)X^gj?E
zqxO3_u$88?h=YeRLuVkR+dKPqYv<!;-74RCo95xK9DT=^k6rzlBP&<m@xa~p4!!PG
z!8a_QJnx}N&Ev~Ir1Vn%mK!?r`Gy^)zn=ey^C7farC9$*$NZ1ROKIO~7xaH@Di<oJ
zc1B+Rt>%9e>i_7NKIL7S|7k^Eq5hAK>C>tfjTdCPq5qTmAIVeHHYv__=23XSup^jB
zynXCY|EHiYlEzb|^yT%hWd2Q8OKYPGoxhj&$wq$a-R_6O4{DA|=<DkvkljoAKReOC
z^#H<C9gtB%pB23}{NGOge}<SoT#HQ!eWoF0(Gm1_TmNsz_5VvGozCZX%XHfQ<aCJ-
z(R}!J%}&D)gD}x2o_PJm#@p7k=DQxa;#TXYXD|54Ghgn1&Rb!Z>jiMT)_yM63qX8~
z!f)iEcm5y6vCdz~E*XjEO80ba<R*Wv)P5;EJ_PKyb7Wi3?d-=hi}ktao&Vq4S|)nu
z|94;WNBue9-ueH%qm`w;ZhGhcdDULioKs&%z4QOHZnoEJsdxT=uWO|0o&V>cfp_m~
z4%=K$^%A(!_om+Yf2AeW*x$x`f8*!vTxsr-P@6$Gfaf<_D<PooKnVH|n7EcnvRdde
zh%+n{4;YN<Xi|s6^GUP85KkD4akKes)!Vm!W}un=gK>_uBFq(p%RHawaP~o5$(bKX
zROUmz`Cx@BL}KZAuehwYb_yC13|6hhsJ1Z{ty-tinAH~6WKug=r_*G1m<>*g=JdOE
zHL3GlKHgDs_r@vG-BUt<jx5tGv5(r>`3FPIgT{Y_ZJdcW(txwahK0e|UE;Ny`5
zYm6A88mJ)AL6r<~CbJ=)RO=Iwq}HrUB(w%IbY?IH9lItBJ+eP>&$P^8-D=-Qed3xX
zwIguEK4a%Kv$KF*t`YivffnegY=O>%p30V<%9ftWmVK+TrKi5<sja@Jr?O}F)cDB{
z5;aoK)5xuNq^EbJr_GU`9!q~}vvk?lrFtrR)?c5SSf(GjP<WDq#%0nxlM~kP!h*&a
z2HIihdn2vPQD~l_?~SwzPhS%wo#^%7ccgu9_wJhZ-Jq)1SWp3yey45J|K2z~a0^<X
z_>H!c*bRH(F1Dxk`UEzowI~+gll76-vtE6-<I(e04&QsngEu^u{rVjX?|<Qo?H{|%
z^!ZC(_zh<D%6q7*%eT|E-BK=ck50d#yYUUyQ>S<KZCZ(cwfiFOdK0G{ZZbR5|GYo+
zcQx1l{8aD1R~{Dimz}Np()QQOx}XH1gIW5HmeszxUvjm+_5k&5iq<!~qc=mT{L<{3
zsC4e<!Cf0~c5>`rJae#{vRC`DQrwl$>igj_XiB87oivj#n|*Oz(($lA^bN*_=BhOX
ziScy>iK$E_4F!qinu0{S(h=o)1&Pv$T!r6O*t`yXFqz^3S+YL3yuIz}_R^C(<gO1+
zyG6IlE4Y#XY(JR&yXEpb|Kv6uL0a1lnq6)(>x=$7P=)#=4rA-8P`g0W)`d8F>lVFr
zi{83L?^6T4FUhp{l1z7A>7T?`I{6BAmvvWmIlA8X(;I(!<4<q=>5V_)_|xuFH_{t&
z?|QIHpkMe}b_6Fb>5JJxtRvI+!4p^spM!2x4!agI_%GtFAiJB#UWUAIPkS2Vq2{r}
zA%Rfyst~^4GcSBuhZFWz#8heAd$Fe?i_jG3=9S>RvTA*1ay^e6=kcwfy`Fp0{6}YR
zNe5|4wSx4P@Lyk3)F;I6e@g4m+x`Bh()Fii-~Xg(ncn)-nbO}au0Q=`*VgysoaIdM
zqpdwtr6O&5-#^f~-rz%`?nXPll{N%kzOxlo(!zKqS1UF6?pw3(=g|li!Dv;i@hnld
z+5Kmr7Kug%W0>?sKNea);(|*Kwv3RDM&oH?-l~G;Wfi#Wle-a0+VobhzrOc*=h3ud
TB^!55dBa}M;^;yDKF$9FBbdg0
diff --git a/src/azer_robocup_project/.vs/RoboFest2023 disk image/v17/.wsuo b/src/azer_robocup_project/.vs/RoboFest2023 disk image/v17/.wsuo
deleted file mode 100755
index 18abb0d6b7187e5130af57f579342f13ca7e91ef..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 16896
zcmeHO%aa>N8E<9dkQa%a5MzP?C9y*Qj~G3yU2UArYW1)Utk>R+?Ep&5NE)rjdZN)f
zF<w&?2TtTbQXDu@zCckNk{pslDx@fiLkg<caH3LN!hb+j&UpF#dZt&i<CS(+!fUJ0
zr}}2Pd%FAU*Z1h|Pam!A`{erDkN->fU`9-c8|(LpdpzkOgzr-Ey+YiLFw<_Vudg$C
z0*K%kMrZ^UMH9b{2#cI(;N2EiIF*56zu2=e=X<s(_0ekK_{8eRp$Cv}_MnsbG$=O3
zC2<neD+uMptJ~DF`SX6PNbhiO{<?Lng=8#W35q`HwnPmrXbTg#gt0Jyrkk(V>yWrB
zv{U+NRQ8Em;euEJm71{7f8w%-auzA=UWDCr(kCC1FWHW5vdhoQcz)<+`~u$p@pb@V
zPK`<;Z!35Z>7NBY1bi6CG291y1o$ZMbHK-d`+=Vaeg*hNAlC)!c^vOA16khzyu(0m
z{ff@`y?Z0DoBZFJc)82}U&7=A6#wle|93!l%>Q!uH~5@-o^pWvNd8wm%aur8W%{7}
z&pHU7K|S{Yo!I*N{hqK&8^3pV1URp~@LqS`IZxEtJDq>78|oLzed+<OE3!Rx0Chtc
z_yq7t;8Vauz{9|&fnNna1N<8B>%eaSj{r4b1o%y$4vYe0z&J1gJPJ$#CxKJI6mS~o
z?c)sM&wJA55Y7Ozp0wi#pK#;f!u#96?*Qk4r+^E<)4(&pMc@+f1>jlWIpBHVi@@&!
zzX$w2umF4s_%iSUkZm&XE&@w#+}qBJNV9;KeBT;~H{G-!;!PXDcH=C+6Ud^b5^Rt<
zEFIc^=cN@=M(Qi*Ula9ONVO1ZNE#rs3G@N%ual_Z9SRu05^AnV8_1!Nk(4T+xQx`c
zw85M&PJaLM2<#uqa@rp3xBvTA@jMcL<9=Ac=wHIDE`ydsn>HUu!p1yu1@(s~L7#j<
z{s>AxhrG1c2pRO=04-9pK>3xMTH)mD^@RPmAy0$S=NeJ<&0)NoqKf%9FalMN^{I07
ze;-8uL&!oI5tM!fdCA*^7i90N;4KSd&;k96Xipm<u1d~Af1v%#eW%hZ{c}5CO}y@P
zGo~@u2Ik-jD6s`gpxeYwz=Wj9VlHx!EOC*Bj7i}+3E8rFO(a480P3PW9JK#s<Yk))
z+;?!rwcU2)(TW=4_J(C-qD7sat73Y9`Tqgv{*A-|>~G1Xbs4nFvMmv$Z|2JgW{B&w
zBj(Un1FbD#ri7RsfPM|-?jdo&Jf!DYXaYaIu;`A2lXL6m(*w}2qpIs}b^Z2V=b&AZ
zm~-lwUhvES^joOx-@PgvAf4+@!A^^N^^HGF9$^}Jd#wNOO#RRK83wZ8_aY?iGJcdo
z<hll=0Qr&nw}jMw0R9U-gbX|vU_1KdRo^J9i&UC`-t@0PCl#d>a&m6{Bz@AQT^W=<
z$4u4cr$3MW)<Ko$U|w6OmmZ@1q0Tgd(&s*iwj#k#pVHNYe62zUHRW7T2YOrLlvB^d
z0P=^jSLwg^LK)b{1;}7q&cBVY0rcC2Oh1nHbYK;)U|tH4+f6w?Rhin2=TQ&!L=5(M
z47xLn)kMu&MaYtN23J}^p7^o83+O3r4+GywWl{FO<_T@i5rMsCgLXr#y|?(*Pu{%0
z`R0kACoe3V`rU)CzyI3b_e6dlpZG_$z4v2^EV(8)FACJSsrSP-Ui)<7S9AH_md;%N
z=VO21cIE#H$cCXIdOU1-?g>esc<iq~{q%ufuD$!~wdc7d3k0<LRe7gCYc^0pH`B1~
z-wCa)@(wfhsy?-sP;tJ8N%PhdG|WBNyFf>pIH!8tQ<1B^wp;-gp9QVLQ-5-C1ri`E
z_dYh{3N3Nkf*ph@HJXqDjvR?0#Qj+vzZLL4CC3${okMB?(v-WHnv@^&DDR|3aUu|b
z1ah8HSn#yv3n)wLmAlTelte;&FrqCliOU!Vo-6!mM0-G)&$(#JUX1EI*mD2Kc^DNQ
zAO~{g*m##lYV7?je-(SQ#7Osm)=ypb;Ljt%#r`?qnF(d8v!;aj<B0adf_|97dLk!(
zIHG-U?vksRkrp_d7WL@pH?@M4RN_8@JAu)ijmzK)az`8eu;ji;h`)^J%3@#I()!|!
z0B;O9GovE2=*>|>t1IY5SMRC#sQ1I^-RRE8%{DofRsaYMwR?4Z*aDF0nT7ke9|3RI
zDN4_V1^mGF4+A{+<K9o!^J6!_NK%~-1v?Ll;O5JQ#u}n1uU=XFgpQLZ?@!x^^$&T=
zFuWVE4@Bl6Q)`e;mfZQ$8sz@BwQ&O^#B;bW;*HU>@{aqeypPJ`?r0M69PTGH+{!Q<
zM<|M47OmHii{H`+u{UX8276!~XT1Dnkoy|;$7PH>cidgN3(|6MsB#RR<)a7i9+zh@
zhbkpKX%-DbD;8rFEn!Y3wNxrTsilsl%u*s*i5iJSNPG~=PV4wxO~>-N$wV%y%}pla
zS~Qx~wRA3;)^t6sXHt4TpU=dvtsa}V>{8yUnfA<TX1Z|JHrsZ=C}f+ZmAcvJ*oEU(
z=fp~}u-Gg%t5!$8^QPU2>9KgYY}wVYRX3K+!pVl!xzIA&M%^wfnnq1N+=R#_yV+P>
zy_(X~$+>uXQp-fsaV??iDJ>nFn$mKye10mGOC|GZ{n{IYx8H8oRytO*(Kc&F$GU7z
z|IZqQMKAAQIYrj&c!+Pw5NF5ZdJD#NFS*gnh@qEmDW5;FEz{lh1{^WW*rbsx>Y5o(
z9Muw(L8)Z9q8&AjN<5V?&3G~u(e-FD5nJ_3<pUzL)yXfi@t|8d9e7IJF1v;Pp2OY#
z9Fsd-`s&Q!_K|ngg8&VNVc^BJK`A>_4?%Np_Q0_P&%>#)qtMQL_5;rL`$Jft{`*Jg
zrjh$@#}hscTc8CCh37k*FQ$3{{x8y(gMQz<WAlFN6^6+m|2x9(OY*jlXOI^q6;kF6
zAn%>&g>?p&5bvew8)a{J33Y3p!u{)hM0s%+l)m@=*L$-%4;zTyK+a8g1ojtqB@Iw6
z!(MXkGS~_)(Emr>Ee553H!2`I68h0UCwtzJK2pVvzOzIbBXd!<1pYUngQ!QfK~}oW
zgJOU-8^OQ&97}Nf|2;UR>C-v({~G&$snr*Z|7%!0cZ&a)TF1k@yS)$A|6yotRdyKq
z{p8Pi$QVjt?g#m%9P~pB{7+w!M}cgI|NB;P>MY7ELO=Q$$bLr?)!}=>8)NQ@PeDTS
z?9q|KGtfWuFX#V_g3_nX4<diP^wkYBcj+yU4uJn<i2gkEZx2czSvTnK#Q(C1o|mEP
zIS$l~1ooeF70ASG9#wo-{y#VCf1V^#8><oSUs-qn`m?A~=^sD+t^J>Rw}JkbJVSW|
z$T8@hv-{;a1b^m6{q*UNOWT8YC%wJiyLBo5vz?6pF%<7L{>mSiK6vD}?b<Vl{lW7b
zMgMmDaJeUk-2ojiG^qbp-LN}mdvO1+QTHG7um7?C=g$2<74HX?KX;P<=a~P8<NxcB
zSN!h><=8LqJj^rd)9q#jht`L0#X<E|-31(#>$>luzFk|@kLas;qh|N_-WKA&)|cWC
diff --git a/src/azer_robocup_project/.vs/RoboFinist2022 disk image/v16/.suo b/src/azer_robocup_project/.vs/RoboFinist2022 disk image/v16/.suo
deleted file mode 100755
index cae79d23543181fbad08fb27aa5f199fd2f477e2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 107008
zcmeHw349yXnfN#tN4QEL5Fp^BK-qO<Yb05R2?5=*<wKS%`G|HE9V1)TF|tPT5t0^4
z51<fO?q8R3wjAMXVJWmgLkgt_g&zBFp?CSymW8%J3#AJ!?Dx%KT_brU%dwNpvp>!C
z=3VpNcfa?3@WMxabL*E+`Yllswh+sS4?aAMSXFR;S;0AR3_+}bYrOZthaY~3kD36^
z1b|C9>?bYYCTRG}63v8_NWn2f%;2Y#9M%#mXTLZ0hic)L@Sfv#{rgKz9T-W+#dJZA
zG%-$eLheyGvl7$Qk|=w&7J6V|Q<p^$xsic;(0$n;<d6BLi3F4&Lr?%7Xp0~K#mB33
zIK7Vi*Fj>h@d@@3ImjhJ#Gw8NqXjuz;0~6xT7dkU4u=Rh4zV4o#TA}C0w=Gm;jUJ~
z;hzN!cobY84ItdYXqx~G07n2|UP=HQ^K0RVbvYKG72q&{j{@MkC&2MU0Bq;g0RI9Y
zq$wOvf_rWJ{g1)%G=Pur*I1^r;Jg{&NB{)@zK8jq4(G=K{42oo{C#{@DI5xM&IUNQ
zB<^`|Jpy0?FauZstN=CuJAebA6QB#g3D6DD1JDc52jBu204U7Y3)en?L4YBEVE`=i
z{c!XH{2Rc(0|WrJ0R#a;0AYX#0QR>i0Dkwvbmux}Abu7g2QUdR1@LizO7DFFo?QU2
zFUS9b>z<UyKeYK$8vjE7I0VWih6f7AKgN|Z{&CzN4}kG;{GS4V<Npi*#37hZGaQBS
zkK-BR3giD&{u=LL!10gA4RFM<gaOBY3xGWSx%Q}c{9}6xu!3)2;aWK2Hy6%@Bhm=`
zwurA0|6@PFZ-U<)=>p<^AufLBlL4?F2>0+?H}lsU;fQnt`vLY5#Op{85dUMp!FTXm
zBOO9I&<cQbLkU0vr~uRe8h~~HEr1R{4}f&50|4>=W&k07JR13H0sj|1E5-ka?<W8R
z{Eu-F|Mvs90XzU>0Eo{E@ju2x8nYDu@jqS{;(xpr@IRgl_#e*^V(`v5K%9R!3C9!w
z4Up#V32`Ah8?J);i2tVnX81Vg!*K_|PX1cJ|0YO@hSZZ#@(diM@;#6Y8%_cKqY2Ki
z{V{XyZuQ?S@EE@>h5^Wzfqc7x*I|Hyd7u@M2g?FQM9zreqE-Aw{%1Kn6>vnwhy@SM
zf%8N8YvlWo%gaEk27wQ=K{*m|%>Z@hI8zErk_G;Nh8P$VfAgj4fc#fOTG+>>)_()U
zeR?HV01KpxHBIs<53*cf1ny2j324YE2KmL<-1E6|@8Yi#^2dA;=Ss~V`{Ji@@IVmy
z4j$$4pT~vr_~-h^T)*=8-@nIyHTlyA)Kv?Vlw>KZcu)9O9{(U6mWqFFgSda^larP|
zQgrcv`6K;D+AlT#FGJjK@ySWcAGr$g0Qs}|V%`|UV-?^1FSz^-f6E6LegLpL4OAxt
zztcbgks3_`&JV-cTwx>B|1gMz<V0%z=RsV=^%$hoU&V4+T>R2s;rp|dmstPjAdw&O
z2}#QzxeM`t`6Hi){GHVLzXWkV$0sK(|Hgj*0m%F9n)1iqEgppSM;=#d{UZ?fOMG%e
zz<DHq(+EOui^K0I^tnFZEUWPxg(X4$OZfIu^FJ37z&?OMO8evYtrX6xWBd(6Vt?Zk
zl#)Nn5|skv&(<I16sh$`9nc^71f}FpRv&+CzQQ-CRL;WtpMr}&^EW*3-NQVui#R93
zVxLyPLWq%&1Lt_pR^ZT)Q_q67ECXjLcnjs7^Xh1SE4;Ug&%XzxCoj=lmS1xLC(cI|
zJ)Bub{;1#EhC%~`t)TIXKpnGC-wbHynrG{Y8gcFk-a{@O^}$?gaPQ);zK;Bz@E*#D
z7^Hmv%A+6UsAWy-A)V6!r1rmVh`R>>gOvO$V?Q?kn){y@^8E>$yR`f(Q-7}hs5?NN
zfz<jZAnr00S|Rk$QeULQIrzTm5^UHCdSU$XLC^tHW!4c5q=dSNTE^d*7#}l-P?&!)
z7Os{n_b&b-{zrLs4S>}8<9n!!!7#Jqd?g<gcr5u5p6%SB)@V1W3_EPW1VaI*;%7C-
zA>zp<Gw4_RFoqcxb1-lo1Spkj@xGlMXAWrP{QIbTz|kk9N3-J|Z6SzjR3P7~;IEk-
ziKr)Nh9{`2Ksx1Tu?FHV<e2=Rn?Xw!H$wdo7bSL-qyrFxlipFHPrx6>Z-p;CLxf-d
z#GkHsXxqT8fBO01$K7_`?|*k=^4!SQzjrwLe{$ryYu<Ss+erAf!Vz^U#{oPFT7m19
zQ2o<T_XuCF6JD^~ck#MwZ;!un+VrLmu6XH}p+mI0zpYs7yz}AU_g?xNs$kf9T?j{%
z{V#&!l>nax_zb{j0r37MaJ&=%`6ax*9FDsHuHdi#gFjvk#~T2y1-K62dH{@%P%YdF
z@$LY)9pH-q$d}+dck;)_;J6#$9)NrKd-ucf0e}bj>#xA^A%KVZ>qp@DRe(qN>#xD_
z>j01Q*QMpz3-`YX@HGGYTX1|9;M@FlVf^pIz2^a55P4P@ukie55clUb#4mhzq0dX{
zJg4W1LQfinaf*~)7*S9Gk+2Ey58{$T0I;kgQOa(iejmR%hCaXr<1lhZA+J&$n~&At
za`7ttqWp6NJQUigQoO?ZNH3T1H_U(ykgs8YBF6wPOqG?Q@Xhk`dhEa0zlD@IJXu}q
z=Z1160dpY@3bEKrc$?!t*23lS08-oE2yq`kf(fB8=N?EOwV;Sw5kC$Bhll!3ln3xg
z0d^@3syg_e<&cPwQnmce&;yYQ&`^6jl!%7wG+1AQaE%&T3|rv)x5DuR0IBuIZ;yQj
zgVg*5868JMn$H{GNBuA6dm5Ze&A$oa3i<nhhFPFCA*kCZVT5)GLi(IW+5t7@bmA1;
z!JN@QfU7s=jX?|Te>5Cd0Z7e%6ygf~KgWJofg1kq<7=p%*B%UccTBaN@<?i%<&JI4
z=QiH?vkTm`+y0Z?C^)mZU`|3P<Qekpf{Ijc((vrUXW^|gybP6bW$0)uL7m}`(BWK?
zN@W@Uw-%Ltb`p;+YW|3QEAcOB=m}XEfdS|bh@(>Q$Mpb?b9wQov6(;K^G0yf%(ja{
zG4=NEAFjRO?#J5$Uw-PSJ<1g=NU+Pk6-o6*LTal<?=xF+lWB*I+_Qx0FGyMF4a2at
z7kIhwES3(;%3=wg)46@<54!9Z^v$t#w{Lsn`sA5w4*lG_?vI;(x8r!5G4-iSkU1!8
z2g6`Q88cf`iP%!&U!~RG4sAgLzTkSnZ|=EF^EKN8sYmwy`HlB`zH-)Cciv4A(f4-U
z-O}bg?K?j*iPawOfpbn1-~fjd`HvKwwb&Pne?a~yYTe?e;#ar8?YH?y((<pY0Ydo)
zO<5=-B1Ynqt$NrFPha6<NXy?)F+D;5CSb#g5eptn!a4Gg7^LKnQc$IU`J;Tk89-|N
zKMryC@(F(U;Y@2<vJj{;=w`8c3Q{=3o602;tS|vroCTR7nt_*6!XF8J6n`;9qJAfx
ziqHY2QrDWxM54UhDNYqeW7)A>$e*D@bUc=2kI`5v#$;7Wm8v-sW8%%RWN?)7d+2bO
z%J_TeY>ZC%!-*in1h@-{wL<#Jak&)G)){_OZH5uX@jP3=vV1sO1BSW@j>UA$LJz&b
zJ+;TO4k&RtmVy$E#b|$$j!+54?}jR515URmpi!z-4qw0(OJRwz3Gm_Rpie8dz>BwE
zCAnOw+_^(X>NlxLPBg!+tc-bDE7q!c-h<=%(Eg25Gg2nx|L3wn)T4h}4P`|68AIj#
zXMoE?i%5ctSm77)Z#o8^Z2*wk|BLx=f%oF--vhiD^4jPJTgV$0b6xqufc#T4{zH4s
z0G~u@{ZYG0!AOcj|D)M&f!+p%CCTTFXQ+QdUxr%n55`BpP@4ZBOLtMXD{$Ccyh^{#
zb=3bvNUYNL7q5S%+&TT<QScaLN~zx;<GXkUxU~N04}yJOss#l5|5=dUM{39)eN`&8
zzu=F8V-V%7xyl3lf76+e9=4~{`lIYoDSvtVAME2F@joYZBPKG!NJz7qRg^wBD+zR;
z*?|2|z&%p?AL@!$@F}3R8~FyrIH<qHoKfF_V-P*9`hg0eHUf1s4DkM(UU)Wpw7;<V
zZ-iS?^Do5zg_yNe|KVcsFQ@mgK^Y454nhg)sXxl<g8W}Ce;-@(EYJ$n`v`T%HzLq7
zW6&N1aURsa8IHm?fYUj{N{Aug-vLMsEgsm$=xc`Uk1dbwkGOFX_%|i%X)B(r!2cc#
zkLziFDg3WmJZtuDJ@((b;m!B>G^OOvaj@cr5bVF};IY*D-v@F3#wS;c{gsA(5MlL`
zsI|$mDS-ZK_S;L%-vVz6_&-HlGs5`2@5~rSwmx*<Z-0Hn7f%me_|!SQox7j@{lA<r
zWL<kQW+U`2;Yfxfil9=bQ&3knEvEfv^NKAiG%L(2&R)^I0;LVK=ycsYeeHLxU77V4
ze(viRKK8}0KJe<%?|$ZlH79@O6A$mqefh;_#OhNfP;{nZ*+4p&2__l8CzeDAB!a0(
zGMI@ot>X-x;^mj!i(3C;{Sj)gaSWra%Mat3Vl@TWXSkNR*LnEsGt-w&S+{=wt_vR9
zuD;v*|4<YJS+M?Z7{vruZ7vi8qvj4jYWo;}&~KSl^wldMiAhha09l?f+7OYNBv_pp
zQWeD54Ag!cez8?b<vKGa!Uo&3hj*O)+K57$?Gi0*DM&F)B!f(r%Dnz1;eBj#v~}XV
z8kAnLP!=a#dElr4OsWQJrUT^T0N`9q<KI9Tj{4h`puw$Gc9S3x$61*>OsBFLI+36<
zWm0i1_~$_K|G;*~aIpJ-d=lCKvHBhSQNZwveY_LObFWC5kaxp?wqHE{mq;8WS!nyk
z=V(7h`|lI{HQsv?&c6Y$2jFP{ye9-C4?-ToJ+Wgg_Fu90m&!d(f0m9#$}mG^R}fOU
zNy%xOc0g+tYs9K+n?x~IG8jwMk~=t}2->cz%knQHIH-U_9U>lW<@wK7|9{I8qW|K$
zOwH4`-v7v-bBb@0hX;~=m+A5|ZgKCr^J7PJm=d?We9Uuom!G47PuKmvKq2Sj|I-N+
z6s1zMk>%-qo)5^M1%NK24J`s3fuN_(^9A+rz|i#|EUx{mQvE&9DoNHq3Xj$5ehh`R
zD8)v_8gUofb~C_Rp`93K>1V)C0$VY%$n`Jge~bOU3T=jkPXbVGj!WdY;_I%s`adrH
zndX^q{^R}*?Ylew^6;nM`p4yG-TCL6{<0>w{faw~K$f*n-e2vJMeYCf)Ss*M&goBG
zb(Cvp^1~13=Yf|AW~tGcKpKRjR4g?LczkNvkwk%kV-EiPjptsuXZ87c{<$1Omsxq{
z<Y_BYE<LH))UM%Z*H!Rc&+v+uAAB6S$8(yeE}Q4~zhR#5&+qGnuyf1uAGCoWzwK-J
z5B<;=Q~sMT|1-b-=OOkkYX6r?KL<eDid2JvBU&UQMOuYtFWY)^%kq0pz5k7Ocb)Ov
zrW5!6|Bs%qiy|~XO6}FHI^|Y$g)O5X!i?X_{y&xJ7wUMDyiVuF$FDzn;1|z7xa@k`
zeC+z0H|y5?QTzEzPG0re2fj--zV(8r#};ZrIFGrTK)a5fWq2n5N33C%Px*cG?&%XQ
z`ubf{J72t*)?Ix2N1A`(_~+Hj-`Ey;@ZInK>MomDtyoo=*X{1J8wXgO8K*)kRG5|c
znBTvs-ygN;#orZMLGXzSKx-h+Gs*sPbn^?Lr!L;S%yY~ySKdp1{mlP4GurI#yV*_;
zBwxPz>Sz9Q%MZm`1)Sb0`Yz0ue&okrc^_?NXEdPw0=?tQp9kQcG+KEjRBPUHUa_x6
zI+z_RWE-T;LztC4FJQ~$gI~m-*d43s9R)Gan2Ka*2AC4?BvJdB?4^s^|4<e|Eg;vo
zI@PP6*)aUjW1*vs?;g4GJEz3f-+k<I=koQg8}4n|_12frY+Z&&=44Rj@Siyl^*<Y-
zon-w_sZ1w+Esy{DZ6BP5e?Iz)#UKB&{%?NYe}C8iy*zTvuAW!_a_eF5?8a$UW%Ok(
z|38j@9Rn&x1CN28bLh8Jn+*)}Kn?}{4I`jml5RcY-fW$HGjrLt-~Rfda|~zP`sjZ;
z&%N?HPEE^Oi48wE{@3BR``*6wZJdZuR>SHu48<(W+{#ybSgUDe&Q8=THISi#i5isM
zDwuSs=JXZSW>JWFc2+AHljZatt!dD%bQajU-B3I~{_w`{DxAB($mi#L%(7<XMXgmW
zpI7?iiz?YmS*_G>Qt7x>o6G)Ozy5n6_1Ag*tru*^ZtxPfgD2Jij12BcvA#R6$38C=
zI7jhvUw`zeACj-$y0P=3%Qw9D+>d_z&3hFuKjK^YSATi`Uy$$rDafZ-T-c}If3_b=
z@IN@%zrPg!Clx!p0{c#RK>shC9q};$<S@%)l;61vZVB`6rRA?HPlUe%`Lp?MforMx
z=OFH0{(WirSD$}^`JP)t{!c;Nw`<BDTUa~@^~W+`5RX-S7j4|n@wa6CznJ~v;L-m}
zx1W~lj|x4jj5iY6C+|OY^^dRp>Cf#qs81W|zUKWW-t5`Eeb@P)d~esith*uCL-W6W
zSY0`<zcRAcS)ApIzFKJ0<yL=C1Jx_FY*(5SdE465M%O2gee1jLcK!bJZ;V{H`-^X$
zcI>*2>p~9=|M%7J|BCf?%jb`0ja$FL2DNbml@`jaVySiQ`dv;Jv~s)SoR(GZWwN#t
ze}0Mo_Q1BAU01&9Z0r2&)U|iHKldI^cPZm!(Gp(k>Kf~GKmYy<hpx69`ZdQ7-(7XL
z{dnI);qjrv{zokR<qyPX@r&TUo&SCUKg7)-;5>=vTEC-`{$23nE41GiTKMxE&kJ93
zKX}0dhtm%@F4G43dhb5ss>d(?;mX!~)XRSw&m8j-PBkiPr@|FQ($d90Cv;10K@mT=
z^a<wyaOzXS1j$12rTl4LMA?1CmH++B-%nolzlZMrRsRit{p3eK*zkqyNu~$>I<oc$
z=U)D}sE7iYp)a9W%bvfJ>o29AjaYAnn{t<*c*o5;E1HK_I2!6%xuw*8%UKY(@2-TI
zfVhIvT<f+}>IHc&crGsnv}Y^>_<YTttT!&I{uk$Xp^YKHO3i538HFy!`B|VZ^Tu6!
zez8><TK3|=+P<D$k3D>I+a2qk_^#sVt?#{P+H%XYui{rO{DOFn@>t1iLH1HaPJ_3l
z@IUhUk2u3tJk-N~q0i!P_~aU~{v+bzl>qql`DCje&V#1{{zeay!gb!{^<PR?p{d%G
zu<lyz^DheLA2ne8m!`E)#Nz>^et#px71n<#o&U(qM?!uJy}b5|`CmmCu=>~Ho))%$
zwfR>P=YQ3l|0*a(1LuF$oIm0UVg6UO`tK*}|I}Q6<P@Z|f8qK=Ro4HhIsc}D{2Q?T
zPZQd|u}?`I|DzBW+W<p^iUxCuY#^IVTS$wcHA^vBhRSBam#@;4ckV_07uyl*Q*S7I
z|GIbfo^t7L*MEKDhi9AKy6OHy9lw3*1U#>n_AT&GXwTzD4&5re-*nl1V}Dux+21|#
zle0~$$0k3!c2m=jZiDlVKm64H(wplizv0_rB2Mz0lVkehcb~Gwbj{2YcL@3Y;H__T
zy>R_kja%)TO-%2r*RPLnc>Sioo^7&s-M;&zul%WnFmE<}_s7a-9qFBsH-C1vY5eMr
ze{MVTKCSP~vrS*T;r?$vdBMsrz4J!(atn3Bafs#5w_*A`;hB_UVfz)v7ryUY_Zl$&
zr)eEzQp@^Vc8HJt6+;8oe{Dj?h*JTi_J0S&eTQ$LM$G>~Arte)0H<>XC&WO%6%11N
zZw(h!^jBX0by?F#p=EG6JE`rTfVhJGxp4i}gK_;=dH#3h{rB?wABYY;_9l)$T+ib)
z0IB`2!Slb_^$(kC$RB;>ScNPcrQjc20iaSq`;P)1wg5=2|HTmZZ+wDM@~>?D;MRXR
zgU?%P{+B`gTI??*Xre0xtp6r>h;t65=Ko2E`wu=rDfK7W-Ie$kN)DJm=8G0Gsrg?6
zai8PU+28rU2-E|4r$Yb=`Nu+jt`aX;$lF!o=dhWo@qc3cTP^(G5s-eK_ru(3<NxrD
z;`i}Rp1<d=xL+}zQ0(1W-mfLDSpH)7YDtr;S@Ew~y>rLro){Ch!D6G#mK_Y8$l>~M
z87dLX#wMu_-V+&JOPWMl4_88wstXtNXwuCH#jBuW`=eQ^-uDHoOTAu*p8;n4eVBuC
zy2?NWNqtbQAW5B8q0zU86#8hiT|tII?ICTDG^jP2BZyBmSvr*PcfL_&v+6WfQeo0*
z@Ewa%VYHG)&@>yBW`ojZv(@qy&<2ZMQ9I7pv%#!UsI@kOLZfL{D-24bQK2%}j2er|
z+OAV-cV5%@Qi!Y|IP0q=u~b*=lTy{tOjj>hfyAcZNo=Jn7pzcaGYTY@Xr^&pTA0c%
zG3UJvY>U=bKm6Cc{$$1JnJ^Y@Mzz_ZCansCUT1=_s5UALCOxSj^;%M?vFa^GGr9Bg
z4Xi_<Ct}s9L_5w0q@XpajXH&yG^!OErP81<s`Pq=Rb{j34OWBJW>oH6xB*bO(rAE*
zB^P&8cGKZtBE|r5n_UFeo}fcg3Wd99a2y0`5itlHfpA3b7JYi7U?<}C!sJ#M3b6C(
zN7=j0#5wTS%*qM)9s*KEoQ1Jilx?p4K`PC!<Lv1K@lz-}20WHZAVMA42y^^k+~J*@
zfA-4%UT=Hy4;_aHUKDs=2m%)&Tvt05zK7?PIH*c#6u)11CA@(?E5iIlTz$0K&Mnw$
zxSqqN^clFs6NC<dHxct&5cdpdg>?lOSj%CauY36tIM6kt@J}&&RsF2+9X!W?$MUyu
zM`9s%{!F+d9Pxb*)Qf^Ylvwfs*A4A~)=q%MEe3ILj|bfS7I!)jcAzC-Y%PaB+~p2+
zf#-vlkqW#)tl)N{fnO5JU}DeJaE7~uS=h1~*=I(+ghse$hMug1H}TI5Qsqw84#@I<
zdKGYaN^QF$+#ZT5G*PWu5z?!)iin0%sX{7JsgCFmSt9)3;!$jw^OVG(A7o6yJm~Mj
zNi-sbVsA;|3iwh18fG{5MsyWBZ)wPWs|iz^XP|>r=}`<NNo_=>GN=@kS{G4h^6OHD
zg5h9T6V`{rnw|U$h8kZ`vx067IjBj=IarqR2ss*8yDXTw-*lE##$0;LO+iYf3u;43
z_;Q+dg$8+dgEkUXv{S*T+MuB*wbsz8RFYba3Rf8`3f$fmg|~1?byUGx763svU?3gE
z$qT~xYud<bpsFM-CC=Xu6P)_0Is8E@V%QP$nflyQN~e94HbUt2c*m^XQ8+X8<F(gp
z9EO9rH|AFM;tk_aug9(IG-eo0Sj$Fnr~1953F1tTPfhBF`@I;eD{LJb2~Qb~7N!R-
zbrxedGC5(#7kY9YuiNCajJYzt0oTBEuiNU<s&tgj)1Oq@R7q#I*XrvuyV^T@OcUNQ
zleV`@(`L8ZT~l;tw8iY!`KC=K*I3FpZKQl=P28I6GHLr`%3(ciwmZ|bv)`Mx4jRXX
zv|-(3NIgXMCEC5g+;(fYt*g5$sU|1Y>BwYzPTA8IV}^&@x2Ns&n6;D9+fAb*IlX0i
zQkU(I#r4UlkR_xWwx=hw3F|~6JP}m2Wv7yTOp9qW?6CHaTDLn5!R<ZUdl<*4KFUOL
zS)D7brBV*(43h~)U7h_#a?Ip*$Ek$3-x%&5>#@cR;{)B(k<^GvogPW3sKiuX=kTCy
zno@5cFlDBa#;Kqq?F-vx$UeGrRPRz7+QOtYN~@h+6S;|&w8?5Sr&GN#SFGD$w%I4M
z5mJ-!%!CtN7Ng#!QbEdOHr$<{CY_z>7EQZpbYjSq4u(_8i3y9h-=2yM^%`u3E^V8!
zlj<EM2RdhhedDyDt=DSo8yxFZN7TWQA-Au6Q16*gjSs6BlQrR)vi0WNhH2GwyJIjL
z4~8-$8G6(`<g|FLOsK^(o%AF|by>zvc6XYGG%n*%OClYLMs;odWHRaS_Byp~?OD~J
z)0x%e(qpEON7*|xqv{Vblku33bY}-L(U5MyISB<Ar0wxwufw9rC5G(Ny;Dh7Ead5P
zsJ$^m`&4IpW`YWiSv?c_?vySv6`br&j}K8=gCk)g+q|locK494d^e*(x@)j&%sMk3
zaSZnQdYzM94pV5<((g+2jp#JpjBdo4oSM*1b~9#YW~yZ*r%ss16Qp+{XVkisQ+8KO
zLk;Py+lR*@y(aB=doLa9ZZmdyNKgB)b695|W`f$Rr`?qB#{1IQ0k78DNp^;N)9PS<
zi0(CxDXC6xWUOl@G-}I)I}JXSD{AnnOd4mu)7%+jx<l4}TWWB-c63@t^~BpWzLb5Y
zj}CcTEODK|({F3<oanV{hI@1g+T*0h#s{N`ajSlCIHh)WMkeDu+CJy7DHrdw_Drba
z@o}9g*4gE@<##hmrmR!Rai2GyoA$Pkk8Y1x)Z;V_gCw0Eq?rLSK9K0Lws}aE&TG^g
zMtx+LPTM=K(YprSU8;z~GprudOpGcW-uTq^c4IQ!9~^gwwUgOSyW0^=8iLd$In!on
z%aGI1P2wZAn1yl-t9r*<I$fg!y;?(bU~J0Qud>o9O|0J$v#1>t+s9nJec?$et`4>6
z98Q%xsqL{Dhg;}=r9S5xG$-A|lZp0JIMrq9QyIFtnc%oLWT8lhv0H0S4O_|K9>&HX
zqA^+%wt=`O*Pk?-3voZ5a}?oHlpEP_=$_qhZh>>l0w{q*(6&^43(r{{$q@wpt)>l?
z>4EepGBuE?flLhw_O_F$!9h(8j^$|q9?(Uk4PYVUg9exmk1Mh#@M}cON&r<dkP=6x
z3o>1h>4HobX7@Zj$fblope2P5C&3Y2*DXuVdE@9pk^nl}ruD_^C)V~@rU5bykZFKS
z17sS&X^Kvr^|&~3&-36xf$sa3C1T(zSn;gs*aBBo-bqsIUj68ON!uSk(KydMz8(3E
zZY28WEk*n1F$aeIa{;unyqG)>-_$iC@6PIGD<>}@O8o;A^xm?iPPWu7l%-DYUa}!r
zHU!It-~-DLykXW5eBvGroU^V$i{~|Z2tO~Rsq94N3uL{Fte27XGO}KV)61O8>*euq
z6#(jGaEi2`vX?!|8thTl3iDB>O6HyP!Q7K9G&;^Y!EiP^!$o}l$kLhJ)eS4wg<<wg
zf?aVm$j+z1MOyuE7x#W;;4c8{=qDgvh~IIkbk)>-UmJF+6E55!a4bjQe6H!9SQxjd
zie_7VF(wyGc(S=jjBf3R(^z(9fTj~n>p(0GKe<@i6%5CNqtvEmAC+NX3g9`UwpEFL
zo0`qAwpT7gos*(+Fxfq^so9kaC1PPG1!=^o)H$I~!{nwlkmDi5m+PO%DP`lW3i~gM
zZ}cEhKX*s{MGklPbFg<7;}81hv1PTyaz3?K<*aNntaWnx<4dm!HB4lAcCkt?b>alf
zww5%bdCy{1gzri7_k3OKu^>_d_ErtOsn{&AAL7jeWDeB<Gpb7`Tt(+coune2eU!~Y
zP4zkeFiFY0ux<1E?v-)0fZ-NA16iLX>$7BimaNZ`^;uPBr0xGqeDU<Pz(SENe(u{s
zr!NyNnP|yGOD0+}(c*}f=)}3jmbR))f|I&zsOT)a#)w2!O1Iz{%!_=<D>cX~H7vqP
z4WB@}A{$QJ(+XBbEu34H-qNtgl&&e;bZW6lrPk;38mqD&u<QrCP=3HN9gyk3BG7@=
zg4+X64e%hZjVG^-M`ip~47IbXUQ~%AQ(9T6BP(@erH-uBsbYy(><PB3xcRtENAgmY
z=%tLwWlSz(av77$n4H7p1)>shIDb+1DyOneN!BUJIwg4(=mTgKXwmiE_V4)%rHjFB
zZHUK>h|v<f;1@D&nU$AWd6|`$S$Ub2=U92sW$5<lgrX|jBa0~4<y{WsJ#XYaZ{$61
z<UMbe%$_%*&YBCb3z~?qQ0p9WQUA+XTW*yMq{NX$3t6;~MGIN9kVOklv=G&TmG5%U
z9EmaU=2#NekMZ+H*n;(^rFzz{E_laf?(lp0rBy6+IG3bSS;lXVWgWSYA6J2i$Fl6v
z7E8sLtV*d;RWC%t6O>7XOe$njA(IN3RGdhdbQ-mSBrQsX(MlR&8)2i;Y*5;4HnV!?
zj?FzWCTxo(s47|_5w^XCrF2RS8}cqU`*=U(CRlDxbhX_5Wo<QKNnPvwOk(^By^Tp_
zKVmTC{jlZzu;u-*577OvMa@eG<1R<?E*J7H7xFF_@-7z#)-D%k2&<wKC+-=6iInSc
z@}#`Wh0*9v^?ON^(P*9?pPJMU%Qk!2W-r_9Wt;tyvDsf(Foi?d<)ZEx9JT4@0!%Df
zgMMyJ;WpF!yQSM5))!?z(vkQETm#d+ZmUbH(os53e^O~vC7s<~>ps@i$+{X@S0n3c
z4p3c9rhq00r<T3-Z}-5^iCh*ogUL{dU^X^Mb+oqn;XfTq(c$n|jP@t#2$f)1OMEuq
zbbA6CrCR0i1zfRI7?$72Wg^i^4_ome_y*XFYO_U6S``Mp&ZN+2)JBEDq$d@mUP~%9
zR=ve&CU>5KHh+;E*hHFKjV@N>Q<S`Zy;4c;5I&u7<eV{1StVR=*z?j#f;b8;3%9jW
zM;1#vp&_gUNhk;tp(8Ye8qP@gv%ro|3c|=fGs2k?{>1lxlD2;o1G_NUVPB>!;UIFb
z`;wn<v%4_S#J=BgDG7T)jS>`;!Ncwt6^8Q+TrJN2QH#C*)5|Vy|EDA7v;WilOmFqO
z9UTB`lJ4L066dj!c|*L!c;^F=ZQG@Ku02sdg>7UFO4pH;TKxURO20r`_3V3YW`Qb_
z;r$}<7!~!4l)9-8^ql=-CovJfp&E4)^FTfgAz1UDAGg@_OQZ(utr`?W_tuX;8}bpj
z6{Do0%ZBaT){wFnI895fz%hiW%`?zJs`MSoj`K-U8&RnYDg~w1MHCt`98m<7I-P<F
z1;fFxCae#KH9Pqi3^l%>R@V9gDftD<Jce!~sqQ@ZhBYGZ1{Sz%3!T2Kl9m19WxsgY
zFJAVG=d^aBn-uKl0%O^Z2L478tx_>|K@*qBl}xT=awU^1nOt$?N^~!U14dsZ?~!e2
z1p}j2qpDzI6fBH*JPhDu0LuZOVUqXA?sHEmo%T^0W-%D$^?K#?dgb+c<@I_Gp!Iq$
zo!tZK&^^21+=7H_|FSFa`&c*|Viw>`8jVa#mXu{lS(cP#Nm-VZIY}7^%xsw5L1P{^
z$%deP5o7$kmfH<1aH$qLeVJ&<L`x=G3r)1-xlL1jox_8+X-d6)z?7Lv8mEGev@dL%
zA^YggQN2rTXbY3pD6MvOP2?t8(k83ToKE$|T(NG0*=C>2Mo3M@GZRj9S&Vv@N)<_s
zklAo|LhiZ?-F1y!9@5i3>>SqFhnb)@>uEP7yz#ztcEGE(c9QaZS1I#d<wneQ+TD&|
z(h#I3$(c4oTZWwO&AH<vwwQ%-46CFxA{AGMT67Ml%AM5q*o?z1biY!ca}Ao4?%~Nq
zdn%mjGWDqpUENG@+#9k`q{G;)HK&HH<ZusTV^FVav?go=aZj#4X*SRDru+%Vw2ChD
zz1aGFF)!W>6-?CNK`Oc`__l^HuT@I7;2Fs5tIWR2Jb=stEODL7J|SJ_7Em-{y^F%>
zul~8Gzwna;D=SH4b%e+3Hu)@Lu1qa5U2k7_a-Z}8(e>!}@7a%~ix)5C&WQ_^wo)B!
zkLsc%8EIDr)xni$i#w+$7S7NN9nH4-VoWZW@MLq57~MKRG1&o{PB5(lu{8YTVrf?}
z9EYs~T0J=$*NL_Svq4N@!)A=@p|aUnYLvkzTjGI$2_nO_Ek-3Gxb84<_-ga~W@G2)
zY_}3=VusiZGrH3-pF2ftA&!L2k(bWw@*XHz7G}WHP&PjiCK50Uoq@jqQTSxv*CGX+
zy3DE`ysWAUR(4RqUo#Pb-pLSgIExXLMrC1Nt=x+Pu$}@1{X0SI`~EyveXwRR7=z*^
zvl1O;>q2xwY+Sh_0GK5RH48%P;CdErzK;2`;ux*OIK;#8zLc<1X>DY6rH`a~BO$d_
z7FCwGsIt#kRncX$=dmkmLv&enOcpNMm8sE%a5XwXnF~31xe%EOEfy6LxET-2_2HUW
zUf^upL?5izWh4ezZV36o{eibBUeAjGo(;LREbwWgARCHve5jcU!<uACR%*<$oSpbe
zWpS1Z%&%N#WBJIT7dQu^db#M+VdXU%{^Bgpoq;0*9Jr5NagBsBD`7P@43%VB6r&OJ
zX0cuOoOCKe``O=sQmNw>^_#uK75GktclRzKIXN%UqcnnFPfTXc$Kt*!s1l$adzMf=
zq)JK*JMM&9d<1Hdsi785R=$Y9ub;@D2iV@DgL^qhEkZ;^pSH43Wuy60wfBmy?%(kB
zzOZ)?#EFZJ1xn@8a=OG(A=I0tpiiS5&tc2FoK3KcDSo?#@ly+?M|_k8O-Xf3U#T8q
z+JXUM4C+8Zx??bc6HqG#FutHQ;8qA-O1N1nPAk^_5X7O-yPjW?U!`8Ihahl4*Qb|w
zowyM*S1-f0Xx$ZRVg#L68Ybi*9S#B&$<tjw@P-tO!Zh6dI57!57uS*1KwN>At%Y>&
zt&pw-`ulviE@@-2m>={3rA2DXY2dMsCg3~>qfRLC(NN+WmQab)P}+RyaCeu+b2zc=
z7;MWtxBZphA5rQ$W%90!@-8&ndl<*4KFUOLS)D7brBd=PG)r$68cvF>2cH&m1929w
zq9hUk%#Jt~rElbF5`bwlvpDvY1;Jvq)S?y+t<d6A$eFE!TGY?cAYM;F+l1k7jHO6^
zprkb5dZbC5ZHb0e($QiZ0J+W$TA2XQRwb;@u7VZReQ@Ps(-77!&&zqjy6V#2<YYij
z1~>`o^x&dWzib&l9Do+cL0e%D3P8)?ak8jCp;zo>i(2Ypiy}d+fD#P>H?$CzM1nX0
zQom_Y>zjstiC-eX_E+SnM)@x!@KMJ@8LwK@GHzb6Wrb#idBxc)x>ukU4Shoh;uwI&
zb5OP5o??zP&rKu#t)%O+vwUi3widXy%dUpCt`WXngvHcot46HBNw{j@>q|ALr~~HY
zZ?uS~;JXVgi)+o?*03@OIIRAbEiJiFPS|>*fo)eVSv7-HB;YHQkV@tIK*4jNOvgc)
z9$mDy>l(Apj7J=Uy}n-OWS7Gf8nyJh5`7~&O*f+(aVDoG<Q<0&&>e>^gpMZ!!7Fz@
zD>FC5AS^azWByy#HO4u(a@iI-ec3iH+r}4WGD-P9NX2qpbx&1-I0P_A{gRMC=?iVE
zoa7S)4l}QBS4#9xtR&mz^}JH72xw3J{Gw50{30RXBC_y@ax}_7Tw4en!x|{x*A}&W
z#e8gWk3pFkCArGn;%X>=<MgaHq7x)!<1FW@g7&LczPWnzkx;sCEo$pl>cwI<r0VuC
zZi8@bKTa?e-xPbg-7%Pr2Sb^W3_a=|a$3ArCe-4YPI?lfx-4TSyF1N88kccs>8Y+{
z*45{Y=cc{w<D=Un7WFty1CdOp2We)2j1MHbtZg20PSzDV`@H-=(0WX)+MLQI5?k;T
zLR8ch#U9H#z|zpT9h)$#;*9I@#D3$sSMFJT{_9_|J$rb^*{_W#(8@Kpb*gb1Jgfx<
zA^%#fV#y$rr84>Q&uyVD#*Lw9RM*x|CX)_tuT$IBo>dJxomovTJ!T4dl)XbUs{SA|
z8ISo$cXl8X4e17)lY?WugS0&!>~&Z)xx|otx_2t+iiJFV4z)LCXrJnA&rDFkF{@`n
z-<{G$rh=2*>G2^-Yj7k?WSdtt)9xNxEPprI-({)#yU60GR9h@R7giQW$40_aMx%x4
z0ro~`F@__P6L#cvdUAEmg|)k;=+0=1*{$<Un@p~;lyTZf`OKQQHP>a*_Q#aNdfIGv
zrfFyY0V9sar?$5nli~j0xI3(!++Ws0$#6uo=-0A9OWik)V*%%h`R|w=t)^zHy{xj<
zJ_5Rh{Y`5xYi-JXKkL)l+}O03bHwMerEc7{=NDU*p=B=)tnKUB_1MEVx81SsiSH_&
z-um8)rY*NT`zj*94@-g)f<oj6Z(-QEL%B)0!xl_16ukZ9qSn7y7Ae*vRnpU-9{_6c
zIBnRCk6(ZEz%QPEaM|^=`PlV0Z`Q5(qxSQcoV@C_4}6zweCvgm#9FAMV8`*&{pbrd
zk?4U@iw*K%)Y|zL#?fKZ@gK9jy7$?O);|-x=UwGp%HYoLopy$I?{yEpdFPXlzJJFR
zVxJYP(|Lc%6JD^~ck#MwZ;!un+VrLmu6XH}p+mI0zpYs7yz}AU_g?xNtE<3)<7fR^
P=a%?OHPr`-*75%UumWdp
diff --git a/src/azer_robocup_project/.vs/Roobofest2020 disk image/v16/.suo b/src/azer_robocup_project/.vs/Roobofest2020 disk image/v16/.suo
deleted file mode 100755
index ee965ab05856b0af9f8abcc4824e9e0425bd86f8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 71680
zcmeHw2Yh42wf8j$2(U|AN`M7S*=2X#)GcixE6I|3lPuXX0wc+iEL+X0ws#j6LIO!>
zA1M$XDU=YxOCEs-B=883Mu1Sl_mV6GNJu_f9&J-d9^`xB{m(6{EBUI~W^M0k^gEh+
z=gyro^~@<VbAR#7(J$Y0*Ga!;^4vMhfy_TYJd`<D6n+4%30QX&!>q&w-T(8$4?m=^
z0|CbYa9efDqz0@^6u%@>%b1x6{5UgBO<A@(o;hewdmmEbExEkQ_1n5#H76n7&U3iP
zeZUxH#+i0t_v6aUOqEC__wn)Qf#RypeJXwTGX$givd+kse}V}!ZscTwv7rv!03ZLn
zl((8Wpr!_qXq-?!O2wz=4+GZm5nF)C!=#u1em>-2kj3t0#&DP5;}r91=TQC`BzP>J
z=*QsykGKMHP2>4+gdYh|1E{VjN<I9e`D=Qg%1m*o-9HXE2(T7F>FMCF2ABY+1C9q!
z8fU^!1L(aI`LJVn|5W&g0vZ8Yzy`o60D3RQ`$zD86a2IJYkDrY6@1uPysw9UnnXMy
zzSwU;JTt%oXa%$Z+5sJaHGm#~5Wg4KeE=&TcE0GD5T}@b0UzHD-vjUhMge{Ry%&H#
z1_%Pi0aRb*IvPP4zzG2AXWIbu?soV)02cx-0_+6r&G8?5vO4}z<x6V(3w=tU142Ke
z@lVf$@lUis82>U{i^u<AxIdYXpFjR74PpEfEyy4LG`@?Ce|n!>b^LSnP|m-2{MVv>
zi;e$_@%$11wfm*;KLz+S;4;AFfX@Iv3-|`$3c!_ss{mI6=$-!q|60K3`TH-z{}SLj
z{{GAGZ{V+Qg#Q)(`fKpN&R^dQ{}#Zl{QYh4zX`Y-@EyP%fPV#i8$dY!4gQ_{^<D7q
z2mAnV58z(FeE^F0efa+Yc!0lukoW1C0FNT<M}Qv#eggO@KzK&}<G6nU@N+&KL3_=;
z0i1v2@xBTFEx_CS{k!mY1OCL{{{{YE0q^nme~16yfcN?PLYc0%|Nh<nz2H5QObC2N
zgoS{J114Tv!E$2gL<QX1Iq;twkk*TQdZqE}3Sj1ffuEHph<BtuUNkcQkmjg=^4F#D
z69-w?5q`oe@R)^TW}lpa%cuF<QuvjOJo}==m4Kh}Ku#jcte{$iOz@%k@`r`(1*7N%
z(~yZgkd8vL-WLZD{#6J#4p1(BqN_q0-QYm&OfB#cChChpTtyH<ISG}Nf`FgJOSs7i
z=@#>ic&M!5Vw9?XMH%(K0!eKIl#Bly#QhD2o+*uA!N-*L=2HGmNQ&lAa=mEdai$y6
zXB@H`)jY|*^q*iHO!2Wt@1KG8KM?*wfO5-!5#k;IAZJ1wyMc?a1%Wv;D;He6LjPyY
zq5Z}EuUP%JAV={+i1~2BPy8^Iom`3N?Bi+t$B*Y?7Q@fc-{L6(e+|_o@$cp0HzO|b
zNaTv)cd*pInNbxP0~_X0|DOZiqskb6S^R~^0Pxq)cw#HR%=*6_@ynTi1pG>-@Ekz+
zPe*#2`8>cY>Ar^;#LJN@r~Z{i#^GG*|0dvljK$6{`z!s|l+pjV@mG5OUEcJ+IRB>v
z4JhVl{yz~B>Hy`^ACmb=79*G6|BRrxKFkOcOboLTaiug96?W@@pGM~=0OjJ(ATG(Y
z<ocNb;33{40ezkudbkPvlP<OeKIwuxaZMCk+R^ymibrPy%EeD9t)@S?0hBQY%%uBG
z;5xz2zg{*6KIlq`1EJcLc7&hso`uKd;xBjpqyDdzM!P4J=AXlGcT@%V{{?Y{_Af^N
z2!Hb)Owt#GpUN#b=~%g;*W>m&KByG_QXCNBCp#GF$0!ecvPHcw;OQ%Tj8gau&pccL
zb1DBdcy%|ApZY?%Ky;4c9s!@qOKpBMfb@4H1CUOT<N#uAP5^uyK(yf`z{vpW^IZUv
z)lLIYS~Pc(e<pnD!)y3^8c*v0>-l>c0rh|e{=Nym96;2G-dDj_^Vig8sozlB(KF)B
z$=?ir3x7@YVk^MF-<#mK03HEY0G)s~Ks%rVK<^WD<GKgX3m5?O1FQfWfS%joI{<@#
zA%GJw3^*6y0-OgpA20&A0N@6A0A9c-zz6UH0)R0<5HJpy0E7TxKm-s4!~l~3;!%l4
zCILb|$)CbKQ7$2DJFa&CE)<0gvNU!S>17}VxREwT{o=5~46;_J2+A5mz9In1VXjlo
zf&U{}VJn|ZdGueL%Ut|wj6dopC@$-ApGw~yir|m&kHqwU0P-hCjm7goW6_y_@Kax_
z1pmKNd>(&(`HALn64)qP-*iuNK@6dsWK1ocbz0!xfLj_Hl!xq-!dEFg63SnS{wNq_
z;e_yn=6@RNm6V@o+BQDb()d+;Olgns6P+QRqTKRp5LcjorSTJ0DeMS8;U)T3E`F-f
zPx48Y#!tB|>;(KoTg%1Yh`9gclPrc`kbhJ~`T)rvL{o{5mW%&~h<j2Emx1!|7w!WB
z|3kRRaj6#ne+-ZRRy;~YPdUuoe?>1YP~`dG7uH`8orz(FiL(56T%-jqo=NZ@4DkcU
z04N{DV{=bw?7zlG9Kd{&!0grq>PEb;4Ra3VAqlESl1VPLo1OYx_@9T6j6naS=x;H4
zo1X(3f5ZbF3n;hz=_2^E`qwDRA7^QQK2GsH@UwV@J|KQp=$iP|Gw2U4fq8x$|D^f5
z`IBE}#x5CPZdZKmYk&1$&Q_6-uHbWTW&a&5P<^^s^{to9Y#evAm?wzF8$p+%Wi@WF
zEeV8MF_F2^$+B0uF|t|t>7_5aw%z5|{qBDG$=bJ5#3khvb5B)YUSnc2U<U$DhH?Rg
z3v+gq?LlFLax$a~YdR=XZKzidn}sN=-=1JYDN7^=lE&MQcc?Ft?wz0(d=K@`30wu)
zl|m#Ry4Wj{P#aOqX~z5Vx+kA~cIMELhhKQWds*cEf?3X19<J_L#Qc-aUEmGN@mC<f
z6pzMxmH*#2`2T$R$8p-!`?;CGfW94rWI*(RMh}IG9r2$u7#k3{XHbeGNL&+Pk@4YT
z&OTrM=mW+Gek6BTcruc|Dh~K_2!jN=SwV*&g!(c$8&W3{MJw*8lvHveQ#1z<-RK9k
zAi6@fiNu~+ihGy-b--T>pJdT;%THqn(OGvcR-6%a;cE6gq}7G|P%daiCijHGh?<c;
zijx6^w^{rZ>pw_e^9eqQi{W1axEw(F&v&0e_-6r>=c?QHY5+|u=?;(xl?tnoh(|$E
zaizkjAJSaX1h7TDUSHhR9tkE#V(z#*oN(EKVVBKrwf43e2Iz(lZyZlVBP*atVue$w
zbmtT1f<Bg3C6i5*SQoJ^x)xfgDU5n8G*d2648a|Riyr|vZlwJ8gBaI@i#RYpJL<^}
zngG=yMv3g=Nn8^hhyovRBMMlfT&$HejJ&0Ix;%|A2SQyUeKzIK#)m4P9c#q<+`kSs
zJqmH*w=kR&@lC|B#~=yzvH2{<IT3xMx+P6V0K5~ifdQV6;?@;2D7!S*MO;gk)l7L@
z)~F{s5lq6zh2NJ*D&<Of?PxGDQ5y`q13nkuuezhjU^L?LhTMt72zS#Mo8G=n&btQk
z^ZtZqJGN_-s(P({yTu(!_%1?2udye5@xFM}9}M}{xLTs#RM;0uCS0w-WLr6zN+jd%
zq%SaCK^9ahb-hBxWkKk7SJcdp|IOAU*YCLb#f2Ic?D8Y5Wu9yk!~<}=ZhO7|-tSG`
z>UqTU`07uuP>}Lv*6bnVu((g_FlhYOfMVPXf4<dPAq^?F^%cr`m7J@2(xPT_dCffQ
zKMJXWX8*a!A2f>QD?eo0zmU*oL;k|ZpAD44ieE3}C<CYrh0v1`^cA92LC8*|H5R1F
z_eY|a?)c;L*X%s;oilE&`N=Bk2l?|S)*&on{pC^;;{F8w$_08v)M<>ROf(XR4<&9?
zpi;g4*Sruv?o;bu80po&wDU$1|0POCp~c;Ei(LLupWmGdB}bCsm`biTG$uhm6TW0J
z7zrfS>^k)ayUcaQ+N;ig>(4j6>imD14aP06x$92e`MUFETvN%y?nKg;D-skh@B%+2
zJ{}~5b&ox@`*x{3zkJC}pT1(nCGJO+O~!*B-t|)Rn^*t)sr5}pr`36c_<eD?N=yG-
z@7W5>grDS@_f8Byb^0?;{p0*=<i@*y^_df!Ub<}C+qcP$_Z|7)gB{&R-8_!)#B~j?
zZq-DazxY)-TYuDX=zXCKu<C5BM1vvDKa$zw4%l;*y^spaj{n7>|FjNcdC~ve$$z=i
z|LpuT0BT2TFA}T<HU#dg7F3@2OOk(yugrfwB6`m81v@Nf|NqTzp1$t)-QV1`Zo0jm
z`rd4sFP48Ft$_YHF#DLnUAowOhFN;;!(0>tWj3<ZnK(wG(WGo7og7hc;#%_WPLpkw
zog>r8Hp+~$TIN`pT((7~lerL=xKc{Nhthl4d5ySmN~0E`R6nE_s0FW18Ud;$uH;YE
z{`gh>eK-92XH9J%zWV4HJ<=uT#^O2fg!P|yEK2#0LFtt!wTpQ~5<lrj_l5?w*bFS@
zK*e+O2-^z8ad0*NpAX#ijOXn`&U@z0$S-$Iop#iB>sDKK{rQF;-r#+_&cF5$V%KK%
zK~8^5*bg2&`Ld1gUh%if&RnIq_2|SYvz69!<$pMv$o0HLxG+*yW%_B_SDM6+dhZB)
zPOHt;8n;rz%25Abjh<jcf6&PE=n;bK>P9bD@OAu?w=aI}q&IiJdrZ^%yT>0~`=L~w
za^swylyap18{i`y{MM!h_xtCY&iX>+;kyoh*3PX#rqLBZ?l~DTU*{!GzaIL>GZC;B
zKsgXUD|EdR4{qZh=J#^3K2m5iVI`PUdX2pPQ=oEEaq`2&^sgBHKC~RkT>@7sWebx7
z{~915n{K(~CyVk4XK}f*NQ;jNpcc$e9J!_Cctln=gLJ56CfO!gmy9-taNTL}FQ;Aj
z_={&h?hQ<Te(EWij}jNEMDRI;dH=Ud(x1hrJ|2vjD6}ma{J0|OGXuZ6Kt1)<+jPUL
zHaM>)qhEPN%qwy3?PBx)^HpBFBH4a(n?DS!28_6k%58DEGs)a#S+T(~E6cw0-0xSs
zb^bjM{K;~&z5m8HR$j99V)@jGS8doidBnKx-4{rCKD#dvYkr{6+J(%XW$T}Ga#g)b
z%c&qlTB`i{OD^BBU8PpkE0tVS()H8IGAa8d=?}8}|3cT<8Q=cawf>3!eCqg}U;pF1
z&fD3gV*)7<e5{3=ul;y7TPikb-@kJ6vum%s>Y<<2*^a*D6JNZVDnmdn_!Ks0_k?8s
zUkv(HsQro?-G%jLanL*JquE_o!pab`g(X>iGugugYr0Tj8=<!&-Tfv2Rj3eN%%`<a
zYx$93V|yTnD~(=CtKLY9MB`b!AFPqJfw}RVI0TYPk}QgIfM00Ha`BTKOgsp=O7L@b
zIa;AkG6Go)8D{Go@YmqIg9$mDnBT~^e>&K@NP<^#LlzNM$j}?3DE%0wV-|l689{_P
zpfpQ4!p$nI;GY8C&m?Pw{3Dn3f*0HhUx6v!2)~vQ<OFU89AA0kj@@6m_U%Ng?x1z=
zb=nAzfLHJ<mH!^7VElYk^4}^jGa8iI#ZWC2t_>A9;&rf`tmip`Z#@0L|Gd0@;16H(
zX~xbwe(l$#>Vv}aoml?6b&<>8%gP9a_x~zO>;Mqf9}4ZCUskDmT6^~d|DZH}T3uV%
z(fTJ^=UoZ@?;`$Ne3He+p9ytCb7T}+?pBO?qB}9H{wABCunqzAr-pcM6`uz_*`gll
z3W+%&S1$emU?+-5vOtU-`P}=UzpVXVfm^BrF0}YRkzN6R{?aY6q@8O85<kh8!8aps
z+#2z7a4Ohgk}~hyc*0Vg;BO?y&qsr6_6o3%mg{U=x#Ed~^*gR&KCgc4q+2$;_V?4Q
zT|a0wnGXKSOLuIE{>LtAWue~-zQJHH&iEp6IW7!_p&px6-foB|)Lv~r1z02fc7@Sk
zFinkT(mH3qeF5hGn(F*7)P8y6Zz;|H5zrFa?U1h@x_9^UyWX8R_k-P;JDJQyYeuB{
z8iifEsN*kx9RbIuym8}YXZ7}GZoKZMo^@^KzMHxA+n;@_#<FJS-WTsl-`jBhgHmNF
zW4(!_t$1(KgoUQGRrx>Hmc3-<e|bH>{vdai%6#Sj()^c}hD$+L<^L*|KNpz)%i#eE
zHk9arnE$(Np7i^{8t<a5zt82q#QbS7&XqLkVP1lJ<GZi_?nk5lc<7FGD_*|lgm-$W
zlCpC-cO|MgiA!4dCamC@kN%QVulq0$lLmzH7=Vs}(}j>F^VQq!ou59nY2|S9FV<^L
zdh(TPFL>JA`jwVTx1RjW&mTGYAllM5E9aA~5zXjew_~~^&@|8;ZyoH^w<HEEfr+t{
zO5SAX3~RL7>iX|_um7&0y~#qak?10`g2eWp1z7)$zFZl8(h1XkMRNJ=-;F*OMxUm0
zfW@+@l<kS$hy86&*{KdFtdu((5d`~RzMijY|DzQQqiBH)EV1R=|JI{U%dLOf+rIWx
zu9d`XoUebZ?*FEB0!x1XH+xQUeFg1LlZBxFUOM&*VUG*xB1t9{tlqRD!2-i`AHrx~
zD6N>MbKy8k1z9A>UO;msGknO)w%606@9aEX^Y;TkU>D~LbXoAZUZ4GcNRt0q4Es&t
z@{3o(WMgq<estw;Wv?Imu<Kj@s{X*e_R{OAWI|a5KU><_S8-|kfBv!rviu9jKy+h4
zIH?U&IGlziiYR<qBS1zGPMJW9nX`9uRV~cUtVGC{7PS2qZWqH}zI5}8ubrBDYwSvO
z^PAt?P>{1+*!)+vK8PA=0p@>p{SB=_q5dY(M6-exc>d4n@3W9e1nflELazUzr8JeC
z|0L%B@&}rDJ}!6ur+Qz9Jd|7iL>P@c16W%AC(A-%NAmxPcwEW;N7^pJD&gSg_rL1-
zU!0A7DV+b6J^zDRSK86}8)VO<zDG_vR&MC$aJ!J_KW?rN|LutXBA=yV{a+}5Ip;re
zr7y4i-ve&J{#YJ<vT#Z}I)6pLU2gr~grpwhlgl4}#8r{}QNj7Y72_AvzXdq|w}$p!
z3Uwe|OSz#&++NKG6{~-;jg@=;Zzc9uvHia^{*v@}Pua_De_E^aZ)KLB-C)bdmGXr1
zSB}TSa`Ar)asR+4R;v7DS1;!1{28I`%f;V^1YhA3ErnlQ3^|@r`RP0{;%Umo--@`x
z`u|e+i=Y0);;*>;Uj=?){;lr+%I>||ANPNi>VHaZi#>Xbw400l=XE4?H=kT-{KY8%
z;ioZ5R;qHx-+v))IrA^kf7&r$%+00z?;ycn@QIc#e{l*x_$kI2fO5<KXT*IOP|O`5
z@lYy^)-p&rskl;MB!(ObAo?lV5`WZzN~YBL4>dSgl%1ZW(w$FOb`K0GHTLcEABfu{
z{SuuqL1$WVEAu#|kgyY|bS*6QDHi#q@Ioa;d+NFGY%KTE(BWy+Mzjla-)Nr%w6&yC
zT%Kp9(1|V7yAtphQ-4=%f8P`Tb<CB|JowX17hU?d=Voe}S-G6%2eCu-v(jF5{O6wO
zv(%_rb^I@__Su)le|Ggssg>5HRyvlq{)V$!MekUZ{G20@I_a+Pj;0%<q!7uGVu?!a
zS?88LgGr^VSM#gjF2Wn^DLmlNNMlN;NO5UDH2ul=Umx80FGt+){a-zE@dPVqh^Zc3
z)B12KuLtw$a-CDfy^u;l-)c}-T$+cL-TEy$gCV>7|IHUxUi8p++-*N!b;8QC5Bq?Q
zBA?Zhik-0~oa46GG(W#J$<|G$E^(6La^qn0OXI~@BRPZPn;7Ovlxw+{DTqBkbPgc3
z(J%Lb@^E{=CeTv!tpScZr(Ighy`5-}03A8KZ?sYxqliXR9A%;t*Qi{--UnKV$`ywe
zg7)>&`dm&IL0Yoq*8hkf384gjl!%iN$rtXICq0)g6BoK%=cwE=<!g}8@pxD0<W%nc
z8vf+A!tV=TG0i_Ymg6{RF912NmkTG(5cMtZ6noOYURdD$y|oLH@-KFktU@ZFKc-+^
zIT8u!jtO`x&L#Xs+3$-_^l!wP+XQC6dA)4sbI>Noni%n~Vb5LhEAcyYO^$r&x9Dj%
z=q9%_*aq6et&*gj%EAc_^wp~a*`r7{vDS=jI5SO&^^<0NC95j`5&ha1_?)WzSC#*k
zsx(`b{}PP!{J(f7zYEehSuu!1t;&D2?{2BLQdRydWszTOO<t~CA&jo7{I}FbR&F~L
zmfx!KU#ah)kqjbsRrwFKu}tK@qmkA5$d77BkqXGBM#z$CXv7<Er@*cbl2;AvGXpQV
z8W3i}w`=5(Jn3gb>>aS#jY8_qm0M|}SrYjosg>L&RvRt-^*$-R$==d?%;o$6&PKV=
zr?|{ril=?Rt5C->=8l?{jdJ|98I%@tv)Zg^FgB}I4GKkzyun~r81Q8rgWRN-TPzln
zYRC4?-NA&{f{))M&e?9-=%TOTBwTLKJimmqeVdMTCUt{KYtc8T)f!cUUT!cnDD@VD
zx<zT$G|RO+o?fiHR{D9F?c0!ftx;uYZZIhfss^=Ou5U0Xb-D(#(qhr+&3diHAm8!e
zqLrsQ{)N*!%F#B8wS{v#Id7?rf7%B~<Nv^AHU1ZGyl|hWEAT0?GO5=O7j3l9&i(8V
z(WlXtVR`u6kHmfM5Pe#$%pySLUqfnta>1?3X)2U4E&JMRcx2v(9>eqFFDjg~_b#@2
zw@=yZ)_UX(K9yS2peEKpuO0O_XnbzJO0V|$R9bzbT&~cnmD{=gnf?DbQ<JyPVFez7
zqhnf@5{;STm^@berAA>uU$7v3L4tnoH3Yj?2n!nXzBS3-3eV8w<hBPL#$Ibi-q{+6
z()q1DHv5>_9>A4}ZVdsG!3nRTBVXyg4zGD^*qbpJ=GZpNmtG!N9UJ1$q_!lw(H)vw
z4BpZ7WGkH!Y;D5_ca09qSlg6ibkuE}a#;E_%F*z!DxpcZC-h_T9?#%}(Vpx!I^{i6
zMt_%Oa=>ZS+5@c}dQ+#t>Tb(SC%e>PQ_xV?sWqn)&ZZXQn6||coNRYQ-72>;D(`Rx
zViudaeNfqK55<yc^YFNTSUF^$>DFjwG<D61=1$#AY|`%ccVwDJ>$F~-UNO_0(50G#
z(ZsZ-GnrER3`vXK**4bf3wFihQ{4k*Kw2N}(f1hpGkxRgRHS)oe7LpW+&3WChcbbr
zzAxx+AG5|h(}`ANX0#dKYi;%@T4uV;iixmZ;b~QknhmWzQFnVlZZfv{C!)dRXjrFk
zv`%=O;o-0~K0F!K)}>o?mVmuQ>&Te8+J{WWOh-K6Rn%ov9Yf=t2E%A{$`UsQl?H3l
z<gqDQrs8_d!1#D@#^jqcbUV6-W;(i!eWoGnsHNFyi)wqvd-{j$#{P8saJ<(PwoixK
zo$2wgX}}Qmr2I|x$k4FgBsWIJl;iH;WamWZSa-*;+|<<D)f;vXxV<y_q|;*$PqgXV
z2U^2z<HO!SsyXFJWKx5%ptmm2(-g0ZW?Jlh{oOroi^o}K>F)D*J)xPpI%}r2tk2}`
zr?jy;HumqaVGMZcR8!H+w8g9+>zEAs`kI|S>x?&%=~g-7U20!%Y&zr&Mq}ZY>1j)!
zQ`Ix<whT74bU4O4EFnWQZt7`|j|~Rh!!fftXpDjY4YwI42d87B{)B2Emg(p=bWND`
z;YnvG5T8-$ZB1%jU)mq(2@lyoTc*bx&QX7hrYqVS>@)<+InJ}IS(&kFXT~+kaZMs<
z)Ak!P(P3>mI*{?FdX0fjqgkUiTg+yEd#lc(^F|{Um01xQYW1cveGbo9Lfzz_9Cx?$
z_S)oLx4g|`4|K&eNv+Nnv$V(jeer=Fn{8Cx(eCJR>r<^W)^ylD*wx$?ZJq9z38sf4
z?FxmeACpCgN#Pru9-Hp!Z&gnXdj|YgZ@0TA>PX4^qe**<YG$}SqO}gW^;3FpO5GQY
zc(iKmcsOnA8S3p)8aunxJ<67Gd0&q|>}?y4ceeXGI;ZWvab?orR5v-N%<a)Oy+@-P
z4EScW%5+*6bb7VAp3XW`&!nfb%kD6l5^|k>XvWep;PdvoouRmOw5=)FJf04<c}&w2
zq1NFpl`-68RO)a<YJW?>sq3GKs6y(AusNa88MTfcyH4Y<X`_kZZgbEePYtBoTicDj
zGn4L!)jE?(`&FI^iy@+Mj&}?N$AUIfDr(jQl>M4?Os@>gq|<JrH#A|6hTEE_o4WeV
zX?x7rnhNT)dbw$`C6!Jnn|(vpAyvR8?`iUPO$Ga=n%d+}MKY#HrlZ;}M_<2rva8P-
z=#93!jX_;gYoFYdXj6{qN2AKR-jpZa-rAKJNE+k=j)2iQ6>3l0Ek;duN6?+Nw)FOO
zws$$>T^Wx@VdxpMrTU{Ov!iYzP^a|9GSlkJ&`_VIsV^9k>r$cS?&iqogw^M9YU><z
zZez!ApP|cF*U{dVwl_C9+Z`&i$uQ)b^0sO+&0a%~&g$=-(F_KaA!SSx_p~{EMx(xN
ztS6~+v?u#bMz?dQW1v3~7|bZAdd8itU7ohSNuRzc*d6o69L7+4S6$0wFljLl8dd4G
zFj`Sw7wyu`m@T6QhsHQKI%bi3)d9cF-5VOzMVp5Dl<Abe+uG(dbqyuKV5q~B>LJsB
zzAG_Y=ZGYyyc6nBb8uSM?++R$f`K}@UFUEFQ&UrJgU{$s$rb+Yu)Kdfks#jJV6!`V
zt(_W^v%P&wR+!?hIFah`Jz~}RM}!zH%UkJmq*$7QX5`$DN|5@9(_UKQQ<X)ivItcc
zp~@muS%m$KMOY_d5jNtQ4#8|$PKJZSMaS*5&yN}SNK&d!^i@6C($SMubpcgfz`m#p
zsP^mqyI)szL8Wv-hj5Ef!QQQZ<S1Ozk%MGr7lOI7Wpa1!mcIgZ_NhA3-!#l$F^UYZ
z=YtGALtpb@@NtG#thXJ&J{}vpe|`jOXlW-X?UoRK5IsMgs*ba&<E-j9JyE|ekyOf+
z^4hWtqE#Jdb&0@!vqYfEBJSTTVwFYMhgpQV?P3@KXDL2~FQ2R2d%LoJQ?vJQ&F0l}
zcYw9y^r|3suI2LH0!L219j8uO%jVV7H@qH>)F-F457)jFbYAz|pBj#Rd-d3zo`A{!
zou9vVC4F*Z)>(%zZ&$P!w5S=ps^bnOCTfFWcfjYW>bR?GLHF^spjAe%%INKjI_|2D
zvy_hWP(hgqj^a#Q(-$qpI?nxhEucZz7l=Bs<C@qmaosGQ{+CV{74*O26VQ7M3FhFt
z-r02NcaFP7{`^b#JpI?F)|@x?yB{C^{@ur4eche(`LjKIW0n7(``HQkb+;V(fBNF_
ze0IRoX~gseZWm4wi9+@X;y;4BHda0yg)B6HFgl%b1Sfvbx$SgnG1(=AxZIBTUq9bg
zXPbQYy%(4-$8Njg8)<PL<wn?tA95%5=jXRj4#Z#07Uzw_cj?tTQ~y~1*t5?6TYJ#%
zAD{Y#KfM0_&Z(x;t~`0u3r9b<@h_87_@x$%3wc|v_MfwGa!Zu$17X%)G{K&M=D;Z}
za-8GR$o^AZ(22BCC$PM-fbzEiXC`ysbD!^73=2IHUwZEME8aT)o(KM9x!K--;~Oh4
zS$na3>cp!y?3_GeT=(t^mrM1QoJoT1`Si^RPHU3Y5LmGpZz7m<S)<}zc_Jf(NbcOe
zjoTC|mlxYPx?{Uat*BQjw_Dtygpachj$+i!)?SdM@eE}B5scxDXy4pE?7}HNQ9Mb(
zg6Lz<<u23*@<$}m6hGdf^h=!`m8(G!k5d<Lhfr?`{6=!9OTPArv&BAG!5PX9w1gSw
z<I-pAeJCGME+6ctG1QY0KwrS6{{V2i5IT+LV~9nRE%)v;*;d&(GL3Ab%qXj6R>?Nu
zS|M{WTV!&XN!BVOzZXxo!dD=s5q>RBKBCjmRLpw*n<d=x!P}NV305P2Mwt?&(4!Q>
z7vS8;FHr+3JA=>eQJ<00yDdha>0)~+H?t%}^Ga_2$;J{&@iCO*$z@QAKAe?BEy11Z
zOs5-$&{yd@%`~gh+$)rbzC-(>WQodOk1$_;fGCE*5iB$n=8iV_jbsTIqvtJr`RF9`
zJC;oOa;dQg)GrEtK!cyq<LKm|+a>cmw;nei4j_iyB`_XF*e_WoFg^mvv3Pu(hTPsK
zncKzaJ-M}8Xs#1#n!Xe8?F#a~a5LO7n6;Fc6<o|I_%A%C3Gsv+e;heZR*+*l^^f?B
zZpHz6X9M4nMvKM)Y21^<5JCI8L0yQC%HEBzRFHTL0rMeCs+}F^iPRf8?m^tn;wM7h
ziC<efvr-`nC-AXS9BuA;Glp4-5)FZuBn~r|+gpg#NfYx0$zC=$3RJn|A-!IYVOAq&
zKU&nB<<_<|y%EM#{wTV4_w&2nojCV{-I+U?%tdQPP?$YBBSGe+uxo3i_`t=j`&ZU|
z^1U0nk6*c>vFY|F9$)>?Ex-Kj_g}yMX4$GQ-g46Yqrdu;R9Rx~c&YDAONw8!rK;Ro
zFVx4mRhl#+DzgbMw59z~$@bfO^Wl7CM;dwT$6BaE3R>C~v@dI_JLru^6H$M%(Gg6f
z+#y>sH5!aI_Tw^`oF0fqLy5+LU<`k$V64yWoq&nAzSe;=OoP$L7KOG^PQUtEQ!129
z#eG{MzEm>q4%OH8r97dax6_9-CVY`C9=Xb|@i+SwicyUmN821k+1OG5`vBG$N~3PH
zrJ{6}YKuGRrW8)!OmS_#WHJ~DB<RUG6C)!=<P-Onpf5Da9{hFK;mm!K?N|QzlN!A)
zRuYbZW|NkVzC)Cio?Ou9k;EKB=!KY>6Ocq&@D)7Wp4pd96unB4?#-oF4M6TD89l+u
z-y!gJwa5W|CydiN&VEiBN8!0JtCG(C+6Bwg!p#xX*56;meC5)r+!=z_kCWDebXBB>
ziQpFm1opJ_QONyg7cuvwv7{Uct$aB0bHThhjoCd)yndcxjsTX2z=bX^jTv#E;XO4)
zH1c}3<>o8zXQlnz-m|~x|2a#B*!q?~L!+wEN4YO*&eos{_B-m!0njwE=_OfdhjcsH
zuNq6GO5cYqSARc_bV&lqPp7K?-M{s}rFHA4qyI|JQRQ<P^QAIktzs^n5zEu!C&HX?
z3Uc38V2+{@k-}IbeI;iJ;B-hXW;1jiYUnyP$+~0;a5q9~$0D`YBvYGPUCc+{l{<IO
zRnBlRxfYD7{=dBMZW0f;ki9y~VUxBfYaJ)ee^vj_&0SUf|6*EWtNQ<i9r5DXD%Vyi
zej>~~r2k($ZTj-FJnVZq^rd6D65B0ud#?D2&~wS6QPuyGWipPLoxbZmH(s%Fty3<C
zD5fg8ttNgldu>gX|KFGS|LjV%e7jvO#x18KTAY<(#cjL?A$R-2N)RvTfv_gFkrA{s
z40FsP*S-gmaS}h$8Rgps8Rj_T?lDO@SX#%waH|n=rPX7xB4LY6hqVc`Qh;>Sq(iF(
zg{4{z;fnHKi%_y5(CUI(tV|5ziYy7j40QrZV=Yh`S`#=QOGqWPG+R5kkP@bt%8S=$
zOP>?Npr#gg{Fm;5)$#w)t;;P<eJknN)$za7)>c<)pJ5n$C~(QzP?w^;Sz2c<JwNVE
z%3rnX(n@}s|7Y8E7na|p*_$+WF|4QPE4dRQ-|k18dDhl`Fh<|LV0A_@s%{C@K)L(#
nIQqX7>1=gfPpLJCxtxEM{uieI`vQes+H=?vB7N1Ypa%XIf8L78
diff --git a/src/azer_robocup_project/.vs/Tomsk2021 disk image/v16/.suo b/src/azer_robocup_project/.vs/Tomsk2021 disk image/v16/.suo
deleted file mode 100755
index 10a04fa5df6b4e7b9fc44fa972df536a07c26d11..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 151552
zcmeHw2fQOy)%d(+0fD9Wz94N0yXlRxkluR_PoI-yl1$npnRLLv0wSO&ihd#p0yczC
znu0WOMMXY9SWpo~6cxlm6O|$&|8p{#eR=OCZ}ZCT+qcZk@8r(hnYsO*d)mEs<70b2
zd+n`<{GOQR))PAr|9y8CVyC|F9pHKef4>(&901oC_TP8keHVlAdJVwg01za&Wl#ft
zf`MO+SWTFS68Kf34Xro2JAl}6Q(N!2na`g6*e9b0dwx0Q#P?#{%U(#4A##KbQcuH`
ziD+#mmZ^^qfF9Xj%~PKazDte~1a#l7^AL3?b%CHEeS{1VfO`^vBEp1}P!VcEHaMDY
z=n1{_kAXYP$1JZ-dGtQW13JF^Sne|9gMs>>048(6y*KRtn0b^R>;K7KgxF^9^i?2^
z<&z+c4_e#1J`()>02}}k02RP00DS)_@DBpmA7EdAw*c__Gr->sU^M_AAJ!A5cNqA4
z1H28O1UMdG9l)^w?*otntOPg_0KXN3e;~jS0J{Q60ipnV0*C-G&V9hI0hj@H2f**%
z==qq>gL~H)&O0519XB9+j{#xWXM6*~KLKHi0pXa2e@Hl={$l%@Ii|NY$Nye$3*#S}
zr|Y(r@z3|aogr6)@W3t*j^k~v;~&TK836qF$8oO)!1OSkL*Tl9{A2j^@sG3zhV%L2
z$3KQmAO9FOef%TN!10e`ADuA%xq9gT$GTYobI}d}c)cU|I|1z6yWa);T>*9jSk(*L
z1N;L4_5#?u7lz;N2iN=e?&aVg1aNTg{!s9b064sNe`L=;s^`BS{9^!)0}%JZP5^%m
zz=^&4lfYjKu&#G6?fEkBtpExDWiLz(z6L<syT|;U3fHFrtOxiIz!?BK06hRcH-c{h
zFaub6VHj?QYX^W6z|{-)^n4%qet-x-5Fi8)2EgwSVsIS?I1^yJ7yd!;CjiLaeWK^%
z`&--1M*kRrBC5jpnav^nfpowb0GRjL-sPXY0~aM^6u{|$;$et9Q3RT+3jU~c5BD`e
z97e1dgX-(znNidK0DOS`8eI_5Wg%TBl$8bQAwXoiG+2#Db}2)adu`}Hlz$BQ9=_d%
zzS#HRWJnun9CS$cA+1>j+LHwO&kXq}z&*Q(GNd3UHNpV7Li(KQ{!ixWWT=1S!wv-)
zwfsSNH`U7k_Bmnv3*&zf|09h58jQo)-EtrQLBKa<z%_`0*d7i<9Fv3)BR~T<ElHQd
z8e9)20{-D~->CQpaSk7w4=^y!00l@r2pE^k4GHPz0Uu@{2kRgo$d8l2dr~u`6AHk;
zMu?2h10U~f{ulB6-V6Qddm)X<bZ1L-GIl8cSR8JVHy*Y8$a5pUMi+n-lRaGDfYPU+
zj8!Oepj#&>o1nXxe;v$F4CF7>oeApQSpdIZKact^!gu`qGne#nrmVq?U4?vL56MD2
zXUrr05`4$Y6<vg%2obAily`OCObD+Jb;r#keFkDXtT+Fk2{UjC>bM5ATO~dKIdMYi
zu$(9n<DOs`1LehD$hF2~b~N)yA7vCr_oA4M{^I3NC~Fz!K>jp_G7}`HM(uwW!@Do{
zA{-@slx+-im_Evb-VZQp`k#h(U+hIVO8O`t8s>QU+g!%~Wf0k1%8$I}Fo)&Gv`>H+
zMlJtG;oS|r2uCTuw)gHJ57S3JnwQfJdS`mrxe)MgK9GgZJ`Tth$;%Y{Pe4cKNKF*X
z7P`bJBBU~)-jDCF<EfuMrSjBI&wc8Zb<Z>Ehsv&3qoLDZcxmnTcX}{My?}A@4Z-^o
z&aFjn+8+jao_Qx=Y3_deNALOj>BMKAedGIBkQtFkSEpz;znU(R8EQPhB$HHi+{q-#
z0-b<N=DXu}oX~*;nWLpSsTMv<13tykEQ47LVRd&*BgMZOzCx}8=a}&>{y|KP*ms<8
z!I%74ABWIF7bmb#IsH#2;9Uxy6nfS3#z(&L%imKU1d;To&V23Grw=&&W2@Sa{r-nf
zR1P~ak^AdUPaRaUAB`D^lOe?r<dU0%$05c9gcu<Y$b%Fi94UWnCQG-ZRtUv6u#}iv
zl$|36n}9|_E{cS2@a;gigy@IhT|eQ0?{zTC@=vg-N9lixuCy0(Khxp-=Z0Wj&N=gq
znIWPvg7{A_c(n9Uax~as`g~U#b^IA1eUwe08!dg5st$ITz7HM+07gy!SV$XXC+J2?
ze>eu1Px)<-HZOl1C4G5ULeJ%Q`ah<BIs{q)MlC<$-q(8(jh6oK83@v!f-QQ{&g$XZ
zf7YDRA3g)mCw;_||D03$v-f{9#Kn(+UbM4%FTvBNdT)$U|FYS~|E9D@ZU1W_?YWG9
zSyysAxJcOl5V5WY7&ZMb!@E!Pq8qLJvyT5cm%jmNKQyQGXCMEkL0me3QOkcPy!$Kw
zx>3rnm_-9RpZ?#0bRX@eK5F{%-rF-hY<~v?2LVPc|C{jcpL$V^R{mM>Pj~+D%pv`w
z0ss7YPU#P)fag>H$3yyap?{T&JpNCI^yh;Al$)7<!cdo}7aq0!y^#K^bEtpiX7EoG
z;yQB<=|>@b)Ty8wrT;03#l=6TK-w$jkiK;G@jqw!>pmzqjzM&zlwaDl2H@VG{*U7y
z_5Y|(9JT%Lhj-^P|46&q{pl%l_wyNl55T89|210qXC41z>mWWEz^LUvAKvBZuM%<I
zbC+GDeBb?-efjtcANwHjxkD7kefh7Y(^md`?GGNwe&GYVq7g<nW{jP8Ja04se8?pK
zg_1nvJn~&mO9qm|(YUQJ@F^&JM7DFh%Tu8p49dDVwo`HV8GC+N_2aRp@3``@`;NTt
z((4Yqu5$iM8^xtFpSXX;FXfL9%3+!=P{)oNnPk04m1^v`g|1oaiE%%ZVDfZrJjfK;
zyi6jK-n~HnYw^q90l6B=*U%EF-sSx!V0Ozw{gmLx*(h=K^5l<B{mIKg$3+)DvOfF8
zpBSs(-0;%nzlolDwCL*H&i}#Xe?DhWy_m@YOI-?2jt#L?)mz=q*kwPFZALbki}=lt
z)E_?dmFsso_j&gfAAjo5&|ODg9zCe|r!eu(%JYKXITriRj4px9Jh*etmT68>YR>Ma
z^V!LCm>~^v+mh7(<~e4R4NT>y*MXH9+wG&5-|{qZ#ohXy-r6g4;jb5%|CNXQ!F$lH
zq4#UY_B;8)1HTpC{i|<%YEbnK8&`KOFnyF>&@hTo#KV^_v*)4b9(eEvZhKL&>meUc
zmsUMk`c(7x=iIc9R{OeM`oP18YG*Y6r0j&3m14Xs^kg1ajKg?FeFEJVHbLC6!2Hi|
z{7tsMd6@VGo4O9wf+8UfDU~4v#VV(+T)$GW(y;QBmClv2-iSGPx2tb{uJQd{KfnHn
z4fm|y|JMAUYxjT6f7ugnJp6xu`+v^ge0WfmL1ff4iH#pZutiX|!(9#QgQqJ0eU#L4
z;GP%V^2nioSZUn(tk|EGUw`482TpwC_FKMt(W=)5C5VzC9OxMk<#7X@m?!?8tiNFr
zCvN<p+ziEvymp`M>t*l!@q@RWS2-i~G50rL+wqRC5wBi(#F+czr6(WqU)H{FCvkO9
z2~h6mVrn!~8c!BTmYr}tu*CgmuH~Nqi6jkeP=H#I_SMF1j`#oi((`t9|0;g!g)f_5
zf9Hf7Ui#Vdd#j##_O4&QeBZ?`6xq)hb2y95um7WVi5sy<2Ce`=8wQ-}Q8(8Qy!}^y
z7=MlSka)ch`1=Cv2e3cD0RU*Lbr8V80EjS9-*p(k;Q&ah9|^D;;3xppg`oc67y#5|
z90!2*V<=Ncy~c?ENaLYhDvF1Z-a*?g)H|STUIu_1B64X+DX9QZVuDmNa?U3MoC1Jy
z0_2s~1E9?63;-Q~9>4%#1TX>c<wM^F_x8Rp7hJmmkl*9q^TRdD|L{HpKJp_GfG9u=
zAP#{1!8pJN06qvX0q`LJ5+DJP1V{l;0BL{>Ko)=o$N}U53II5%^XZo1x&nYhh3yOf
z4qP_@S^(`{_*vke4e&L98vwon@DYITEaLcyba59-<B@uq=;@EKuQPz>cscg$vi8vw
z5r(Jtdh+=H=xV{Mt{H#o>zTbj!Cv~ce|_xR&YLqU|B)y<umWa`mg&?J*XK7ZV)?hW
z{(-~ccOSB&<f?yu=C4<MKjAt3|LBd@_lG`d`3(8TKkU2b4?cUrS8x!{sEbMc!9wYm
ze*Cjgt;^<TmEn4fL3GTzw>s`yceueck;<<<+q28xpEzTe700iC^g3})|MpS3-{CNw
zQ8UBn)ESIJPb2ljBKDv8@K1$d^vfXikq3q$DwAA*qEM{`8a_Z-QydaFbJ0I7b@^v%
z>xzJFc$#wp@FHR#PJ4z{E}WHJ4e%mbx1e^Avvo1|&BcE``T2kUQF_UVmztm2)3f$>
z=M(?j`=)<xl&7wJ`@B!DKWm4N59&9ZesNTe7g;Ti8atZfzY;)e*5HRM@C5k%+}M+$
zC+V-54%Gbh!$*DX*oV&l;B9Yw<;DwkVUx#Q{?{LUV&aG|4a!a*X?kiG`(ioQ!>Wi~
z@oh+(*IQyOVB#70q5hG#M?|WP0>FDDfy<C7jzh<pX40vEdcZjy=Y8x*lTU_Unc>m<
zdXJ~4kM@t4Hc}`&`8_-)q!m_-?FldJ0WfO%d&9d&@6N?KJpDGzScu(l)*bFJeM}eW
zm{HTm`J0b%dipM?YwW?e4lB~D3kGKMskP33jWp4UvBTgN+jZ3RvA%-6DEf8aNTp-#
zbMwmdelr(DGgD99KTpgD&*3;9&RKfFT4)@ss8IWj{S-C!RL_Elvk2HyW$INUaoQ2u
z{i2ff?tc9{&%^a|bJIDZBFWaM>T^2&dk#T!BXyJ-M_nei=NOE`&w}3*>x?{&P49og
z_~-G@dt>~g@jU7RaP0=-kA*s1|8W2St`FhYe;fkWxc&qCDz5)P{nzw#-Vb58{sV0z
za199JNWpFS8ZZE+M0-aTNJ5lLG>5J<;4$4)AMKy}b2;_t;Jex8Kja~C?L`*yGHlN4
zk99TRy}723`r`e1kxa*bXx)mmQ64BBraK$XZUc_?cl$tGD6_zP^rD^Bv%=GJdv8on
ze{=NK^oZwU|7w9qc=~G;{S(SjhB++%3V6u3!LaxG!~Y-LJl6{tC4H3T40DkFrnE<`
zf0V}F*^6+L^w9!-n46FO??Q-dF7~g|u9RzV4p9GjB0NIb*r?^d8{YkIFS@1Ce{G)A
z=j6wz_rmeWf{&DV3a%#g6#kgCa^2V{RMK$!$;F^c`)xwUfoCW|UL_!jRQ4$8&wbzd
z##IN@5BSLE&ad03|L%NeeE+Zi?7J8L<Eab3D!TRIOMi+@!H*x_M^0^&l2vU_yl}1R
z8qXP~)1Nx@$qheP`Kh90w|m|>r+Lmfh>~W+J6-=dO8=K*L_-{oe?HHn_P<9Uu0QqS
z8zp_*g<+V({*UQm9!E|833zud_(!@K`wNsoV-816{~z$~s{rUmEB|l|I3N7;dr0@+
z-PDn95aj>wW%>UNi-<qxBmaJI5z}9MxkA6?KWC*<2ikDDW}A~OJT3eD?N8nM%-cKu
z#ai_J{?hudY#XB9eLas<KD*P$e)*yg6-G0#@w@Ndzr^LAPJa%jfBPj-J;c%TaQz+X
z2od+8oBDL{9m?u>`g3~vTuLY(<#xi+0UciMuDamXIACbM1NmbY{~ri}qo$9dzm>g6
z0$pk!)%@9RDws0LZBXWkRD%zskSG^L-iQSW6t_xzG6%5!YiuvLJsM!t^!xdr{+e|`
z-$MB`Cy8AL@+_1^lpy64$g;R~9n-lYl)qxSyqnC~@T*nu2m#$}>4&=|uYs%(H3EFu
z@rxAHOt#CPodM-v4e`t+{oUYwp8uLH{RpIit-z}-upXv6**waReearHM6;!DfVSs)
zVi8u(fU*Sc%b@+m^)-Au!1~KE)Gwo&4eOv6?X2F;@N|^=?@u3b8&be1-7I%&o=XDO
z|9AWUk#IX|`aAWdzZmrIFlv5M%fZ>4OhEe~cTe`w@TJHZ$)l=o9zQ0!;+P*u?u_kG
zYh3#8^RL;jbiVmDhx+^|mXhyVyw6LhVV4qOY512(`-z$Qrv#J_m(5LpZ2{sg>^Hcs
zI13@ThO_|x+>G(m=YRW+qaUe!`L{1Pk33-Q%Lm{6xifb>;cp+(xYxPEgyj=xW-z1d
z7}oE&nC#bx(|4`@$0cS~{JXEVH0OW%nzz1j*FUQ}TyfEVi<i7}<&*dYUwgdItslD@
zo_qn|Jb)_zJ_c|B04li<PhAB5rvN?y@JWD605JR-@Gk}U3;@cVG3;~Te;(j}diR(2
zeAG}~*}J|9{AU5K1NbHYe)sjB|J9z4Vc&r3>wEXNf`2o>{{nmq;3fck=N9n44RBlU
zUIoPL?QnfZ?|xsnzYDG(0=Ng@UV!fb;J4oge*?e|diOsB|3?5n?%n?c{09O4uXq1&
z&qrqMQGmyK_m6{*)%!Dmp9A~?0OS8<&;J$pzXo`^cmE9dzXkYR?;hWM9<HD3-Twvr
zKLNY}@MnO(0sIvJ-+Qs=qs;bgxJDl0?*Om$!v6{WzW`qE-QzcJ!S%lZ_JMEy13nV`
z$onIr!smkiXZ~w15qI*A*WYYy{SV64_k>K$SN{WDU}VRSf5Zo$2H?m4WpIs=;JD`_
zLmn5ezX(3!6&(MF2XOo&&iOLHH2~KFATGeMkMDmCe8dO*_`jhSek1t(<Ns#3$MKK2
z0mnb$f$sqD<Nv#GjWRwQ|91oM<NrRm#&r6}|NRh#;~&R9K6?Os9Q$~U;~(({j{iph
zaQx#~$7j<Wj`{wur{EsPJs<WPxb7eS&%*uh0saVp?;s99T!7>M1pplXF9Go1^Zv^a
z_6h)we;mWS!|{J80Du1`T)!Ux$Nwn+S^#{;kAK7k0~S_KHL&bjTTWCdvaqh)KKm@r
z%6rCA?(WKN{u$VSOVlSR6r2%w4N{$}R<b`=MUpPfA!*4;5-wfL&E$gf!Femyj)vUf
z_}K@bx3HY8RmmEaX+xra1KU8fHs#CzE8qvXNcs5g9UDbTH(dVsxdnbdKl+U`Wu|~4
z`O+e$KOg(Qo5qq0TK_)3@=vATrmjDk$@B9&u;3O5j~?^68;<$+EA@Zfdwo@Y`XQqK
zt}uN3@Q?gxk7VuB&YVg2Hguy+<BuPQ7}oEQKEb&h-SoW%kP}USB`ZhQ5jXwkBC%rP
z=@%>ixz7Cs>gcytS-+&c`iKj@O`jyu`(K*C82D6pANM&J)egxyW0!$`e#i^+8?MV4
zZ25y}qYW;{e;f?gn1gQMX8w>aeC9&#rRipB{ps_2BJex$42pIgq`xmbcpt#1=_AhM
z<HDX$0KSedBe%~Q+GuAWl(W&AzUK@7E&5=8Go=OkpAEbA!2_J*(0QP>b0B%hKw8uH
zcni-WcjmrExx<RF-Qh+7Flza|@a~FU1n<@Te~@lM_g>%sXLh@)6v#QIuh>C-9L^tT
zX~kiwdv`nX$ZD8;c<pJwzDD-B9S&T5>5a*ob8p=J{H+f?b2j<0B-S(b&3?BN27+6f
z^&3<1|K{w<IIbNzdTzA`w}t~{F(?`2>Y#AxL*M%OgY93`9;qETcGdx3edZ&73qJal
ze}CrCBlifDCJsb$az-6s*h2SzS?K(SEsQum)0H#wrFs7PkmG_Q#Jk7U8`mfgdE~jz
ze&|t?<tpR(XB_(2!y69Wu|B9Y;~*y-b^Yfg{c-ss(jTLzKehd*%c-~){lr0%t9E^X
zSb5XuFHPS=RPR<NUwSvqzG;6;y#HU`_*v1&)d9cv(0tQAcY)=<X@TF5qJNanG7q8t
z)dE%F0>JT&wX!+45^yi_F6c%}A32x7Za(@??7k>7K{rbJirMWy#%vG|kK4MDZsrf?
z9-jUiEq&?G7_t8C@YD$~YW=T<^fAxqX2<`d+SS^=^Q&F<+j!eYO^qMF$~^hjWj8AR
z^P>9gi+;1$P8T=7{ntm43Exyhd`)>3tuF={e*dlMV{nkH_{00$`9BY>`RGsg`PYY!
z`1uY8tU39o_7gTfvf?4B?xhD69}pqiJ|mUQ;m2DRIDR%4Kkgtnx825YjzR4T&NiI2
z@YGe``~JVNlD>X9`^$rZw-~;@`sA_Hp&z>bjIaEbcDuiOz2z|!P|T=>;k%vjtuc!K
zNr?Fp073pUvxc$$wgvgm34N_UUH)@&?^(b6XEFP~4*D%p0GnG26NFVI0iYIeH$4yd
zHu!e~C(JK<nZC;&A1^mA|9wU0#x-BHpYPRVO&@hE#`qhizna`XkF!SWU+c?Tp;Z?q
z{^`&DAQU)=IJ@E$@6oTE`HjzC^4h7FZTRJyRr{>|#)Y>veo^?<_YVL2%NHKBBee1i
z1Ta)4(4T?%(lEp6=YCm=1+W<d+%DIOBzsWx+1rn}{IJhoN#AyDyZrE5kN)GB&mR0+
z`j@vp-|_B-BH0;*SO=xh!1pqcWXik0abltSA1p@sr`rGZm%0DR^xYyq;%Hq^Ic4q2
z2kw3MgJ1msY5Rfol>a^AvWsv1*eSu*tN-~13W;Xa0MnHdp#GWPA9sHFpX_R3n4PfQ
zQ3`@_wP0i-SA$$1+CXg1q5J`H-^yNYH}^h1mjL~N@1q-q{*iVi*WCNl|K~&hp<Ety
zYUoBuALVJo9MWHEc&GuudKmUzfB2qI{%d;yqoptD|9r|drmut$OdH*lheK{QLiiKC
zXHn=&h3?9!$(^s!-h!8hu|wkEgVFvUdHLf3Mok~}9Vo9uH%j^Co2h>T#G?loHGR~b
zpsobnXz9zjmk0b~{bRZ&cs6SKf53R451<<@{n^_e)8!o(>+JvUz+=>}q3hSQ^z<K#
ztp901DqB(iqfwtESMwSmLH{$D|Cp4&?0;lx#VhSEd|>RFPh5Dwzt2B2ar;9O^5nyJ
z!M?%wDc<LGUX!)DR<cm}-G^>0|8$@BD=)t9xC4S;K2|)Cd*Lkc2Je47zxi+KN>|g5
zo3CAcz2fpS4twOkhkW$dZ??a1!k=!Bw*P$Lv&a7KhTUF8)Hb6oP&0!1Grn#(e^mL`
zr2J`q`JenA`S(%j*Xe6daV^EW?_ynWdtgtkmC@G!&a{6TmHxq6Z2=%61Z_lb{p_%P
zu7>T_z3O*Yow)Z$CfCpAp<PzPS~dy%aDN^AM{C0aS4vi%3|lryVC$xn;h&$;?;dHD
z#c=&Cw~N}y%WeD7?Hm=`8s$kcWe>4Uj=+WXAEEsh#=tH2_Mcz>8HRDtavCih`&Ycr
zzJ_b;&R3o|;G*#}zi9tV^`1Yy|IY{Rah>#-<HFZ%V8_U(RzEiZgLfK^efQn|UGe|0
z&B^a@_XeJNp1S`?Kh~ZAi((4in}EIfz3?A^Yn(lJOHnuAE`C40&l_XEyW;Viul@Ry
zzd7*5*sZ_#N$}>+KKbQS9kP2iT={j?PdkI^2*ZZCi{Sy<5TO&s|9gA<_uE^zp!fB!
zKN!{izn=!;de!XzyTRlAdaulu{xY%upMsxeFFE&gXYf{yQ2r6^|L2@O^6Py1)7Re!
z>wo6A{)Zm}y=I-&>$g9et^aS0^*__wf4T1eaun1!w!y3w-+Q<>_($)~-Aph4<o;75
z@BcFA@z>w})6?JD`#+J%Je;YtE>LI8uw%B7tdYo%9d&AuVrv1aR-;Q97N4xoPfY0H
zWw<xfR3X*f^kLWC7Hq%r&;sc<;2bwO+<$43o9(BzhVLCUKl*nv?uC9olYgFV{SZ=#
zXnlnzQK6i^Lfp5&`df_r&xIQpB!l4T-1+Q*i}g3veiv;vOHe~dNlySTLU%2jkP^pL
zX#6guDEmNr-Y>3t;ra`|ad_18i=_4C2cNwEyZasW>i+v*@z^n;Tt)t^D}Rrji^uJ}
z-@lvuLie9o=>DS%ZU4ov|2zch=%8LbVSR1RmEZ;5{%;Y^e;h;G{o?>e#s3Vvi)VnN
zTMYZp10Wv0Z*(Kw%pb0ZIJ)=FDDr>REb-y~8^^-)H2|ZQe=7gS=X~?NVE?i0umAZM
z^aIpIMgiUg=wJVX>;I7U=(qm}0)7<E|3&RLp18g&&;RYNKVJ=PJL>o^!n<?Pzv9}0
zVGi*RYA3Oujhg<a;oTQ|5srfYP`^FQ&1e6w%OEmd{xwSaysltS9<cm-zysdyYS25=
z!#)ZD8+w7Gq(9vH71LeV`LCm-FYE5EKfM&(J?g(Pf4IJI)b_`-C+Bkhr3@^7hB;pU
zj%lMC_FjMZ?vVam&cBpx=KL`+M2<3@QPanDps)8bG+O-+AAwN*Zo1vVk63<OOUsYH
zZDIfJVf@3S{oSyA-v?Rbm59CmXwKh#Lgd}uZ0Wbv<c-hY@Z62Jo35TH8&CQE3eE91
z4Ad#~nq$shib|99LT#c}ENhLrbv0m{*&f@xIn$YLS}2F>k)m@>8$am(KJs|=yxlJN
z<Xivxx<vn^;#=caer@dyZ^QL$5nlrjF@22Z@1IU?c)hJynfr!Bf9$`i|GR#dPgSn?
zuvCBh#&sX9pMT?DE>TML_D`Sn&W8`%`};?%lIrhDJ-XpjJ360!^$v;N^FOz(yy?Lm
z+~hwc`hTvu{e~y5Dev|8e<k{ledn(a+#LGB2O?pq{%O%&N4?(s?LTh0N234LQ}^%u
zp~f>Ged&0q{+i>TeCF`j10Q}FzWwpU;XhDZd)UL;d%1k~U%$2SW$`C_;b;>!4!T>E
zBV+z2>#%%=0iN(SZD=*%0$w=`?~a2m7*|&ITM_oxA4Ids^~)9bz(HaBv+4ezcL#+{
z=6h-kgWeq!hF=W|Uq~44q>a0l;F`pKTSGKPT@5o%3n)w#a2!~(yxR*d?g!wsDWiXn
zR)&}s3xKD0qwXFp=DB^gR(JJcb=Wrs*I<rA%b`A>YkAa`72!Q@U#K=b<=S}EmPZTH
z@vd|LEfz2hJm`sA?Z4bhkp%nATDLThZU=3GHX+zXDC^li+J^<n6Jn;NA2XxSAIh!`
z4P<tJL;AxD;W2<w=^r;By+|*v(b8XB`VaRf$2~F7jh6oG_Mah$2g@^R`M-+^LmtqL
zmj0~tU)TP0F7#gj(te6do)G9iuI75xF=ubN(|_}sGjYbo*@Xo^xomOr^is5drcrD+
zrNMouirvy|gg(98=$YwllLYDy&uQZN9k&zHaxW7N^e58MNCo_SJ7^<L(}w5w;p!2N
zx_M|jC>O4kLW{=jqm>#kif}Ylp-g!0FP7`c?Vy#gT-7c&h_;ug<;9XMN8E?2zY9=;
zG?Zv^j^*zcyG++WEg>n2a*%#G&oEh#Ape1i8o`Y!f9bISGh!U|d;D<JcL%+<H0N(m
z+RtnY>vu<6zdZRCXF2li|9-Hr<FVhp#QdM%J^b+Ff4JoTUi;(!{NTb@j+Er?Kj!tf
z)}Dne!*gZ4KU4lV)Bb1p`Wf7(r`WSw`Agz|JC}a*!aWw+erT<~<)v(caT*u8ec)-d
zGMXw!#P&yrCvDfeGt!_{L}NWc07uR8JKiDnG}<Yw{5tCSevkO40Wjb1xm~2^XGMm!
z!|z_ZNg%=n_Wm(<2w2Ly0cPeBpH6oi^*P!ZlzYO*2)*>Hy$g(1f|BG>)}yWKp3hnC
z=5)><q3{S(v*L=i{@>b7ce7vhe}mWPCi>3GTsP$!rQB#kn1p|{jw5>3aoB$+y7+}#
z4cLul(;sFqcFw84e(tR=nD2Yx<ehi8Ip5W^juB`}>w^Bx?M#pMiQE~SxKlo-1<1i0
zr@{I1JAi*mcb~$uU}tn0oM>QzQ`!~qFNIUx_1$YZT;XX~MkvQxcnhED;0nv712BN>
zUjkp^&j9?>Ie-mQQ2SHU<<_rrsY0TV>hK*&X7;Gj#@#~t{p9S2*ne_fLA?j!L~g%H
zJfN9Bn-5nD6`(iryJw8HHgkN&?w*KOrjCm@FC>V4p^jD&-y1WYEP>xyI+@v|R+yw~
z^=gHDtyF51tks#MI#_e4lNhuTv)OErpL6!9E}BiEWiY$`Y{SXpxDJjTClljFCRs01
zrCPs{B$KCW<3Xm#=4BF@w0{lc2use-K1<Ve2E|&r%B)?hP$=bVwGy3ftxRjyDU33c
zQY}%P^TQ>~zLQCk1)3#stKf03dE84@Yjm<eLG{4luDE&eh=sUrDGI@_Lme@X-&%@1
z&fJ=L)GEDm39~z3Cxtn5h8wp=&gVCCPhzl(#}<Y`mwNxph1y>)wEkw&|5litNto3p
zz=nbD&Z`qJ*CCyb@|yk~i8(&x5Bi7R9RJdNvD?nu|KVpZ{j2{q2XWf>{{D7*_gC)>
zw2uGk1vmrp{0HxM*N=C%bC`~h%M43Rq$rlol;(uFmU8x(f_{?)l4U2lL30>uONk}I
zG>h{FmvQc94mrPfYha1#y=0bYo284(B=-ZljitaO=8*e~7pLx7rd~}_<5da<-}uIR
zH{Q4LJCC2Y@t(&&DVsxH?pmC@EQEY3leO$bxj>dmuu5zWSqJU{kF@DJ`i!N-Bm*XF
z{^*BM>5Wx{al0Tz_ntw-CKgQQPZ@E8=;W=C#H*_YZDRyslXD{6tm9rfn-1$e{-(rX
z$uK%_?m#GO3T5ERfVaAgK^Fs0-=RuX;7Gtl-2uPErmM1wq{@c@{*o^w)x!s^T(hBx
z`9c_IPnxpvWK*XzvM#t)8+FN4qhi4i{MJCouMeBE)>b%`BK57X*{hVLig7usWXZfX
zD{&_xd3~tn(#It3mOkw?SAsFUDwMI<wFZaIPg<Mpnp06U&^nPrWooc7u~DB@8N+nN
z7G_8}8Dk{&Sf*?aC~Og#I#ejv8m4$I9hXHz9hXwsQHs>8+M(%`E1`7S-c+YVs-#9M
z?WkEzT}?A=Tj{9P6_l=K4#lilHAOqC)s`!00%&N9ZmnDIYkG5vdP&{N#VtOQHz?5-
znwgr`OQ&sFe>u@+E&66k4NRgsAvJcKCTYH?l_o6mlu2iCGo&peG3c%7JVVz~MU6CU
z$tPmPc+p>tR~VJ3Vbqv2A)_kXG&pThgT866W|C4-Q*Mvu96DW!X_>2fTBh^Y42gi$
z*s5xk!Ca2+7^sTQ6?R2Cc9-63i274zbu7TBJUO>78q)h3ws_TJD2CcaTda{Q8iG0|
zQBR9QrD!~DkmyTUS&pPDj=Ur5vd1L`vB&8tl0h=r(bi&#P%&@S+Jcs%H5X52>gsxe
zZPp`YIw{Jy#Z?j0G={uBmzy*vVj{E4n@A=K9g)c2wCFPlk-Wt;+h&tCYp>9hR~@7L
zog~|I$-`Brg7TExg&570i^g`_?2XCYanc+S8|~qo-CWQyRfF4B%|>W4UN)I%Jp+{-
zx9Td9b~%-1<-u~(?$SB)CT+12D`cu2xi%nHXuOSd$z6;Fp#Iv~a4eNJDxHjlcIfDe
zrKC(nT;aAoBM!Q%mWV@ZWP|2RK3kVd#5zY&sZu%BvZi0v$th(yB})fXK7Esks~SwO
znXY^E8He7aRG7>rQ`%<Hm^DeJWR{zxg{UQ2Z+gRtEUOTwD>>5W@dPAEQesVnGS0HH
zrqTq;W?MP!tp?qJKuTe^h25mKZt3_N#ZbhlwlbEsy+b#mC7V<#_rZX$8>CdEoozdP
z7DX$b2&Vl>7wKlgb%~Fug^cn}+*VTgqolT_P1Y4&rj$@ARJmd!;EsBnGQGp8aLbH2
ziPxPjCav+R!<M!?+94_@tLb72aja#sF;;CtsflE$j!M>OXy{l{rExn%26rXlaE8JL
z11r&JqaCw7NF{w_tWfo*tYTW7YZR;rLpxuv#GP_|QLL9~piX_pOibhJl;j0PzGz~V
z8oes)4r!F(fQn(`E)%Vj)PoJ1#isXkDrCv;@6;P<c_MGtm6WlZJxXWkfT7Nqlo^>%
z*(hsenNFiY>XU`Mi78suZL!m5YJ|#qOP$uJv=T$bSZ}a0H5K(o<(YuQElxXIw6`U;
zN@CJlSz2o_Drea1GgX}4SjNNHNIk6)Tf7njYn5fSDMlvp)Du;k#n}wjbdq2=qmQ);
zwnoUTSGw#p+3*`ZUWd&Yt2&#BgjDB_2I@YhZVHR?8IdemZnhQ8Xw<6|d+CBiQ!l7p
z>QXB2rxG!hC@doN_PAH)q(pX`wGmQ_W45r|WY9&aR??zus*^gm#-H|dlo481kd>9y
zgf&L#^;%KZUDJeZHJ?FG#-jG1kIh7yvX(m+vp5r0Z-vr|X;(Q}4(kgxr^r~LYi3hK
zFK<|j(25cf<J5Fa=9Dh1)JIZTvm~j=qywa<5YaH=s8`mgr(J$)%;1dH0EsG!6-Cq#
z)H>O?C|s(wl6gfzO}90^G_B9m8IdHU35V%=t3~Q4eY!4@rd>seFUPWo-*tgd*yDF7
z4KbVTv>s)%$vcR^#q;NGx?i*5O1M7|?v2Yq{<CvWUZk5pIk0Fk2$Q)mnG2J-FqsRJ
zxiFb8<z(*OG?{ZN0dPkK<MNiw?aRp{If=*wC)GQ1{!{*DC5Brv?x*(Kwt4=fw2$X^
zCR&ZCIjyc6jrn|BoC!FBN>d;aXWL{U8S$0#6}2PfH_0>RTqNnsha%BL*khnY4mDFP
znp-(aR?k_Yi9j_`Wy%WRRdfb<NZ+*Os;Rn4Uv`pI+0u@h^8shr(hhh{g(Q>csGRv+
zs2pP>MSs+rH0r(9tWqtfL~3QLqZS#(@k(52VaQg~Q}OCWor;vKJKCaXg{3>rXjB}o
zWr~KF+gz{%!Jp8olu1>}r4@IStk<G+QZ-YkX^d72QngIfkcum9aYHOJHANPtA(Pfx
z5u3jrZh7RXQk;s!Vy$e@t%;`+HJK``a-@qkSB}zqj9SXr5z(5Gy&W;rOhYBBkutVw
z(JQHNzF@Ny!ePHQ?<yD6wQ#l8QOmRqDwJ@`R55MDWDPo6X`?pBs4Q6!bfimJDOHc!
z3er>*1UgJnqD=T=?FiEpRWu<GfvOFrtWTm20AF266x!N!xGqjN<RY7aG>g)t+i$Jt
zlg?U0;%>MDm836PjcF{j(bB4`YYA^DU5{G*3d)<+*z`t)&1X^9outfMEsKEnMdr%D
zIwMj&>F87<S~lUP{TjK+op1Z?ITM*{Fhx6^&iN#6ecT^5iOPj0Dc7ngZH=<3%>}2<
z9t<<qrcRa0g&bjqa{F_MV1S}65}#VH32Cg1R-;Ne3JO^ur}tFVEjQ`&M(A|S7t{s)
zL9<OQ%~~x@RVkTB)#WL@EGB|_64TncJW(zB+MckX7H+V%vNI`yRwSia+Dn_n4n|fk
zrrl<ft!U{aJ)M}#7clz5zDgloY1AdLfFYxyyn3Cj?XXlxO<q^-#6pH#)J@e@Ax%`L
zDh2(%jDc1vbTW%6R+cgyFO`eZbgP*#+vEO1J*dw4A~mP3tju)c?L=E7wKR%l+7|X&
z3L0^T@@CkqT*Jm2R&OgH(L~rXt<H!1=D1WJt~O-~H&v?!n2b2mB8%!mJz}Lbk&4k=
z6I*=dhE$WvN`qpl+N@S<lg$X_Ei{brhFE1R`jcj0>QfedK2=SMRqa|Oka0TW<%V8b
zZU?N*kk62JrHX-2L<Fq1GVZt9Q#w`0AuVXMk!CQh1aX_l8gwRTgCQ6bi8HEPMyJmE
z!fZg}mFZOCw#uW)NJA2XtfIFC^%=i~Eo6#8XSpFVDZ{=_ISOKtyw6usP$^qF;&t0}
z_LeA`2|ELlnynnRt4nks9TYpOI=WmjCL<0ypYpoov_`9Un3Ls@%wl&pO**5>DNh?2
z3l$3%<7IC)5VTT>V#%eEgxYFPE8CERh^J)Kq^&KPH{|nl0*Xw*1kw~gscTa)oxfm<
z8IlglU5JFdA+^R*uIoj{sLv|N>S2&t9rduyY_X<nd7D)gSH!%*QbnZ?cq|T^gAr#7
z4P~KjuocU(f<4R{6y9L9UR5Lvev7Q`2-9SVF@_}Kh{hi-h~nOoQ)$-Z1JP7UE^Rqu
z4QWD`S67OOhRNU3HC^szlGIhShD<q-@=<L~$JR2m6;wH&Yq+x!b6Dr~tIR&eR&*9k
z;*u*NQ{{rb8e6ue*{HNrk|mwyxZh?fNW}#o<!T!1tRoYs`n;(`4rCRsf+wwoE}{vW
z6XBH6CZd@_)InxURe2>^3dlN*h}l?W?RHV2)o{D)4HfHZl0Lx5rc^iyvYV>STW+Y`
zwMs>o5$Qk*l#I746_10CQVGTuZ`loY#?#D*1CgXU?rNqAPJ>&M^J>+#RxD{SYMU|W
zej>(JDYGV<zNV^6Dj8C5%$HT+n5UpkgbF%;yRBBG4S5NjhdkLdAl6D2vN=zw6>LgK
z+GAC=8;W|_Z1ju76_F>Xl?OGX!(j<(Rppq+X>#P9o>aIJGUsBpj9HO&kc=UtvVic{
znpP{)UZujFU}}|;tDS7Pnkt=C+%8x2^<Z4?R46<?x>YR)J@!<rsn5#mS*^2d)CB^h
z)mZm+{7$!#mWCxdds&jM=;BOCC6nr7#zxB7B)tZOKUz*2O+{-->-AcaGOI2pQ#TB`
zCL3#O=%g{-Hkq;|UrJ0$Q}v8IZVt-jMk>t)bn%?I45DmZ$*Q6fHi@NdPq)jCh|O3i
zN>o(ZZKtydlP;3ViQ-~L7iH27E$y|~)G8(pt(7sf#i0lj4Qdn+hu2yW$;D}@HdPQY
zG-UyqYAa}Oc@$QELoa4%ZzH1#RvRso!9juq+@tff-T7*qk=Tl4lB8-8T`L$Ai3}==
zkvpSBiOyT9MUo^dSI1euwWck#{Y{U_$Jk9~Z`;<$#$*<kG30BRtSLjStnjHxU(26N
zcXUi!6j%6s9=}Ad4>BfC#MLZBDW}E^LO@m&w<Kt>wv;T|DWzWO6_c(^T4qtEOvwxl
zJ);>@>y&iGNSTam$Solqj8>H?Hqs$Y$l%p_{5q?SwB{L=N=~x*oL16NHX%+|!|wFB
z+~T;Ql}sC4x^yOysk35lN9L|4ifV6MmMXb|u1-c&DuWC=m{gYxRMX~{rHqu!W0D3l
zx^`XB_L&`Vzl4_L#M(|s8xYyrfnq?LHQVbFYcwOTb($)bR+RIpK~nA$CCc`E+3ij@
z(un{YRJH;Ie^DBaCBiaYDB$z8bdI3Tn~pkCMUO^NwMWV-zg(GXYaE(Fg>u=W>Ws*r
zc6(i7__R!B5~Xy$)h?#Yv9Q-GmXwtdXVxxhCp<B)!<V5X?qFKN=ILY+<fBa9Mme&%
zfWqkSw5*B(Rd=LABEPBAu2l_nz1WpxoG`S?31xv+M;jq)LTOaQRavQAQDK6PSRvww
zXe1h`L|jQp+Iev~nU!nvNw3bUF_Ud?Q^rX0p*ZS#Mv?6$``7^*t`A0bV43SR_nX2G
zR4K9m8s70O!VqkY$OXxvAUPByhl1o#kQ@q<!=;oQUb~4@ShL|VxISsANrfpkI`cE8
z++*rz3nvn0$<r2%T#y?IazjCGD98;3xuGC86y%12+)$7k3Ub3ODL4EJu95D#H5*p+
zR`wgW?9wDW6Q$kTQ{pSDZV`)b{?c`4tu21<=f@n+{^5JC#CI`V^u}q2etG{(-*N7R
zIpbH5@qTyhI^OULI9A~<BP|!5tTHT<uC0RwO?9#msMS+6v(5*Xbgdm^m;$>lNSE1l
zxc+U}{?CI>lN`L`^NDZo{l{nb`Qn?F^Pl?e#@)Yh<ewjY{U7&9N^hv{xc*l^`NMY~
zACz<42Z<trb-L{34naC;br{w#!&0o%q^fnW0$`owq}2x4+qqt)PAgIMT9quEwAx!w
z6lhS9K^gK?>9mAIo>r#SX{j`&lz?shj#v$+4JkdIsnl2WZqhT?|Mjo>cEOr&emyw1
z+OK~4$5-!@l3x7mZ!Y@PN2CWF{G|o1znt^B>^ZFK*|T3{4BthzgtT}I#w)OY0{bVh
ze**g_uzv#kC$N74`zNq}0{gcm*}oM$4@Ioma0FZ<!)07%+wJVe0-sc)GHsyCoeQS-
zw^U?;^@m{nAy|J1)*piPhhY7&l-3{5p<uG>)@=A2_<DdPT{kws(qfK#BJIuz{d!+5
zd{x)*9Bs+C1<{`%`V&Ndg6K~W{RyH!LG&kx{shsVAo|;qqCY%-hj+)r#rpuZu;s_>
zn*W7uO54-?Z`#au;hO)!`PowO3go{){tM*4K>iElzd-&A<i9}v3*^5*{%=Y0pC|p%
zKdJ}Dt#bYgT5C^t<Gqu+GTqZgio|L-Z3|B#q2PR~)x-%zt9x$o8nE1UZTb{#748S%
zPJ`i2{u<?b8mzFZVCBpLq`}sCH4%oiSU6E^>zqtD^W47WcxLhRQY7I_KRltGARgTg
z+62$wL#ueSl_!Ypp?z3bbHSaEy&T63j#%tnEckdv@$zhw>7{7FS?r4KrZjj8W3gMB
zjmr=NOm7=Ju^i7=<@y~#JhB~>iH7qlHPGj9{QP`7C=Z@bRwDAyQf0sn9ClldxSH=}
zJa%KDG==S^l^QULa5PnW=gDF3dvZJIeORt4P>p!TGM<u6EnD0-_^j+AP{-U{yBzWP
z;4%s4e<xu1>==wbo^RUv=YLN>*>Aq*KkhaCL;L)Vzqepif?{WyVh4MlaQ-`v!0tGF
z&z=7+>;y0D1TX9a|K8mRo&buOg0rTPcf<c8$XzoKGSwk&jGc4pub+GC3+DS?IC<wC
zZqC1pZ;TOp!@mpY?n0OD4!}Q39%>L$?n#kz@WyGx4)7z0Q$WH(5N8oOLIyHr6QPEC
z6Cowmg5^72%i&52KO-T5D_!@Q4z48d(*YO=E&Q9iKLfmd4(wc#BAUe1bUj@TluH$o
zgH&bkl?>ut4R51#IS*G{Sr>MC`C^>@?F4Qt**o8x1)tyVV;n*YaL>YTg0R3f1LuEp
zyMUb9<+3Sgv-QO8#I8N5``oF=aY8r=;Jtkkz%v&T#J(^_RuRkL?7?Nehx&GLF!VB8
zki2EL&zjwca=>xobJ#v<tLT3hi~nEBkvU+AST|@6b-k3mWUy)LsOQZu?^EWMGWP?z
z%^cS{UOZ+zSpvVabTYF^tuRT~>eUMQTB+11S*tTib#SD-PGZnX%x1Gee$Lsax@b0O
zrVA9i{_K9~7|YbFNou@Gf#7R=<Gma2+xVTw&)ay<<DZnxp$Ff!IC+7?&+0`unVlWC
z&^2p4G45v)Or9PI&y~qqcA{J$OC`FLnM2lryBI?b108+Fa-c(25ytIeF5lZrR%>*!
zKtZppQB`QOM1RMeS7(?qti0y&rAR{9-$U5nL)hO#*xy6g-$U5nV=4FdIF}=8;D61A
zE5X$EK)5n42Lfm3B1xBk9MH|599Xm%gvnf(%!SE(3r*(2NwDwjlVCmij6-iyDokdR
zDQ&Z8%$g)qGRsZULe!G1H@)Em98C?Umgh*L#}kkwNr^QP$~eo)no1KWn{DN^w;FT@
z0x5;v7Iu@`x~1c96hjfG+R9kk_72^Eql=|dIAc0hv>T*Uq@8U$eHKM4o(QJ>Nf+s6
z!gUE8Z5%SnJ8@e{<&ToumNr>ec$rc{rBLOHjetAqamw@pF((jnOG(V>HxY9L5rOMt
zk(gUb0p<Yp!URhf=OB68xBrBO-nR?iR&98`wq)FbY2P%{KEbq0FzpgdyWYE|T_c(H
zc!h=CTXJFdF?}?{PH>H;HpXp9Rj1Zi%5}ZS81-2tS$)Ljw>s)!o7rMb+4454Dz1om
zgQbc}AMjWlHU}fl78=Sz-C!%0V+DJdH7LBnYQ3sR82lDl-4UkA5@QTW#1V}@ToA>*
zC8yG?$p@mTlw8_!#v0OuF0ZZ>6AhEUrE9v}%_OO-XbqWiAmyXlnvShyXe+34KG$$(
zBj&Kq>sOh5jIHP_n#3hnLZ-?EeKodhO|wyHrzA@{&2hiYRFH}bKFZZJ)>%g;Q1y9*
zJv)RwJA^$u1k}DIQG2ovwbO8YFrxOQG=mz%c2y~|Fb95czbX7+@8g9Zu|7-AwrJ#n
z=0MOK2%3W}q&W~I$6{KYYZR;rLpxuv#GP_|QLL9~QW2NWn2Bk8osztu$QMnlQlnRe
z-64%K98fWA+-0J5l6tUVv)J^WPK7M_{hfLvEl=dlx{@-Mvq$MH9Wc}xlQJXoDH~<2
zEYoQ;NPV)9H!(%4x-EA4OpQ=kZ>iH7l~!V?80!sIrlz9)s5}#pxW#E_i}tp}R!K}+
zD@$t)MvxpYrR4b9O{Bt_4UfU~NlQ&COtI19=KQ%@Oz$!EvxU#&v*c-uMlQ&W1-Y@X
z*RQbGudvszu-C63Hx%TCg4|G$8*WLt;a_kggsxk&VZ}=BYDtg!7$BXSgUGOV)CU&{
ze7Eo;IR@uvOT{aY{{s0hkpBYtFOdHN`7e<F0{JhH{{s2HCCUF4{7wh3B=88h#)C7A
zOSh-a9JgxNYx*7!(fKbB3^2(gRUPk<`&%tyf#B~a_^ab=txDFYOnVMI-)?>1I2YUj
zwp3(-^@m{nAy|J1)*r&ULBaZCDXl-A<7B+>zh=YV;2+l!F6p|#0hShX-1uyF&iJGE
z)xuZx4bRb*j9Z`t`zb-HY!OxrzUNj93ay?lW#4nHPN#W+|KycTEzMdjO<~2Tuwpcz
z$P`SlWrv^CwW*lSU$DgtNeAUFL_*$>uwsz4+eL*|!|k#+RIIB>`i%L!DHTqFnRnIZ
zEjQHeTBV}Ph;-U$CmC;7Djo+Nr4o!S-m)9)jHj6q2O>#z+|^7KoCdch=hdoftyt1v
z)HY*<hEc@WDrMGW)7MmWNhL$-jrp=F9P<>kiBLi3Z@1N|v>`8{^A*@zMFV@MBn#P`
zr_>5IB_!>!D%%Z3y=*r6MdFIc6V%Fs8q(pg1huMi%;PjU@=i}GTnU+TF<ZtgjCyUd
z8KJyT?eT_KWi0xWW|6_2vgq@vYErCf*D8UG(-|)}^wM%WU~PtchP*3P41^*gu*Fx#
z{Z@NQr|LMQ1&ub+45pPer$S^6Iuo?P5R8e$8C5Q$Q|EnQHlXp!bSiOM<<VrMA&Eg&
z(c6OhjNif*GR2^?+z^?RVPB^lg&lYDK3_>erEKYl*KO0;TcT(t><mb1wsP36F42K>
zQ0%Pg=yJuFj5z3g%IlKT8m-=8PL@M5i{0Hc>5M9;JZ)qwR4iDGm%Z6Q&`KqWC6`7L
zYO6i1Y(tI;KSkC-GKP%GA`w(Sg6c<5{cK6q&#Fy=0D?FkuHT1(faSaMe9!WW+rEBL
zEWZZtguighufg#O?7zVN3zlCzBPbAp?`=ZRs7a5sVUWQ7ZBOi<LTL7A!1dlf0#4Wh
z_`SUau&@POVoP#;rA-EZ1aS^rXOQdLmQIe^Yx<E>Yx+&PW<8v0^fv`&OJKGHW^3DH
zwgmBS)71*vTONh=Jt_Y6Qn@Hix0(sFJ?<~mgX)|wQgiCc%1kHTPP9c*OQTq(ZDFsa
zpb>W{Z-&jvHEg_L^|k^MO@uAe>U_v=j!X67YE!0gQ?+V<$%rE@vZyZ9BUV}ysTj>Q
zvBhU@NHwXfu*^zWW+g1M5_E0vNo1R;vtn;Y=B_7->TQJU+6J?v^2?REw#K0;R4A7{
zs?Lb~X}8xUR<;6VGLtB!^R0F<WsZftUa_RCj5xD)Nju?*c^$qCC2<GS5;jjKi?EX(
zlebZhY%ZWM`a3PFqCnLh>5#~8>a=TBLtQU+B^f7JR+JOU0<DfVLe?$H2=;0Jh@;>J
z!MLmr`Q8f-D5fS-6ia7H-P2<2{hZ?5PK(`wQ3<@}=)C58VX*B818qM5)W7qauOx_Z
z!cQ<n0w7P&L=EnPa9t!=xR((U04cGWNWn7<Sq*-XAVGRV!J7eiGYLP6sKT8K?rQLy
zAxdzU@A@hDpMW<otOjo}L;-&I{yL&ev<c#-|6C+iOg#N!<v-WCzd#-R)++0lv{xT-
z!MEv?Bzpf#6OdKn-D!?2u+&))er$-Ps@^J-hNHx-L><a7|FSGr9XO$cNvNMfx4y`}
z`ba_;33bRqeN4W^)xxRpRT*lfM64%vCw3(a#K};W5WI^u#lkh&EuFC&dmUN;->r3@
zSRkbuvC#6*-j@A&?{DSho%;!>TdLbu>j;0;n>6aZ)~r%3r$lOHtD_bf#qmm9X<^7#
z(^K*4MV*S2tUKDGXoaOa&S+E|u4RgbnA=>iE5+V~R;5g;QZB8yqh!4nrIV_eLQP||
zT9B$`qJ~slX^R_Tk*O)NFb$cs)`CMO>fx40o+`zuNG#UM2Hl!?Dp8ZE!YW6)XmjN#
zy~n7fj2#iJDcRc*GtD$qvKlF4s}{YI3g-(pOCcQgYxAyhL0t=1YaO*r+n_=Tw@ek&
zMoiYALy$FXd$J}$*02;7o`Qx~(D14iX|Gb@PB67f$<<CaTuqfuDsGo6`g$-fcPbPf
zAKj{!gC2V-*3@U^^{m!eHtGTa(rT>xI)10yNK3;KoxLncS9Ecvq>@SXF=Hd;Y?5As
z!XGUsji#ctr1g3&Ntsoblc^hqT$7EpHFVOLZktTmk}oADrKx&G9ybT&awC;y1G;$5
zTvmlLx{_5zC2SH)*`98f9TA(cP?V^swA)T+6DD0Gl@rCqj4sNg8(P|Hv8h!|yvo`$
zhPF5qVWL5e0^;ymD<Zi#E!Cz9B8H|c?0Z|VD=_w3jj`Y65BU-{d69ryyXg-v(5>UV
zv+1zj<8MkFmJFi<=MIFjCcr2<odIuk8G|kcp1wnss*-L<)E)3kY`Q9|NUD4o;xG9^
zQayao$~7CBm@kBZ_M|BrPd0TrBkO`&wNaN$H7XYTASn%m{Q9stYi)&7&P1ybHK)~e
zBP=9}i!%X7P-zMz;%u8NBqP3ZzM^)d{3dzEoQouV`A{U92zv~)$f0JcMRO}h$?7>v
zG!dvKs!Unot|W8@d1!PIfWQKP<VtNYWqlHLfF|RWM4_!shwI{WLoTuzNV6zSy8YIQ
zKIyDAB<_YgP)Yit)tJUY8!fH6x|Z;k()Fm-ub{kHjZJS<*nAds-AT&K)v^fA&dzF6
zjDdAVq<YfPsYJAF!cF@%a+5pX_S<tNGS^^=b~>H&N!<FlKWq|}3r$k4Ra4p;WmTIC
zPMtj%W~@z}DwPX4!VKm1=MupHMO!33wO$j_SQ)KGm2wmmvOrGnsj6FU(&vqgw6qgt
z5zCbep8z=kN@ccHZamzTh46Brepy2QlcmjlqFVH|Jz+yF++b~GXHo==JSol6UfLvf
zFtTzn?KYciMN22?>BL;VfYBHBRSM}!qb`XB3>gjO)$43+howSl^15;-7Bb|bZmO;d
zX`(t+Dd_iQ475_AlUYo$vXtovETF&w3WtGiou$3P(oTT|+}701+pD@sF!Ghb3Q$4u
zDsWwb;$>MWUQ(b5L@p`=deuqo^z+w;92XoR-aW3~xJG%%BhP*ILywv)R~gSg<Iu+*
z-f-xS^+DR{x#-W!pwC4e?*wuA*wX8K*{+5jb=DNrL%YXMe&Qg>RlB}Gti0*-m!|I_
zs&}iCFTI;)-?YCa-v2Le{H$o?>R_R^1LrQV{5LJ|`y8|b+9<4!ocW2{KKj1KgJZXu
zU(q~$--f6Dy?5@etB=|1>uayMM}FR&gIfHpJN9(;+fV3-bhi$xUCRp2wj$l_U$ve^
zg=B_BMZZ>k9Bcuw6|>%3Z6066A~q$PDH2?^s~@`}4mX?LW4@y~O0$Mclz##0(xo
zvR^;o`@QFc;P;Ly2EVeCCUgAfZvDjA#DV7E_y6zk8+SbZ<Uze^ZZ;h=ao60WS@Uau
zTen}<y8V;{YYMV!H-uI=oL$A_)kLo!r@GcdRj>|Xp)d2J?wvn=@V4_RXQV#n{^n~t
z-tjf!)hmw}bAP<_<U{_;+V|}wt`4f7srE7tEwKM%3Jbx&Gwq*>fKPa9D3s6@y7y?y
zV}aViIt#(*o*>LnZ)k5dK^P$%@kEJ0i>k@mdG3veMEf82!oM2S>*LR~?!EONuOIxU
z3vW5<@`?Rl{MkD7&T`EA;<jI)D^En5pb)gBpYTAQbzm2UAt-4?yfoR~lR33r{PkDQ
zd|;<P9A-TCwTmyk{>`0#n7iOhKfLm^ul)I;L%w_a_kMyooo3ASE@#5EHEfy&k4<iz
z2#Y+n*w$Kt;e}v$AsAjTw6~Gb1gnjf$>1OxrnpDvX}j~)I3uwY$s|eDBDz*EC=wY|
z6eD*=ixQo;REs1@R<4e-erruzYWtfWlaH~R%-*)Gk&VeLE@Q~oGFelGT3O*!lfITe
zneOPAwkWRf`8<A!ULRyko`|bih*D0CS*g*pqPQhNi?yX>(M~D#Qm>eFWzsT>GG$6;
zXxM788B^<&bj3)SjBLm)AsviXl_@sTAx+5O)q4CotB$nh8I?*-viY1=(or@cPFKV3
z^tjyOxS^Fy8(g|{CLtJJY&F9RoK|@^$jEz%ZHfD}eB(gdOxi*BKUrG+-{x(vkw2^h
zt%kJOc-L~6;{e8qQ&+BEsaR=PdCE%XO4$$&4Vz~+o9*ahyLMY}zV`QzERg=4AN@F+
z{r_<Km**O2pdkxDOM0PZQ3^6iM<#cB{*s;9Rj01IBE0Ga<?sIW)>(w<OAoSl-1gE<
zr?0)_mQP|;O{F96TVVNrzQFGnqyG9w1NJZyH~|*^lTb&vo&Y6N$Vs4dW^x4pul=ug
z#}&_UaHHvH)4uzB<4xA}@&*68jsEam#~yg~FGRO1FS=J^IM{!>{Kac#*5kYHPM81u
z`qcNIE86>fYv;6!KlR90q^GZY@v(2;b-_7bZiw%ED`3A`$;ELD9OuBmFW<-CttP^-
zenbN6N7i-!arMc8D<vyWhLIwH5py#9OYw7XcQ1(hTKM`EVg1K!W4Rw^_e-jl7H5yT
z5@}fO``3R6_CK6tVoR(gnu^!@TciKUw$qnzeH&uiZ6`-l$L8efF<g_^zsAf9(xh>i
zPf<35GXd5$1z|M!DAngz-OWulIErkB!{P*S+7a6QqLTIQN$Jp}3~=&lrZlifvNfvu
zoR0r~s&sGj(%toA1hFS%VvJZpTn>K!+Q4JSH@_rT(Ep4>U&0ld)8~Id|0C#sw*B?L
zo5v7~y<S$(|BMqnCpT3eFp1|UuV!L=LH{G@e~^dYHuOJ&{AaQEe-Pw9bKO6qpBob7
zKU;Uti+A6h+|^`0{NGIde?OnwFEg1gbwJLk)#Y3W;9ZCw|1{VAS62P*suTDA$Rz)S
z_7z)G{?@k@%P8#D>J-i9SJOo@LyZ?nx`c<WpT(_0>|dZbbE)DvXKR!v$&_cCaTA!n
zrHl)ezn%guaP=a}Am?KPHoFZGK^zKsd~y-<IA1%j#m7?wp@p*jW)aJV^4!rZ@Az7u
zDq|T&*>2u4CJ7ugO6wC{X$x=RL=Xo;32$4(66%0kN4ufT?a>q@OklMW1kHg9?0AAe
zvwH|PNQqD$gd9JH_ggCQ@p95Hf)GKz?_9)u&$rk0=W?nRvHuCL4dHW-TD51lx!f10
z)cD+O!(`CvliD%@?g-++B`M|R#|dXIhC1I8>>`G9D`)*T0k&>5^uYpzqK0U4q^yI|
zScjH|g}NJE4@(e7Lu`)>jcx8bYApn_PsK6){dTq<J-JI&cgHjWRc^N|Vu`lwh~!5i
zp7K^(#9U98R!%@a8f3!{B-J)jUsLDJe)_Qg3144(L9V~Li1jt!R_@OmXZL`XtR&Pn
zpVMO@r}r;nPPg_5<I8a{l;b;#SdP(ad{itt*y@%bPJ)~t1NOrx1x)~UticagGfaTb
z&GLL1(OCH2ZMuv>SoeS!j|Crhi%h{)|LmY%<le@$(sH5yw|^RWd**PKEB*i8prDsG
zm$UuRxQojJY+wA5K>rK$e-bFYNjp$nSGMJ?w~#X1p6UP5sp#d)DG2ocLbU}mwLMlC
z7swM&fP|H^wwnMhYf=(CX{E<=VrZ?l)K<?}j^WmX{Su1FxA^?;1o=Op$8%2SGS|Sf
z#pF~*=cH$ouq{;T+RuebpoB{^j;5Ays&u9UJTuB!Ga&J=LytrW0P@o`Tut3UfFO>8
z5<Ijdl7OitnCw~8ty%l~A6L$LZ$$N5z4HBWHz-eOPUZ2yDrT%B7pzl2`75r&#`76Q
zKD&b0AJSYJu3?zOXwb_0VN#<LAom;QRAXC*!WeDc3*YXz>+sUh_M`S=&MKSJ3*vew
zu01&Y$5gv)0#5%}Tv_UTG3T)UHL3rcZ65%Fct4b5F{K^zEyq+H3)cK>`>Jr~;%p1E
z^3;6yW5WI?Gi^z5`RD&E$(WvM56i-wGj%T|f;bHF_P~<lZEhpg3%z9a^%DfK2b5(o
z?L{KpnGJVQ!r7bK3#8HGwaS~gyxC9b!L_LgLa|9NgJ(T$3mT;U9*g>k63nv_&|)e#
z<x~YZeRLb>8B=pwgnaVrDW~q>!ehtbD_9B8#Sp~Lmr<F#U0Mh^mKyB+HFYl<^ArsX
zuGw6gam?2{KZo-B4NcBUbK9?mfYGv46x^sdsUP=Sy0VwVrg5`!$y#D+?x&8ML>HGK
zrK1B{r`)BhaTPP}XBOycB+Ec$<26N8HkD;>D9as-I9?V5qmFon9kDOueOurLM{fs?
z8clAh{e7SW=PzRIk4~>mwYDXQogpVzEJ;qL_UY;JWuD(S5%RY*Jm1v(O>OH@d+I@u
z`^D50jo!{vb3Y7|58_{E+K%;0Vvd3mZ42}4uo7*4ZW?AWdH|GcY54PDWf{~uo3q!@
zKq;1nT9`|ZTj*)0g8UcpC66Of{<{>ixb01zxh>EX3zIN!|5|{hp#|oGp=Xo-ZeNnq
zrI{_K&NEZT%c#3J@t6~(*^6mQyV&&EBsC+*f0zFJ?d2!`T@EtSZGl=Ht`!}1j~`A(
zy*)}*myb+)Ti{El$_a`?W!iI-|1J&HGj*mNK1Q}pndvZ^ZE{YXkMw#u%S^WgOgnXa
z%wKlBE%0iimqn2OqQqx4=vycEeVB?}1o`ilq;H4e!a)+o&C7om&i56->Wfc%s&=to
z*1QXFXU}Z|x{0adeE6K+-=in`&K>dgG;z3p0I!*IgY<544Yn9cy6{?E_?|+;`pNXs
ziK~CnwuhWjiX)*E_bo|#4a+y*$NGEQ?EU+AD8rAIqzwJNXEr-wTzB07Ogd@f%<-jY
z5rlR$yli6?lw@gatA@44B;_@1mxBH=u@>^bEy&9jr!<E^Y5YU`6{op3_Qq6y>*M)$
z6yS|AV9XCgyR%?Jp#XakzLi%)y#G5i-mSae!f06HaLDntz^QG%UFXUlz<yH1Cl|3T
zMwcc|uX(<Az&5?xq?^x7HLSe`;rLOsq|3Vx&ZfoQ7WllmwAL_<i9r96V81yI*q#A<
zuC1~9a9FEN$1ei?H{0nli?QBqxU@~6|F(@WF*j-!>A&Sir)?>Ub$QRd!{#f2{u`Cp
F{Qt;LfGq$3
diff --git a/src/azer_robocup_project/.vs/Tomsk2022 disk image experimental/v16/.suo b/src/azer_robocup_project/.vs/Tomsk2022 disk image experimental/v16/.suo
deleted file mode 100755
index b2bdd0ffbd7977d118fcb479aad68a5f912f278f..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 102400
zcmeHw33yx8mH#twApy#kG(e!mq^wOGYqMk}Aw;|7MV2MYOHeV|EXi7}#TyBQmX^|)
zuv5xX_9>+doi+odg)Zr|WtdKZ4ATuhmOx9Fnfai@e3Z8Tw)}tRJw5wLk?kj0wiSA>
zzN2^Feed1v-Fwct=bm%kfoD%Vc+Xciyvmh?9o#DJukWtmj+CSyfj9vlPT;ts5m^QJ
z>$~s1OKB8834l`;4wD{maVh+AToY&Ek~n6#DSDN%!+P$0Gw-``fu}C{?c2XueemZE
z&A9J!y>PP=C8oGh&W_SY5VLTT3*=IKc|As?w5yA+mOuLrk}-VwAeK+%rnxw3kl{iA
zH~PW{2o>&|>u^>T<)4Vmj&2Zg?BVh#CC)|BeuA*1WHVASa}``jNBIpj4{2UfKP+%N
zf4LL?Zyhh(N%Q+Jk`AP~Oz)BUN0M9=-GpoJ2iy-hU2-2~c^=?>fYSiy0@eYJ1FQ#}
z3fKUUX7(V?KMbHcd>+RiNUl2(Un@C3xe(uiV+(-Vp#-P^YJdjN3eW;{fHnXPv#o$_
zfbD<_06PE|0(Jrn03*NzFas<AE5HV52iO4}0P2_93P%TyJpdZ1lwQw)X7e=QQowG&
z9>8UQj{z<REa&{+|DV5S^B;X)PxD_KyQ@(znuF5uEaCy0zefRR4j%)M&i`X^F3w+Z
z{!^aP`7f5S2KQ22I>%}Li`QxXi*Y)#`7iB_>f#H{|LSsOPrX-q;0nw+n)9E)@k+oa
z3+GoAj#n3scj5T=fW3fg0M`QOx$AMf0dOPWrb60hal8faIl!%jwA*pK13+{9&O+MV
zh2y<Aei3j#;2!{A06YMo{0TpN3Gfi$9{~>oz6|&$z*hiY1MCC*Gl1@Y6vzF5#{gdk
zd;{<};0eGt0p9{V37~rkzFUYRS`_{j=}(op_B7I-0Xzpd0Qe7pm{(B-^PIelXI=sP
z67VYEHNY<bl+Wvh<2;Yo^pX)AZ;Fd?32?g^9Od%8s3spy2mjfCn26g8$@3nFpE|E_
zM<2?~qTDX<x><0-ZZ5*};5jb9bH>?ubK*GSe^=q269CjP3*63Mw&S1Z9R=e1iObJ`
z_x6Dgx1t_#oM$<4We`G5a^MeBxP$Jb|Jhn?1?T+%<g)=Fw|u%cawPr`%*dBonkeKv
zz^?*=NS#0pQYa~k@}hj{v!x2p(!T=jr?#vG$SuDc_x4<XKLj@ag?UGPCk<@=3*%#<
ze>VRQ@A=;c+>!>SAq+^khHx<9xB&jmfN`v_6+FjZek6RP3i$`oIWadkN(vNk?KsL0
z@-^84I*<Y$CfSI9`?i2Wq=2D=JSNWbSeWwIQAPO*JSXD+O3Einm4mb(gL+U8MUc<V
zD#|CBMFIVD-B^KpxTZte+a#exxP8uWP8H>oyo7i)3L@`9G*paBk3`p{=M<;)Ag%Sp
z^pg-L`a`lFqJ?7mDTtq1IH&dC>3|OcJ_I-e&;&RWuo3WKz*&H^0p|eD1)K*sAMg<X
ztvNRXngLq?EdVn>EK7|z$$#klSsbPEA4=PXwCw;QdpiJ>wi8DKzz8q_D9wVS6<{lz
zQ<?}}NF&<m0Q3NyfL?$L;0E*q#Cr!29|Q~mb^$H|cmW>;TnrcnTmtX`{D1%;2nYd2
z0AT=)uP9&?5Cg;k2|yB%0;B<BfD9lD$N}<zalk!*Nx-K8(||pI-GHwGJ_Yy~;Bo-X
zaX+34<3~7v_@5+dLh~<=bHW4j1&qH28V~OWpsQx)^?c79jf*r^D9l~{6x!<Nv4C)A
z`F|>(%BArnw|sg>Vs547ccZ-tluB}NDKAF-TP|#_V*ag0Mtz0MO3Nqg62hp6;q9jQ
zcR}2yv?j&1(4hS^Jbim#SGI||8E2MVR-51?$~y(6f_Tx}Mf6#kp4Xg~4lbG`ra
z?U$BV96t~3KM_fUL*%xf#_DZ_Y)act>j4_`=^4s1e=k&ne~(4kbM?Qt9-%oH0xVP?
zE!t1-lloI``;U>7Ki~dCKWM)en6wr5&lRZu&&5T;+j7gF!o5c}2sz3rUn|^N_8;ZX
zjD5M~Uxu81Rme?F`TDXY3yIWzqESSj<d#ok@1zRauMvtW`(I7_KaQOKT*ysM`_*Mj
z780rb)b8_eD!2Vxac?E>pZJxz0+s(hTx<f!E&oE?TS@;basOPQn*OKpe4vowMVR41
z;Q1U+b3Tf8bO8espGiBi!WBv*DFI<-fg7H!O<NV?|J!(+@FfK|a!i05q1<T~(hvHx
z1sq2fB__F7P3qr$s`K_Q`z{JSc|(8m!TW#sR&@3G9lKoVwL1^qeAD+S2kLcc*#5#V
z|9Fe_+rQp=)T)1q@$~2j+(pB^k!~*uFE*I4Kj#GxoCUt4HHa5A@q#WD%S9;N#`jY@
z_!2+PXy=OfgBYLc1w~DvB_vByVa1}tuZizJN@?O->En5vCX_>ay;K^tU6ANd>%BbJ
zL+cT`=AG$v>X)pf_r2i!thnw(T84io0X!WlaO{FKX$wl2;_kTq9p4!@4Z6R6Vf)ip
z{BZB+MWgrMV12&px`|cO$9((!qi<1XNc)cB+CA4cn71nMyVRhvT67wVve~H9sGF5a
zv!dBxQ5u>R3WLI=S6Ho9(@gVe$k4)siVvgcVKkmdcob(zzCl8jZrkSE_~u0q+`RX#
z?KkfK+4-gGEJ4mS!aqj*U%4?SeTCqE8cve5LV9hk_{q5Ng~G)iyu(p&)nSxXDr4*C
zcW~#*MM&R>@`;`*0MvW)-z{8Sj{}WK3MRZcnr~Tf?@@Hq#38j1?wjksn*IM`{*%VJ
zl)pPv>gPheh$khD1V2C0W^04_-#}}tBLQ;TPknVq2~mG^@934AM{X3+(rf_>#1g)2
zE|mF~tEzqeCgguSUq%DJ|8TY^8_GB{sgY<rbhg)=3gi=^WG?HqMRV<Wzt@%Wr()5Z
zw=b2*##9QGswo)F#+srDUpUkhnoNf>QM%t3_a=PN<d*c*?n@PgfLWtZl>CX;_Uu-+
zsy1tsyRE)>HZ+^ss@H5*2#I&0lEsw^Qso|5B>uW>k<Tv{|6PS<(YPrWK8<Tv13m-z
zJHX!q_5!W}Tno4ka6RA#z>NTUmp21G3%CXFIl!#|8t-@DNM#VOekXvs^KQUBfO`S=
z0caibMS!#}-@|!VNt*O}E#n_7l=CExP0LhfE$d%RUN5ik_*)G96?m37NL?D3F@qz~
ztvF8tiHji$eAlrbe&^d)zjfcvC(Y}Aetql2)3@Gy;i|{qe)>~B|K?Zv2mTMQxoEsm
zAHLT2@z;KN*+}p=n%~~5Jn`LA-n#!|C!QF&Uf=nN{cn8n^x`M#i+>?4I^re89`W%U
z5%D#|>Cn*wzR3mYl?{@wKBUao|D`O2(4&6{?EK4%t5!wL-akBiUCTSab>Fb=;h$V{
z$~iCHyZJ{OBDSv=_m40p6$)TjW!q+T0hu=7CV>J!S;ynAo2R(~$L-~a#}hb(Vel(N
zjYY{cjk5>M7T4gK@9X~jl`U5c=N@@x!<#o;{<B*jQ%`>UGs?%F=;4>JrStPYZVELf
z@va1kPrNW<f3!38wU;9so;tH--{i)-?XO%g@z}{fe&>miy_Y}qz~`?#=E8FQOPs->
z{HwH<bJbszSFS>~VqAJ$Qzo6{eMGax`-uNr3m_T92PEl)&rdC!Gx_h~A^#OPQJVRJ
zCaeKiG|i890kcr5G(i4Tgg>R%=86-qDy{|H;B5(alf;q6E0s+AF0H|6r@a@eJz=Fr
ziy2W%U^Zvse`#KQ7)f-Ty)t)<#;tf&%6WIACZq)*NW!;ZeMhozT08}@wxhKWVau%e
zD1RjR8<i`DqKk8!ki3G{EEMJ|zn^b;4tSdQezEQJL=Zg_;cJ5Zrv{p1m6T6?Pji)m
z9QltDYw5WH_W$_yH{%AX!(8`C(}|z_heCq9^3~GEi{jPnKVFI)8i@oU$Sc2Ou_Qdo
z|5N)nBh~_t+x`jM`&0gD+B0%voLMmDtvDL6`s~EN4JmFO+|WSo;dh)050o6Je5xI-
zspXdM!M&pVd9mw<11n6wpIPDZ=i{H`(BH4DaQ)f-zkL0Jdfor0HFYKXuY}EM-A<u&
z|Et>l-!qVxT>P^X`@d8+1-?-W{7LD5Y5TiDa}zx8^S818J0Jfs!0!SlL4QbEF7QF6
zU8|~s|D%;(x{%R)<(t6!37lvG6p`%2@_5cs#D_uur-Ak}KLn8b{)zO<;lHHi)9wOk
zB+;rbE#U#7|H<<|qU@;9tOXBZ`9%L`$Dje6oe%j7dkF0)k=jo@a*i*V?86S>Kl!y7
z$+YFRza96A^6%NQ1+MpS-2bO`Z>*sGZ2y0j|10KSDwiKkU_Ox4tytnTyMriL4IKnu
zd${iM$&oS&{rPegt)y{FvJ{e`xKn{ZDC5oMGQM0WJcZqhRjbx><u*48cXp?8_copK
z*W6>X`!B<pkS|`rzDM!<Co2G2^9qu_hwUA0-#g>m?~6CC-tzdq8((-<cf)gUPv7{D
zPhI}>Ywx~FH5PCV%|9`aqz{{wn_E@Fw#k*ar_8nqzo!$9<{~S-ekg7~?S{(P|CP!D
z#l3k^es|yCuXVZ4w>3piz3-AYZ%Lf9cJ<%A<GRfF`tA={4aqC7-dC>I1bzQf`GcJG
z|6=9O%f0_YqlWGOh_hwokHzDkq+EjbcLW$+P$(Eh3R%DiI*|fq*#6b<y4QcY?iZOq
zUG>M`UbU^|OJ_X4CH?kWlgFL@;+<dq<l2kNjd4m70`VTSS4d&wpKg5Nz}K^<pEi2U
zzwiIvi@!Mg3$N%Xy|icM+5xWZ&ad40FX(>|WhlkJrTb3;WvDOuC#CY#*A%MVzhCVB
zKdnQF{*i{z75Fcv70b8(uMqe7#m#>l_v~i-|Fz!#KNIh`Uh)4~s2ky(zoq^EV*Jne
zzrQv9ulW5-DFf}wEv!Gl)_=77fB3HdmYe^ly~=vU|KCU5egGi+|F3`t0i}HCd+l7R
z3pBqjxZwMRtk8rtfZ*qeSJUJ*A!nBKP}9s5)HHJos%ai7^N_2tsnaPpt7J+U=C0Dd
z72ba<#=nQIzeuh&h1C~%MG)mOXAVu>x&Eb#Po6$FG`ZOQ+kVKCMcFoK(@*l!<_v5I
zqgeHYFazcfq<>9wg7`W*&VRRXwGRi<L7*US|D9hF3J;Y0i~0u|q{}V;5!@^8zsoCM
zUsCGqX*K!>v|s;Ug)HQhuj0L&&6X*RQTa+FP}?a;ugw)dhzox$Tr3`c+Z%T@Y8p+A
z+ZwwXm4$h}U489CZ+!3TPq)85(DI$<E;jW}|FFg3dU|uqH#Qu2?d5a$h6rWP{>!(&
zNH0<_{9Ikl--Y=n!P=$|^O+<qS$<x}vFnpXZ;MtpuA%m^jQzP^-=n>|XQ$=DpP%;A
z{oif8CZRayiNEZb*t6#xdUdmXEyVu((cPC$-fj9!$6Kz;TF&;KAAEexgGX%o#1D@6
z@=N*Ob-cps^_M<FiuA?r|4{tnL#>$nr@Z~odqMv59?<{b?O#M4xB^<6_n^R4QU9q1
z{vR4>@3#{DE5g^L14*F<{vUYz=SuZIjw6L%1Ud9)Zt6=u8jffnxd|a9!Y`EPLg7B#
zP>KFs+dLVH_NO9lo9EV<OJujQj+6ip<g}kWpv)CS{-cue$(CkyMdj0oDIci(kKoc4
zfZYDS8TVFZ|EelqI8w#-GyPBb{Xc<z6Zy|t>3@C*dCGnNQvFZTLn#bJ#3~PqeLjsM
zseHo6Z=Nvvt)$s_{l(U8#)flMr|Hl6v3d;gSHE(8-&KG3+EYoycRYCQ8n^Z@6BPg0
z70Sf3=l*i=;)k~xU%adPqb++MKhuZwH_y7|f-}|!LeK2oX5=n@;gbg*-M5dAqYP>M
z_9L(R)w)9Yvwe`>DdvMq3x-46pWeCPLnSxNmR)i|#_3$oiuyg$EXt+P^>XQpNvkAJ
zY5l}_tGZdQFc_LudaFTWR#{qg3hkcT${R&Dlo`Dy^`70^yQA5F71D|9j@{W*JfDlE
zl9^E4my3>vw%U`?+;G~L@g=h2I<(ql2=zbNLst9zQdqGI{YblNRJ(E`L`-iid$kf9
zd@4oUzeDs3l0rL3uCO?Z{4KD@&O!o0^{rucga9eab_FQA^w&IdHh)a|48<vwdOeW#
zyEw}D8yEPKK1cE5nOOQ(PsAPF(2Eo_6Wx#$2|fu(Q&RL`NnWp3@n2`y#{0;+6cz?5
z*c({j+lB1F{~vh4v;1v=KwC(yYYqf8x@L_opl{X(wEE`Owm>MTQ7e=|t#&mxCEqi_
zLX%K5(B6G%8y`%XX0_I;Z`Nqa&i^aR&;PPc5f&=CjL=bLEpDoPEuh2&+~f_!ec9}=
za5AIBC9_tQIE;|lH)U!?T4gdTbmQ`LQ#P8Ypi)cl?z&O|Up$%xRhfB%d{PCq`cw_O
z%9;r!1Ce3q4~~Wcxm1Q!2<5)APcB70hSO8INQ(EvAampZ<MIB42}wqGt{l2KmNT}v
z`?e}oZRChWsniBlD!r;12r}5LQ3is|K7~%#9P;}DzJMkG+^N}9ctBs_18UyYO9dsU
zm9k4P*U4L5J2Nt8M?{0{#6PSNm2yhlckrj;wJkzqceLNw<C;)(+QKOVg2U~JSUh3G
zjJ<U3F&Tz%Fgc<wg@Y0dgATV#VK-#5nt+zS#g**!D2=#ha&%(6ZK&5ncXb3TkzIia
zgTb8b#;MM12n5H+Z1g~P-tBQ2`^^z&roYeGH|cO$oLZGGq;vNs6joKDv&&=Yx0{@;
z_HN^tCt}n(Iy5ado6R|qvX5*sxpe)LMx!&5G)x*o{U%M!lJ78Td!ve>wv@@%nND@~
zdeW8wLu61J(2e`mgUX(GtH+n0wgg%_x;heS<+wT>9B<7lx?7^zp`q64v@I2}*t2am
zV|Z7-%{)1-%k@TMZHWoL*{>V2rN^{!%UC=x=2NxgCK5f_O~!Db-O?MjOn2&i)7{hE
z+4gYTNH&<y>6~e8DB0dQmCg7@oc3OWGGcVOVxhRF*AVE6bX%hO(Y~(9U~-pAo!%8!
zh2j%E_MrjmWJo>TXUt3_3=_Wgbbr7)rR+)B!);Erz9pcvjHJ|^9b@^iO=+XWYDy;^
zQD?MEZ?f9Pb3vsh<DLq{JIsbQr%HvKmAODyJT%^EPjAw+8pC6Q#<VYxRE&+8J-xPM
zbkL!<>N~V827Aa6R`%Jad_AKneT&0l=oyGO)Iqgx*PyGvb)d~XrWzemWsR1&d&26-
zyY!Q)$=3FPT+HXs?8>CVuE9>T$CCALa!)4Q@vttJwJE#ora_I<Ft{n6_K%F{T6&d<
zM7zh)scmV^sRlYbbDDfQV)VNej=?EauP-|ui}ovBxxUPZU)R?;jtUH<Y%!ms-K@#S
z2W^v%iG(xiclWfbJyCt@guOL07V<?b?y<J6q%Jt&8}CYw4u-V)_P9~m;!#btx(54|
zAxF@!wrJY=P3HV~y4|WANOcT!L@ZOI!S(@1zoT=!quuBan|qz{o?SXkS5~*HGchry
z9q-DTIx`cScIDM^(`a1j8Os~APQ`@H8P$Xab(ZO&NYG)_j<z~d(XJLlhg<1x9qJs?
z*@m({ZO+|ljC*1|>0F;jYq2Zs0Y_Tx>-DD`hKM3$_XHyyQ~t0u6R_+1Rn8H;M`hG>
z_I8@=(QKFB(rZl)Ol!lFx=?qlMbn?OP4%Swo=xVMPVeruw%W%WHqB7CE}n9CrXr&Q
zBk@s7+rUs#-DwYw$GWvWokPZa%x>u(Q^jJVI%Cw{;j-!nf{Ligl8i?aNy|iHwBHlU
zPkLHM!_z^tdNh^7BuS?SQrSLbtS{bSX>lu6I**}EAMRIn=(LVeO`CJT)u9TuyNA>f
z%~)8`?ukuIw;B?GUf-xIpdHWIZLW4-LhlQWE2moYEg9t`hDmIfHEIsE52+lZo9xbT
zpF^u3>5EMC_o^(Zq$b+i9yP1m$EG7rM^9ip6jS>*>DoJ0u7tMRY8cv->Q%Jmodc$X
zYiK;)nhYd6j6EuSM_1N2>hYUH%63DS)|4EwD2KYU)+`|!gC%b5i@Ec?36qJapfsGq
zH54xg_-`%I9p-xBBi7BJdI=NKP(dog_6*xIY|pSg!}bEU7k3H?%hOIH$%M@HAq_c3
zVyf0~(xFn7a%l{`GxW~TJ45dby$k65H3>t%3W9ht<To#YsIrVew^A|lx^gn=hK;kv
z$g@(=VmY>)dl+3{bb-+YMi&@e5a_}^Gjw6|{znlf1ut{mZS);4B?;JVYWU#X)L`Mu
zj0P|oz-R!Y0gMI+G(b5+13t9hRfwBcZcqHEg33<AArXeS8RBM$n;~w7xCO-h88GW&
z_#7hV1I%@)6t`B&zz+)wbq#0E&BV*Uh0z2?6BtckG=b3sfhIg45(I9;{>KaPr4aPj
zKqvV)fhdTo%roNqd9s#s52FZ-A~1@;C<3Dh0!6rF#?))|{&B=f-IMG|u?x(H<xo7;
zzzR&sHFHMTf@wH4VoLnXuxV?Cl^9(!AVxNB27rmv<KN2vOt^awwk3kF(JQt)qwfXe
z&;qhLqp)or`@MRyo0RMQbw2OU+hOx4d@W%IzR7WPUD<^ew@>&w%~N&dKc`#`#qaOY
zHy+6k7J0;?FY(crxyV;}7-NupWc~cO$khKl^rmm~)Fq$rN7d6$AFch|gWqf&{_1y+
z+plQcL{1kD@tVV7?9T7M{)Kn1`{Srtqj>cJ&+qnJ`1*$)Hb4E@HMiQ{xaR}s=O4<X
z4wlQc)Do2L5iLMTy+i8JtJHrmp#-{@+=MU3+~~Q0<G<avn)`C~24JSGUixfr*6XXq
z7kgz|ihZN{`UbXo^M(4>)^(lCs{dP^RJ{f|LEo?6rc_}3_*DJ2q9AaSYsOhGLu;O;
zqTHrDvC=Q4oF$9g%0FH=m7le^ELV3q{*P4P`Q{4b|A)K-RpS5XN4Wntg$(7DKfnJW
z`Z^?imRYX<o0o8}=>MrujYS>Aw#zMlQH7OxcroQ4>c6H;0rMm+_IauQpLZTPaKb|V
ze{Qy<1I+)=><gJ$Le3`fu^G()NqV_7x?C=OF=>_LDXHE}w5<OR=KqIl!ditq^wXEx
zngEmhN;AW!3SUbWz5*_EWT~f^neIh|MhyY;|5NPAXMV3gvsa6sdQbTO(YFOOilAEG
ztPO=iaKhu$HS1N1*5=k=U_=*G_=8HNkNN+pPs5D5Qwy$3Rfi>bcQsh$)QjDIop_1?
zRbu{s=Jznf{Qtny&#WgP=KqKJ|6%@rnExN<{|9d2;H%D{vRZT+i?Z2>@9;KP<sWk~
zg3k&({gn3##I%H%mJrhtVp>8>OGwZXV*Y=aGf(EslR5Kb&OG0f&ODj_pO7yORUO9m
zGq#_x{fzBrY`?(vGygvl89#IM&m8?TNB_*x|KFISf9C&(eW8VYp@n^+g?*vrF#AFa
z^Z&yHI_#@m?5kbut6l7?T^z^!|KKaBOvKN=<H)|_$iCyqzT*hHJ9++p<b1z<vHp7&
z`#kgili~uf$&44g&>U|5e`@0Yr8fRl3f{%bc_~mg>lFq=vr2C@Xv`{0t4^Wa!~DOb
z0{KKJnag^^(Oe|&_hwT5R4khF=2D4lOr=n%nu5`6tSOrCg+ooD$#f_arTcwx?>zo}
zGlDvX+_$g@|1Y)i8Z>uPnE#i`%m8XLiwp0g29}Y`|BH7K-l=A6IOg~NQVz2g+h8uj
zFIa6xRE2cmSkBnu?%S$VwUIT9QmGB9RC-l&NUaObZ&#_(SNMRMx3{UFgi0(o4;KeQ
z<p$8&GJ|iZHiKbVb7ne;Oec})Br=^urjsZLEFig<3GXY!%!f6bR@Pc|SB*)PZ!{Fh
zr7}}kkSt-=)e)<5<hy44pJ^rh!ZQA^#>2Pr1q9>&3<iV4?TJ`CVZ@BR_%nD+h9MkG
zj;Kq))Y_O@8&hjzYHf#^TH6fshX~{U780`0h$`xt|FeZtM4TA0GEa`{a=p=5TVld*
z_Undh=`n5GG8PYv`BaQpF=EAt)nP`g%KA+?j5m#s$>uV?TqsOpgu&V@@d|r&%aL()
z(g{YL+HA(~u6)~)Q76{JOWng=k#0*=Kibzd8BFd{snfgSs!)8Q$38S*oeZg``;3{1
zgki$hp6(A=r<6S@d$`T1*0%(dmXVaYvtukjwkd72SWW4qBkGKH=}lJKcrK{aWZYAM
zc!$~0=2WSWvogoDE17mB)2_tQRM@Ow^k1$T7^D9NLwDZoaT)u~5oe~q&)GNWa9NyM
zl`f=n_a+opRid-YW9he>oUQh5<CrI6)H*seEjF9YIgzrDY%;lY{gXzcGm<n+8bbXh
zP0W(-Flu|FilMfY$<~=>y8nziG3vyq(+qVg>-KF0KHkT;)6SeGpN<&)ZiQoTO4aMj
zj>n??N>{EgGve3vb&d~290Ms^%;#t~Yx40y+oWS6;f(s-J?&~wRNp#bZ_SK_d=ZO#
ztgS1l3r_gPyV9eBA+5eWZdA5-R8y_4!G2}P5oGh9&HuWa|4U^Tur3hyWwXP=7iNbG
zMg@_Yimm4PdhN0m#Kc-mLzHQVG7V9tAu0%6%6hF_gpJrz6O0OM#EzFzG&~nrhPo|B
z8H|K662?dvBVmk$2_&qn)6GQ~L7G-V3I3>p(PVc`ORsHijPNkR!w3%}JdE%Ngr}_2
z(5D*A)qd7-sn;jN*=VAI_0wAE`l;}2c$w<BT;(v5#z-0?X^f;Xk|vO}2e98Igbn*2
zFT~6%u*+R`m#YRYijEU#8lMwLVQsga=BeMZ6~qV{BWR4EF@nYjnn2Jlfo}{UtlmG4
zxCtl60#+4LIS-fRLR<#_UU&uda%PVFgXXyeQj=VcPq%SV#M+VS=fTUlaF-tt<F9zR
zKExBaH^!+r1wh3$aY0<8lqMV#$UBT2n*ftsnlF)3g)+z+sh4x}B?tIxUObsYt$g?o
z;mN6jw^9yyrBE*~>KMn>EPlgWH_~#2D_fBJ6nD6JHC?Dx!|00;+IzTqZC$v1S$>o(
z{PxOnM)PRa$n^`#^nVnyVT%ym9ql*vxa#X`=JGMs(qJhoKNFL{KM`Oa!coIK#>wHF
zFw?>#xc*w;<3@p90%%PN?GbTped5sC(i}V97|}L*hd#93$5Sz)MMRzG=mD*BaXp-k
zGl0s8SBmf7g8s|$Z-1TXzRjP1Yr}=V{?aRE$C_jBz9V=2b7Rl_`$a$Bc=acqzv};P
zAgcRrNyz7-sbnS;_vNDFp{@30G&h{~WqgUO*BwoGVddfKu^Ia4giq&bCDGI9VKLg|
zQ1#N0M!hr_pK$4(LuX(m<9G1cqbYm5FM0e<DHb`4GvZCtD5Q6g!7&T2rXO67f>UDl
z(?5l|c%74R?Z;rlwxaw>d9TvC)Q0a`cn;897{=(2puHiqmoQx%{9P996-PeDQNOz8
zXqf@?D-GU~_&MS-yO2WwG2;9BkY@r{z1(HoIPz3;R<4cH%y4`sAm0OX<hu~Jx->mj
zJf@}G=iGehFj_|aD9{Ivdp}yWuZmXTF}#IpcxPdj(eo6JuA=OE;Qz@=vKFJvikaGm
zJ`mr>NyztWRrbl!j54`+j^oy%?)TPJ-M2UHXw)>C8n-oeHL7O%_(asRo@eqbR-%<F
z;i_`$78|e@t$xLka}-+jR9*FcHTX$vO=De;C8DsjzUR2(0kyyb7El@uXpWfgame?%
zI)0n-#^~%_V!KG-lc`y|N~JTy;0A~@h(cDAL@YDYpCoL0xn||oUd}YzQ>Am>2|mQj
zIe`Z<JSHHCQwV8M97!rQ+r~9$<0Ey|#zSdGxm7C1twYW4Sk{`~-LN>`_t=w~9{BXW
z@t?$>|JDcp_l-}V!n^e=Sy8<!9fG%Qb8dX|q6co?`_}dw_y6qtg`|+hD=Us`sPXuH
zW|8f$?fSQvQp`OUq|(-6`gg_n`ybr#{k^B3roHx0=RO}--G15H&)%NA_rJgMd)?!2
zA9K_Obl)K>-rQsVa@C`IN3YyGa^rziym<Y)(C2ZOTLb2FhnY3|J)6uio!;GRZMBa%
zY?`5NT|DLPOhrZqM&hHEwt=Cfy3-yUk9BK%I){w;nBCGnri#Tzb;hW@gPAo0DGB>y
zwYLANjr#gRNU)6$CQY+iYt=VvG_B>l;vZuFwG4F&EO{A=-)CG{-c^~*m*bAWaQfl0
za#0I$F~xMfSVBdTZrZINj*#|Hle`Q(3C>+yD|`U$sn<1-JSqJIxbzXgTylDjTaPx@
z*Se(oaVbbrqmW6^nq*cYdkorG8eXm|{<HEnr>B^T7gO<KDqc**i>Y`CENrdqKWk&+
zQsGB1XY4;~T6%X)b7O>u5gtZ(7~x@rM<6`R{&NXFXD`^OGW*Xl8-0Y1c{yf2_?eee
zl~L2#+i9{#vt52muQfR^tqo7=Lfx?zO@Goh)ste|0j<eEvcuS;(sy)aeWM<~Iizei
zbZJe=A&YXTJ8R7XnHUU~xV10l&i5uvWCv3e1d_(=KXX}cIGT&({oYK<pNd6u-ke;w
z{LKDysMkZotRrBF><Ua64CZV%PIYEOAUMtl8vCdWbKBjoa*pUdjG*-d#zQf+f0M4g
zQ{_r%yRC+yO{rc*Ti!WfO1Or`<252^3)+7c5y8XYq?|^sbbnLW!z{HZS)9(<QtDhu
z>pq|L0JHzB4{I5Dnw9lD{tj(HCH7M*{T6CF7c1X?7Aynj(ko#0f7M$ikuQK9u!|Dx
z^O*hLp}rTE#?ENrIl%1y=JQk`mx43<zqxE;EAjuq?Egyb^qKu%t$fZ@WB<T$%>GaG
z4YV}$Noq^FJobMpWhYm!I=7i3i~UkNIjL>X><kihiOKHp@U@MrFAd>ht?t>rE`1}F
zSuQgBKdBcoX8%XrSm}t8&heGC|6}$aG(!J&?LUYX$z5?3%S<_rY=vvRlDp%Zw;bR1
z;xk`4;+B-@qz~S;UAOjsw6|S-+L8bD*Z!+F{_nHTlSsYPJh?iLh(fCuYd^hM`Pso0
zwsT_de`WVFR{H%H&)<b*SEaUm@+`yZtCf}UeF4yiIDVpxdYHF_EA?5i8hEK1?lzQM
zgoem`emXA|-A^`Cq~uS$wr6+o`<Jc_ML$Dt9C%{=si%Kw-#I64__tq8c79`>?b+#{
z{O;xO@1tK_`_xURKU5AQmtyF-&$W8>W{q~Y)fdl(?n72*d$OU7Gm{#L#zSX&&8a{^
zC)5_rwL^)<RZyboTTqE+LC1fI?3U4NY9q=d-#6s1s$eFZ&1HPKP<RRs-B+zz&+%P1
z+hJ;*X0ui&RQMW>OYWB0(fZA*LNl+GHj{jSEzLT=^0_ST#RwyQF8fE(XRbj1`PUSj
zc}>;oI!XFmZini0%FQa3@FK31c4<Xb+E-Whn}_Zt64sgG?Tcr<p`JN3b?5q*E<Sns
L;Lzk^`H%k(444cl
diff --git a/src/azer_robocup_project/.vs/Tomsk2022 disk image/v16/.suo b/src/azer_robocup_project/.vs/Tomsk2022 disk image/v16/.suo
deleted file mode 100755
index d08c75fd4385c4516c4f5d2045f1df3a3d38e14b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 63488
zcmeHQ33MYznQl*x$s_}TOfE7xCdo_=JhpXNhm#DWTe56fhb13T@JcONmUUV!`5F&|
zBm|NTkc5y;U_-)&I~QR|HZNK7fVX5>0))5lSn?nX%v%D>k_Cn(%Uc4o-`~ebmL<C_
zd&b9f>HVti>gxKds;mC`|G)mK`tD0_{ply3yx?a{joZhZ#Qf#RHs;i-@Rq7;<}D2K
zCfuI{_{)(aN9Ykf$ED&nPz?l_Jboo+7h`8~@Qch0eM;HxT;`Oc-~99?-g=Aq>9b$_
z<_}wTBi??!5ZMQed1jn(1N$hh?9B8Msmy<TF0<t*eyaBQPwU@3fnYRW-bwfgZ-L1m
z2Sp|U2%;{cfP@&Yk+-`E{8l9P12MrqrVK0@CWZ17B&#sn5K?S}SPTdL7Ap4^+{-P0
z7;zn^2??A6@6Yt2=+40X+W^u%_3aD9d%C99aWY^Vz$=EI4gbx6vj7*0Vbc3MasO5T
zp*s_>15g4;&ujfXcy<NgN`PvSc+z`n#L)nZ4dQl*;k}}N9sGR&n|Lp!Bl!+7+zH<W
za09vl9zYL(>VUpUAHW9)0i<^&pXzr2Kyk?p<NA6)93X@={!i73lNQ4=@Uws%AP*=2
zCIE!zBKVYQ8U7?-3b0)an}vVAcwLM6c=rEYw?0|v{|00TPU?Thv;RK^yh{J)tN&<w
z+=|<!=z!Foi6=M@Aho4N{h#QD)c<L0xELVyf1({kOQbX;U+Mn~>!jKKe~DPXi}nBa
zAkIyIn*p}~J_MleOnu~bz-{9FU&Fscy#65kdjWR>?gHEm_%Prez()X|1^f+w(zzf0
zM*$xb?;n8walnIsM*yDy{2kz70LA|l{7(ZuBi?@w{^!N(M@9c}(U;nz)OOaB|8L0S
z55;`0r+n4s#e$I}&!jP~M8RvMtGEwjUWU66{0H&<(pV$o6@JXTN{rYKyd3cMfY;%`
z83dUzo(C&2EYBHnOC(7A*BOXI^qk7JM0EafKmLgyAxC^4ad}0Is!{M^4&)<)d+zAS
zlt4~Oj1{>e{y4$^S8~;b@}Glrh}Ox)??K$xsnXzVNS8{Q6;mGIxxhF=r;vj@@{t18
z6py`vRd|>FRU-c<jqjBwjjx9A=#6uQ=Z*URkMMZC_W$+V@1_1#umArTb-Ld2uJr#C
zyZ<jHe})+~qh}0^Ngx$<|F3F7{=F3`JuRjt2fvz^@am#x-rfZMhmh2vW#QM>M~+b7
zZ)t^3e5+jg|4GFC7GSlUM04cguf{8<{`J)_)Li5H^ZExV4>gbL-je+MAL5gKjBN#s
zbTk6fSVuenjj=5LZGcI%K@~$aT;EasOSt8hpJ=d@nw;_*q{wxAD*tYTlfFVO{`Vp7
ze~W3!!B4u4Moy~#y@((e{|69vIrx?Szj^yVwHd{oxHV8MdvHvIaq{wp3ld2TccdMR
z0n&VEgz3k17Bo7I5m^P$FuS1DB&wapRf?A)lc3rW$Z;gs5EU=tt`~Qb6ca&chL`#{
zW@O%zc8mlwpiQJ-dIl0#7v2mZZDDMP;2p|St;DpNxh5?n$y+1H3CY7zzO+;i30!A@
zUAmHD<Z=nNl*;G0_R@6e&B(Y$rP1t)r?~X4R5qGSM1pyiO%x-&d5RTbGf|Ek5pG6`
ziD>4=mU`usU&?3dzAwL?Xe?|5BqP%^(%q4aO)~zQ08~uU<&(@!y8W{NI{>5yBw3h5
z%kuyvlfDfg=>chOfb`U)2PECpF2JRL%K(=HXdJE9|B;U1YQWn8!~xJ4*9K?@bO2NU
zN&in2S_kL^5WhlWHjSG!c69-+0bC0p{eN|N%~kg{T-yPTsxUXMyT$t+_`Luxpby{!
z_yGYxb((ZNfUxVU!v7Z65kL~~Ucd<8uK-a1#b@Ck1S9~XV%Qk`6kr^X24nzPz$73K
zC;%n^^gajw{eZG~KLvjpK>cYJZ~!m|csJl(0D9*}_;UCU4dOL&690kZc-JUib$A>%
z&xiqX@HZZP8HVuhLR!>@C<E}8bUDQMwHQkderi*V9O0+7OYK}P{u1KSw<aeCKeg*d
zu9^CeAu*}^wG>u9<*AGcL6pLQu^|GAnLrCATAyIdXrC!jZ<fP7Y0)G-Ry(dq?iT0@
z!@O|L*4MuGa^$WDUur{-R)fZ?(9_N6jcWY2;Z6l#h15*=ckn+eqeU$S^09n6Rf_cI
zvs}8C_GY80Tzg?=?tn^kZRFSdNzV?>srIVo9MKGyaM%qxySmL{&}rM$YMZLfY*(A{
znptHvsT>Z6ReNym+TIk$I#QKdkUut(u9fC`h4Jq5drHs$;xj+EVC$i~AHA~o@GBiZ
zI{niRyms;Vht7EMiS$Xo?x!!$cde>Fjmcrw*);Y}gXYpZ4uy79>Q8pYGYM|r9NR@Q
zRt!`li64!#r2oueOsDa?jC&fv7jsuY{-*JO=(^nYzYTFOqk#mjG5#5}B;q}Y>NR%2
zzasgs7Jo0wOD&k}AgXn-L|^N<y}(an5|v4A`L`l2|5@SJ;<xa9C<=OCKrZN8l)wR1
z=L9cRIUVrRIDIuhE`H*#=pAyk_y^E7N%ua6dT3N;;Z5OxR}=ad3tl~2Or#cn{qk31
zoPRCRKjJ6kmVZCu-UuL9i=XBLsLdBp3lg;wV%Gm}rv1GGNj0i}X+DDbU;?mIe)MAd
zr}~sz{(V*WgD5R4&UH+p#Duj7%0&G?0}3G3LOW(?Xr`vb&=-oLT~ZrinEg$B{~^3b
zZIE0qxTg>}!CHK~cuFM9#0W9#|5N=F4@%@jZuy51_aiMrde@_dNrEZyef|dIL@Fm)
zL<vtQjHDUr8A6*7-lczI6Xm}F?-AcfPR{tR7NXYum)ifOcq+I2??l}1h^bXmmR|n5
zM#Yh!9aM*?Kn{5<@IJN2TBm8E{9BNSP6mD}+8?zzk~y7dbu=Osz<rZcCQ+w-`2Hjx
zkh~$c{PgL!FB3nF?$jfw1r(|%BhCL2-kr<9PpBpQ%jti4lswLJRAqi#pmb1vzTA`t
zaul|d8$^Ug^rwmOzvV<9|JzZY8+H7Dv}I-bhi3TS=5<4F+%R`w`XTG>-G>A3>ev&x
zI{xgo&uwYF`RiLAf2I7}?w@>@TE5T^D*qHlvUh#W*1c<9exh(RqLR#%1lK7@1rG$Y
zdx!76?W>Uw+;-+OI}dLkZ*_g~;nOaA?A7yrXV*X9N8<m2l9H4}IuQo+sWMj0*S9%A
z!$_wm>Dm<eZ)Ngdt^8%nv*m0eSK=bhRLND2MHUrM8!HD%Y@It`<egR5rqw%4Z8}}2
zw#}q6o7*b#pGtr56D_tb6@CZInq>&CqWmX^|4`=t_)b;#kNB_Mc(PIF|3140^pECw
zv*6`u7C_*;M{u12RT|+rZqg!=wn&h|C0)^u-#>WrsZ%fh>hJG&u~)Ls1p_y~?~*T+
z-(Wt*&R%l$mrtXQuz(`g{bbN22{w=PRGzB7870!Yk@}txmniy*`O#D+u_w|LZQx49
zXep7LX(kJ$rL#~wn^G+gNOLbX{yRmfx)!*0w13k7lTJe}|4F)MDWO`qPq4EaMQcc+
zg$Uzft;{98OFrpeDr0>m-Rd~C*DeHnS`6$%s~d+Dngp&|8^16=gHZeS{QJv*|6~N6
z0+3sNC*qO}M9#|h0uCc~7-det>aR8zLcB)*()y2+5J4{f>iU;M|H%TQkt6$yEAVi~
zGVtGrxc@FD7>49u(ZQI}|D(L_g|rYZPyphNf~c1gexwacR>@wJ1nED{gimu$a?2k?
zT&ezr_FvuV7g}A?oS&f8rWr`mNmLx<|CXz8OZp7C_-P*MX;A?tul^fr03`bt(m~3_
z{{zI8=3f;4f34-8wdMaSeNWK;3q3*5y3jaGvp5CNN3wpFv}9;LM;-bf-|ndGkvsZe
zkqe@gL^H`z-=>*%(k2oX!c6^*bg-oR_G1(vT0pdsJ8C5^yi5Po{|N8JxR#5*n*LSy
z{965&kEb{^cNK>=lE$Kn-bu<qGv%kUOZuP1@Q3-5moQgAbVss~r#IrLnK7OcLjT%w
zp;&gg<)^ViBtKFy80#auZrS-x)r&_1WL{*WTq#k!@}g$nCp|3D?=4tKd^`O498>(L
zG?h%1#vn}<#p%A1G~Fl3v7~=_7Gvk}wizKkF6EziN^4hy_&qKXZOrNg`yBm|Vj`O_
zCCr7CHGf}S{#(BN#!9Vo6#4(emj4y|uWG9`w>Zj4)?J(5{!8iqCwBi2W6TunI%#%`
zY$<!elL?_}%+j^P#?#3En_zE4Khh@`!GA;$oA@@eYWpUQe*D-gtmh_an)*)lEbcDY
zYE2_8J!};SONvj2IKDhA-e?C3l7Knzdt_-zGr}cY(+q4L@yXs)4eOr>FCS-l{gJ?t
z&LC_xN->R`>5$guC=Kz{S-vzhDbo1c$dNx4`#)4+14sH_vj1Do{4c5H){Fh0obOLI
zw2d6fzij#MRwVYcn4lc|ORfJR+hCf5l`H=|gt&(Q<mBPk3X$vlOZt~RcqSMBClU8s
zb<%1!i112oxoMCb+Opi2s^YKoeVvG(WYQmg=?`ze_2uVWD#tYz|2uy6U{3e**A3t8
zvOMzppJvYWBrpEOqg@t4?f<`-SD${$XlXs=y0b}+rTkaQP}q-K*=}L}m-jv)s^FH5
z_GrYFyelOl=`y8z@>deJocAT1Z^6aa0F6f)!Io>iaRH=Lru8VqY0$crjXC<)mV(6^
z0pidEJFkuYO=!gteIr^)v^l;H2RjeqwKry;QTTt3S#NZ#_M6D%|JNr~t}PAA&p$|^
z0{67hsG5{}Nor0(mL_SKsKECD8@)Wj9)@XrDYBgxsM}`H3($-dt^C{^UuhB}DfOlz
z%0w17R4$qi-sr85%2fmxM{|ax(G_^P#dRO+llGBKR0bs&MTsgpHtC*WHcy#^l2QxY
z_+_f)z)>jCHPWIArEd|X$HwnV>D2e7dGz{EmkuTQlk`zS|CcPjh<Zl>wbo%2Uf+&-
z`8xy%d8;S?%H3mtQlPn40#OPtiWiiuiq>q%*{z@*R#zNGt)^ZIi5Oji^>yW6ShY&`
zi(ggV5U!9Ov&5oP-gcMEXUe4%!xR&lXel+B=#o~u$}}5BX|Z!3*&+Q2%9q^+6B`@V
zvhlWf!W3)Msf?X%QG?FZ7K<6#ILpShCX@aoW~OOVV<EfwT(jA|rFkpo1*k={>uS?V
znU(gT)z3wpogtQC{4IP%YBSg?VE@{Eo~?|fBiP@i))>hWU9Hx~H5!wqEul5U+jMF)
ztEkFgXiLPRY?RfpMwZnb6kjkc^93z$>%I&OTD5Epwl%CkvHP}ct-bi;Hff5oX5~76
zD?Mu$F6L5UOJ870<#8tSW;ky!G-eMaab@w-eaLDaf@k%n0xB;Bm<PSVfXZzyaynMe
zM+xNoA+-f@rpKoyjYIwr#p-75W5eu}*=*x_aci)d+4$sylV0d82SWi%*f!=XhWmZ}
z)82sHr`H$~hM+&Ia%i%io{&B4w)#5Vy_Sj4m__gH)^#|YPTy4CJ=$sw7{b#Qi*GDv
zo;D}KR$ba&?zZUtDb<iMZ*_VKd5=F-un(BW2KB6AGNv6=_hmXm(ekXF?db05&T7?@
z+CqG?v#jdvNO40$owEgJe$4LXj802(xNNjdPZ~=8RN9!GirHd@A!lJipRrG5*omm7
zqcoN6<613A)@Ao6?Xw<JbhdZ4mvbeJqg=dPGWZJmM9$@z;fm2wpWAO%k68kNbRrY-
zo7tYRUVF+k-rqAF&kbv|h2e}Qk(ugq4-Gh`6WZB+OK~b|o{G8(Vb(FD?#sKAMxWNy
z!K&?}d99~=qCC-Bu-F~eLe87=rFu+OhjX$NSL=$w88*{xGaG#x4N_K@*q%&c(&H|)
z>N+jSi9t&t%H~uP6Sk1wnM)0NO%7AHzQgQJc$4aW_e`{JJa6jo+Rc3fV_t1s8yy}D
zggXa}!3oXykcP9^Gr=i`w;V7{Yo<G0>yrnrm6a9wfBkEm6#1VlRM)#+P?7)FyIsoh
zmH!7y>1eDtT+Al}gC1MR&c#}T)7fAqX((|{b&uORsPmZzTQh~&=%}H?ug+#&A+JZ@
z(OJ?AcswOtxiDsl1y$a`8I3>6O{P;}b)eK=9E}<JJ(C!K2J+5y)a$b8%9%muw0A1&
zOT~hHE^R1f>YQ?S7AF$XF?(>r*poBFr=pWRh4I0J-sH+y)EyzsOlM#)tWJ32F|A!^
z3|npG$%4zF9>{kObdT9*#^bI5Z`kXZ>~>jVNt@r7=^HladN{+dCp$HvpX}kRp5j#N
za9NwNj%U=NiLzPmQ%yO2DP3aFV4octi+e5l@lJ0()ze|_4yuElL!KdnbBK%TOTkV{
zCY0_gl=?$@yIbvMy#;O5AIp2qW2%HZ6d&uJi6tFH)@=%Fe50n2#-j81Jyv&$>xtR@
zj@-bkJ~?ej^rky>;hb}(FCPoF+R_G7(C_GUPk5cWp<Y8KAN1tM#s@|-<96e~P)_S{
z$0yUh`aaK)rJQ!#dnYvM^ti#2a(4$Drh&LBWwqxrscg<Zl^qX<(&gz;=Xi29ZqttE
z^I(z+g@HWRuTJ-8y6qi7wZ;%K8%@cuy4#@lj_ZuRfk3w=?g|cR$8-}(l`E8<n(Z`a
zS$}jq!0IPUZfC$1&6=W#N%c&JsiUZ#2Ahx`cBE_x*O10L-s<)x`@MS8X#dz$*srnY
zbGnq@m9lAF6SHGJZy!6ENNZ!Q2A4+@$m)9?=AqWSUu7)&2CUh@&}61F$L6{%eHv4D
z4;LK|#cT<+%iN>4=7#L*p<d3x5u<6gXB_?MVA-FwS_QGD@_#<lQg2=x5p3&2JUpXH
zq-;5Bp_o}OOkwX8_Wqc$_X>Ge$omE(?}K$nylvg--OjAu$rbv#mWvkM%VCMfYc~cQ
zA6x7CJgqYSlfj$=%{tINueEk)eDIQgE1myQRD7FW#kVLAK&?-i|2uK#{}d7b#1`?7
z&i|ScqXtb(JKAQ@s8~$dMU8DyR-<oIcN$DOjg}pacIuS*-*v}_D)YZf&M+(Uzsmft
zGXL9P{&#(<c%Q5z_s~i$S|vtKnKf9gS%bHtHhW<=E9`7cYp!YK0Ij^HQ`u<G<#EKg
z4mD48e+|EH^8xI3qQQ<hcAT=J!@n99ZWjJpi>u|G-_VYAS5y32?+RYpZ<=Hh!b<KM
zrm;cM@wZ!Se)rmIdE0o}*JvO1D?fb=Y%b@YJVE)Pb6{!jwv(6@Y$GaX<C{ef=#XiB
z_h|FhcTHz;;UtdSeOdpZ`P{Nyk+Qgx(9&BV$=||KW;M^}DK|j7*d)e>8=<B}<HY?<
z_ovlLVC_`qn@VN6sB%zI4sCknkYW{E&nouB=@p9o&)h054^ixY6!ZRdW!`_{5HiKQ
zbCZ~ND)v8(PAFBDcqvP~nponci~))b=mxU^U26WPvY=Se{|meK%Gu{2g540RPo5_I
zzcT+rr&=th4ma2QPm}Wuq83-~RLJ}pg%wPUG_7`C)H+*kF;3kan;fiW{U12!8`qU{
zhUxKPr{C`DSD7+X$&$&J8g-8a3b7f^X_<-}a3G=~rnb%Y*wyK*NgZ=)<94&tn~%DW
z>q&{VM*HDo)0~b`vdWByvQTW*7m6urlTD&F;Vorq{%jk|V4_-k?pY~mu3Cw^nrSIS
zSRulzPK2e+coa>IqNzE~ni{3)Df@tI(0xD-)7r%vXIp^{9owwY<ua8a(guI~3E@pG
ztEIsV;w&2n&I*mdYFj$5SUA5m1xsT(zm-lhrE}GV^RzjfnNz}vTqWiY=YESl<otg3
z@a>sh>K*^{<ej(o=LfHN(|f#qy_eki!rYxy$I|&Sk}uVJQ&y^y4d6;^9L!jrS^mzm
z^R=KiKk$va?*8Y0-2VR16;Evc&8H4ta?_74-hI}KJ1>9V72jcgap2{8rB|sA&Z+jQ
z<{VLR`^62rGvi7(<EcAay8;DM(@kPk6nqO5M8_sO;bW6*GqQ<d{~wD4#6yMaE>TeI
z|C?XNV3lzoa@|T_?ufcM^ztj<U5;B#OGEcc5w3UvK{Lco*%f)guE<K2ET|I2B>u!U
ziC3mXHp!I8QdUID4zKFz0(1gvw;p-EEe)NtMdyZ>kf$V0NNmSC@EN9d)ilZDm9q&3
zUpk#vMRY;$(ah;JyRmXTk6)U%jVi*YJ-ov>wS~?bZ|DCBK^mcSu2d8`qqC(X9m1RO
z?4~te$;~j^kdJ5U=7Y}oYJ83lm5rSBkzKd!e5dL~;ZFX4Hp-O}#Vapr_Wg^X#G~_j
zT5w|R?eO=6pi%M_^P{OuVo$`DXUka_k8lxZs^luy5!GDSsenotz&DKX-*IL3Ts=sG
z#aE`?>^Z(I3feL{^*)1p4!;qm63dS~Q~u2WIXrQ&&(R+#CbIcb!dysM^Y_)&|1JK_
zR=dpg``rTp?cU*gZ~JQG1Gk;|%+ABx$6H-reE77>9((n?-`Vxg_vKOG1zJIQ=PgCB
z-viY$i=OoK8kfJm{`FYbzeGI*3U~S!FTCuCedxijg}#0A>_1&r3f=nftHZyv|JihN
z>b1xN=l^fLnx-&vLJACX8m{lF+y58q{l}*zG1%>*IqJ09`Rhfyll0&7dvx`a5nh8%
zG(f7|t;ql9k4f#WP5XCa%qKcIf)SSXU7<4(=xnzDdL)&*ay36jJ9dNNP=`6@-ha)V
zW;A}~=jTsHJ3jWSEA3}&zx)gLO+AtS&p%vu<WIiTdr)Bus6%D+tl)oqjr+gL#t`dJ
z$<WdjA`K5QR9vM(uK(ri{?}r=p8xh9-Qhcm7n#E?_v-Gsr~5hA$ZreQa}GaOuR;)}
z!cICG@D_1?ys70fIG*RA-Fb*BqEkCZa82iSj__~LOb^ZI&=ca9g!vo>d&@A)Ep^*#
zEoD=L-4k2brSno(^Hj(ABO3#*j$xjW)e=`bClwxp&W*2dY>V-APV_F~BPF^|yDNWT
z(cCwiw)wGFwp{!h%TtHH^VFmFUts(2EC2Gk@$_$8@v{$Jb?+Ua?Y|!Z;zj6ZrEDzt
zUpnpf*w$Y4=-7DNlxpqNHT&kvA>-AAzCy-@J?6S0G07Via=8RsO67B0duh5v!k|W_
z(a778X{4BlW^SxI{>kUro~iqOEtPNFI;j0F=j*<4!`y-Chpe}E9}c{$V^8Gj__NzS
zx25&wuWxz$mGW=9e?kZOE@-zD*2t}f%qB1YwOrjFyFU0|mXXYRU%ve1Y0t-heAC_c
zT&VfVM-p=*FP!|kHDEkUIhL#)Bwv#LEB$Y!^}o{cOB~w>_y9p_ozIpq%qb|xM;E1G
z$A^a0mokNOdmpcxKl#0&87q*=H(YKXU>MqqvOa&c+&s%4KPn@bKxaGd-}|;sHkrEW
zERE;L%f4+hkL$1bcKNA~?tbpQsxK8CU#{1FDQvz|l--*|***WW3U@N>|7q=G^Oj=_
za|Vy8`TavRm)>zGwBv<yw!Zegmm_yQ_|mc2KeX|w<*z|`NBduHVOC(YBuDXA*Un*8
zjFwhlHo$hN%qpu%<#0Hx+Jke~_NF-20nID7Z*HY!s#<>`cX_3<{zEy8`Gh`<Sy}&~
ztp7O9>pv9xUz5YEvuW&|29^HciD&<-*ncVZUwt^ISsT|zhX(`U&VdF8MJQvFGB$0n
zv1uv$uX-Z$i59le?Y|BjT$%lsWIxud{g+((u|^vBwYDFV+IzG1TWf9qCCxcCYyY-V
z3lqiuOPU)YU$OscW*@$l*nd^q(ltMqpxBQ!vcPF(mZ)~7kzpF!k16(F_13@3pTAz|
z{OMZTe=T>uaCv)G9(((-Mtn`P*4P`(en>veoo`#TTJ}Hlt#Rh#V3?)tmyXZezdQ>K
yiI>|*_Ct#Ok7EC`aP86Y8@~k0?dI**6#JoON2+ExLy5vw?1$u!AIHZ2=l=o7yjLRt
diff --git a/src/azer_robocup_project/.vs/VSWorkspaceState.json b/src/azer_robocup_project/.vs/VSWorkspaceState.json
deleted file mode 100755
index 82f43e7..0000000
--- a/src/azer_robocup_project/.vs/VSWorkspaceState.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
- "ExpandedNodes": [
- "",
- "\\Soccer",
- "\\Soccer\\Localisation",
- "\\Soccer\\Localisation\\PF",
- "\\Soccer\\Motion",
- "\\Soccer\\Vision"
- ],
- "SelectedNode": "\\main.py",
- "PreviewInSolutionExplorer": false
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/.vs/slnx.sqlite b/src/azer_robocup_project/.vs/slnx.sqlite
deleted file mode 100755
index f87de84cc98e481f450b14a33fe85de23e950367..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 118784
zcmeIb34B{ey)Qmz(a|ntcbvsmw#H5(YnQB~N$Pk>oYnD?rKzjRmYk@uEmxA0y1SgD
z8>OW!v_NS~X`nz0-Jk^MLLn@-_gzR>%F_4X0(o36m#y&b1uonB&74K&9L=#!`QN?^
z|KofzN1ESvW`6UV<;?H*n;G@&-I*(8eM9-eNV?>Upjn7v&?P<}LTm~lv=si!e{<mj
zFMok2qxx*{5%Op{6tQ5i_xBhx$NPKN2VJwAI~;e~-(q{s$_ab9yV>8d-RyRQEX?S?
z9SPVltL2p*Yt4Q?Q{I~!&1A3cnHU+!54UB9a-+FYE<c(QKaL*m$PH&ZGpSA*$lKc8
z-qPFd>)hSee!zEVMtStR9X|Nmv%4DK?`t@uEw;b0Jl}1tX$3a7(%Iad9?8;~^Gw5B
zCFL|MVPL<nb<fV7xUReRw6^rLH=f$)vex+h%<cYCdSE!Kue$!bTVkfSWy{WXO3mGl
z22n0mLhQ`+gM542x3+ity1F}ewR9i!?Px#fYw7FV)43b6-POLk7r27|eLHuqlXB@Q
z<d5Yt*@8N?j1eKY5xUzu+Pm9#x3>5A4#_FnM@zZVM1O-40a-Lwl0dulb+utZfEZd^
z+S)aww)UOv6_U)1D8SwOTDn`ewRAUxW3fhsVVzv%Bpu_!!xMYQ)5E!;TsA|?K-(U2
zx*gC<D)N$REK^$s2eZXuZXh>|tEh-o57YI*DqN+?Y60Sb(lb#kWk(bd>Z7nIeU;R2
zM3y`AgK0cIyYt75D<Fef^TXpKqt!UKqp=)tT5D1;u2N)N5l#owqodhEYkKfVHdW2K
zILh=umv)rt$%gr;#+*J4^u6lR`x{U6Imn2+xu1+UeJcHTr*_2Y$xyyL;`Ai-n3=(d
zlNyx9b5+mLj5t#6^d07}>o4-KR#!^y?;37opwfBeh^p~VALP9gV=zm~GnHI~Jo0*T
zBiUjpJu=qs!;?pE=PsB@VZOpW(Qmib1R9x1FX@SLu{ggJ4&(@tF^ANweY-pN_9<^3
z8KWdpbuXDFRSe!Sa*W8bZX!{r`?7^147945e-hcOHI0qTi4CM5%9)ATYromGeNanO
z^+1`RZloFXJh5je42#vNOtZmhXIZ&irK&SYjGJt|)mqc;mzzxAkyBmiLUy#2!qc{%
zW}~*B9;MDf)s%V|UNF$!&g<ZHMp)1%z<s&AMX=T!g_&=nGV`fMp;UBrO4a<Qf?K8f
zNJ}7nvT1UgrfySIV!lPaEUGFNONGv`Fb%_%l+!Rr6MW-|ZM-;1w^k-;m9+GHTC@X1
zPgV^PJ?V`N6_MvkFQ36m#IOx2a~+w-@OldWy@4gcUpK>d7O`yleOo%Kmw`Le#nPUE
zVzzKB3(KO)%qy)HeQoU>EqyzCeS_l#GGa^eG!271&sl3ii<xpg874j1QYkljn2c#@
zy)DM@+Wu6HBi)EB_r(N8gSN4Y6>8S5We$>_Q@je+XxUUTd)0V$6jp88ubige=!gxY
z(D~Ov%Et0m##*y@F*BJZc}l!V4_1@MAt_eX<)oN5t*7lv5067T>Uw7%=xVbaZubZh
zTsh?ZvHN?T#qQs_a_+Xj9fi~6)RcfJ0aF5|1WXB-5-=rTO2CwWDS<ad0`3vUiCQ7F
zpw<(6!QgXccCgf%A05ga9xs3;8T5*O1IkjFQa)e-<mC18_X;vU+5>uIZZLZ-)>oHH
zfGy*>;fyN3{Ah9cdf%abV)$F$Q%V;~<6{yQ>#1sLtEhEqL6Djh07Z<6+(OLqk#ugf
zX>4M-WKWbyz3IZ?YzY9P;4hGASp8F{!R%h|c3i(&;E_MlTF9nL+033o-&h8J2*#Si
zp{8&!9Px#c>%+nI;js4{DxlVK|C=X+8kR?a*`3(O28CiLMQeV1v=ojl<9XCNyDd8~
zepsw)u@AI)Ni!|($W9#UKN03EXv57(tm~5;MBh`7Xw5H1vf064JUr+RMdJ~FG|?RN
zrz3+Ie>NTqWrN9BBA$ilfk=2T-kk7<n?r;C=wLkKPlwYXe<ahKhz<;;!<lHtpBRV?
zWMY|!zc~^Q`=jAR#^0PwNBxOtcqp6=hhy=<us=C4)SMhh4*Ijn4CE9{L?EX~+#ks%
z2EvKvp;%}z;UA2Lo8yVWL4Q1vZuUo$gK>X4nF;#C1KFWSJeg@87y{71P_#J~P56TY
z**Nf&2}8k9kHJtlGZcwsve{(PpGk&8;lWtOpG?HkkP{%&iCDxR9*Q-`2ZjcQf{BDb
zGc=fpB|}Mnb0!Up4@3w31DR;dABx8^;eo-S_)t9L&&EQrcr-iYkH(s@oQ6XF=0tc1
zM35MY#nRbi93+#?G>4+mWWpZ_#$x_xd@$rs4rPOWC~R;r8H)rXp`d>#5gTky3<Uj|
zOf2e;4#k81WE><um>J3h<IPa1a2Uq^!hpA`!IKLZ9<8qF0T0YbTC&Q9nj%dxHzZ)a
z{}XwC4T1S*O2CwWDFIUgrUXn0m=Z80U`oK0fGGh}0;U8^3H;4TU_Q(FS^fSNW}B1Y
z7BiBiM{q!`1n&{#J>q@P`)%(F-s`=;_?xS!$*?H_Qv#+0ObM70FePA0z?6U~0aF5|
z1WXB-68LW-fq9&t(eDru>bX{CioGBfOrK(l2EUlwz!>b#;O27yrfM4iJBwRw*K7j7
zXY*O_zhfya_5R-b@7`hWUhjJE3NPpRj_2c^+dU=E4$pG;pWWYcf53f%yT{$+cDR1v
zI^(*_b)~D_HP`tw=eL|^osT$gaP~Ws&V`O&JHG08(s9a>c5HJrIBfQRvA<$}&i=6d
zR{OAhuYJ9JiS3WJS8dPO9=2U)yVTZftF`{g`hDx?tnaest?kyu!t25pg$ISA&?(eg
z{@wDAmPahdExRl$_}}qg;or|+%kSk^bARN%#(jvpk?Z5uat!-T_9N^G_8=QzEzE1o
z)66YQKNDf>=zCD7hARcMnXwDU;ka)Kw&kS5p<r_`5o!)bBhjYP)un*#we5>Gj6LgZ
z$kCArjMgHL7$L(^9rAEBG7*ji<FRN+SNc#jG9C$rlA%O6s4YEffDATA6Ix`3j)c80
zVO{Bi6l6FUipAogm^RM=17tWB((#!#LPml*KCdu9h7ysuw)B({GNjM*asy;Ash3i}
z5i$`>YD>S&2pJAVw8%qrWH^$H>hiqQ2pNiowRs*iK*Fha9r6Gj8Hy%CI^=!>BuGiu
z?)&J-AnaGt)xFOE848DWb?-Gm2IE0po;?&~C>ah$6MAH~5i+i?`(7htEUYWNi;4_E
zPYUY#{2n7@ET)s^ZaOj%iH3B2ewP6f<}n?gJB^Uhpl-bHFhYhRA#JbQPDjRL5q)cR
z8X<!*UAu2HK;m6_+S0cgArny@pB;2$ESwDMW}9{+WL(c@8wDAJJ$AteZ0yv$J6h?;
z5KIiZcHcrphT@S}P?u+m0TP->+v}EGBB0gkcbtZ|#hV4R3aDd4<o(xlWidahy!2`}
z31}G*^W&wl@scLp(v1RY1X3n@EJrSe(5CcVETDjx?I3K;Eu}|GMS@7mFI3+K5dr61
zOS!>ed|<njEx=w|1+i?sfI^VRV0w5sHI^=<hN>X5lLDH}*o3Z*COO~b%>shgdxf!7
zAv=~YlptpXvnnB=B+hv_T`Z>d<%;;ikD^!&6>Mo-tfL6Sms<?0b6FA-1uK%b4(A8d
z)cH{X)dFjfJDlcs%k0%gM3Ih{$YmgqLLn^*3upy!RV={<>FnVNHEn(f=LXG`%V?s5
zVj;Wo_-+m*r#S%u&4sdu^M{qV#Z9=OZ33x=R9S^s?-!6CvM1{x5OsP)Y^^mi+jZ*%
zv=#t8xe?qH)%3L$I*2JZP#~?Xq_0^cprufHPhWTUo~<pt^>W6`8^z|r8TTG3WQ#}g
z!x^z~1=G+V_H~RIPLF0r(uJc%=ndK7Oqdj-AeXHcyNm{@h16d}uWFsl(TY_9ia}Mo
zv*}?yOA4rdrI-r=sRW_`mah=&MF0xZgcSu-zg(4xN@@yV*)p*@a*wDCQ9<T*nNO<m
zSRn^jAE;PSAam-)-T*>T;^x)~Xg;(DW>1M+xl}+2sQJF0{dth^Sb8uEd*n-5!Zj<G
z7+fNtJ&?3FKT<pj``5$1Os;s;mm5hR&ib-fk7WyRn0vI89#&GUSd1k~Pf-Ca(t<*&
zY!%SzS^;guZ0GX>`60O31IiE5fDH?Am%+e+d<R@DMUZMvl`Jo+5zq!1DdrfV<+yx-
z*q~h#r6c%A0y&m0c2p&=74yZeBPnfkP~$uSwLpO_Lona@WGPasd(m8|F0u>f9y<5*
z`BUd!Joho*x#!Q{b^exfPc>!ogICK<l9(f)6vjPx?y2+T^ONUq0_-Qxed^p(KJxMD
zb1$BsgfIBCfhJ+yZ0LbF;qy52^C!-I`urUsm|*0Lm(7Ao1HD%on5jNx+L9rwy&!3T
zl;pviDviT!K5&8_4*%o%TmgD85T*cfFN1F%z9T5>>l_vDmV@a_2F`QCq=tccxHhRE
z$2PhI)C%Pkb0g!!=~BLsf>|b&$ra#SNxm?FUrsV~g%kQQK*#dMY$}s2<_=?qU@}yL
zRy#z#wIFy|!3h=8;pN=w?4pb`Kvliw+XS>2N`seV)7a=?IoW)x7%iP3k)vw`nB<{y
zop9ZmG+JfSGK(ne4!9Cw@JOmuNR5%crYdVW4;>83QXpx3tOi%VBIhL>ytM!`FkULb
zeQG7>(<<t07A6BIzv);}E_V?FZw5%vlpo8EjvPy6Krvoeu7|K@9}6Ml{W^qbiTCUL
zDqi4z>-~mvx$|ack-NbCBloQH0QUgrW`EEAD`?uEXP;&tWN%}SJAUZA#CzKNJm}p?
zwwC3XpK>>N-_Cr4%W_+t|Lpvh_g3dWF(2b%-s>D+XYON)P8V0t9Aq{-laBvze9rk<
z?_qDJH{e<5LGC8k4_*K0dd~Hr`-1xm?hm;yaxHRQ<?3+>t_@zVmv?)e&$xc;YV!QZ
z^CkD&+{fK}Jx@6wb-vS`a6Ij}*D>zc??^fp+y7|)p8aL}qxMtw!}d=5TD#l!3)^|y
zv$l8Gj@u5}HrSS0|4i%*@3-D!%~^L?{Z=p78onlcT)1DD5Dp0&g?fRp{Lu1w%VU<?
zEJwlS5U|YVf6ae`{{;Uousihgn_ag$U-yi7x;zhHago1TF2;6cN7KWl3Ao$~UnsGY
z^euFMHKjwO+_#+D%66sIX<(AhOr$#CoQ(*1$5L)14I)zC?&DfnaY2wO4(Cg$Zg^$p
zM^a%}zv9&$x_1E=U~&J;!3(Yn#)uew502iNEv5R#Qr)@3N2J2;o)5c3r0AX5Au0B4
z^<0Y_+fjhrrRs&n7c#obh#nFPyAxMI%2l0Sq`q}Fw~B2`gSb)&G3JiBT%#Q0m&$mH
zI!?;>_DWnlCB@xV$t^DCc55XjCdS-SiHV9aH&<dJV$7*ZjKsl7ESs@(=}2k}Uh$)F
zPYH}PbOPf>$cl_KKoL#i+8)hAGo)aGDHZa=!`T9Ubv2CuzPyl&R8et8H!a~dGTWf>
zQ@ybAO|_DBmAp#5v4)E<gZU9ym}gTh!(&I%sbdjHbpwef3tPDp<VGO=`h{GK5tph`
zd;#A5K)7x$w^l=dxNF@o90x}8aM}pIT~o)kFavlM-ZC~;$iqEtsh(1HteEP_UIpZd
zS}vg@!(wI-C|57#HZxk=T5~871qBJu#K&pWXe=&Lk5yB_(gr#pv7wFAsgiD9qEn-y
zURI=2Bcdi)pi@H;qD@~#rAC5LqM43SsbR@llBZJ>qAg{VPK}3&zCS{zMnye$m`Vi;
z0Fl*EI#sf8UP+@WR?QrpDp@j*=&2g(<zYQlW3e2fQe`V;mPS=9lbI^2+8Q}XrOFn_
z0XkK(I;N>q+0u9gohn%uQ&g&KQM{Z^m8^*UbgE=Iyo^Rwtc8c@RLMekDU~W)1rO4w
ziY4#>l`32R_S2|}#cv;#DqH#bs8rdq*IPwZTl0G8RLO$ZO{YpT!(IxtVyWw*Qzh%%
z9vW4#$nB<5Wh>k+I#sf~?W9vBYugSQRk5&br&A@XS|^n%Thg}Csgm_<D~+mH%sS{)
z$x7B<MO9nI+Gte88rDjoRxDs!s8reN)iRq~U3uf+x6CE8xK)hWy0zKEEn_Mctxa>d
zMn-GJ+K8QFkVI>_x){fH>8({87H}a3UUxbR)p~)O&2)7n@yl4rykd!JUc@D(cdp+0
zl)(IGElzP<DUFpW=HO}>(Xtf9)u~vMB3^C<Bijq$6&%J$(P|XJAH{cn(8|qaKp_fX
z;ixS_P1q5sw*vX`3IUr`RMv%ca-7O;vDU>cWn>G_8eDapNN3e)#O;UU6{CpSlGA|2
zsiBg!*J>JD7vU8Rv9IWgScTDqpkjC>=7&To1(U2-C}34hmSZ+#DORj5%LsQ>mKGnD
zCNN!Z<K~kh>hOw)YDHPPird4;I=|W;p|PAS!8N0y7OM$rlffdqR5Y@R)MAkvT0$0L
zH!-1BTR&>B&?pv<1(*+-m190$DjHih=3zFCtQm9tT#A{o31bd!H@)3rHs+UNteAx*
zU}&N6k}hdvmhj+VBibU|*yCO_HbA&=yKC$Ovem(fi_lV3rUnPDi-rm#*Y1RU__9#}
zf0ir>Ry@L0<^uuOSuqGyYz7wGMshr8qs<X6#}?9~MfkjUHMf@)>y+A&8$6oo$QO>M
z3mH-IOYO{xCJ^+H!|{FeIY+_f{e$;6-d}lt=>4wuE8bVUpZ9*o`<(Y_?-Sm~9JhJj
z<Gs)OHt+4;linM=S9=SvM_|Y?>Ak{xskhg=+lxH^>G?mNpL>4n`GMzuI*)j_dAE2k
z_Qt(I$Gg32yeqwR-i6*dUYA#J-0OMObI$XM=d9;N?~gqn@VwXaZqM62cY1F1OnR>K
z9P^BMuJmL)mwOI)x;;BR9iB@(>pf9Vlc&M6+_S{9z%$F^@bK<Gxqs{amHVgeAG!b8
z{T=t$-CuTp$^BXP^X})|Pq{zrKJ9+U{Z98i?mOH!yKiz|<1V>J-ACL5?#tZ!++FVN
z?l$)(ce6X}Ugy5Z?Q<`3&vm=p7T2GgEsn1`f8+e6^Ix4maK7gJsOOC58P5~WyIlX_
z`nl^zuJ5{Db$!|O1=ovUr}(Jr1FnZ$_qpzL-R!!-b<8#D8ggCk+UMHs>Tqpx#a&IV
zi(GZC1ul<EaQ?~hS?3GRk2|h$KIC}BQE(o2=A8$eU5-yVo^gE0S?iqbv^zu2M&~le
zw;iuI8OQ7RAH-gv|L3?({wEq9%+Qp8DFIUgrUXn0m=Z80@V6rYE61|vN%T$fd>p+h
zz!pvxeZ#^bmbrv+I=+u1)-W7?)G@3CKTn=#m^O#>p;dZrA<s#sg*;ziHsf?bW|R08
zz2ZzFA9?}(s|4Q&--GA{=3;m@pfk({DcLrOzEyg5NY8eWj$W3YpOKy~IkzDj<ogp+
z9D2cd1^N1KVlwn;>G>(=jU@i380JQ2Nc6Ml6VmfJ=Y8;tThF48!_x-Ok6~C6J?s1c
ze0>}}Ltsy$r=6dN?@ywqFgc2zB+-wfk4n#vc;AN>0Q?C|_Mi`YKaH#&^dS-aLGkgJ
z`1pYMc)$2KO){87k4n$?NzX^5=X<5+!_xC1>G`1ae2?^eKzhDgdcI3~zEgVMFFo&*
zp6`&JZ<n6;O3!<w=iSotZPN2D>3OI0e5>@lLwdeNdfqNQZ<C(4O3z!Q=grddl=M7_
z#Sla%#K)xgD2tDqEF8lGZzSOjB)p!4*OBmA5?({X2@+n7!@zM89wXs62}>j_lCVI+
zt4KIT!aNB_NjO5nVG<rC;guxJk?;r!50h|+gjo`1NH|Es0TQN3cm)YlB)pu2{Up4M
zgoj9YDG3jf@Bj(-lW-pi`$*VJ!X6TKlW;EyyGXc)gu6+&i-bE#xPye-N!UrkZ6w@E
z!VVI)ldz41tt8w+!WI%<Lc+}?+(g2SI80tl!VM%`kHe-U37binAYq(@F&w(1B#e+S
zOu`TegCq=)unC82{3KjQ!nHW8Uqix15;l-<H4f`8BH=0$t|Z|K5-!J~u#AL064sNj
z4u|Yg5-uU(ViGPQVJ!(4lCXw^3rIMhg!5oDTjuf{V&>prHV$Utzzbe`iwA#l<G_Uj
zCl2tDeuo_biw%FZ;y}Qix(Vh83y;5YIAA#r*+u{VdHniV<UP-A<5qJu-mf}db_Se_
zAjyx||Ka!}`wUydvdqtzZ!w=^o?za=+`!}<*ExRZeb{@3*~YA5@AclvUd<kG-tT;?
zx9q%$ZS$5KM;#wx*Ezo9xZ82SS@d4+-RxcIVch=$y7n`!ce={1A@@Ikum5(}@15TS
zoq2)tW#=E@%)xJ5KX9g9|KR+Y=Ubjny7TT{_d3seon6jW_bf-7W2J+)U$CFGpSGX0
zXY6hEm3H2C!FJYm+IG^Gv9;M&+IZ^)>sjk*>q%?I+Gbs8<%J8vS>d#BQpgBx!b*X+
zT(F$AoVJ{_WGro#l^~4^{8|1qf0EDeZTw0Q&IRr)cbYrNWn6olANCybZ1miYmh&wv
z&6y4EM&fXvM0fi5jZ}yXc<VfV6)QWq<;Xi0@Qq5O?Dd{RZ_z|6W!$ccR(#)+=r&c(
zlKXoS-KvU|Jm8b)7FDF=1fN7Vt0E;o_#`@|ij-X8ljvj(?`NvK;V^y9<ySEpe|Qj0
z*7F;gDxbIP5f7sBB0f@02cuXJ-Bbrj4ZY&Q8*3pY%`YCjL8MfB#)H={;$x6TO_6-#
z!RzMpYqccI#I;_&o~ii9gV*@@7Wi)HArDS0<`a5=<RcGWUC(c3Dn1Kx0!8H|4;-h`
z6+e047=^C#ln2HQ=<+`Hk^x=vmIsOkbje>HC>YQsk9puKI$ide2gc}h*=rui8_*@c
zd0^ClE_u!aBL;NIcODp~(`D~@;HUvzJ|T1^m9BWu133e_d>ZIT6<y;+4;-$dYy9Ye
zAv#_5qzAH8y5dU@WUA?EZ+c*mPM7`ZfdK=$e2yngr^`O|z!e5``OHp=PM7`afy)i(
z^7)*81G;=F=Q1i?@va9B8PMeuIG57tvWGozkV;p4?12Mxy6j~S?5ENdKYL&woi2OY
z1ATP5>}wD7R@2qq_CSvTT|P0>Z9tdjm%TK4#pfRAGN8+6V)jt!ir+o3n@*QK?}1$g
zbotcFP6N7p)@28kuK3>r+YRXQxt2~kUG~8Twi(dnGb~%Fbj1%J=rEwm=T+LP>1tnm
zpp8mbyzzln8olC=4{V{+WsiKIWiG#3^?t)X`M@P}U`?R$$_F;j;+HWhzkFcRJid{s
zIM2zVD6p}H50Hdf-+c075?|$=Pi|Pqhv5BP<)2TkxAC(X@X$9U$+Cdrqi<?n!YAc-
zzursVlpx$`{q#+7QdNzozA5J7YZ=K`?~am6R=oA@$ZUQEqj;KOZQ%|RymZbZM81eV
zdw0;z&t<UJ-W?#~Q~T}RO$%W4Rpq%~<0tDE>`+&^!q==*VpTrxHETWmQbzIL*RLUh
z!AW)Q_4-EA;+Uei($zlv`UWC&4V|=J{c0+cbr+HK7tNDjw~D0D`||5n65c4@{JIsD
z1gg5#Ehj8hd-R26BCl0GeZfbh4V4h;9sGQT`1OT4vW}yA_SvPY`8^EPYp?O`vr9;A
zY2JNyv4*7f@3V`@ve(GN&(@MAG4%1X3zza6pbq#nM75Wnts&y1`1#odgd>`#pPf$@
zy~e(NcHUBcBQo~(H}NT;&h*5>ocX*T;JR}PvlsFaM0NPr@te^U9)ChMJc;0O^0i3z
z`TO`*G^Nvj5uZR>@4e#nC&g$S`*>54e8Rv%>ZBoKzrTxLj1<Q|`67G%XYnhL#`RCC
zFZ=ja=LIaad<?0b{~O|bj>X=8P~>aZ@O#;rc>gESL-_uGFhQDsrUXn0m=Z80U`oK0
zfGGh}0;U8^378TvC16Uxl)&Gy1n_17Q~&=vZeX)&rUXn0m=Z80U`oK0fGGh}0;U8^
z378TvC16SbBw+gg&2C^yz?6U~0aF5|1WXB-5-=rTO2CwWDFIUgrUd@}C1Cph|Nh(9
ztf46ZQv#+0ObM70FePA0z?6U~0aF5|1WXB-5-|P$W*;ymU`oK0fGGh}0;U8^378Tv
zC16Uxlz=G#Qv!eg5-|P$fB$W4*3guIDFIUgrUXn0m=Z80U`oK0fGGh}0;U8^37Go7
z*#}Gsm=Z80U`oK0fGGh}0;U8^378TvC16Uxl)&G=1kC;afB$W4*3guIDFIUgrUXn0
zm=Z80U`oK0fGGh}0;U8^31I!-VdaoF?z!M8x<2Td<=o-8)BYCQYgSI!%iYcXj_qc*
zn{qNGFmnm;PuJ}CGv&Rx(M<O0o{5ox{BT=#C^wob<?^E`@#E;>j@)pzGn4A1fxNBV
z?Jd3SzRulk?FW2^W|T+2+u?)1J-e&%{l11n+G6_~%k$mVnpR+QE1k{V>5(j*InOlA
zRZ>pF5(f7BTKDYSiR-$1Pisq0d*i8%E^Ce7&)n`Wr3Z$y`l{={yCr6NTej?Mr_|i-
zXb|O6CB)85KghSYeQSHSudBOrS4;On-;VZ!zLvh;J)OHD+g<Iudx0zX-?ww;Iw_Z~
zLjG7TlP#!I%NP-Y8=<?sqrJO*cWZl(?~t6LeYBJ-P4qV?5s*b=B?+`!UsoFz1c;%v
zrLA2<YHQ!wULncMhyvWbucf<nTT6FCI2LPE7}m*EPSP<xJUp>?JUyHn%4IXO47BYb
zr`rL&q#`f5#xk{Ka4=ge<_2=ZxQdEc^)Ou@tin~QtQH_1C_NL!Qg%cUp*{+W(pO3S
zMr64&KbXejvpav>xB@b$H9tH)GFpvuI~vOYr?n;p<0?hQ72$L+Jvy2#w5A7-WK-3w
zi=#{rbZJMKo@|(pYRu`=K;Nq_y}$89pM#9JoBPR#)2GsZcWOtRo($#7BTi3JkC_>a
zIH^HtJXiG`&4?rAPTyhvy8a>$YjvgM{;uIh1}dFbj;I>{^g-S`F$S}wJX6U<$Rn>e
zH<B%u(j#O2K0JB!cJ6|i6y_`36a98;O`wsP^pc(^7mM>t;XsZM8FNU@+PAxNZ=dq!
zkugdlRriu<QpMmMBgcp=>n0L~x-VNO!a%E<`6rRhTGQCboY+A6p`4kRz4n`3+XuBo
zRS%R2>PDJD&l7uw!mwDK$}}6Cc9xaPRjN9Z#JI`UTdg(ie!0o?9XZvNE@VebDLifK
zX*O#6=~3z&R86Ue;ROTj?Ys_NXM_cP0^FC&TLf#(QJDEADl?yI6iP)`r&P^<D!5gu
zkF*5RCz~d>Y3epbCFWby%c81cu~g^`3)3)MNjVL3G{HBX*v5;KbZcdjR!K|Gr$swJ
z^kmf#(UacTP!V~q^zs?3L=4-YGS`uL46mo~-y2vG{B<*YXA#S$-?yc+dKtJgT`cVx
zC}s=Cval?w%)HWS(bv}A(bBiG*Ecv`AS1RUPt!2S^PIINw3sQ^lVQ@6EtPVkhsl_h
z*4ttXukBCOIMR*Ca$ihfG-w;USfOU^TIL|>ImN4Bjh0OnvsaC0M`6{b{mN<jjgHtb
z3Y~u)q--p2Wvn%e7c-MtlBdL*^k6l49Fk&HT~3O5(|X#z^zb;eqpo*~FG-^RqdSpz
zx9d*le>nHpLe}eqFY-INM<DV||GRnCJnMYF-}aXM$8y=@c&*-*E*&9nz+ASd|1QaS
zUuXM%pPuGR7u}BioxR&&+_d&}_jK-Sug0|WNJ}+5zIs5#Xzle?&j-Gi9^aO9QL_f`
z%&hYn4PcCK75lvi5#QiA37#f2<kAseNB5pxRi#!&F04oP?5=zjxvYH0oOd-**waEf
zdYb-ncB`@=>|b}CZ}nxXeemdNQmP5{P}jP2q>@&wpROq}x~qce+`SvL(CwgiR!IUQ
zJ9<E^ub`nalvdiP<_AwYve;qDaXaMJ?P*%GPRDmq=@@RuRLstVNnq88x9T>64!RB0
zWO`@ub@q^jY`I~swdPPGQ$DC$RkWo`X+tfjA|DJ}pq)y!M8iC&uW6heo@gt1$YSnq
zJbCpTvVgs9t?nI-6IBx6Z}YWtq?V|fDz$_+He*)uP*|<JVx<>WrE|T3*X&=ZdCQAs
zb?sy|NAuQ~fnqhLvZPF<NmX70)34*-z_Lbq+4Qy_&>D_<(}ly?674NyV5w=%kCw7m
z&m;v_9Ky4~>JZUb9mw*qJ3Ewx$zU*>Qa<7SAR8VleM2G7Ga4)kf}Rz1xF~6$wUo~y
z1E4U3XHCa+TXrlvnn{nAreS*S^h{H;nwDu@s{Y2C7kjNWkqGk^m9<1GX^Ko|84Y(u
zCe$&nP^$;-476C;ej*PW+W4Z6dpX7Q1MM$1S?cBa|Lsgvsm>cQ0V?8_I%tKr|F?>W
zhA+Gz?Dr|ti^1gaUpU(+mM5)UO0qev^jJ;jM<@2_sHPUZ)51_wGo2r%^WzO!I{wNf
znC#d1|DW^2Jpccu8#&AZO$nG1FePA0z?6U~0aF5|1WXB-5-=rTO2CxBn=S!U|9{g(
zZWd@tz?6U~0aF5|1WXB-5-=rTO2CwWDFIUgrUc$}37Gf)z3C!13p6EQO2CwWDFIUg
zrUXn0m=Z80U`oK0fGGh}0<mO#lCzE^@O#Qv#+0ObM70FePA0z?6U~0aF5|1WXB-
z5-=t3rc1!>z5#XdtC9DD_q_L6?_TeE&v!f@_uTF&d6v8X?Eaqn1MVJoliT6?f$NOx
zF4vW=cGq0z&z#?Ko^?LryusP;Oga}je(m_G<4MOUN7}K?(crMz|Hb}_{W<%?_FL`4
z_PzG?_9eDI+FrFiV|&<ko$XRvv#r+pC+qjEpR>Npnzy!F7YnZoUlbk`ibAJQZ~1r2
zKUyBK9JlPUtl)pge}#WPe=Wb4UoHRR{>Xie`w({{*T=2p81|d&N7xhWK{miznAe!6
znOm5CCc@a!_sBnlyu;kK&5T_*4%dCBGH~%&I23FSCPK}@Xe8QHy1Epwy|(?i%T9M~
zYsd+RhD>0z7J0-78IJ0ZhpUl^a4Z;)MMJvMhpLhBNHCNPCBi{%=~)A0usNF0A~SSk
zb1WLxl|D#8hJ&G4EFOw!^BgchhGQWepJ^jxB&g%_3Ik*)5s7O{PZ=RY`aCZ;Kn9a~
zDfJs66Tzgm^vjHp;ZQ`2JVZx^Bgv>P&r6Mvp=el}=RpG`+<LD=9-t#b(L_jx+;4yc
zDe2mM9~~Kt1Y>gDFRj_S^sD8DJ_BSZ9G2@YA$tvw!FW)YXAcD#N``~cgdW*#gp4b7
zmrCDjgp7rCrFT)0A?Qg#xz9^^?lD5fVzN9X<Ze1L5s8N6J})7686aUE)A6~}2pJ8^
z<6X*ghY>Oq2}!+9LT;xc<FSaoH9L)v!I-Yyw;3R#Ff(aO-)e+RM0I?2(2=olGAz$F
zQjOY;ka0bqZ4_iM77hm^VO{C1bYuu723@;vp&~=^NGzz!v&8@jO{DF0OD+-6YV|u#
zBg4g;1+)sNV?*Tq*K}nuKMF5BQ5dzG1hfo@`SH@&cuA9P=|%xH0x6R{mLoq5p-t(#
zSU>?W+rj)O+`vCtDiTCe&fB*^M8LhXrQF~!zOcQNEi{cyD2Qe21r&lj2GhgCsj+k+
zHB<$eofOb)#wK)iG|BlcZx#@|;wy}$3fZxIp#(WAm{kb@C2`Kf>0&XpFIU7r_)!$A
zp@J=qi**!X_!6jLbuLR{qF_bx*5UksnmRu!pju!Ja);ae-7<T%5mBV$CGuk+kU}9X
z3JYija8)c7(xvR-2{mng2<HaPl*?$MgJL1O^7ywoa8ApcdS7~WPC!6&q3q%OVI}v)
zO}L?L0;z^nS%p~d7my#aC+i>(b$Uc>tu->+b?XGQ763iD5!@8j^tBZ_h$%NvAg!&W
zuUR9YrBHfLUw8MOtu4Lva>mOW#pc2p_Z}%^i%0Uq8L@B$)6gLHb&MHKk7h>Fg`-92
z4cXyLm=vQRm#r4Nj0UNN)L%reYMso{id6!NK~=l6>0v!f3aEaim<s`^1fl?zuMq1+
z01DHD6$MkjT$PDRY6@W4GO;>xkEje$LFRUuPpa`)AqPKwpkhUV%&8Z90|-Tln_DNK
z`OqGiJtcDGQUN8P=KFf~=Rv|_>A`GI3A!TTnw3ioE)mckNZOkpDIN_6!(m@0S3K&=
zjie7}ec7wWvIV%zeYBJwR#L23j3r7>Q2{N|f<mfn70~Kh0d2%==ko*kA^2qvC_hL8
zHY~(l1_KB39q`jq1gYj!$?~Ea0d0_xVvZ47j>{K_4cawPI+7oi?r0Y~s*=}=`C`|B
z3qr5V4wmSk#(4s2fdX5GV7~LoQlwVbvja73M0VlaL+748f9l+e=RW2;_x$<0&fjwG
zsisVR@M_y@U2WfKJC#n%5l{-_9z6Hd`SSV6^EUzZljlBl?kOMn`1H9K&riY^kXA%W
zC{FNov!Ms#gwNy5&!0H=>GOAlV1kj`df6<fG|+pcftl)4rY#w=+6$5fNJ$>NsnR(7
znh)Hd510Sr`CJBiIn=lr!4yEQgZ1shzY&!6b&d|@3nOIem1{H44U-xM=HX|P3Xq)4
zX^k!cwL&??+{pNFx|A=ZV3tW`as?1{zA%AbPI5^roY0Q}I+ibHQ<-crcNjASlc5^4
z+9C3-1;NW|D0i52csV!kFiKz<X@Kf_4WpXHK!=xP)7a=?xfb)S0>aTF>D;IaT`RyO
z50&eLpF5LAt6UH$_gk^BJMe|XM^dFiYK-(Xl6w214P(!G0kSfthNN*n!_~)f7BNdW
zcxwS>V7yd<-%~3=pH`JWn}x{$%5OSWl*?Vjz?%UQH09wk;gMq?)8bK?hWGz>@^>QZ
z6V^%V0c(?$7rrArCEP4rCWHl><-3-TS#GynVToH@{15mu{GHxcy-#>2y$8HaULGt3
zPkC<kT;>UTZ0_&6&$!>}E=tydAGuy|z0)=7>T)%?yv|=b&pV%S-s>zl_d0{l*^VDM
zKIyo_k#n>=mf3%8|C0Tk_N(mM?JI15wtdz1sBO%4vCV3Il}m6Q_ET&ZTgUtqy=L%l
zDtifg12Y?yEv#KAq(_VCLFiAT_=g`3<Jqg2lKL6ER+|RJcJN1%AjrX{a4;B(M&rq3
zG!l%WCqeGZpXyKB*&EQyC<obz>jCkV42us~G*B~WPBupp;b!y#$)KzEImqB9bXcDO
z@L`-j8kEzw5Bv_&Um~W@kHB&*n`#*zJCaTvix{U5Hp}TZ^`A$`@)NWJmp^rS!Z9(5
z1j?|sBq_*3N~&@-$CVUo%dp;T)zwkQUj+>u9YCq6(Q<P%CqUhjYm?l$Y*86js4HNe
zO~Ei4B)=_|3d+_Tg!n(BKFC;JhEZE6o{S|T$uK%YS}1zU(u>NlXiXX>BS0dU)n!<_
zZUCVC=GIpcXGxI2$vE0?jFhUEj5Ie#B5{e8u<yl1WmxCxst3zCF;jmT7Pwm>(=P4e
zT3bu>tc-6e!%DXk@URHcEVYdiAjG|d2B#}P;vG7AC<+}77q|SWz;na=)9}W~PFtMZ
zvWY~ZSxnw=rMM8B%3LR@aWgR2Nyr(P>m=k1%yklS2Ie{mIRkT@gq(r7PD0MWTqhxC
zV6KypGceal$QhXHB;*XtbrNz0<~j*E19P2(oPoJcLe9WkCn0BGu9J{6FxN@Q8JO!N
z<P6MpBC@FrDghqv;u0&>GlIwc_VJOiR7bvWJYC3;6%d+~mjU52=nH!*33lWLkIK+g
zckb|!Qfg;bTsBn_h035$>@i5RGdn~}6D)&vv8$41Yqpf?>q_;M@*^;Xr*@}6JJj*E
zt_<o$yKxd7d`%e?jg~5S2PiwKzA+tmSs4_L0D$+7;~E!X&2}uL!>lZWt`P%FS9UZ#
zT$+Gi@x(tU(Om@tMWY(FvkC@!Mm4NM2lJOf)evWRO`+LzW+K%AzrLlzH<Ur^*oc`-
zSK)QgwdJ|uD_VY?59dp%Zcw}PBdIXhz_5j=(#XropnG5m;RS6DH0CZ?bXQ?O&A=Ga
znqy*z(P4aLP&ND&4DKH~$eJ>!8o1@8Y&r_SktFS~Xb7nAwWjPb2J4%!p$sYqmJTIn
z9TQ8+pogpiXj>ZpYDhvyt}BD?(O4n-^?cQpLGMrl^t{xS?P@?gr3Wl2+teJybsQ`y
zgW^$PFjhqd1*AfbR*@}ga-@pPtI7Jx)t5o{z;>iD_!WlK7??dq;kVmh(F_YOapqZE
z2K@t@p+<<sDpiEeRm4<Z<b79WR+m8)*$C;6=ApZ#Kp86)^25W~0#@^zMpXGMFC$SM
z*CW-1t-evrZ5xb=R4*70Q?0}_B^&w_u9n{*W>&TERhM|ZgwZc`RhSjmiTZ!F2A~4f
zT`QW()F3t23$GDSJ;-2SG#>=FfGTC<1hyOi1<%$kV`GIpo)ddY*|B1(Cwr9|y7Fq#
zl%s{h@@`O#YB)|?Pc?@UQLtfa%)JfA=*Va+u1Aj3kl-W*^S$i;l-^e*BV?6}rD%kV
z>P_thBV<Hx=)cMc8H(r}Cu0UkQ2%reiM#<aTydC5bssfCCKT72gd8zK#=|;S(6A9Q
zs&|JRH9&&XNZUpZ!GW&ie&XO`{5|}9%Wo_vY<F13gpXTZbAFP4fWJ<-mj8wj;Td5T
z_fy+1yiZv^ZwWgevVGC=xa|nv>->(zW_#Fq()GV>Esj4re(d;~<3-1Z9QQb`bz~fS
zEQk1?JDMC$`;YBkv47l|<&WAQaBg?55#Gl)SXS{{?Z++m^H13K*@HqK|AKRleXebu
z^*7d^TVI8}0nfR<Y%N=dtUIhBYmM+*&->i#gr5nox|X;s?$<rP@czKP*ma{T<+{Wb
z_k0_6EA)Ccc~*Ixt`Av1?EbR*L++d0`z*t{e@nOrn1$SJ+!&YUws0#r8~Zc%^XwDs
zDRzL}2zG|oE9M050o24AD0fZaa5$D|4uRSej)3-vzQmqDPqN~8*E$=t`c+dXnM}rm
z!DI|ny&!rVeGw8KV++~gd^)4kASEm~kRtI=96gD?0NCv;``6hr(B~l+p52KO61!7b
zG2&t9tC8k#Fqw#>7tq()+t6HAt!<+h*t?ON5%X{d+4rG23^6#@!AC1IlIGqGMrg@E
zW`!>{2Fz02W5|x5EZ2u-+to%X_E*{h!1%%~L$es{<_N$iff?f#paqQHQfKD|fDPF`
z$JK+&NM(K#xCS(j(OSzmh;vH33eN-Em>>9HC-oDKRq+8)b_TpqNlP9r)-oU)9-YD|
znAbJD;WwOYZglWTD2G%9KG_(sAzO}v7f0i-Y2sf8hujpF<9QYqsw#_=zYU~Qd5>Jf
z$Fz($p~w0EB{5!1&Ep#xM(m}EuPZ*6UoA(<9<JCNeg&g$Xg4}z=jSpa^AqmSEFR2>
z>V_-1_*zEk2MhSMOl34+1KZd{6rG;MuRxl1naejKeY<!V8$TD3=J4|MjJk50X7S4y
z+{&`6a#IZ-U>KbV1pJE|7V;selg`uug!Q$2gkcnqrpj^Hyo686V$hCYa2F<A{9>fY
zyn)}vPCIyDxV%EdtrSKUT7ivv(I(Ne`luj!ftky%Vpz=*3QZE`)r$&j!o+X;>*s^O
zrG^#9B96UwF`oeWX_gZ5`rvAPnb1+J??nPTxSVg1YbW**?F<HU)<>7|n`H>;F;$3<
zz}(KVx^~f_o<*Oyi0_1abgfhcdu|ShLN`c!d<!$>AX&is88W(4kiYD!tXuBj=Zn!o
zT^+xHq0XhGufD&Q53npnCq(Zetw8e;Em*|Ipf}PyL-QB$38b4DByWYYj^C^&sV5Cv
z2sy-*PQDf~qCW2?!%cD(hlqeQ?)}YVU_cb)q;}0;Y~yDGi!h5V;bYK3RDBk`lR}iW
zSaiSbGBiug$|4w2tZrQ$#r==j<a!XfWR6$xt#YRk9p<{%9lYS4)^|#n{#WxmAlY>0
zYV@**)34lho#_Si85JASNU6fSM5(^4oum7tu3e>Lt95ep4j;b}4}dC^q-~+M6Fx)^
zh>BFLf1`UzAEapA=pI6$=-TM+g}k3-C5lSJMsFkInxg$GdNsPs5F#t6=uT2EN-lD}
z-a3z8#j;w>8QrmfZ&V@`eHpz)6RnhSyDGY(8>8D)Im;R`x>Xe^>%r(2Riv!_qMKEb
zvd)W6sUl@f7oDtuxr43JZ_$Z*ej~%`wNXWHMP)J&^mIHB&`oualrdCNZxknXeJ6*M
zH;^esGvyO^6OgVWbD(y1*C>h;YxupY5vSS!qwDdHqgN}_rD}UY-Ack2Ue8rFWsruh
zTR~O>J+R%No>a}yTVGqxZ-yDy&_};;DNMs6o#L6ValKNRKzw{FV&H{m<QsN*-immr
z2x2uWFlYegk{ExuY8Hv=CZMJ;?7V|rZP8>H`(a*SHqV0fhrBk&3z)|m`MvaahT4~i
zksoN}_pq#Fd)1gybyiXIAX!aFNHVbKwovq*m3+IBr5-2`zX!+=*5;<Qd!l#O!rF)>
zjViWvO6>V{3`?3D8dbk1roM3@tg~25(Ii>JaAe_8ArL;+tRsz%W0h?&n!P*?WO=5c
zlZ5rFy$(<pFhTX!UK8hYf2r03FOzR7tf})_O9m9Fy>2g3-73)iKA>strF;Skrs-yo
zKR8p-7sVw8EvFrH5ksPom`F3wAbOD)tw>B7XjNS<#HZv@O?`nig%gQ*C<vPbVZ$qU
zOPOs`SHt1>B|5;|hrpNrJ}=|>l&8aer)Qz3<bK}$6UTQQpLaaxxZiOd?CeiEeD>ej
zU$sANzsFv-m+S}ZZT6LR-gd!u)^^%<(w4Ec*;d+k>jmps>uKvrYsT7UT?xAhE(m9Z
z)51w1BeV%C1>SPOa@KO%a?+Bqv{_bKc>V%^mOss(<THF5zmn&<3*1@mG<TBAaBbX5
zj%P2hXW7&2NjAgoV&iNr^AqOt%#+N0@;_r8J&_DGCz_Mtc%(Us&M*l-zn`U}LHy(o
zL~%PEfj0%{^8@r{#%R#-qp<xG_7Eh%LcqiUJ%v3X4krR)v1q(GhP}~DjJ(Fxdv?Mw
zkD*EQ=zN&q7&QTJ=9#^c-^#G(-V0lNU?b4Ecb|Le+)H>Tkb0LB`bHC<f}}IuKs09w
zXb<c)N7AE*d&aY)Fel_>TW=J7ki0Oe_7eK3(RCM*`9SN^RhuA>Q|T4M<1q?dZEhSl
zpvy+ak^x;dEfx*vvO%$6K$p#kSJCN;@o<bzS4@U^1G;P|95tZJ=D`sIx@;62rqdNu
z;86p*d_v|*D!p<pCTBpGPs1FkqHE8<9Im2kPreM%>B@PRER|k4<&vqUYtFU|(&@^H
zmH`90e2yherz@vdt}vj>XI4^lx^hzGas#@2KBeD)E}u%dj7qPZMLA?ZmrtNvN~bI5
zP7YG(mD45%=yc_b$$l!ma<XI}ovxfG>7&z?QzX6Bbj{h39s|03Vx-%EF3&G}X>`@;
zkS+tdd?sWMm0mdsvYSp<&VTGOpv$K|b{f#-vmQIB^vVg3?FMxDTt_FJuAJuBW<ZzE
zaBQX0D<?NP4CwNCjrMA~=9ESom0mfU(MqGMPGoGM)0J}=EyRSXyx)jh?Gj>qqwlj`
zN~|~1v4JQtYRbFR;IKfXj!&|z9HG<I&Y({dJ)pOc9~;i%{T#5JqocFC1tzqlvT*^u
zkLUqZkgO^^LacASg<N$$L*v84GVHwz`3PbtyMt$~=Jzm+yt!Gu@mjk@d<j``({Su!
z4N0><dJ(aT8SXgtdibRbqZ~C@Llg?xS)<xRzic)bvKg5GD<yZ>yNG?rc-5ASz@j;v
z2#1^Fc+D7vZE)`~M2DKg-~)<AW65|DHtsPY;}s^XA47@eNHa_+NjT6M1at)QzQibR
z@F{%?)0v*?UH?8H9F~<!!;L-YVWR)jbWr_v9`w+EV|7(+YMj;t@c&+erE8rtHkVA@
zgQC$WD0cB!a|oP=XPB*1qQm%%b0Qpx#giedQ*=y?ZiX|Li9{$EB%4r~_9@Y!L?RZ1
zY@>j0hP>ORM#FG|wFsQJh7}>8Tc<>aL!fEJ!=MXA$vD|k!><Ly#~AEUI-VXrD%%N}
zmU@1Jfj&1eh{q3^;H3UP#V`?#5!7LZeq=O0@y^v~0yMl>Bo@YPu@Mx=Gsb9e6-VJD
zcQhI%^4b7sV@(r)u_V0yW6epT4Xy{Yv8EYnZib!eu%jauB${Rt&{Jrdp?Dm;*YS9w
zIgDqxW(WI!(3R}3J=PLv-FvuC*gwMknsalfY`=!x`3ZiB{Wh-7lHrQ>4(<v|tLH)X
zcb27&I{vo~fq#ZuZ?ChQ;J;@7x%DpoPUiwA@2YiKoxk$_)cY;(S?@F4m$=uwZ}*nG
zz1~gU)y`XZmv@$P)Y)Zui07?GxF<Znu=eo7{BF<xun+Og&IY(u;6>+YK4ANx-EBST
zc+?t$y8*r_yyW<q^$&1jAS0xNEy5aMzU7aW3-+(sUlbk{P6<bZ{npP~{>k#B?VGmG
zSidd&o<Cu`)iz|i1a2AlCw`QV@-@7d`#sndzQ8@hy_>s+%R@c=oR$4&_6+-W_ImaT
zHpV)b?=c@^PBU+1Zej+QHjp9w@uMg{y;C2(3OG;YUB%nbx@o*x&jH4U9qH@?Xtv6s
zj-F*diI!IRZ7zmbYgf9!ej6=-hxRn(2KEogr!Q}8Vm(CLN!EN=TgKxMVeRTj@*B`X
z#XDJXg*JnSeKD=pV-pE(1F92Avi~>^QBE;xD|ZE|l|G6`C1Vg{1s=E+S`N-|*CD6q
zy$W$5<P;5ALHK9`uULSOqWNm4coRhMToZE1UT^l_kPZ9m!Jj-!?S<w(f;@^d{2K0I
zZK=<4AatCp$i<Lla4xr2?=ro17Pms}_C_x(;6fBX@_PPS$Y36~R`28nAGa=t$LDa3
zI+uHVHn&=ek{s@_I<933cji1RH<viX9bB#CV-M=fc@meSv?f`-ZLHxUl_*sM>(idW
z`D4L*xfL4!JkC?MZ9?E?ga0sDEY9Zw)t>haOSz54Zm+j{xMeE${U-h~WW)UhS4*6y
z-3z&h-rI+LiOeS9m&hq$d|azm?z%Pd(<`|h`b_jEWYEh5G>yOTGpo2=(|h_~sW3#f
z$fB3(xXn^FG=KR=7I8_bKj?QJBxZ4|G_LQarQBv>AXn_fYRh^9?yl8#_0<cx7}Zj}
zs)h?u?9>c9kMmbqsaN9eL~#bKP{1lf`*Pf+fRlO~H=iV`Tf%JwKM(y-XDzOkp_6Fg
zVs3+xi>L-OMRCS2z*VNX;^$jmS2|KX*E*#`sg_F^`0O#i8ZVP(pU-S=m1gS(njD+3
zJGsTm4iYc7O0#<g4*+R848zi1=kA``QN?V=6PeCuvbK=w!IHtzI`2fI?cZbpq?T|S
zjJ80a_g}>Ak(G=oJXjCnAtEiZ^j@jTR{r-a<Jyf-qNe#kJ=dZ`XdGAX#`8_}i!onV
zzy+#3Qw%2yMdfB$Xa6T;!xF*ms&$+~&&Atjug1eoy^&(`YHn|3t}%9@zKXe5zZi8>
z(nptbTc_C%^NAH)=X9H4p3~3V&w99e*+T{XfyW$oAfb?5#3kfN9Utd$r_gn;2vqzA
z60e^P$0n}F!$_=`v{#{kPdkoHT<7NMD_gU!!S64vMAfe72`t6wwk-ntA8>9$H5A-@
z&QI~_M!4Lxnd++lP25?kH}$*=4_I-}g0wZ@R3%2*7jTj=q1*J1PPo>hIU}l#?r0Lf
z0ri^z<V^yojNfm1Du@%^gr`TN4FWgTaxwZ|op)AKq>VcFS5u@tEcaFFCheoR6&R>Q
zO8Y2osl-T|C~iiGEl~EfsCK@*10at`Jq2(4uiDjeF9uX^X}JeSlj%cqcI0kVbY(Nk
z+f=c%6C!sR(IwUKP6N6)5xo^tH5*v&P{qi*SKgwIlk&a25~u82xlI)(=6@^1+$H8O
zOB!OX7h_bCKC11IfH^3}$Q=@5wraZ`#KiQStJ3v=TBE0uu7?owC&R7>#P8E442b^+
z&BRb7TM@5^_@_1T@`M3|cUIQs@a^^~CJZ1S#pEU~rCEPfZP`%!x|wa_EL*kdO5H*6
zT5ZY>il+!l2gT8p4vJ?5DO%M*@!LR(rFKvwy4*qW970Jib<oPdl+GsIF_&v(#m<pX
zI;c8M?x2;pN(Xf~lHmWp4tZ-m|Ki=~`Ge<f_y6_`d7k&o@@#N4I_5YmaGT$k?H{+_
zWj}83u}AE)ZU1ijk?l3x%eE(NciW1#K3f7b?cZDfm-Qv<Bi0kvtaYok(dvY~`d<;A
z7VZ_sh5bTOSPVM*_be}49<`jZ9JX{?)>_>BFZlEPv-~^w<NQH>1HY8}GxvS&bKLv6
zTeuvz12l9m`#SqI_T%jR>;!uV_UAXSbD7^VuP~1>_b}JV|FE8p&>C+46vh{o$%AR0
z5-R%m#XQv^2x*^UM%KPnn!;+63K&$9Xssbsy@!BF;NGM5<r63Wqj){7jwK#`+_k54
z@S|_|xzu0a-p7jss&^lK5Hq0n?fYqw7vXiV;`bmoE2{kZ$7xi>tADJTs`BZN)2Wij
ztwg6v{`?}HDtYq@bgJabzlutgJ^5o)s_e(l)2Wgdf0Ry@eE1`Ds^q~Rrc!19{ZTqq
z^4?!bqbk1p9Gxn8?vLoH8o&KvJyqkiAEHuapM91_RXp~YDyrIFKS-s@-ueMLRr1xR
zsZ`lhe+8W?`RP+ss_dn|oKBT|^!;?I<e|TeMpgXthv-zvJAWyaD*NUS(x{4O{s5IK
z`{no3sESv9AC)Tm<ol>p*(2XuMOFLbd+1cj8{bW*N;AV=3bo>i@1j#BKl~mVRq?{_
zrcz}e{4P3G^1$z;QzifV4jNVQzHg^fCEt4|l`4DQx6!GR-+e2Ms(9Ty=v2w)-d;sj
zd)(V-RK?%kN}*P~?OUi++1K8JZKui`2YcEt!P-34&%P9!RYWg)6k8r;m-^U5WHFaC
z^00pzn<}WDbJ@NAKD>ORBE$vSBUqE4#<BihtZ>u3>Z`at(>v9dV6~j)QeUhls2%Ey
zuv9B93f%=>UKh7i@usiAMnri5Q}LxQ!^UAb8uW&+x0icD`L16Qsr~3objl@{dg~(-
zn-Z$tQ^~}^KeNS%0gY>(2~Cx&#xGC2@;^1mmAK@8TJ4ed;Rc_^(fBa7fk+sQIZfw`
zf9SuN*-d4epUzxrWN`coEOfY+Vhi10WZjlceJb<wTG{;kUu1RGncb!^xZwdw+iw0}
zXnI#n?iI7!l>J}5tAN=$6;!jOfa#bLs@_k)v`+=qY$jmZri7|@5-_b(K{eY5m@U{@
zr`toov|#(U;aH!^peFv7s;vV)&>W4YM)k%4vTdMjzyc_is$ByN-ZNmRgKD-6FnGg&
zp$@9qFTf<-j$yRM=3%{GalY)l$C-ETcCK=~?sygM^WWoGVgC>Nx9yMH@3i;Z{q|X)
z_rG9!udM*Sb&uzQdxtCI3OIl6UhU=`x4O={9`$?{em9`ov&j7e_lLdrdk<Ofv)(5x
zQ2ztQ%s*2CrUXn0m=gGJECJWO%x`CSQ)v=5t3~1d$0+RWgPTrwF8j(Cmi!XZpPAuR
zsjyIpN8-^0+=&9GE+K>Z*Z$DA6EZkH{l%a-Z72pmYY~ctrL?~({0Y)-m|@{?m46U6
zC&t5YD+|eB>G{ng&%5qL|2e}9;CL78nv6#haIcP-!T727oPi8JIX(9zE&M1(A`F|2
zam|;G-}t~6Ano-CUdmTbcWJ14|Do-*gXg~Z{l{I9PP!ph@66R}Xn1V_2jhb9vn{0X
zgXcP4`s2F-<re9_SN&;ZZF<bEcze-bazm_`Vx~95ifDsdUd1b;XK-Dsn9EGAYn98R
zUb}kfviHyW^!AxwyDDb(7hJn4mkz%kpu6-{`MHF(W%!MN%GJ6C*C@yhv#1Qe5unQ|
zDiha~;l~2-O?y*pT6kOrH<sbY12!vQ+8!nayQmDm6QC<q`Bet!+DlM!a<vefgqmpY
z#9cnQiF!pFxk>^!S?K8EBKMzKS%#kqz*pE)eHTjo7M9_60`MJc((zq5lr7gLREFON
z*o#TiTnep9b5R+7GGK#28Zd?^nb(!!X9MuhD2Q^Enn(rkmEmUt@GXdH507}bQ)X~o
z8GbbYUw28hG09da<>t;c<z-@1QsJUWN&#;w!xf@A$a}iV>xMhrfh~5>aCx72PpDEs
z<6SXIr&w3+gS#x@PQWQWSjr{)?jo9%ERV&?a8DlYBz2Q;3AMom?X@4dJpRPSNw{;G
z+^emfwWi)VMdq-DbxW@(_$J|2L;PbV*!Cdq7?MuKD?e|sY!a?D#J^QSlz9zC`7wu8
z<-O21v2f{U#$_3LYvA&F+z5lin<nA<QSl@Gp%Te69h-zXNIN{>g7Vm82PGDMszAB6
zT)X2Rid&Zd^}lY4O}0;yPOYos3#cz$7W!4bGd9^abvljWPSW|l^3pZ)Vw0`Yq*H70
zV!HcZde39A$t_c-)2Q@fx+DL2<0bWz@XlHb;?(K)<f>3#<gwyJu)JNobGCYCg07z{
zuQgbkwc#d6mf==rxS%{X2^WJ`HHk*}DrFrrcko<tavMpjxi5aoEexbfE^j*Uwn%Jp
g>(u4p1()hD2yV3Z&t2ErRQ8D%|4w6oR%V3%3y5O#fB*mh
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/1afd4ce3-dd99-4473-ad13-6824332ea171.vsidx" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/1afd4ce3-dd99-4473-ad13-6824332ea171.vsidx"
deleted file mode 100755
index 791f3f7c27e94d261e99558307ab6034fd993172..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 49913
zcma&v2b3gL*}(mM$vNk^^ukv4ZcS}>Pu!7_sqVVnT|3oPy;arIJv%!VQIcd3khCC4
zOh}NR2qH>Sf(bAH5(EVll_iLPm=yyGNKo**_u>C-zsK`_=X`U9=XY=2P`7SZ?q#>;
zH`bXqZ{B0~>7aj1+Wh>F{_n6oB{^?x%^nj21-n)ls1{0v!g!(JttiyILM<%R;)1UV
zeo*i`g?d=1$Aus&gsKn*g)lC(;zAS`+FqgEEVRQyJ1#8m6p}(AQH3NZq+TIag;Xc9
zLON5(nuRPbWJ#gZE_4fpZcylU3v*5(FD$T%3x)?5R67f5@d95h@Ph>^Tu_e}1oZ_$
zYeA4LXnPCV(SmleAW;ia{TnPu!v$%!Ae~u|sRh&CLe*PX4;D7n!Z2PKsfEis3scW3
z6s?7IYhlw`7+MP>YvFQhVQN{9WzAXEY1SgwatbErlr1N<++~)VSxYQyNoti{t6Z_h
zEz7G~HP5PrRxP$_iRCNH4=lf9)jg{oSoOqeD68pNO=SgBRuEasVk=Zu7+7I!wF0XZ
zTamKj$ZC65JFwcJ)sC%pVlD4jiL#Q|N;;MvXR53;u+q><Gb@X&EU~h#)k&>RW=(t6
zw6eN^)w8UgW6fCBjE<hQ!m?I4*2>6Qxzd_*tUR|1wp|#v3!Yu5+l9z3wCzG_7dm#K
zYg=X8s@axrTXoxN*j8j)v2C?&E3vKAwx(@srEL$|_ONZY?PAd`I(Bi~UgX&WWqY7v
z4-DD^L-xS1JuqSqjM@WZ_P|oRRJ2QuUDm%PyIi)*6}voWmxt`~uw5RtD~??$*_E<g
zso0f4yE0@~hV9CTT^X}0OYK3&9xT~|6?<^V9vrp@N9-ZT9xB;GWqa7MhfDVGusu9t
zj}+|@#~vx!qmDhQ<FGyE*kdJosbhOp+l%a4(XM%REw*bN+xKigu+_9(ckQ}o*F(GM
z*-d2!MLSq*2aX*?cF?wi%np?u26h<REoHX?yOr9JXGhA8Vms>Can){nc001$v7N+r
zs_Znd)6h;cJ5zQR*jZvvdv>>JcY8(iGhQst78lvYMN?+l+`PCXEsk5o*-~+CZjqfX
zT2xuIXljwWY*D$is8U=sIJd}47ELW#G}T>fr;E$Z;&Nqid2VrKZt?K=;^Dc)X=Zjw
z%X2K>v6_w*I@WT>(hJSU@5ra_*aME4eLc&1D)rpyX&Q3|%FaN=DHWZP<CNx{a?vR}
zPPyciN1gIgr&4q(C8ttxDnm|X*r|*<l`&_q=nR&e!69dG*clvihKkNm$r&1UhD*-y
zm@`^(#zvf_CCBSJYT8MalLn3+UX~1$i)L-*v4QgVKzVMUQW~gK1}cLCm7#&k@IYl`
zpfWm885<ZZ4Gazq3=R(rjtvZz28M<QhDHX42M31721bSlMoR-@`gdetX=$LU2A0JG
zi5f`tZ!j<ux`m=!vfPsEmP&5fa=ohS)m+bay@u<Bt{1sp+w~H++Hk9ZTMgZ6>{dH&
z&2wv2x2D`$)2)SWEp}^(TkE)f+4ViwSFRtpe(d@kSNX0AT@|@%+EqQb?zwf}tp{#B
zcI%1TP;N7Do3R`CZV<Ua+YM58S?n%L-6`dUH8%|0Fm_v>+fr^Ta9gn(DL0DUc*>0<
zH}1G?&uur|cHp){w;j9f8FzWdO*}WLx`}d=z)fN|>A0!qrpis5ZW_30=%#HqUGAot
zo9dz&H}l*~xmn<5v704s)^j_VJFVPq;`SW37rHZ!JL9=}x>h*uiqOsTC021sD_)YS
zC23qL6iWrCWED%6j&r4A(d-??alLDn9INCEl$?^@KlE;)cLM#v>rcHTpL)rB>S;-)
zzU0<QOVU!QsH>LDPq|d8luCo8(om^1Tq@0#%1%k|cV?MW8gfd*`gg1}yR0-<)VpD6
zZnl(HOL<btQ+=K-TjOPG&g`4Tx$@%SvSXS3$SIedO4&)vZmH}JmrGW;R5W{TX^CE0
zxl}HfD&^8(xulCG%B8t-DKD2T-KJcg)92}OrBtqz%auyGGFYw*l`F&L%1F5~TCU8M
z2TSF_x$;n{JTzP$nkx@G<>69!c&<EBDvu18N9M|-#qwyWJUUt)bIW6;@>2afTwc1g
zJYFt0y>dG)FYlCRmzC!hmGvPt=T<B^l8SQ`$1)r3lq=3)#Tlx&Ud2r-r9!1tG{>2l
zBRwF!RjwYoIqXbc|IAeeoyuUTGB{`s#Gz7UXt*+5G)LsHTNy4@Mx4rsTN%-b5p#@=
zx|PvVWlaB;D@#k2rNfoPtIRE`=#k{zK}(Kc$I=r&=nM_I`gd%wR5Zu5{xmB3cVuv=
zG&nRoI9wbYb_a(`gTupvBg2DZr6J21vQu-!n-5KoWoW2WG)KSrT<3<$!$ZSGeI^(h
zE)9*8hAn5fSR5|uIA^9%pD)~@VRv|VNjh9AnzM!86$W+Q8P-3#{+Rw<Iy^o%oF^le
zGg2HMaY`f3(1<%UvLqcT6-P?WNU1bZnj0B(M}~_d!_LUCPLxJQMn^`Sk<q!4JQ=l|
z(c-v1d5t=QqfR#JmPVJPqotxgEsd5+qeG?9;o|79GirY3M)PFMa>kspKB<kl!(&U*
zF@5$bmBz}$V<V-prKPbvS!y{;&8cv3>5_D5si=4HrKQr+($b~F#igUp(xszI^-rFR
z7aV=%AGaL63yhaX#@q4uTydgMnkb|bmNQ}b6IOk~3MQ;&6IM83wI;0ggq2KKy0kZ8
z%}iJ;ChXFLU7oNj6ZYVQJv3ntPuL?9_UObwd17F2Vqk2dRGcU|6XoJW*_o*5-_pcj
zX=1oEF;<#LyoppzOnVdE-o&gmF*~r2<#+|hvx}Zx^6b<rm%L%e8*#i*#~X9JrH(gV
z@T#6yRbDmps<Btiy_)iBQ(i6eeB1Mjo?r6(vgg-4-}ijw`GM!ho}YUDwCCrZn)Vuw
z*O>4c%4?)vqw58s7ero=ddp%jw7szCg^oGthGj3D@WPrG23{C?p)PHEVd{k)FFe(2
zDX$fHQO%2dFH&CA@S@a<GB29(+K$%_yms45Y%eM5)3%rRUZT9D;U$5WOnFJ<C7G96
zUTS-(<E6?=r@S=v(#*>|FY~=jd0ErT0xyfbtm9=fUMKXXmDermtCQCay>8Fz=3Z~Y
z>rHzzo;MSDGi`4s_f{xxHt_PAm+P2$`E=DPRxL+gPO5HDbz9YPsXFXby=v8~RXtVp
zlB#M{RajLzkyaadHHfOqR5jGUaW$@1<Ed(#RO74~cdBu(ng-QQR?Yoto>ud$Rw&jA
zPR%OTEJt6iYUNUG*wOP<^ICddYF<+F(wdjms(G!ZYRayuVoilL71dO`rjnXUYt3S<
z>C~EDt*L6wu-1%g&7{`MYg5(Ql&VcNYEwaNDzAlJEmXBIsD*JY_G@uii(9ogs>SVE
zoYdk@Em5^3s3mbNwQFgymg+=a%j{ZKtYuCub8DHZWsO?atYtwhi)&fCmMyPkNiEB2
z*=#M#Yn`yxNot+6*7a*MwOU@S<*Jr9Yk5%1!&;uy^0byu`+H3Ig`!_@e5<H$F@4+i
zZJj9lcFnhyZ#R8A@a@RA+rFLncIw-iZ+Cqs)7Nr+arNEQFIj%M<d-Y{xIVg4-wXBU
zZT42PVKcsO`{qaae&Va5uN;$9bze1n75Xajm0q6uYT8#_U-f*In;kr;`9aGMB0q@z
zAn}9L4>CVgepK+I+>ezX*ZsKV#~nZJ`fblo^l!~il%K?YYWr#CXHCD;@~2&YHt=Vc
z`E#@SSn>0!pZk8U{JiexfuDzd-tzO<&)a^U_<8D|+Es<3Dmbbzrxq$@6_w>EYg}1#
z$}T9|Qnsz^qO!}%t|)s%*-F`UeV?lDO7*>_zQt7bw6eRZSX9Mv<qRuF-vM~at?GLk
zefy$ZrQEtQHz<L=;m~&y`tCuM3aaF)(yS`ws$5dTOVo&?MwX~iM~yB~V~!eIqLw;p
z=@K<=sfmJ`(9u>C18QPSO(aUrIBKG+_ObNYL3yD*lJ)VXyj)dNRqf~<QSW)G7VAAm
z`Fkk8pnOYzw8}3k-!XSm{<!i3J-N!C)l;KXL8-P<sj7Rb9;kYv>X~W;s*$Khu7XG{
zi&bc=u&6@E+=Yb|6;7CAE>tS4t1wWZuFzIts={2gEY-4AtEgIzYPqWAsa8$3lxoGQ
z)mE)kwKCP3Q>|P@1r=E;vQ^}n{UTB-3RIM;C{s~K#i453s_m(^PQ<F6sidG1OC`2S
ziYjqb;;LkcN-8RuP>E*_$0Sfmq>@A>sY-PHX_a(UGN+PUr3IB*Dz#NwRH>s<SEb`B
zozO?H-fopPRT`)?QfaKxRHd0pJ1U)3scv|h%50T6DyyojrZQh;N@aDG1u9#nvQT9$
zl|?FxRn}Hns<KRF9hFV1tgEtIb=qpWpr)0Y4%Bp}x>ePURX0~XTlH$H7pPua^)fY+
zs@am7EvwmznjKWLLuz(d&5o<tK+P^wvu!n3RCBJHn^pP>kXKb+)5nd<>nd-mJWzS4
z@|MaYmB%V?t2|YCrt+@Jb9HLBUUcil@w(&H9aVSIy6e^5YTd2XUBB+u>u#g&26cB?
z-3{w*tL{d1H?F(wy1TsYCUsZW&+2Zc?)K{LOx;~k_iA-7sC!}EOY2@oA6I%V%;R;?
zs0U#^i0VO752ouuuO52!Fs{dTJuaI2^te`!lX~Kr{VR#<S)ra4>sj6?I1M{!*ipky
z8^!U4+i1AU8gANHVl^hZ4KHi>`dU|LD7&GG4dv)#si8^@HQrDY4dpjf)KE!7r45xe
z)LcX5dK`_0YBaM(5H&*8i1csXNIbJUCvhXS8)>nTI*rtAblt|Rr7s$dJZ$9cMxHeC
zv}rAF+Qp_l(6mcUyWF%ZO?$BEdQCUc_x<KM)hp_o&89cp^hTQAXww^OdP|#L)b#X+
z;hF1z3Y$%@8Kli1Ylg*U=rqH;83)ZcY{p45PMdMojJwTt++5yiCaRgl&7{-J3eC)J
zX2oWfH)qmjUe%Y8X6`rhAh3#o<pg$7KimezVo)3p77YfA%fUcdU)uFmTwfZ4Y8X`Y
zx!wwDUQko|suR>weW?iiJ@g4Z@QZ=(1pY+etH9R_=k(c7p91vp7Sw~F;RFp=&sxxE
z28}3a%m!1HAoPP!1z`}h+@R$JEfutMx*fFApp^x!PSENFt=v4RM-%1-B&wP7ccf1k
zdN0s(9|TbtM6Dod2T>YCSrAPJQ69wAAP#~!3ff-K&VqIxBq~UfAW4I?7Np8dTN(su
z7NmKQ6@tvtuK|Lr7-UY6xk2UySuMzvp4uP_f-DTOILOi<%Yv*EWHUjQ2h(0Kt%9Bv
z^i;4y|Hi>wF_?3Lx$$65A5OJpaj+~-mZkBsPP%NaxNL6mvN>nj+`v?E$<&f`s*>oN
zhAHJwsccHkOf{0JMm`lpQ$aSBwx_zqsobB+)2UOtVWAk>h0xZq7TUf!N7_MXw?ezE
z-$8`M@o>>1{YF4P?&^oz(Dlu^(`|%qGjs#}^ccEXSa!mS{&n@eX*f{|CsKV=Xl@p~
zM(F8u7J4&bH4dx#>Rt$IR#<b)<E**Z>x)?}^k>cMJ^d2PT#$;Pazg#QtIDAo4%JAg
zMng3gs->ZtFn1qHUs3fryRMH3^TDbxRIN}&p-MuP>JwS0bZ`1pa#~m~hIL1u4)iW%
z?pXrgoa2Lf7&OA5r9ad#NcFsjVId6dFf4|lqx%ZOAT&RD7|G3MtZ%{e{Z1o{qcD!c
zxE;nx7^h*Jg>fg0yJ6c3+fLYa&24GB8K%WBb<H&-4a0Oc%&ahT!)!dv>R}dySsG?}
z*im7p6;69$UJdnKRqpExPM8N_-VXEW@YHUr;Iyn_%W_(Fv1ON9_DIVfZP{byiev{Z
zJ8cz<E!S(gNy|;mPVFsgd11@b>8$1TT2<Am>N~ThzBw~5oK&%;9P`dVjknZ3E#<dV
z)KW=Hr7bnvQgiy8tM~6#khFrV)v{WxnO0=CqHZhpTXEEivsOIaYTK>0*J_8YcHC-b
zt#+r?o@u3)ewEcqi>=f(m&A0|Totoss}r_*^;WOZ>ZPq--kK@5X7bkDY%4Fca<7$F
zTe+{VoUOdx%EMM3xAL@gYBwqrqk<Dz#mLezi>x{QN>RTzjO=1$mm<3y*_Fs1)bHRT
z+c(!+Jw`i?+-l_dksC&C8kJ|GiW^M~L=&ZGq8v?BqKP!}s``_RyjE20M74sx>y2tn
zea{-zlBkyHd&J1End`P6Mt&UmN#v)IpGW?w`Za2#iji_6H6E#&zR!qM7^zmI+WPXW
zud<QqMyjVTSLRevkE2FGpZ%joF>3hwY_8ARQ6n|?0_HTK6Im2GQRqftMej*bD~%!*
z#ZeTeQJh6_CyKjK+l$(rNWUG_w-0IEoNv-NN;^?zM_Dn-+$bwY*>IGNMA>MRjYZkg
zC>uAAJ6SEtd~?T<HO(n13!^NGvN+1xQI<qm8f96Ot&Fl<zix~=#i-*%oodu+Mbln1
zt)gxa%{WnBjq;klQH}IXO|GvtVU)L`Jc{ymlrN9+B+An$&!Rk!Ehn~%vFpWdsvqj~
z(@|Viadnw~R*9?nroR@~R9u^iYn|BlVqfXok+@!r>*I05jvK|e;lvF$ZY+r#Ufih1
zjUYCcv^)-KaS-YAU_3P*PX+PRsd4D)vql{H`Zg*K!#Ire5g3Q<I85WP6Nl4rsP`T_
zZn?49<Mpnov!}*ULBIcwBRh_YapahHV^KMd#^Y!rj=VThvH7(0r=q7nj-$4oVm(uF
zG#y8|ImF|3+_vL(F>d>DTgB~`UJ$p_xIGgmR-D*zQj8NPPTV*dj}t#mRGb8HG8HGS
zI7#CqixXXQHcs+5b>p-grxS6i;xvfUnK&!NnH6VtoE78DiL>!IQ*jo=Sv$_sILqQJ
zk2?*sJ9OG{Cy6_0+{xl;6?g5pTa3F-+;wApXm<7flE*ze?iJ&n6ZhP>SBZNn?zQ7y
z8u#)zuf};h&eL|G*tUxLCA+x~we`zB{Ss3j$~FD`(e@f<kMpv2wbS<9w(rRn+im5v
zRn%5VyXm!?Gy2Nc4zhO8Yfm-Wp=a)7!@$f+q(6y3PgXll+Hu-$+wHd7ZmV`X)Qj5f
zZaY!!Bxomk^7KWrXs(l;tliZUS!mDL?U_n@CTY*iwrBEo?zQu3J8!r1?(!sC-tm@q
zf}~JPEdA>wb|JBgiR~wLJ+Xtt4imeT*imA)6FW)lG_ku$aXeYH$UNn{RsAkCanoc;
zE2%h1#Z4y4iRYMyEq(Xo1&P;6yeRS7i8q~8^&_Re>CO_rkob;%sMC*aiQm?bRf)0_
zRWvUQ^h2$FL{rM#@u^m#BC}a4O;nbs=|t)KEq&{jnwJVeBMDkb5Sfh*vLslMOa;kQ
znoQ+MXeVJY37sT#lQ2(Og`{OAEjwuyla`aTyriWMutE~q`mB^hfj-!hcsz-#`e9lh
z8%Z1{aVv=<J(o$G=-ncTbMuH`GI^4glhiYh3#q;V$&##)WLA>dNmfiUC&_B&F(M0+
zEKRaJ>8PX=CY?+kERtlVn9R7z%y=^6C$m8^yDXV=lDXL=_maGp<bI-WZSpY5TY5c7
zo+Wvn7K&-XNekm?VJ@|bsrf2vJhhAFxx}ugcADB*S}f}CBvRK)-I{q!arKR=ex0qa
zjqTJ;Q`fwiP0NeZas3X<PAAfIVphKf(yw(=Z#u17Y1O>5&@U#^n%*q^ZB@~H6(EnD
zsTxm}Z!WOrDN7$%x`KXUnbyx9`hg*>>l;&B-<#@NPyL!i-#ey(p9T$mw`b0#`Vcg?
zS-mt^kxrT01$_sgujOfEnRgZDw4%=iowV(x?I3OE>GC8^9rI2|KThd8yEHf7D`xsj
zGyUaEQGYLC-kN9Q`i)VVRnx4dpWw|?R~G9JA<fb>n@O|TG|SUYA??)CPMCJuX(vf%
zqI4!rXG`g9kj^ekXWQwl?z^gAmY8R?T;DT>Y2HfnD9y91P|ONWR+!7IVrDs+HJ;gp
z%(gPSnAxSwE@!r9Ua{!MJG*Y~G3_w3Tjq+P&oOq}jMSdaip8urp1EG;R?TY|w~@J1
znVXp5yWOl@%F5-eT*=CV*>Eu%cCz6-8!2WZPBxNfqs7d;49&B#Vm9VvV|li;m@Rd(
zrFk}PWnLlk?93}>o|AdwnODucFf(t~vdo*!s-AvnmDQSAO-KEfr)%D;>8DTgW<;O)
z3g!zg^VnZCZ-$g_ZXe8(vw2Q76KNhq^#iAVw#@b8Syqp;Mv^t{tm&FJbLPoJ@7MYj
zoq0mYf+=%{Wp3;BHBg^Z_09BjwoKos=^IdeU#M?x^kc5R414BDH_~U-Y8JQ5MKX@F
zxUJ9HS)6C)Y#8ekkp9jsOFVsq>%%xp!Yqk1bIi<Ssbijx&2E%s>8##J^Q>%Ercd58
z=H^{rt~>3llVqJVoA!8y?#5Y9ms{D4lj-kRX2NWyoy``q+3{>PF#CwPIG3}$lI4S0
zK9uFdSw52Gqgg(d<x8{N%krvzJ(lIZejLwo{luvsu=G>HGJQLl<t=?-&+@iDxb?o4
z>3t^eWO+Bs^G>1Iv5TEzQNO<IRJ~5E)=^eR*&S8vD5s;`j+)T#2s({sC$Kv~%?v-(
zuQ^pG%sQ=Vr`6K;2>Mp96ZxG;$(HHw((O*W*pX|7elgH#C!KcQNo@1pE>WE%=p<1m
zNjgd1N$pNr?W9W2g#Kc=li8iD*vTCIg;FOQ*RL=;ncvA&Ckr~6uF%ngo-P!p2a;*m
z)8CLy59zNx^=)z@v$#-yDXrg9O{?Oxa;B9(t?JWi*|ZAv{mZm!>$fD+sxz(fX?5y!
zP@4|Y>7+BQXQ5E++WNQEEsl3xuj?jVFX>i0UHw8(RlE8LHt@Say&Hu3Ca4=^-LTjV
zoo<-RHMZD|oNiR@MtL{3x^dD?oNnTElc3vmyZWc+c6(VjuXgi#HxIgb)Xmdwp7pFk
z&nosTr)L*?w%4<3J=^cut)89guT*=k*K?~qx7Kt0o_rD5bDKRk=(%CfZJ8^%+wQqZ
z&rN&g>$mA%rP!;uy@^V1Vz4(c+?yEdO{C_%i}HIa?5VV;-%~X7YnZebWW8`vFLZig
z-fMg2ULaZ2OPpS+^eL~GX1#Q#mlb-M)ywQ&R_tX?FDv)5N-rDiWkbDexR;Ieve8~P
z*2|XmvYI~ddReYtZ})muFAwx>MK8~03eJpGoUw~Du4gVCdUq~cGnIVC3unCUOf8)8
zgPCyAOz6ynY9`EQqS{QPX5x-{JKk0^$)cIWnMs0~)SgM*nN%nAi87y=4QBGPetkZZ
zS7-A2jDCNR>m`xCJDACnnLJxz6;~*Gh0@Vkp(a+SWQ9ssgwBdEUlB(u5@$so%of_S
zg=E(9XRUD7if66%td-8%1G9E{)*hZ6sLT!w&JGOCmYiArHDS3tTdvHO2WQJev*qF0
z^2ls?e6~C{TdB-e=4J=yW{2ixhv#O;%d_L{*<jIZuy{5YFt29yA!{DO<Jo0%vtcmX
z_Ga6$zWmJ2&dtuvt+X8TrzE1v%B*1itVFRmS6pPi&0B2G4Wx6fH|N&o+{T=n>Mttw
zAoTj?ba!*(d7+qF<GH;kx6{0A<?^LtUT)^)c3xhdSFC){%7?6c*vdz&eALRvtbD1Z
zzj`q*iS^T@7n$2APhUy&+co`gsqX<h=35B;n$5h;*Uy@9?tA8yn;+=+(dOFjcg%Zb
zeZQ}I<~>|J$m?;gUq3ai+`I{D>Nh@lvz<4W=S_VhpkI8L*BSY;I1fGZx-JazFwR?^
zd2iPW@|I3_^2jsq@S<2hY~*n@kEe2TW8Kc<>Adab?WTFm)pw@tBwyai6Hni1<w=mI
zo_XPy>O`n-TFq%J)t|Zk{zRYp^?6y}dd%edL^C~<PwQ_~lDzBX-9R6fc~{?(m_Ln@
z=Ruyg@?1Zlq^DT^DXD&I*n8i*f8c1|3T<tCW#_nqjy-PWek=W>4mjb6gN{7@xRs4V
zk8hr^|H|a3{f|2Q(BoG=e$C@|KYqLZ@5;w-d;A{#1?u4Z0}nm!@cD-xvERW5tvvdu
z;||IXJm|PX4?gmsW8I@qzVWW@7CusY=6|-?@7>jGz}n28b^3q*TZ^>$Gf!rj`SVc5
zb#XmhA59;od_&v_H^xnHQ``(U$1QM6+zPkGZE#!M4!6f0a7Ww;cg9_CSKJME$N9Ji
z7H|QYKLTd9Z!cVk7TQ?EMYtFp96%SBU<u1u!9g6tVI09x9K)qJj_<*}aRT3q``{#c
zSj8IpsIZO=Y+``Ra0)|gVT3WZaXBWKVul@@#xC}72F;OUKCf9^iF0U<FjJo6zPKOm
zj|bp^co4oH55`0AP&^C|#}D8UcqAT$N8>SgEFOo);|cgdJP}XAlkr1%3Z9Cm;pun=
zuEI0%Ec`H@jpyLGcpjdQAHfUoqxdoWIDP^@i5KFh@FKhzFTqRk(|8$Pj#uE7_!;~x
zUWK2-tMMBAJYI`m!0Ygf_$9m^Z@?SzCcGKHj9<Z9@K(GHZ^y6V*YNB34g4nFfp_9v
z_$~Z4eh0sccjG;HFMbd2!~5|8d=S5n58=c31N<RAf{)^3_#^x=K8`=ZpW@H(30#d&
z;#2rEK7&8UXYm*K96paP;4kq-{1yHhe}lip-{J4^5BNv?6TXCh#=qcS@o)Gt{vH2;
zui&fr8vYYs$2agzd<*}D|HilR9sCcPlcD~d_dmE6u8r&9y0{*$j~n2IxDjrQo8YFn
z8E%eS;Fh=*ZjIaEwzwT`k2~OwxD)P-yWp<48}5$taStru0^Ae#!i8v|jYV99i_yUW
zba4rmu#6QP#33BU5gf%aT#DoP9^4xz@V&SXPNIiZtf7wz>)6012Dl8TFvJ!{7-Jik
zV}dDW*uiP+Vh?9<1<vA1oWu8Fj{D+%xIZ3%2jW5aemoct!9(#dJRCoON8pio6dsMo
z;IVid9*-yB2k}Ha2~Wlk;VF13o`$F68Mq41#Ix|jcs8Dc=i+&IK7Ir*z>ng`@Z<Oi
z{3Kq8pTdjqV!Q+|#ZTj9csX8ySK?>zvv?JL4zI>*@bh>regUt;FXET*db|N|#GCMD
z{4#z8Z^2vfHoP6bieJO8<2Ue|cn98zcj342+xQ*)F5Zpz;Jx@gybtfk2k=4sK0bsG
z;}7tM_y|6VkKvE-$M`t@1b>P@!zXYxK8a7^)A$Vj9G}Hs;B)vqzJR~P7x7p4Yy1uV
z7JrAo$3Ng7@lW^?{u%#*f5pGy%lLQv2fl)@;%oR%d>!AwH}Ng}7ycXH#&_^PIBzYk
z|F{;ejqBjLxE`*L8{mex5pIl|;HJ14ZjM{vmbevejoaY1xE*efJK&DE6Yh+=;I6nE
z?vC?u4=msU+!Oc0g=nFTMO=i7(ZK<9aS4{Nj1?TjAsogL9K|tQisSem+#4tGy|@of
zqK8$ip^pmd*uW+RxD2N-#1=*vV;h%af+=R$!D;Mb4`*-%&f-d(!}np1`{I7MKOTSw
z;z9U+JQxqbL-8;?96x|Z;E{L~9*xJ~v3MLFk0;;<@kBfcPsR`7DR?TLhNt5hxC+n2
zv+%=sHlBm$;(2&JegrSTkK)Jh<M;{uBwmQ0!i(@?yaX@BPvd2HIbMNR;%D%)colvQ
zuf}We^LQ<O0k6X^;+ODxya8{-oA74*GJXYb!CUb*ydA%aU&F8CH}IQy2i}Qy;kWSH
z_#ON%-i`O*z4$%65AVkZ@Im}OK7<eB5AcWh2tJCB;g9gg_&ELqe~LfDCvY`BiBI9v
z_zeCWpT%F`bND>IfWO2S@mKh3{0;sVe}})vKj0tnPxuo48UKQR#lPXp_;>sVzJjmf
zYxqxm9pAt=@h$up{u|%Mckn-G{^w?WD=_bWa4lRL*THphJzO6*zzuOD+!!~(O>r~a
z9JjzNaVy*!x4~_3JKP?3z#VZX+!=SlU2!+u9p~d7Sil9iC+>v{(Lx)GxCj@cg9GT|
z5-edED>#TlIE*7WietDG$MHS5H%{PtaUYyS535*19~IWIflUl>8BSq{EsQY6HZI2m
zQ_Qe~)7Zrx&fp51#g#aR@53DT#r<%9JOB^GgYf-$Fdl-3;$e6=egKcaBk?Fa8jrza
z@i;slPrwi2iFgv8j32^N@KihvPscNG6`qM_;fL{TJO|Ik^YDE92ws36#gE~~@e}w-
zybwQy7vaTt30{hy#>?<>yaKPp&){eAD*PN?jo0Ak@ml->UWZ@AFX8og1Kx-?;m!DE
z{0iQJx8iMhJAM_vhF`~T;5YFOyc6%jZ{fG`JNR9^8}Gq;@q2h5-j5I9gZO=X2p`5D
z;1BT;d=wwUAK{Pjar_DX6n}<K;A(slpTei{8T>gui@(6<@OgXze~B;RukhFS8~iQ)
z4u6k-z(3-j@Fn~+{ssSvf5VsY@AwaV1z*M2@SpfPzJYJzTlg>hH@=PU;D6A3!7tZ;
zTnpF6b#Pr=57);Ha6{Y(H^xnHQ``(U$1QM6+zPkGZE#!M4!6f0a7Ww;cg9_CSKJME
z$N9Ji7H|RXiF@Hfw9v*PF2cp=-~hU~1WQ=P3J&5B4&w-p;utQ)aeNQ%jT87@+y^Jo
z!z$L$M}>7Xe`$`{Z<-k3GMvH?TNq)CZCs8CrkG&|r?HDYoWT`1iz{&s--kKwi~HgJ
zcmN)V2jTniU_1m5#l!G$`~V(-N8(X<G#-P;;&FI9o`4_36Y(TG89#)l;Hh{To{neW
zDm)X<!Vlxwcn+S6=i&MI5xf9DiXX#|<0tTwcp-iYFT#uQ61)^YjhEr&cm-aGpTW=K
zRroo)8n406<F)t&ybiyJU&8D02D}k(!kh8S_!YbbZ^hg2cKj-S4Zn`xz;EImcqiV4
z-@<R>cksJ-H{OHy;`i`AydNLH2l4y(5I&4Qz#rlx_$WSxKf)j5<M<Q&DgF$fz}5I9
zK7~)?Gx&3S7Jq@y;q&+c{t{orU*WIuH~3rp9sVBwfPch4;Y;{u{0sgS|AsH)-|-*#
z3ciZ3;Xm<pd;{OaxA0&1Z+siy!T+H7gMD)S$F*>6TnE?1^>BUM05`;qaAVvAH^t3x
zbKC;A#I0~^+y=MB?QnbC0e8fmaA({Fcg5Xscbt!VU;!84p12n-L<?;!;v!s(4i2D;
zOR$7xtl%IH;V_QiD30M$9LM+I-Z+8p#eHxRJ*;94eN<S-1~xIkWjKW)wlKmN+qfJP
zOfka_PGc8)ID;#27FXgNz7KQU7x%;c@c=v!55o83!FUKBiihFh_yIfukHn+!Xgmgw
z#pCdJJOMw5C*nzXGJXh8!Bg=xJRQ%#Rd^<zg&)SV@f<uC&%^WaBX|LR6hDR^$4}rV
z@k0C*UW6CpC3q=*8ZX1k@d~^WKZBpetMGGpHC}_C$7}HmcpZKbzl7K04R|Bogg4`t
z@hf->-io*3?f6yv8h#zWf#1YC@J_r7zlGn%@8EawZoCKY#qZ&Lct1XX590UnA$%Br
zfIq}X@KJmWe}q5A$MGllQ~Vh|fvfRJd<vh&XYl9vEdBzY!{_k@{3X7KzrtVRZ}7MH
zJN!NV0sn}9!k6&R_!s;u{taKozvDmf6?_$6!++xI_y)d-Z{ff2-}pAZga5&K>v8?Z
zwQy}*2iL{*aDChWH^hx_W84Hc#m#VY+yb}6t#E7H2DioSaC_VVcf_4=XWRvM#ocgs
zoR5280T<w&xEC%&3vDdoB3z6P4xo!mu!Lo-;2;j+Fpl6Tj^R=q$M@jgIDzlQeQ**z
ztYQs)R9MFbHZj0uIE5j$Fv1wyxEvEqF~bf{V;6fkgDY?rSK=JL4|CiX_rv}106Y*6
z!uR9BcnBVfhvDJ)0Xzba#G~+NJO+=&<M4Pq0Y8W*;z@Wieh5#&Q}HxB9nZj3cqX2O
zAI7us96T4#!}IYYcmaMCKZYO2Pv9r<Li`k7gcsu_cqx7wFT>063cM0OgP+B#@N;-I
zUW1><Yw-(s9exqNgxBK@cq86~H{+M_D|iduinrnI_*MKGejUGo-^4rcPP_}hh2O^S
z;CJzEya(^a@8NxTKR$pD;`i|(d>DU#Kg37yQG5)4gg?f|@hA9G{24xhtMN&E3ZKSj
z@aOm}{sN!F=kW#nCBBHi!e8TW@VEFo{5}2w|A>FWm+;T{7yK*!4PVB;<3I2fd=+2A
zf8y)-2EK`J;lJ?T_%^<S|G|0dbN$D)aBW-%*Twa4ecS*y#Eo!c+ypnp&2V$v0=LAi
zaBJKKx5e#nd)xtc#GP<w+y!^V-EeoDk9%MN7vP?_7cN8#Z7kv<T#OD5po>eegk`MY
zAP(U$j^HSc;ZhvO_u$?*f$znAa1uSNVhw#%SjPr7F~DUwg(0>u!Wi4Q91~12!wybk
z7kfB^D{vN9;vBvYbKDpA!~O99JP;4U_v68M2p)=u;o<lJJOYozqwr`v29L$#@OV4{
zKZqydNq91T2v5ON@iaUg&%jl9CZ2^K#<TGpJQvTy^YJ5i0e%!eh9AdI;3x4y{1je<
z7vm*(DSjF+!^`msyb?cypT(>2b9gmggP+H1@e6nzei6Tf*W(R%Bi@8J<CpO(cnjW&
zx8d#hRs0%$9lwF!#5?d#ybHgD-^TCYckyn#2k*u2;eB{NK7bG6_wgZo7=M62#7FQ^
zd<=htKgP%LC-_tR89srl@kx9NpT=kK=lCrC0-wX@@df-PzKFlVU*m7^xA;5!J^lgz
zh=0PD@Xz=c{44$qU&g=VKkyZN6<@=D;_LVZzKL()zwqDqHok-Z!Fd~S{l~R%ZCnS}
z#r1G~+yFPkjc{Y!1UJRaaC6)Ox5TY*YupC6#qDr=+yQsQop5K|1$V{WaCe-Kdtd<<
z;GVb_E<_7$EaD<uj1CT<i%YPCWvt*J4&gA4;3$saQXI$k;NCca@5Oy^5<RS94SiHt
z#|Abrz-2gvA+|8W7~8lU6HGC~4o+hidpLtDa28kM9KH{8+!y!5{qX=i5D&ul<H2|c
z9*T$I;rIbO0*}O_@Mt^+kHzEgcsv0=h$rGncrtznPr+01G&~*8z*TrAo`oOAv+*1}
z7th1<@gsNveiT23AIDGNC-Fl36kdcE<0W`0ei|>s%kc`l5<i2V#jEghcr{*wpT}$Q
z3wRxV5x<1j;|+Kt-h?;fm+>oj3*L&i;qCZU{2G28zk%PxJMd1t3%`Zm#_!;F@ou~a
z@5S%oeRw}UfDhvL@gaN|e}F&4NAOX641a__#>ep|_*48DK7p(8Nqh>Q#%J*7_$>Yc
zpTp<z1^gwxh`+*L<8Sb{_&fYP{sI4pf5MmW&-fSoEB*~%#=qk~@D+R&U&DXm>-Yw~
ziErV*@Zb10zJvck^B4EX_y4#Su8r&9y0{*$j~n2IxDjrQo8YFn8E%eS;Fh=*ZjIaE
zwzwT`k2~OwxD)P-yWp<48}5$taStru0^Ae#!i8v|jYV99i_yUWba4rmu#6QP#33BU
z5gf%aT#DoP9^4xz@V&SXPNIiZtf7wz>)6012Dl8TFvJ!{7-JikV}dDW*uiP+Vh?9<
z1<vA1oWu8Fj{D+%xIZ3%2jW5aemoct!9(#dJRCoON8pio6dsMo;IVid9*-yB2k}Ha
z2~Wlk;VF13o`$F68Mq41#Ix|jcs8Dc=i+&IK7Ir*z>ng`@Z<Oi{3Kq8pTdjqV!Q+|
z#ZTj9csX8ySK?>zvv?JL4zI>*@bh>regUt;FXET*db|N|#GCMD{4#z8Z^2vfHoP6b
zieJO8<2Ue|cn98zcj342+xQ*)F5Zpz;Jx@gybtfk2k=4sK0bsG;}7tM_y|6VkKvE-
z$M`t@1b>P@!zXYxK8a7^)A$Vj9G}Hs;B)vqzJR~P7x7p4Yy1uV7JrAo$3Ng7@lW^?
z{u%#*f5pGy%lLQv2fl)@;%oR%d>!AwH}Ng}7ycXH#&_^PIBz4a|F{;ejqBjLxE`*L
z8{mex5pIl|;HJ14ZjM{vmbevejoaY1xE*efJK&DE6Yh+=;I6nE?vC?u4=msU+!Oc0
zg=nFTMO=i7(ZK<9aS4{Nj1?TjAsogL9K|tQisSem+#4tGy|@ofqK8$ip^pmd*uW+R
zxD2N-#1=*vV;h%af+=R$!D;Mb4`*-%&f-d(!}np1`{I7MKOTSw;z9U+JQxqbL-8;?
z96x|Z;E{L~9*xJ~v3MLFk0;;<@kBfcPsR`7DR?TLhNt5hxC+n2v+%=sHlBm$;(2&J
zegrSTkK)Jh<M;{uBwmQ0!i(@?yaX@BPvd2HIbMNR;%D%)colvQuf}We^LQ<O0k6X^
z;+ODxya8{-oA74*GJXYb!CUb*ydA%aU&F8CH}IQy2i}Qy;kWSH_#ON%-i`O*z4$%6
z5AVkZ@Im}OK7<eB5AcWh2tJCB;g9gg_&ELqe~LfDCvY`BiBI9v_zeCWpT%F`bND>I
zfWO2S@mKh3{0;sVe}})vKj0tnPxuo48UKQR#lPXp_;>sVzJjmfYxqxm9pAt=@h$up
z{u|%Mckn+rZ)2|ixE8LB>)^V$9<GlY;D)#nZj77Yrnnhyj$7cCxD{@V+u*ji9d3_1
z;EuQx?u@(OuDBcSj`ML3EZ_p%6ZgV}XrYZoT!f3!!2xt}36`*o6&%DN9L5nG#W7rp
z<M<xj8z=C+xDQUEhgGbhj|%J9z$ON`45u)}7DgCj8<%5(DQ4KgY3yPTXK)41;!2#u
z_hF9v;(oY49)JhpLHK?=7!Sci@i06bKY&Nzk$4myjmO}zcpM&&C*TM1L_7&k#t-2s
zcq*QTr{fv83eUu|@WXgEo`dJ&d3Zj41TVmk;>Ymg_zC<ZUWlK<i|}H+1TV!;<7Id`
zUV&HQXYjLl6@Ct{#%u8NcrAVbufs3mm+*SL0dK^c@Mioneg$vATk$r$9lwfS!>{8v
z@SAuC-ideNxA5Ee9sDldjrZWa_&vN2@5cx5LHs^Ggb(8n@Q3&aK8lawkMPI%IQ|5G
zia*0Aa5X-OPvO(}4E`LS#b4la_&mOVzr+{uSNLoE4gMB?hrh=^;2-f%_!9mZ|AK$T
zzv0XHcl-ywg0JFh_)mNt-@rHVE&Lb$8{fuv@IN?j6R!Wb7Osu!;JUaTu8$kwhPV-K
zjGN%5xEXGaTi}+s6>g2&;I_COZjU?Qj<^%<jJx2jxEt<{^KlQnWpDjIwd)`KzemWA
z{@*0$&D+8R&xA=6=9^%?G?|x~FlmBhB}i6U=s#ll?mwU~Pg1_gN)_pJ8<RcT1oL-1
z|F8bcUv>WODz`JKjtMdxX{JB7dGGcoo$9~FY2LhjO^{VeRw`d&mQ9)<%_Ll7vQn=<
zQt7^J3S^n2HaA)Q2SnIF>FPd{mA2QIlr%GciS#`E7fG;=$u%jdr@vwP-vzR)zxgsC
zY0(e=KdMLvRnv3Zgykkkv(+Xgoyr{a)?_D5N+w&{{?`cR*3@g5)T9Yg`9hO=*aT^D
zzDf1hCM%cq%0x(ktXC?_CsQ*m=9_Sc3CEZqEp9NWt_gdZ;G57NmGmOr%N8|F={Y7y
zf0HI9E%r4jX(mBFb4g8_0+|R2{VM%ZsUicHX_D+J-Rj-(%{M8jEFG*e*|SaP@6W!;
zW+v=v!lVh8n;_FP-=sD$!Tjy>@9rQn&(f)^QYJ#eq^V;5R{D3xY@3u6$o9%+^@9vZ
zLVtUu)16H-*&_27zRxp%JNmn$l4YLBN~dR;lq{3(hfG#JZW*o=NPn_{lcrSGCI$DJ
z>;opqGHJHen(U-WNj)i*$&s%5n<@qVntC!S$x2%Z(rG^{{YeLsI@7eh#01$(&M_%D
z2G2IBwh5=3&|mNGO;#$Kzfk_&(p4sPk_qN7!GHIIy3wSh*-a)TBb{86THB=hS@U=7
z&r>GI&}2Jg7&7{8O@U-DFsa8)xYLByCdfLZR7NT(nfq0yR6hNF&$3L$B2{E<vTVMt
z`|kEuO-gpm`6jim2{IAVm85Pk%VZApGw-~4vj0g7>2K03>xcf-$uTXVUn*mfDzZ$v
zmp!px<^M|cJMC{<f8|ozpYi`HkgQbc&tJc?6vznvs~4#vK|YUuy?%i#lkP8^r+=jT
z{=CS<c1(fP+tZ}>GC?|!X8xM2ESp@D-O{9_tqiRnJX0Y3NsE3;vQn?VN-32VvP@QK
z{uzyTKTD}RX|hsJc0K7uQf*TpJJ)KHnlwRD(v=iQ&(f)6r57ocQ*&loOqw7CH=C5y
zlT(W_S!pY!YnkjBCP-VUC*f?fOhSLz_GX!6C)cE8xc#ABVhZ|WdCFubO^}Pfw3TJD
zj~{3XPB7s?6ZSPh+RD(RvUD$l^wyM0N=nZ#%SKI*PNl8Pq%_m_Ma+RzmeS|UvPl!3
zFhL65UXzlnw4HC3NrChy1@gJ|S1$cYf$VycmEA{1FZEWNUgSXB$E1c$ka{hX8Z$wT
zX<0{qZT&-6x{|D<&M+;c-h7jiWm}k(1nF72lJ&}PeN!qa2~sLSTF4G3LAshWm1P~T
znUrjsH0zg2GwGmhsz|A4Qc@tx`c<T!l*&m^CP!MxNVhf3lnI#$vJPoGX|nzP`Xi98
zq*V3@X(3seq3cYu*G-r-LAFC$$f#s1`@@x?$p~ar(&9qXOx7#o`~Pg~n|jjUW+o+@
zAY+kb5++T7q%JZk=~PxEDcLcl7g>?Cl>!-Gzk6A>%5)%INmd3fEp{*kvg{m_YMU^*
zCiF+&UnVJ8r8JWub1-L`onV42lkO)?R@N&$?`X18I)6<{x{~3_sATlgVzsFvy-2vs
zWGB}IX(3tZPg+P@39?LjmQH0Slmkp|dt{jmsNY{dC2NxoWF69rj76GB*wzd~QqoLP
z+pk$BSs8|WTAnG8djA#V?ohgyAgh$NvN4j96-igJcS?V&O@F=#lWT&ES(ZuaYEy8r
z37486%le(lilkFnhh#HTPr8?7lP3FLLB`TC1(NEw?GId9NYDRQTWKbP?3YSg8JeV|
zw7(8n)~_tvAwkX4Kb{GbYeIh*(x3Dq%lextllWiFq`&@p`<45nl3papG%Yl%lxC|<
zYLy9cZkjYHsV6D9ZCtfxnY3t|tPE`}lakP%_H)d#bxe@TlO`oWRwT_<nPrlaDw2}X
zOEby-SLk;oTO|GcR|oyBB=wqE$G#@?H%5*g*-BaVv?<-z1X=I@6{Lmq_kyV>$DpiN
zruYrBOlCt0WXq*gx|eX9sj}JxsUnv<X?wO=_Jj%MpOJcZuI8JRbRd<bbkZ!l(1dGD
zkg>=}PcvERdD5gj6Xa8n^~!BPf4$P5oZL<{^&~4@NlI2Zxu)QSHK|)oN=myXB|TqZ
zQqoMBl=LE1Qj?Wo$YX&llTDT3N~h9+Z|cdis!2_nAX!OC&+nLJvI&xX&Sa$nsb~IO
zw0ZJWwA!TPA|hM9)MRA@@=PZqkn!DX3Z$NlKsuEI8L13%(o~UU(t#|KtQ1IBlIo9M
zO51CiNtO8~D<hSxbk$#_6v)bDi~7gRex{kUkRVlLQza$KWNk94b4_I_m19A=mmpaw
zkg1avlcutKGBVjxD$7Ppf$YH2R+g<c%lf^@uCkU{CL@&{&itEX^R_p^H^KZ<X79ch
zG5>hkyk|{ln;@THe*<qb%VcAw*Q8!BsT0;@UpJ}cCY)`;gC<OxaE}T7L7ri<{b`cn
z%Dk9=p6%VazucsxKvpS1#v)^uth{0<n0kkpATN%dG^wr$lP1WRFEOb}6J+XSz5A}o
zPOeEwsdO*XE}JjQq{<7Xp0qv3q*j?A16tptCQXpjlC+iaNmf#_<&u?pt4w8CCYApy
zC7+QrtD1Uk6K*p>RxT|h+-sIeO7=u)Az@onAic<(O6sXKmF464uZfjr{Y_P-vb2!O
z{hp;tW=f@JDUejjER$)M0UczrlP0V(VbX-NO^{8!(xjyA6DB3=kk=jOnXL5EUnXml
z(`#Z%W#F<(*$JgyKS+U0lV>W=H$lccXHqAaAQ!_)le*4?l_tojWI(T)tZ#xe(|<Vc
ze{aVNCM)$GH>opCkhZcN&YG+&lTx|OJJA%#dP^oHJxdiC=~`x4(*)^xOOyH^6Fd{7
ziVR4WN!v+N(C<Z3{UG&Znq=iNovTd?Dd-1p&9c`_YSIKr$^Ihieb6kEdj0j@W|qkr
zM5@S`rBj)EIc8*<&NTJpq$S%Xn<bSqQy?FiWcM}MMJ8-(f?N_-nUs{u=;xcPlu9!h
zfn+Z=1(PPMHsKi)IwnYV(xjvp3743xj8uA&O_0xRzA1R!gkw#Rvx@X0pOGBhvTbtF
zl>Jm@=u}f#F0Qhb{Tb@7Qc_h@PnmFr39?MOmy~R|q~ts<Df#fEzpG3O8QMW6CC#=q
zDJk_$O14PCq{&K<0=ZO5<)=)6j9yCn%U(4Fl9hGH%AYgK=1gdtFu5kkDy53_CsWd2
z_JXO`F+r+GkPcon%O*{bth}?7@ktdqh@Us5l6}FXWLJ^yWh_!~hAEIwOR^GV<+4io
zKxGd4l_e$1q*Ut3#7f9bf3oFoo0M&WRF-=3!AtkDY|@lUIKiakGnXotnyhTh873ud
zrN6UG_6`$dV^*7#Oz~qTHEBX-f(%lcy<oC3L))6vDibD6kQGT=8B2d?{kD?o2kB4h
z$+8DcFOrh(rL>=YVNIzllTw-XGfYAM1Gvy+Pc&iD1UXD4C7nu21}VMNO%?fQr9cM0
z+ANdPgA7dyWNlqjAk8E^WU|TxnOON0<N%Xi)-nZhDwSqZdZt+><Ga(Oq>A(`Ep{`@
zWEbeK<7l%?_Asd@dxUf<Eu`M>OqG5MNxfi}on?ZQO52jjO6n<-T5ZCfCTweh?AdcB
zwSx)mHDS`EBuG6exY{g}QOQoTg~`gcRZU8=atk3FxXLV(fy=UMO;%Deby6VnCn=dG
z=~<S^#z;NsRL+DlLlP!USC^O|E0PZ6R$SW3aAhl{Q|U@}d6~LN)9hsvWM}BFS5_od
zWF2RiDxL}Qakous4-+QWgno<r%`*9TPB*D#ChTQ`tXyWdW3tkzq@=BkRJO9eN@*dZ
zk}CZVS+X)_xjB*DTlO%SHEA|!)-m6N3r*<n0`kF6nq^Y2x+W!Kk!Ahy$#kBurpj$5
z)j!G}HCg#ArNyMl_A9qdRtlt3IV$FxWzvf*ld(u@OH*)-39`L15mGQ|mPuG;Qqn?B
zX>!2Mm}T-o$#$G!vi+X>d(-ZwKxU}_A)jrQjhY}Wo?MfXb!=g>(!nJrCFARFjAW(R
zq$!nYf8C^3n;;!*Yf{pS>^U!(thA63NM#xGOQzrk6Z)xDCTp7@E#5RKdC-|ODJhWI
zm9FFt;CfRaYm)-m7#XCjylF~TnIQeic1YX#W|_>Nq$W*PcG3Pa=~RX*d-iFjiu58u
zCP%jSZBsC5g0z)&NL!f_Ny#c-FjeI4K??fIq`&2+bkYPV=zoIJLBkZhW`aze+}%jM
z+$@t0q|@4(tZ!2C@kpxQU;iVMW*3`!a(B};DH+CXCUv$69TTM9q)Ex9{?|mTWeWQB
zq_SjXeEov{l*qD4(@ZK$fAT@e=X;GQki%foq@?sglah;)bSlkcvSk;LY-Xy=H$e&{
z$XvZ(mbFdT#{?OM1kYq;Hs(xfa!ru_WRP;!>u-nbR|l`DCoNW+thA67$^6L?Dg&2E
z?2lPK1?g1ACsVhl=|Dc;CrnDVTv|w#NwaK~39p#&fC-Z($oQn5to$0YOtSsieakG9
ztOOaooS&thgy&3^?Myh*1evBulj?UR!;p1Isf>BGsUn@q8A&!v1}^oap#ShFlie@a
z%e0jp`1m!wOq!H@9#YviS((2}OiH?!l}j&jLw}nokkMDyq@+}Wj9yaxBSkj<EK^yQ
zoo!NEoACeCWjCRAUR4;!O>8ybpe^+1K!j6hAvkKG^#jE2#6+uzL`fPQghcGfi3+_2
zk$O*Ks5%f^Nvfi)!HESyGF8Dw1cwURo2VG>OotBiHUq8L|1a`xxL^iP)>_Z{-ur#e
zImwR#_s<A-rYiYd(whSFB1WqI>}4-i&q~fCyBtBimSk%h(z6}t;P<ioWU)FhBD8_{
zXX*&%fX}3k;MD-}a*_np6R8ge)K!uxXEkFz5pgtdK{iRd0UJrg$mXpl-&U&gIGN;S
zskn&_W;3rZcOtNkRvR{{h6e{b3L~wPt_C{zYLcxP%g}1b3=ZX@KNnM$BiXit=|{r;
zU)U6}6Ug^s(!K<aM}ptR`n2M*O5F}dAOSxrX@Xsu$TM@gh%>=X@K9hm-j33nXceJ`
z?*dz?z{WSJ=K|-Z#;H^Zyh_=2vmvN=q>{W@_2RUIxhl7Ev1jZ^{b}&Cz<Ee;Ayx7F
zlYSUDZmFEU_oR|XVi86q>B<i=E(jgm$ayzln=fg8_e59(XYgFM-vl$WHBm)OVk~|}
zHG*pMDf__UGM!25h(`jwj3n*O^az=;f6he>x9R(-T#T?z-3hJ-F7-%~Z)T<<PQ}_T
z8@QOw$;hl4!a*74d%3VlFjB2<H%Y!%k~{_;cH7XTi05PF17h8r(-Bl>>7+lO4J-C)
zs<Whe7k@1q)nlb+;4xUoJ{rikPWpK;0&{5ma_WP@CU_wjfiwSZ5@+g})a^jscPCjG
z;P>QWqw+hNZ56Nq&&PVPNx;tt9zT9txnM_<#Ul9Gq^rV(?O=Y+W0UmW!0NE;)MJ5R
z-kLOrmhWaZSw@m-{{0lIN!!3WWK-&3T?F;sq;CWl0_+5-fb{EJraA|7Ef8Wye&9sX
zrvn5RQ<A`5tS7*s*rZp1t9(6Hp$XgBc7pk3$1qkr{YEYb&YHB{nRGV957?=%3!4%E
zek5Ny<{7DWEWdXIzX)Uz@jxmUCp}NW3t%_)$AN~}T)2^Kwky@++0^wrbT4%iJR2za
z^(31-k~9~gKV#YHh?UAc<qTW~KjLO_erjj~ak>POvi&4g<NOdOvaN$Vf>kgAU9nwC
zb-k}7y&QZicr4J1o}~gKLaO&A9S%k?U1nE^H^=IEXOR?l3H8iR(l6$6G&moe3g!}L
z<D}h%9Z9bR&j*fC7Jjbsb_8%moe}oe5&EN!R4$}<rp`9_8PI8oFwEVHA8DPs35-9#
zB)luz@!*ZY4+6^`NtS&fX*c*?poV2zv1)9t<6bULFMe1x^sJZdh5bs>995Q=vYB^I
ziEuGVvMBDZt%^7usAd?djejRXm@jf~NnHohgX^h!@e|%1+GLXt+(>nx+OQ!>fe}V7
zR0*!6O0UT&jR+$Es-z0Bt#T1IC*sF8KN4o_=JkNvzi0lK^ugf8z)MS4JE^m0^ID=)
zJs-QfxQwL#1Ro7_^>EVc>Z#Nb7=Om9!S7fs!mlxsCEA9PCih|O>(MWZkYFV31&Z(M
zUV<;jPC|OIz_oR52n|W4T_6jVDr}T=sy9<-4HNP6OizMs!jZy8U`j`JHgy&FjCv&r
z2>Vm&J;4ax6Chjx(%k*ISVzkS46_&UuV5V<3Z4!Q1|!gmg9l3PgCqHY&n7uC9h^;7
z*J7>nR<;pv(FQ*@-CH77A<x?violBCY`avi3^rl^&iPU>ThqbuY?6LF=}fR2NToIT
zWI;R<Yx0qFF)+tE$-*8?dLclplD-rO)AM#JR%X3m*<7E~x%@StYMeA1(&WfR7?;U9
zvUTi%fQuS>=B&5~JJ8z587sXoF0REYRz9)7@z@`~vAAp}Ny^Uwe0#BtBoisbPi6#F
zH9vjXjuye@3ixU&!F>@=1taiLB!Py`WwT;qndo>nE<k@+`K;+}xk#{YIU+{vy}=0B
zHc764EF;x&&L@31csx+;LrKz~P1;&iZLAh`FG;)FnCCGTmjjE0)fJnruEtuOUbLp<
zIzpj~i{uy)+R*8#*vrAIfuskMUJE`M=#QUNijN2liP&e89-Pc?vX7;zOG1EK5o(-B
zl61ngY>x)!m3f_N-o2#z0-62C`bgE^eBlXWI~@D|U^`F}nB0m#7GZLPZTN4tCnNq4
z%sED?%!Z-vWHXU0z|aW2Sm!!FqmCpA&MneNnyeZc(&U`zY<|x!&a({>5^Q1>Unlul
zfSAsQB1YgFz(<lE2>urMRC7?P)V~K3yp*I6R%^iZu>&lL?NSlKd|;4(eAi+Tze+OO
z<)qI9p9}b1N!kR`+s27hk|#<VSP@u&-3P*E?50#M$myg{1jmAP@RwjKa1Zpilj^GQ
zL#TFR@ssLY>SK%TV$#WgvoObVINRNUq@G_j4rCjFu|%lhVj^^)8WwDdI2A}h8mYU%
zR-l7blK$o^O@a}@nV+Lz1D}e$GvKUUHK;cusK7)IEzW0>xVUDDJ09oZSCLVH{(MYW
r&Zbz^02SDu4-D;k2J@}QyAodi^Y+_+z2Tep-xmJyzrXK#=C=M{%2uTW
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/6bcad96e-e241-4ca9-8356-de26ccdab90a.vsidx" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/6bcad96e-e241-4ca9-8356-de26ccdab90a.vsidx"
deleted file mode 100755
index 7d1dc1c52e4362edb7ddeb31a64246f249341009..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 12819
zcmd6t4Ui;ddB+z}f(IxHsC)=xBj&jDPWod$*AMKSerL9sp6;pcp4*$;9<R%?-Yx9i
zUT*gwh=k!{gv1a_&9^a*ScD5j5{-$`SV&nGWmzQT%912nmPM4Pv7%)tN)wW3p8wm=
z?K`^+=TYt<Pxafs=j(mmr{C_L*_*nC-+Ix+#KgC3I*~w!oJQl?GnG1V2Ep*U#&m4>
zf#D}cGcjg_5p;}pV02=m>lxkHNa*MpX<($Wk#>!qXY^8IXJG8PZPIop?b4)Op0w+e
zjy362CtZ8eEls-h$x>;uRGuumljYiE)tanUik4Zl?4sosty0md7Hv9eMaM2WZqca}
zs};*It)gWWZL8>5mT8gLre)VH*R<TSRWhxzWmQdU4;_0fnc9YF8&kHi$F|D0C2hNI
zJ7wE7ZP&HkitX0yl5Lk9yX@NKnq4*Rs%6(~yH>aBCgs}ovSXNzMThNJ(y>j)F&)=*
zN~Tk$!*VLNQz<!B$Ens_!*ne=EZ4SO$8_DgTdKQd)2+}^bSrhYR(9(q+?H9g-I84`
zIabMWO0H8XnI)34Q>vR~iw>*om}S?Z3Dj4)R#z8qR&2B4n3a-QsncOqEwgI5Ri{*~
znAM71txy-0nq}5(I;yp@S*w_}Ivq~kGV6A=?v(4US*KRYmS;?P{xx1pctPTYh8G52
z*!IHE3sW!5ypHgqh8Ou>S9nR=OENF*d0F6P((4;uUru}B^mI5K2Gg-W-Ion7Y<Qi9
zFdA(-WFr)fFmA+ABaRzM+{k33-|&qoU$lG?`a=4F=LeA=Bz}<k*Yy03=O=-m&G>!k
z%fOeRFFU?WgfS(&R5aS65sOA9d_(wB2w#X)G&9jkM9>nUAwmyTLn%5!M4pIJ5oMyA
zimnuiCz4DgQly4RGm-X0Dn%wlCdI5FWP^GUG8D2aWZLw?W;1QZNi*@9Sixv!vMHyV
zveA@LQ+8X%R7*synXDCJ@s><yL@*OHXEI|ZlQS|1yiPDZ9W=b4(GGk;mlTLFXl6lc
zCJ3{j(+eUIM6Do_K^z6tev$^M7o<Uuc7rqvc8P0xY1^1;+eTZ=wy8=g+tO>xI5eh0
zFABXrZA~bQP&BFOFo?n+4ckW8w!_e%Ef3=;OhuR(Vb%{b8Oog<W2)mNbU!;H>IhQO
zj>tMfb~^O>W}TjljH$>ABd-%RMC2QES0cX?iAE&SNXV#VM6EDt$taDZ%!o1>Nk5XE
zNT!j@BH51{UhMbdW;1S5mm-dgIP&9&6dA`!ocM8)#fgm5G)}WP^WrRsGuic8T`%Yg
z(G^iwB;8<FH}tx3(v8z@oOM&9E5oi#63<AyPJ&0FkqD89P9oAo$RwCff@Tu5l6IQJ
zbYw{?lgvo6Ajx`3CX=2>dQl=9iS!d0()x+)CNfQpsnp9-VbW7YPf04$RFFwGl5T>Z
z22mQsX<MXmn8r!k6=^r5hA1h`jFesidV;b}n#r^mrafA-nbMWXZe~nnUX*zmJ$Cf8
zA!|lvK_kOmr(G6V(nDW;nMpg74!uU1oTk?zlYS=Kne1c|yF!gq?M!BwoXzB}O!j*o
zU5eQYvtA^7iRh(tWIgHc6v@sYoi(Ory>K@0cNtT=MA$c``i-z3h`#LfW!#rtT5z|~
z-R)SrOV;k)&YnG{q|ZV6Ec1NTf+yA6D;qJkszI+GUQX;;GnVw2wgVY@ct1i?V%hPe
zdUPZ`8MxmO-AKGT-9UC@x=D0nq}olHlG2uGS6z*uy^uSFob}{vAa@zEPe*X8kv3nN
zf6Kg&(3qO*tu7u~otvNYR~8QMUp%n3I@elSn>l>lT(WZA%8g5FbH|3qK6C72H2&z=
zM~^+^SdLq`erff_!qWcvy^C|ffu%KhaQ@Kz{?)l;aei5y*dd+b4XY~$^5uTQ`RUL6
z>YrZtjxV3KwH>8P`{(GHN#d55&5`k38vFMjUG)n0j;-h@U0GOIJTw=sEX*%2t<JA4
ztsI!^Hs_=~c+0~4!oEc*=eqNU)|M8Q7n@7VYm0}93kPpm+qZInzOajwbA0Qr{>FWG
zpS>k}bP^p=uglmj`L+|Z^yKQjr?8{Z4O&>9UtN{0<(2E6Wpj9wo__3Gue<1!?a?zk
zQaAO4Ej{tc;q$sjJC{}uuRmvFJ3IE>mG^JybJST;&-62QCl8Em;`l?Yc!78O<>znZ
zU3+dF6`d;mEoa%TO@CX~R7XE%`RPB8_+9%69mzxV;d#Q15}&%WcHZ;2M{;#({|OtK
zxH$X7);`(e_Go4Q!NY5d(px^bZ(iQyp17m-e}4S=?x?h>qoPx?D`ooUMcr}T%EYO9
zj`pipPOdJm&^LB^aAi&2JimOSeG*@|-J33b`AdFl3%}Rs0|(z|&wTsyz^47#@ZRS}
zy!}tM_O6ZT=+OLu)%gXvymVkut}gCXf9c=}KfJ8Ik2f{>(8I@0eMgJSEA!WHT6oOv
z<gTU59(?SIt$h1OZ`bPDq4~ANy|<ifGpeN@F1>wgKeJDwqoVCNHOF=>{QEuspdQ(=
zEq~Wesw4UhfxbB^^qWD&8fiwUKbwxK)v`rDx|D3odSUF5V>{J)#VJ*4CF_N;M~+>y
zN>#@#S7@VN5O>X~Rp@u6D%~@DZETWL`WzizTUuUPdu}iP#5W#&Kz%|!bza`;S={mI
zL=4Dr9lvox$IqJ}a=~phAmfPNL(isxymM(lPDJG6AGXLxOW-SLz<)mT0_KIxS2AD4
zd^PhT=EclQn3pnN!@P`nIkUigEfE4@eFFx`$q@Nh&_MrJ(t!PX8jw*UzMTf*SJ8lg
z@c#l0_)WHp%;T>fh&X<N29Li0CnEA(&My%mApB(}9$VN|8jw-Kj}IgCH$?;X8<=lo
zzKQu}=G8<9i2S!OJtFEf*!G!%*<`kuGeii8IspyLZ?hc|At2%%7$l>FKcazoU1mar
zj1qY%4KQQ&m^+CO5b;?UBp~cv%svqU!v9M&u<jh&dzjZUCG)M!dFFLY{D6Qu*VBNE
z68=RR;M<scnfr)X_Xg%N`}Z^P%MRwPFb^_+nTUC-G$0`I)|iKh5D@;GV33Rw{+nrl
zzd{4`-p=-|%y%$vBO-n~+XLnu%p=TSW4@F5>qH2Mb>79ilL!IfzY7Klh;@Dw1__Ax
zyJ^6GH}gHr_Yx6*AKSmf{5|F$5YM3T0U8hxbw0@a5c9*#k1+3L-baLhnD=Kiu-^S_
zKfwG8=0}MT5cz+_{A=bXh!7C*Pr@JpVgDTs)c-UM2nhd!%!intWqywNFcAVG|MM_N
zK-gb^K?1^lga+36dm0cB{zsX|m|rA9#u52nQqi2S|A_|pFU+qHAt3x;Wqyqa0pWj~
z2IO&d-blp1P6K>``As4Og#X*j?+|gnze@wx_dObjf1d_ql<+@E1AL1410v@AkOpLw
z$op>^;Q!Eoj1vC;qXGVyc^VaieFpQz%%8&|Bp~81VV=$YaYWv^ocA*3%bDjhFC@}2
z@k+K|&Ab>+l2M}GrR*Oi{Fm{(0@Gl=j`?~bWR#e<o&BSPe+T<V3BSqpicE{?FiXq|
zvqq$2qRyOxK?0)Q8`%b7znKWv)$AW7;vV})34eq9pkTH*4}?Ep8-(3vc9=28L8x8!
zC(MlfJ+^0w5D@kHY=f|OGxsnhbDp`tTqNT8-^cb1%w^^QB4m`Pe~{ych)7xGym3VS
zVa~sqc?<Ja=55T|i4YL#fc;09?}U>CMBR50;kuLkqeT2S*gs16-_3py_T6lQP=A~G
z9wOrJWxk*JyUgEb{vq=NM93)N`eQnQAEXoNeTeyC=0}+KGVdeOF>#db`<V~0|IZO5
z8AsIrOV0lo^W)6F;dvnP{+4YJ_NSPiX8$M=e~{x3aU6vIv&_#iA7*}@`32@9L<op^
z|3HN6QMQjUzsUR&f+Qf+$JqbR%rC=90z&-?`@hP55bJ!6?NP%2IQbF!SB{SouCLPx
zc~1~w{~Pn0?Eg0NI|!1DBj*1H$3fWNV}76cB=afe4~UR)#Qgu_I0*Y`=Kr!Eg#Slu
zgRpS|PiMYJIqCnrj3HcSa^6`SA0=GdIDQWErJM&sJ&$b=pEKvPJxaJPq!aimj*k+q
zi#UD>$43d*Yv_bay^P}^?8}L8y_WqT)GL@*GJhUU5)kTDbi(`{Y@2YBfbdT;i_BkS
zT1=Y=IT4ZK(g`dxtIRs{4a_$&uO?#sw-6y9)M=*Aeh_MtZ4l}V^BQKzjF??!!pw+R
zugCT*v(Mbk+(U$n67#NQKM3`$Y=cm*V|yGCx}Hv`vq*%!7pG?-^3FL&g(qH2fV~ao
zHfTd=LujhR>Cm>rr~dR9LK~h-r_&HP3+7qt<`J0q3Q~W^PvFCtqblX6KB4?s>+7o4
z)Tw|#VZ9C2mAV)e9UwZuv|Lkp@a2s^Ygc*(k_XS4w_fk`bxn1k7T>&vDymDt%%?$9
z`&`iL>bw(cm{Fj?doV5;(fkrr`Ee--<Tq5M=o!zJTtG^`?K%ZM{X`UCZcah@kpNQi
zJ<wHDFUs7g4Ujx?9m+(CepvBO$cg*c=N4cVka8Ea!N%mH@Eu)G9>7#7`i0OBwbq8n
z8=_Rc8ngn9d?EFSYE!qSwE>dVLb`zV6%Z|;XnsG&^tbs&>11dF8hQBwG*wD#1Na7N
z#>Qmb--*i)QDBI<1!#qhRR-`4Hu{F}4dK(33-GDAdfg#>Dlhk~U%A>-9UUNVfO`5y
z?SOB``i$)`)rVFAnp$Yw;ztoZic)z2XaySijJu!>;L|s1T;;)fUIAJGGX~Jq+)WoV
zL}0jaA??%JxOVkkX-((pxw=V%7Mn1zn$FV~GeA@|sWlx{+pbRAkf%OW3()d+8by!5
zJOXV8v>ng}&{R>?Mq%UP1w@rk&&Zctzj-=kQ{XPlRh9MSYfWFE?rMk`Lv*0E0a<yK
ze8!k{3rJB->Y{lsD3vd{K5c+x)l~tSnx-{fKzBM4MLlf}$Qo@8QFR0K;zOhi5!kdj
zDsO=308y>!b#)cp)ltmIKLp2I(YPwxFhe~IT2qVbg+_{Q>}5OVZr|t|z^8g1LQ~hK
zE9;_15LM5b?tTEDnyd4)rmN_=_hZKW=s;K5w8aIa6p)e^Sl<^tcYqYNu9~Y$>68Le
zRFyns<3;OlNj-Of8LH@ZXlk2=(1w_+D-YmPd3x?W@ZE#F0<;1&9o1W+t7vT-W~dvg
z^9Jy#HMCZMPgNd58)A)dGxBxUckKw0)qT@Tj?;9hahs!?ETEohW84L*%^n~n|4Tu&
zvFWsNoAPA%#%X#DtraMD<ksqo(J8vrb|kBNsq^%u4iQy5spl5pQx`admPa@Cr*}hZ
y_aIq40%{KmFjbWSw1K``>vtw!Qkh7B;NJu<Q`*GDCCod{%$0w<Am<}j=KOy%Af|}`
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/82dfad02-59fd-4c98-a6ad-51863ac0b212.vsidx" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/82dfad02-59fd-4c98-a6ad-51863ac0b212.vsidx"
deleted file mode 100755
index 7b29827aaa1985399a1c9a59a39290014334b4a1..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 215133
zcmW)|>$0Odwl!EC-A~c~Hjc9;VId@OCzAagAqh|#0||K9PA9>GRIOkA;yqOaNwCr~
z#%Nx~7@E`C^#9Ml{`D_0{NHf=*T4L^-~KQ7_y7KX|2_Sef8{T~p}!FM3(>!t=&y$V
z)l7lkNDyoRA_3L`{&zrVK!bpyfVKg>2lN$0ZNR#K{S0Ck#9I(Q10DpT2t*YWc~A#I
zje?p7wFv5e2S0{D1%ZkJwFGJn)K{Q>2dxU)F=*$YeFg11&~>0ypvOSZfqn!=1;z%(
z1!fJ*Bd}{=A3<k>&IR2VbaT+HL9c?|2K^ZHbI`BBV1r=}oC(}La8qz|!DxdKj&pFI
zgUJRH93R2l2J;%sdoVwOMFooumLXVNu#CYn2g@4#dIW0_tTtHtV0FPd2kQeKMzE=1
zv%%(qZ4S02*tTHXgY6ycD%gi$cfq~}`y+U$;4uY{C3tMXR}g%;;28wZA$Y>c8a$uD
zD+u1OtKe;eg9HZ+j!$qL!SNlOL2#azzy7-TZMa-+7ep?oy`cVr<_lUbXuV*5!OJCz
zE|IuI*@dGEXBW;d0$oIU5$Yn*C6SjjzhucJPcM0PDauP(UaI`^zxRt`7iBJLytK`w
zRhJe{)=T@iXm!!!Mb8%v=U*3-UW~dJeKGc8+{J7c^SGGz#f}%dUhM1AsY};gI(O;j
zOSfLS$E8=7USE29>E}zoUIu#^+{JYlcfYvl;`YnW?Q&z6+k6?-Wwe*kT}C*6UB>t2
zt}b_XxzCqLU8cun9xsc!EZt>+lj*X|mu0;y@5?GKtGul0vf9f!U)INEW0%cdHh0;^
z%eGy%{j$M-zAwAD?Dnz`m)%|V`(<A*4|RF8m&bDXQkO4xc@CGSyS&8ZwO`)i^4>3>
z=<?y0&*$=Qbor0I{Ks7W)BO*Y|0D9h?d$K({XN*f$NBHc{{8Fm_oh%lP@v%H!*fUe
zp&NAR(BH5R=<gBzJ)ysUp}#leH-ZpFs7B~O=o=A&2t}ku<Uk=qH1ID{j3`G`ASw|}
z5X}%R5LJj8L>;0dqBEi^q8p++qAx^0i2gzJ--ubnej>IYwxbxKctY`p1VciigrR)%
zANylK3Lzzsl1Q~kjYv&MEl90MeIfNiEkdnA?TFeLweaT~X@#^!dPI6gdPVwy^b_ef
zG71@sj6>#u><e`WbqaM3btCF#)UBv{pkATgqJBpGiUx%Si-s9Fgq%UnA$LRWj@*RY
zj(*PQ=Fn|MqeY`b<A}x?-4(h!bf3|L(4^30(FErYG`DCrXr9o#qIpLPLW@L8f)<4q
zi<S;84lN^EX0)v67ec=tXhmqXXdTe%&^n{_fnY*l?lz0I0c{R#Bid%PEoj@&wxbOO
zywR@EZqYuV-JyL(`-=7l+Mnp5&|^Z61wA(O*wGh4UkZI$^ySbKq33{}aI&HoLNAJ5
zCwe3Frs%EE+oJc3-aGn@&~J)<KhU2E{n^o<Kj<Ln2+={&5uqbSM~#jK9UpWY==etG
ziT)j+e<Sqo82$SX`ldL*IDlh~gC96hIM6t-IOuRN;9$hTgo7Ce3l3HsY&h6)@W8<n
zqYFkcMgpT8qXMHJ7%7Zej5J0DBa2aoQIC<sXvFA_(S*^A(Sp%0j8=>`jCPD37=2*`
zS9@Xf#^?`5|HgicFy<Hwj1!Dgj5CZC#x2GgV}o&rvBUU=@rdyq;~C=x;}zo#;~nEC
z#xIQD82`ouVL~toF}Y$wF^Mo?n8cXWm^7FiI8->?@s;=wjNr&|Byc2gl;9}EQHG-&
zM+J^b991|{IBIdEadgAc9Y-6Eb{xHM^v0ZEPBCYgzpx-!T(O{7Ff3v$I2Hm6iA91%
zibaM+jzxh*iA9CQ4=f3mA(j+Nh9$>RU@5UouuQScu*|V6uq?5xa6)iGal&xIaUyUc
zagyL9#Yu*f947@%Dx4CWhB&1-<v0~MO>mmwjNpvojN?q;4F1e<R^Y6}Il(!_Imda0
z3xW%Z3yuqkiwqYzE-5ZKt_ZFuuHZysMX;h+ajXPZ@MnTmidBPEi<QA@#A?E7!D_|o
zg<FE#2)7)!0=Ejc7Pk(!BW`EhK5+ZSnqaN4HdtG%2dqb|XRIGszp)|MgxDx-EH)0C
z9h(O>FKph}5^O292HO?eFKl19W4KeebGRFEH{))_y~6#7dpLREf#5;m!Qx@Y?tic&
z*j=%s*hSdI*h%a(b_P3#-5t9Ly9K))|0MY575`lD&kf%QzD4*Z@J-^I#W#m<GadyV
z6&@`f9UkG&1&=R$C-@%XyTW&i?+)K*eBbc>#1p~O6;BjT5uOB|6rL=eW;{Lc^up5{
z&n=z}o;y5`cwX_m<AvZw;w8aLikA#8IbI69lz6G|(%?nmMdQWdrNfKE%ZQf=FEd_N
zygcyo!OMw%J@Cr$D)1`ts_?4uYVkVYb;9e6*9EUHyhV6pc#H9-@n-O5@z&vOz?;L{
zh_@MU3*I)o?Rb0O?SprYcY$|__XO_>?-uU^-W}fWc%SjU;(f>a1Me@qzwv|Mhr|zs
zA3c6d__5)~j=vcGQuxc^r@&8zpB6s{{DePm_&MU|3%>|{h4^*FFN$9rzXX0s{7UdE
z#V?Itcl?_0Yr(Gvex3MD@H@tDj^6^mC4MLPo#MB`Z;jt~{9f>T!|xrxPyCzU-va+$
z@b4XeF#d4-(fDKV$Kp?iKO_F!@n^!H1%Foj+3*L3J@My-PleArJ{SCNPX200z{vkV
zT!D<c5x<3mTnV9s#Dvs@{2=5*$l*gy7?O~aP>@iPFejlVp&?;MLPx@%B)pMuB;lQe
z8wsC2K3xU5N^;G}wI$cl=W~cyloKilm4v2*W`wRJ5+sr&%1HDNi4=)i5_Kfn2xEk`
zglWPIVV1CwF!;~YN4uDlSddtfI3aOP;)2AQ#D>H@iSHzyNW74ECGke;iw6;PMCyYy
z4f%(Ye*~FV^5Nto$Vb9ABL^i1BgcmvM~HEVqY(2D3kYE$Rv~V|Z$sRJ8-#cY@fPBD
z81gWbVR#GK&ycfFz|KRFg(43{=^qcgD6gTUp^QQqhmwc#2on+}ahUKhIl`2L=`~F2
zFg?PIhdJzNsESaPp=v|5hOG_t9GWmRDl{%Muh6{1ZVbCMbl1=w{(0Zx@Fv2W3~$eH
zbm2V=?@@SH;e8G#8?JM>sc^I5=E7|ccNOk7e9G`?!`C%@iSQ-E*A%{%@O=&6GJH?r
zZxa5NSB$Ute#OfbZ&&>L%9)=@L9YT%<W(eBkzPf46^EZ*8D1s3N`95cH94*+xu%()
z^Q^vR$F<<s#$A=TDtT4uRpnPzUe$PQ#kI|@?ReGit5H{DujYLn$km0{8|?Br+UuQM
z@9H|euS;@W((964m;AaE*QLBJ)peP#tGsUZy6x9pU7z&&64%#qeeKtGc>N9ga{awi
zPt1Bx%RIiM6e)@fMesiZMUJA8qJ^Sg6s;6(6zvo}0ha(G0SH3TAL{W$1c?xmALKL0
zRgi}u@j#A&)B@=Q5(&i%#T&&txHpXijU<f{8l^PKXq3~apixDmjWXtKpHs@=<nT@)
zC>5LvL4~9up(3RsqavrGprWGU@LnP*m7GdJrKB>UGNm%3vY@i0vZC^!384w438#sm
ziKIzNlZ+;%cdwMvl+#quG^6R^y)C0O<21`?R?@7ZIiWeFIj4C_^MV$X7MzxpmT=5z
zMQO#Uf?ZH0sWMcJRBcr4R6VJB(U#Iy(RQTmgK9!`MD<Mdn;J!prN&Y7p!SP)gm#K{
zEA2<>2z8n|L*1RaiMpNMEQQ!GQaZwkqvJyFmfkly5jqJvDLOsq+|qfZi=<0Jmy|9U
zU2?h<bSddl(PgHqqN}AVoV@5}>E`IR)9p=nLHCsIitYp59o_GApXt8R{Y4*wJ|ulo
z`i$w*(if#KL0^)-Qu?~n*F;|neLd)#&^M*;n7%oE3;LGyoznN6z9;%#=zFJc7<16y
zl>Qd<x1?ZlzYqGk)6Yyl8~q&g&qV(nL|8;v0(ywBiEsz0I>J982S<2>j1}Qmgx?Wt
zoJAt=5fSW2z&VeEh=hzp5{WbtStRmE6p<(+QAMJT#1YBBhfgp%$s)-kDIzH&nM5*;
zWEROhl0_uTNLG=oBYAk+OjwlgC=pR2qa=xvG)l54DWar|lFD0d%A-_7sf^MjO4BIK
zqBM`vB1%gir@^i>9%UlRWRxXQmPT0?W$;oJWk-~=DCbcwqCAQ6EXwmJFT4{JJSs#~
z$f(GoqKt|vDrr>ms7#|Wi^@DI%cz9^JiNP9JgQ_=<xy2cRTcf?A|)b~L@JF`5h)$1
zF;Y{cmZ%j`Yom6IG>No`^c)!%b+C_7uOc@^Kj-L1qML|rGP>QO+YyZ_8eMc((cMLp
ziKZi(+i01iRYa?eRu^qH+TgfH+auaVw7cjbqDLFOMD&u;OGmGJ^jf0V7QOc9Euy!K
z-YR<Q=sia7CHgI+-!}Tauz)bSF#2IE0PkmhI~Yfd6UHgy5#x+;&bVM)GM+G=GM+J>
zGhQ%WGF~xWGY)V0W&$xmn4nBBCSoQy6M_lJM8ZVMM8-tUM8QPKM8!nS#5a?ONy22v
zBxRB_DVUT@CQPPGW=!Tx7EG2*R!r7R9xOpDAuORR;Vcm>kt|7AlCmUYNzRgjB_&HL
zmeedoEF~-rSxQ+Nv6QitvsAEDvNU07%F>LbIZF$cmMpDUTC<F>EMysF8E2VbnPgeQ
zvXo^R%W{?#EGt=7u?$9jvz)M;vYfMAuw1e{VR_2(jO97Y3znBGuUKBQB47n&1!sj|
zg=9s-ij)-@D{@v8tSDJgv68YfVkKv#V5MYb!pfAD87p&E7OX5;S+R<+in5BcO0Y_@
zDq&U1s*F`Rs|r??tg4u5nbJ%draGp2rbedjOifHJOs!09Ozlj4G4*1tVr|b_$J&v#
zGiz7YK3Mx^S~1-+t(mq=4@{3t-<h77UYUL{{l)Z)={GZi8O4ld#xmoWnVDIcc`);0
z=FO~Oc4T&9c4hX#>=&~y)-l#8)*040){U&2S+}rmWj$lPV!dVk$oiS}D;qEyEE^mf
zW;UG6N#-<jhPjTpJ986r3v(-T8~Zu4p9}lBv0KY-mfak?&1_U`)NHhDbZng2U9r1m
zcgOB<zOnn8O@vL9O^QvHO*5MoHa*zfvbkgPjm;yQCpIr^UfH~}`N0;#7RnaJ7Ri=`
zEfrf7TP#~TwhU}>Y#G@yv1Mk<$`%;^X3K~D`o(@d*sm|PDz>(4wQTL#>exE5b!O|r
z)(6`Z+ceuO+Xl8dw%yn^vTb79%(jJXzu3034TinhuGntbKCs=f{m%B8?JL_KY=5zb
zVh_U}jy)#!SlDA@kDYxf_GQ_ZV_z$KV)j(*Y1wmNPsg4kdoJt=|9`R<W-rQKg1sbr
zIrbXaYh|yUy-xPV>`mEQu(xDy#oi-(uk5|E_sM=!_S>@G1N%L)-z)pQv*Vkcj-5Al
zj_h37dBzAJFa{nTV;SQ##wx}-#$AkWF&<-lkMSJi6(~@QU%(||{5!siIEH<V<1OYa
z7CaVtEJ}}{<u#TxmT@e3EM+X~SRNi$Ct;lAaZ2L!8mBZ)k2ovhoX2?@7d)<b+^n&R
z<5tD(7;6>lcWi8ITx{mpyy9++`!#k`JcjW|<59(<jmJ42kNB?Q`y5Y!$LSM|Cpdq^
z(-BYK@%)Gv6)#=9%<%%}?|7B*YU9<#>kM!{-fX<Nc!TpL-hRc~9`AGf*y1l2f35M;
z#!nZ&2oGSVJUBQapcrygb98XLae_D@z+pTPgBa|IM9s+$PG;Z(oE(7r0on7A1DfX^
zmv20L@ep!9;xyuv16%_%24D+t6#ywfPW-=^iF>Hja;iD)ICY$koKBq1oUWX1obH^y
z06;)G=TXa}4T3&Ia0to}Y9Vw&OoU(ufehjlgdxr}XB&?(kFPwAVYcA8^Z3r=jdR2~
z=A6JS!rgElaUOHO3C<<w3Fj&28Rt3Y1?MH_73VeQ2N#$N!UgpMh2TPRk#Lc6k#Ui8
zQE*XmQE^do@!<l-e{+esBwSK|VJWzjTqazmTxMM6TozoGTvl9uxIDQ0<_X~m^?582
zJdr#}c#`rY<4Mkwf+r<UDxTClQ9L<#^37AiQ|j+G6+D$ZO?aB}G~;Q`(}JfZPb;1t
zJpJYw;TiQ`D-%4EJWF_%@+{+7&a;ANCC@6J9X$KyIpw+Fx#W4m^OWZq&vTv^JTG}(
z@%-TVH!lb;sDC_#;DzKx!i$s_8832P6uc;TQSsv7#WybrFDWktFC{M%UZ%Xvc$xFE
z;AP3nikCGn4_<!riuxyA30_HFCA>;`mGLU)Rl%#0R~4@gUVZag@;c@9hyQ4}BD^iR
zrMx@%o$x#3_YdEG@fYVW!C#WU68=j0tKhGazZ8FU{KNT2@Q>u5pZxfXAA}zvKd!(h
z_z`)eaK!wm`O)y>!;e4wIQa3+&z7GHKUaQk{M`9B6#)?e9AWY45Qu=JE>JDdAus_|
zNZ?Hn0zBW-K2O&HkOKMu%3TZcLwE`e<oOU`C_*7ZDMBMcC&Hh=MMZcQ;U>Z-&`S}%
zMfh7>g}6#_&42`os}t9oxNhPK&>VS?3-;(K5HK7dDw&`;kPSgwLA9U;2!Nm)fOr7w
zfX)Gb1IQNi1DIGu2H={AHo#<nzyLx49s-C1zy*K^APj&G01F`NLsAz^3DydxA-M|H
z3Dyhd1lt7Ly`mXQ5gQSYBEE}w6+93e2~Iqx_YMzE3%&|Y2(AO31D*i(4JHe=2&M)$
z1oi<FUkly{euwE1{1E&U{9A}X2qXmYB+tJYHYr3RL@GokL@q=jL@7iiL@mT2#J7+E
ztV?)C2o4Hkr4%v|G8HltG8eKCvJ|osvKI0q<RRp@NCHnE0SSUb!?hDBl0+n_NHUS+
zA}K^tilh=rEs{ed-y#h}N<7^J?}r;nrAQNzrXtNmnv1j$X(`f5q_s#7k$#IT5E=2@
zG>d(-fE&-G$P$sIBFjXUi>we?DY8mrwa5;UeTzI0Iq_T;U@1fw_@cQKc_Q*u<eA8G
zkryH_MP7-#7WpCaZ@)BJ5K)Al{z5c@2UAE<B%(+~k%=M~MInk(6qP7yQ5>T97G)qx
z>M<{bE_jNi6lEgHRFs)0b5RzeEJay~vKHkb%5PYD6&3N^8sZ#0@=A&-5mhRxOjNn3
z3Q?7!szg<bsuR_hs18wmiyHaKsXwAV#E&21M<be1G<VTVqIrnsCH`rIB0^n-qC&yB
z7Rm_Iib08G7QY^1RpNPw5Au=qBg99FPa;08_&D+TDL$Jxh&VzJRDkpYr4t7eM=Xw7
z96!X-h~pn|e8llb9EUimIE^^(;@reHl>w0f9BUaI5)oJ!ljx8bOH3rz5^tbvN<zFM
z<wZA0__ZW|K=$$qz1OgW45bW}3|ko*8Fn&sGW-dOk__)M+(0b?kqERQ`Cnm3UO5Or
z@|uF0Bd<nYCwX1u6=*Z^1QZg5*D^q=03{-ov;frrNW1h*8^!>625JQCRLN+R(OpK9
zj5ZlPfKvg30ty6t1}F*;4<Hi25CHlEtOwK%pj<K~Su2^A%t&S>>m?fize@H~vQe^C
zvQ4rt$)13>02p~h6AKw9GTtN)Bqv^k^T``nGUQ#rJCN4E$snsiu7dOfc?Hr2<O7KH
z5X2!;pHc)6-MsqZBMW#wcqBL^I2ia0_yE{6*eIACSW+!TBgG-bx0C@qQ!m)~tHT>$
zOQcMt%%sevETk-@tfZ`^Y@|G7637I}gm{4m_!;C_@OXGflE@^LNhXtACWTB&nN%{V
zWdd*cmT4eU;sqf{$Pgaj@@XQ|RHm6sbD0)0EoEBCw3g{1({GstG9xm(dI1UeALMX|
zC~!wvBC}LxnapyT6*4PjR>`cE*&(xUnFlhbo-9JbhwuZppC>X;WuD1Amw6%cQs$M+
zYndOi2xLKIarFWfkV^mv5U$|s7KtoUS!A-vWl_kYltm?rS{Cq@Z&?PiBwo-0Gy$;<
zo=us^GL>Z}%UqU)EK6BdvaDr!$nslOftS93wgU8kKnM@AN@SJFDw9<%t3p<#tSVX6
zvPQE0$oh~!8rcZh++;J#W|GZAsz@p$+g@rcHI<r4OJ%=GXXL2leU^)pE0e2|8<B72
zF^&(n1aw?bIze~?DFsNgJZgFTkjF<JhdhZqsXUE5J9$p>T;#dQ^C`bc62MNApiY7V
zmaY?YB$y<aCwNN;N(k{Xo|m)hBn*>KCRYWbXF`!zYk-jhs`3&c7)YAXETLsWHE3Z8
z-4pss=o^#~54*e^02Tzi9e6kpY+%m7ih=Y3r_~8F3F{KpC(I>mPS{rx%OnQW6(pQ^
z1(C-9AAJt-I1kVpyaY%KfD|AkKtBL$>O=%yX!I!%QVrNCWEw~wkPINWL$HSU3n3MT
z9f|l(WROVWRY~vC;AY@CVAo*QV5wksU`b#ZFxfDFbt1P!9*O);k|0T_2ao>hFdDWr
zNwOr#lcY$JGD)f=sgvY8NdvEU0>*?~2aW=x(lkl4B+ZkwNYXM%t0b+H^hmNG$%t1*
zLCgR^3W*WC3ND$YNtPv9o@7Oml}T15S)F7*k__JRo#a81Q!jo1fQ3W~UI@34r%9eA
zd7k7&l9x$dC3&6XN0NUhMUWKKLtRL@;IDAAMVb^@QshZdBt@ANRZ`SRaU{ieQU)N?
zdQc3>89W=lR+%PcmXvu?7D-tqWtEgiQUysxJ#dBu4&D!6y-JfROR7Anili!&s!FOl
zsXs}5B+ZmGkEA0>N0WoVIzs{qG4Q$LlN?8KnlzwkP^ZC>B9bDSq9es5#al{<Uy<;u
zGQjz2zv3FoG&E_rrPl<6`}C?of=?;(>L6ftuU13sfybYwGz0lKrBzB>5NAOi1(7a|
zHW0HwnF6s1BqWf8K<fb!2Lu|>UqD_-nNC@svN>fi@^=~o7zt8NykhM+4Eza!4dxao
z0bqTA@_^9+e*>@v0DYt)0KDjVgU3=3|G)$Q!2luwgaL>FKmg?RBb5Q<A<r#*$c7jR
zW&$A+;vIxA2uToSAaERM5~K+M*YXs^8#EXwSQ8i%*ayrv%p%MUJa-uRou&bNXU|mp
zN5bV`9Bf&d=4o1_X_=-~nzm_rr0I8>1!+dS01vbV;1$GXFi#kpWoeeDS&?RCnpJ65
zr`eHa-)SDCIZ1QsIS^nm2;^YPaN#^l^E}OqG%wS<O7kPlztbW}3+nM21beV|`0yf2
zi##ogv?$Y}N{b^czSA;DVL<@s6u>&j3E&oRk7bsYd0G}}S*B%`RzX@(5BeZufYZP?
zsIs)m)2c|TGOb`homSszZPR*9>rdLM)TC*DrU!wwh4hHh<48}F1vCrlEI6P#C&Mly
zBqO0;c_4L0jx6l5tA>S~?7C$Xc_A5ConQ8Vqy*jx&j}VM7SKRJ(DXUVi;o~Cf=UST
zA4q$k+*MheXYrQtAmhZZW_a=p)DVy<<TP+@c+$WOf%pNV1GWY<tj_q6@$XConIOP1
zJb?9h8o~g~2|!qYqyRAiH3ASkG8tr&dY0*PJY+}+6)<m*AR*^L4uiA=ndZon00OaB
zF@2PT2nZ1kA_`0|1QZAm;P&9m;HXEI2H+uH?DPf-#s^jfCIA)ylL`|CGXc-~$g;rK
zGGxTB&-m{EcLtvdmxH0O<ylr_S(#;3mepByWO<O~B+IEM!GOUbdqWrln}UIPp5;ZB
zmswtAIgI?yiXbbf*I9us16qeX52gp#EAp%;vZBn2Dl3kx4E)j%@M(ba00bZ+f_cJc
zmU&hdSy^Ufm6b5;JF9}MBCv?$nKuA`fD8~j!I0tZsywTTtSYmr%BsUdiP~lLGpj#Y
zePjp8U@_`wvg4DTYj$qgH_g7k^MFEGWR74}iooi!Uz_rayLBGwJlyhdhsD4A`a7ox
z7S%k<_e*7fO~6<DVihb1<q^&!4eGh)9A4lC4H~pymB(AogPaq;(&d#D5J!NN0HT4o
z4t@sD4>STG|3K`4vDZ02auEO#^{mB{E{Of$i|~wrw*U_TY5{Bk#(d{8$R+j6#sfRZ
z58$sb4*-k+=m2&BICA9jJ5Pc<p<c%JX%f;Nq!Dmym@dd7kQ*T4Lo`3~<U3D;JSBcP
zE(N6xVj09K2tW`3U}C}l!MDL_!7YzG{mwIzXVmNB-rT{6!9c<Cz^cFmzye@uVcK9a
zjy(I$bLv@_zd86+xE73nt;q8-&#OE?^86dt0KHNI%nV>1BsT~)U_>yaDDtAriz+Yb
zyij>@<i&ShQm@1VhXVizX%NB^m>XQIEb_9<%PKFAydpU)GlK2{JP$x3q)G@~V3P2G
zRgqU^UR8PZo!6iIPVzg;@1K0i^SRGAn}2xz5&6gEpId&A{Gj<M^7EdbYZ1^QsG<0;
zAfzC0gbIKf*3OIYUR-f;6~(oMb>iasDk$<xPayXARaJncke}gOYFIlg=ne~Mur^ji
zFIaAY<&7fV3Qh_h!YZg=RqzTLFj0W9kkUbr0@s1Z0}2tSKOpPW1wRTA7J_=J>X|vD
zeF(<jUhoKktOEZ8FiHv;0uk~H6rNoGVE{x7=m26icp*G#0K|ZCVdQs_P|tTgDgx{P
z5gkAY%mY9N00fZizl)T5rOIbG$X<|^AmoD|!$d*Ehfof|`n$+Tk<lWHy`bge8pJFJ
zMi6Mg-C;h#+rg8;KkFhpitM|{Ns&`8eR<;s!v%{3JA-M5S%<lVc{&PM9QI!c?iKC^
zJ`An`FT++AMO73q;JYZPUvdG04(J|`4&)yQDquA*q%4cFD$1j%sF&7&!2|pUC<F-#
zq7Il9T%#(Bsw%3Zs6T}YijEc^UVMP)lj5Moxt0MfgQG;WB!8ELl!QX5L>b<p8la@e
zuO)&|2V#I<8V6tnVXT7X;WAQXw0VWVuXVv9Q&GlS$%$X$<}t{lUODhOCIB(WkP!Jm
zC<Fx%)IE^x>XQE``B93n6tonP*BHG10+0=IDa1%HfB43rGJtRZ!@f&NN*R`tmNN2c
zglBpH?jY+zGzCY2X9Hvg_{jkX-fI;e{sQ!aoD8uR+z6f=AU{BLFyOmPsaG65ngOH&
z5Crlz#A9$Zc)F0bAu%6i`dwz!tCBu}L2`nu0~rNkHn=0q0EBD^#C4e+5GB2G>Ejo~
zCI~tZN+3Xh%fc*y$AUK=WkJf~S{Ae{BCm0JBL?dP`vda=y8(lM`Go0&*@D5}Wl6on
z;=dK#H{26^9=r@&RhCCtg=Iy(N(veb5IDfI6<`g>ArKzGB4AWiR!3QX${%C-&sctV
z`2nehmgiapv<m7fI4VRdLMj5sxC*!Gsw#^75-|v_zD@$TAOK_lFEf+^RCI$yQ&<75
z;;rHYM0qdDdR-gHB7kTRlp$Y1Kms#_`vO_Et^}z>Scz*TsMmzNxCU4opb(@rh+AN-
zaQmPN9hD@N3@dr9Bn3Uf3%#EC11bm51PKr#9N0B{Q6Qh+RYJYC<hc!Cc7Rln9wGjL
z)x%=|$bJA^_9C&zsenNNTmt5T{0RXP+`<F0Dm|)<R2lU`vWF%BI{;4rR)8D}K@?mE
zo+YGM81@bM&x_4IGeOpYbOK2NLO%pra4&e?5ND4n|E>yB71V3e|BX$aK=`Nu;Q;&`
zybz`Zobaeh0?y;bFmJ42j$mqFMqnZ^@i1>NZ9l5=pQ?nB-&IAbih9}1e{=Y5aO-ee
z@R4vW7z*1_Ro_+psk(<3)jquXfV4)dqppslI@da&^`)yXI}~F3axFw_yh3q99Y1Q$
zY5_Yaa3EiT$^h~QC>nq%fJYr(N>2|6WVIb@7uBPx@37l?nd^0~pSJ#>`p;7T`2+P|
z^-<Ty=Leo)z2^t7e~9#l485NBqX9|pN0a<$+z&NDCHRkF_%XU4n{9%i33w9}P0%&L
z)&ysRP=iQ=XoF0Hx(2xh{cO<KpnHRs2CWU+8uVz;t3mGu{f4UC2J;4s2FnH~4Ne=J
zH8^i@(O}(R)8MVauO<>rlr&M<aMEzvaMtj;;jc!JMo^HIy?P<CM%17NHj*@wHj+0o
zZRF7;q)B*_<V{jHDQ_~;WV|V8Q}U+bO%pT?97O{XNRu>8-ZWj)tWC2u&EBZ7Y2S@e
zjoF$`HJxi*)Hpa!&F$LUqUPqB+pQT@GrH!knhAE>Omj0W&Gc#(*DPzZs%D*=_0_DO
zW{VnlZkua%*jEr*nn&9_`sPbD&#`&FnwM-|_vSS<ucdj%&099_srjJh6Ez>+e3s_3
zHJ`otyqeF^{QKLN?D6^^QUBBMf11}n&HEoW{d1H5jPCy+`oDMgzZ<ld5EW1r)G9a>
zA_^S}6NS0LiNdMEg~Fx671ZV`>=fP<A&OiTp-^t92va0hq*h_5u1;MSC}~pk2{j@r
zdMT!0IbSgg>+6a+#eTy2G%PKv*r<3{ocQu6E`0g0Aim7auU>E^Y9$&aKE9?{5??&!
zMNh6|tz_e?j1vO-&VTD!pyHVL$~fT68BrNkIZ=72ayS9xdRO^J6~q??7E~2N6;c%+
zRT5t(SW?JssuZe}s{E+`Mn+W$RY_Hqs`{upP&HDp(9|?AwaU*QOx32Ek5a~WIY60n
zOH`{=YgOx1JF0e8?WWqdYX7MAR60{yD{Yk?l)foFDm^RxOX*$dhthAQPhTi)h%y{B
zZ)Fl?Qn<D<S{b8^Ri;zMDf3gAS(!zdO__%>U&_3d`6zQLdslW+_Dgk{>a6NI)y=Bg
zRd=c$sh+5ws=iizqk5%!tNLE`lj>*HZ>oQ){#344Zd7hkZdGnmZdX5Nb+hW`)a|Xt
zz&CS1DfdWx{TdW*jIkQI8ig9A8f)+XH7Ye~HCi=xYV6hM)Ob_lsK!~1iyAjI?rMCf
z@uS95jsH=1tnS1YfZUb3x9YCd-Ku-9?oQqB>b|J^rtZ7CpT3C%O2{Xorch1P7mQ3&
zO}UzCH8pBdYHHP_)nwFU)zquWsp+PsNllxYb~QcJ^i~sG=iAqzFH|i;EmAFwTC7@}
zT4uE@YFX8?spa&A>MQXDDXUVeR;y8~Rcoi#Uad~8H?>Y`oz=Rkbyw?Kt*6?M+OXP)
zuk3>|maSHsQkzwqQ`=2#v)VQVF19_?_EFoZcBFQ!cH+wbq2^^5YL{wPYS(HvYPV`1
z)b7+it9?=Xs`gFoyV@UWf2sYg_ESAZ_1M&7S6^0r_3F#1uTg!?>KUjfQctX|Mo
zBlQ&ODb=%9&qh6!dTR9?)pJ$PmwF!Rd8+3(6y~cJfoeo3y-_crUQ)dh^{UmYQ7@-n
zqk7%d>r1^(^$ye<sW(<{qTW=!nR+Yrw(32p_oCjLdY|eyRlk|~ZPf>>4_6<dK2m)W
z^)c$>)aR%AjOue&pGAFM>LBWbU8qy4Q>)Xcvs0&2=d8{}oxjw%sdHE7mpb2VaI}cD
zh_+~H(bl4)#iYfe#chjSi?<f<Eg>zb+c0cH(Nf*gS4-b*q+2LH^A$)f)DN}Hw9K}w
zYuV5;*Rru?_m)jiWYe+*s$p8TwQO%0yye-lSIgdQJhkz&<-|8G2spy>DsRQnO594)
zO4`b{m1CRWHX&__+q7*n(q^>Ha#)~l8`$q{v$V|yp}cKAzUou8t<tSBt+K5e!H8S6
zwrXqJv4zlLZEL&M_N{fT9b3D$c5Ch4_G{ap?ciG1w{C3Ry>(OT_V%as)lv|CMmTZp
zIJe`Y9nbbYxA&!;Y&*?9GR<u}n|8jn^V-gPJ3rd_tDT?i{Aw54E@Qg}?TXr!v@7+6
zSF37Q+pcrFKHBwcH{5Qd-Dtb9cB|X1X*b<&w%z)6bM3ab+o#>m_R+Nu*FJXNcIi94
zp!pJxb^G4i_t}1U`w{I&wjbC2`2#VsJ=^xw?P=PxYtOzthxT;sIkxB2o=bbK?YXt*
z-kx9W`E1Wud%kr*bU-yC8c~gjUxL6!;~tt8pd~;P0=4zNqSn{7*E$S!By?2hsDlzT
z&044-gZeP2#M11p*$Q<|n(dlBwIEuAuyC#g*OF*SwTyH|b;fla>-vvwplb~RIs`k2
zSrB(1Mr`1>y2ZLBx=nR^hi{^_)w<XEuJx?-ru9SXr`D%7kO~6d^ac$smS`wZx73%j
z*;relt<<*mxdPs8jkcY(PTNu2yS9_Iv$l)2tG1iAyS5K)4{cA~Io-eYfPBZyK=eR;
zsu{E%tR9>mZh9E?FzaE}!=Z;$yH2}FyH(!;eM9<&^^NEo)wf9Bn7(m+6Z%&BWOZxx
zP3xP{H>+>GzBzrH^lj6(UEiMicKU2KT0J^F&ibzO-RQg3v(a;}=TXmhJx_X`^}Oi$
zm!4NWZ+d>{h3JLp<*k=f|9a?E>DB1f>ecCW*6Xe}tT&=JsyCxItG7vSv)=yb?bJKg
zJJCDUJJWmZv*>R1-s^qPyVLuu_eJl&^uFqS)BCRXhu%MWKlFa;$EY87{h0J))sIbo
zN&U6!uTwvXep3Bp`YH5N>SwK=Mn4bz!umz@E7UJ2$_}705&Dqyo9K54HQM^E^?TKy
zR-drL+**AO`gHmn^*QTv(dVkqO`p3yANu^#=To0=^H*a6VghP{+60F|1bQzGstr1z
zM-DpK3?>Fg26Kaj!P4Nw;K~C8+(KKF!3Nr#4DJl>4IZFL$>5&`-wYlNz8gG2%aFl~
z!7H@x7`z+&F!;;hr@=3S-_YD*2r=XeogRkNzM%@*BSLOMVZy|Oxe1L4R}*gLD$G?I
zIvKhdx<l2bVcIYQm5zq}G;B6(F>Ez#Gi*08HnA|VG;wO;%EYaSd&7|tfiK1s+z4sp
z+oZ^(#?*lO1JiKR2-8T@WTq)hV@%VTW-|?>K!|?;eXLO<By~u@kQ7aeP1``MG_5o3
z16<u`;%htsC24K6F?s;w1Iq(TGWrdV$)hK~S!0|rtM4d<)-g+bWj$1eLaCh<#!6!o
zV^d=@V{>B*V@qReV;hgWtoHA=*4WP2-dJbsXzXO{Vk}&6Gj=!j%h;!}FJlj5Pt&cY
zx27LWKbw9vgEqtL;hWR`E?j3^Z`^3y-MGm(cxg3mGj2ETVceJb`7on3<J;VonF2Fe
zGi_#inCUb#_NA<IYi4a`Yv#_(H#6VOJehej^TW(vW`3Ia^dNDW&2pL*o0XWA`U2Qh
znzc5oGOISLHLEl0Y&L8*Vm4|vX13UDHINvyX|p-A?PhzM?PIpn?8NNU?1R~z*=MuA
z&BL0<XdZX-Sj}TIkKH`}m@j6&oOu!7YV+de)tGl+-UQli4AeHhxdEQ^KFtT24>uoS
zKGJ*=^Ks_$(;UPc)Epncv*v6KM9R~c)0(q0r!(ixoTEAK=A6tqn{zSeFLSQu+|0R~
z^I^^}b3P6D*MFY&FJuE^18RdBy5H^J)*@mNwWx+RU5gIrWrb!`i?PMTVs5dpSX!J|
zoLZb&oLgL2Tv}XNte{8Hhicz;Xf5t6b{7AHmOp6ogN{Dv*|T_traS1PgHAbUezW)s
z8rUp;S^S1ZGE0ahAv9lE5?fMR@{c9oHVkdZZ76IgZJ5~5K#P&RCYDA}%Vw#tG_$nz
znZfsgbe2xgn*m)JmhLuEmcicoq~S}Ptz{h)C|l;Bsu+rfq4pQbd7*3<N^)&1Y%Fa&
zSsqwUEN7N~EI+LvR)khiD`G3SmDEaMrL+mPDX}TF8SJsmsm)_sP+P>dB(|isjBQ12
z726uwdIWlJ8)O@7o5(i8HqthUZA#m8Kyhue0*|%LZksRLJnjEMRs~kUk@ytnxeag(
z;0yr$0lNbn2dE9W84xhw+QX_7fDvE|+dizeR(Fuktagx!?0?e?#C@ybC9smetO=|^
zz6aD0YpAdCHnBCrn%bHMLWxhOJ{Ev8gMT{kF>ozwHn3xBKGqyyYSso|D;|0IaW`v6
zYbR?LYgcPG+Xc2mwo|sVwsW={Z8zI)wcT#JhwTH~Bij?(Q`^_JZ)~q@Z*A{vf4BW?
z`_=ZF?P28G_NR5db)$8Yb({S;+n*16qxQDh+iAzZj>wMKj>L|k9jUKaAEg}|J1P$b
zAqc^(jh!8x9Y;IPc3kYZ*>ShymmLo~p7xIH9osvxcWUp<b3_0UcWdvny)X8@+52wq
zZ+k!N{j^hHCuApVCt@e+tK+BGPQp&LXO;jxAm+i7n0h-oJKgOx+iA1YZl{NxPCK`D
z?(96;d9w3r=Z9U8U9eq<T|&DsyTo=8cB$>6JcNNKx^#9K>@wPAw##al%`UrLPWy}4
zU)273*k2$2gB&aJO}#6zYiL($*T|Rcud!XZU4>nxU2D5Gc2yo0K}d#)T06V;c6D|g
z?RvNCWY^iQi(OZ{Zg$=6`mpPlU7vP++Z8_Lk6jPDo^}iDhU|vzM(h^a?P@n_x5#eH
zZn52j-K5=WyES(E$8O4_unp{Du$!~n&2FRJCcDjcTkN*lZL`~Mw};(6cKc(u!*1{q
z-*(4#Cw9Nuo!Xt*y|%joT<-xLB$M6Ry|epZcW3vXcE8zuwENxelig>#FLqz;zS(`Z
z`@`;Ec7NLaw2#p~?)I_RuK;8K`(^EyvtPS?2KI^Ulh`M<PiCLWK3n^o?el~tY3O^f
zFJWKGzO;Q=`*QZR*w=1fANxA&>$K2X{U-L^*mrB+i+ykQeb^7OA8tRwex&^*_LJIA
zW<Rz44EE#f=coNF_Osc~+y41u4`L5ykJ=s|dlGvJdrEsI_Dt=W*)z9iVL`w+TY#~U
zJT183+1azV=U`7~&(WTDdrtP8?YY=<wdZEf-JTD7e%bSB&zC*l_B`!>o?TFPh;)c{
z=zyOC=rHN9>hKEP%N-#dVI8SE@}nc4j^YkVhN<eP?x^jk>*&(awWAxfrgW_BnC_VA
znC)2CvA$yiba!;@XUA?G8#{LI*aZC;&|=ZCUmaUJ25;Ou_UPDG$DSQ~b?n`--yJ)<
z7<KUo<@g;(9p{}uokU%BcFm_#ZKrgn94IEBFLY|_6c8uewY1Za-4$d$2zi|~ot?m@
z!7RECbsg?H(si`!c-M)plU-*!zqzj4y6)(@)BjWk^u9wcivw~1v8wOf*tw~5Yv=aP
zeRb~Dxw9LUk6~lijjkK#ZhUli-Q9osj`^ARCYV`w^VrR6H$S>%>sHaNvRjppdF$Lk
zMBIaJN8L`kJ?wVc?Qyra-EMqD+?Q_Ox_$5VN4I~v{pj|ydyL&<>fWw<pWTOdAJKhe
z_er`>+I_O_lXst@`?&5Ppj~xm+X1-&NOi(@8oJYU=dC-(?!0&B)SYv8F5S6y=c7BH
z{a=6ef1y4geL(x5?t`O8q(`(zM~_L5MUP#N?>(M+y!C`YpL0*@o?@?!(YB|ir?)=Z
zpfL$rk9wwiW_o6O*7dCKnd{llo{i8X(=!;e_c7{Y)5lXE&wae~g7ktyNpvq%FV<cX
zsIBa!>gC!e6w1Z=ltQIWU!%V1`sUU*d*3{JMS7({dIQ-DbgN!1z1n;A=v&#h8PMQf
zv)+JW2R}gsKX!t9OL|LvH<GP;+w@j@r0Ih)#JjDxdvE{r_5kOB`R;q$_ha9``+@XB
z+Yh!M#(voQ;p~Agx~X?t@AiJQ{W$k`<#W`8`w5P&pXPqr`|0!=;XL;9)X#H2ul>CD
z^Vu(JzY-|)>sRG-+Uojs?l;_Tq~B=2v3`sDt?sv`-(0^v`t47@4?gcb#{S9r_u9YD
z{)74t??0md$o|vzAJ>1@{`2ZT@BYu&pQJx&f3p6}L7oJ`qd#?jn*Qwi)Ai@AKVi(h
zKj;2j`g85itv~l3%;5YC0Ud&R2#x`f0bK_~2XqV=4VVl#8ZaNQ7_c00;_0&gX#@DV
zfzLq5K*E90fy4u;hY$~x4|E+Uv_C0mHXE23m>pO*un`)o2DU<56tp}IF&<($#K{n+
zL!1xsHV85ZI*4cx$B-ODjfNk5XvEM+|FaW{42lklLuvD%;D53~<%22)RSv2eR6VF}
zP`5#igPI1l3~C+J27=(Az6SLf6o@19pzniT2BQWGJD?&nSUXrZ*m1D;!A^r+AuT}`
z8SEP(>CoMWAs7ZVzy^|l>ju{kZXDb+xMgtb;I_f-!%Yn%8AdvcZWzyDCc{jJIUeSE
zm>b|R!|aB69OnBlPs2P9^E%AiFz>_s80P1&jKeYyzaGPChjkv-$FR|1tA}kL_F&l2
zu;XDT!%l~t4SPK7^{_X?-VXaR?CY>^!ww^lVLyjQJG{5y6AT|TeE9GY!$%IEWcZ}R
zCmTM?aM0l-!wE+<fF5>&uwjPN4re!<!*IIc9EWon&UrYO;oOFEAI_)yAK?&zpE`1w
zIQ$EK(!vqqNbN%D!UUT9-Bq}&bl1dD?x=87Iy(E`1&M@<ZqV!sO|32#E|xA%T%5YN
zgJvoxsFTD=?S4qtaMv`j{NozXi+j+)LD2?D8nk1OgF)2=r4lqp*Ve9WLB#{9&b2Fu
zWgvJtjht3aw;=lXS@MDlXe)P+MV#J1-2idHnZNwckHN1TdGb$uBe>_}0;+DENt{WY
zshw$@Q9xFmnSjUvXLIHOEX$b#s1y(wfWH770m%VK0_@`K({<R(LOzQ)*UhfmAx1;Q
zbUkrBh4ANTYu|gT>7DEE5F^0t!M(w6!6BWa&PC3}&i(LfU$EVz8?j$i0I~}>cXVzX
z-MIUoRJjx1ybkS>_t@QqyGwUho)F)yyZ7$y{(t7q15R?Yy#EFQ1k*9S1rKGU-PzgE
zY`YsghG{#i9qE-$@2eSUwZqHVKHG=ScgNk?;G9hYBoG3G&`UxHHINWW=)IRv4WakY
z2@qQHrv>=`e(MJ^cJ}tp=I(5LcILZJd0)Mwku=i#KJOzxjQue2L+yv~PyKM@hnXLa
z{m>%m8nZefDT{JB{as;#GNxVDeKqH+1z#=tYP+u*7<}Vs270-0lZ{Jk{Pf|3r+i-x
zd=>aA^p)~e<g3_MiLbP;jIUB(4ShBCmGzMrCUX6#;zw0KYWh*jkJ`Lhk9<E0{Yd#y
z>PMD|VdADAxBR&0$AKRkKeqmk)K6Rvq$KEL!~K2FPkcWK{3P^~9e$GdiS`rYC)U@F
zuU%g^IOL)`xDW94G7m)U`+DH(z}KO#m9HaT$G+CSHoi`MJ@U2nO$n*3ke0?bE^+|)
zrs|sp#%+Al@lB6E@&*UA@0)>d0^fwbiF_0LM*GJ2CiTt8H@R=tePjJp`@@z$?D<3A
z&&q!0_*uixdb~!+e1EOwul4-3$S-Ps;rT_~FXsI9#2>pzq2Z6G{c*(~&-ml2Kc4l+
z4S#%wKR(kR7yfwcV`K@th7Eli`&O?`Ijd88b=q59RI9ztYUHm*$!gqNjr*%QT1~^%
zRIiTR)$!EoczShQSsl-;j;pKVVo+)fO5;J<9h57Ba$`^)51i7#DGwZX;Is$M^1xXg
zIC|iif$I+3*@4>_OzFY2H>l`AC1<Lh8E;T+v3Ik&JgEAEYJ%ykgQ^)+M}ygTFslc(
z+MwnQYW0EVG9w5lSycyhm#IPONT*R7)EkI>AJmgUvo>fp2Tgx~Vcjza>?LjW2CaC|
zUO}%qI$H-_chJS1Z5KC?gPuR=sX;Hn?Ex<32ffjtmk;_0e!vF4%l=e<6?^2skMTVX
z2JT=mjj`gvU_1zV1BJh65UD|w43Kk3y8~SrU_P;K@HSEV104@^I?%;Hj|bWgOf)e0
zVB`))t-;72<kdk@8x-E4$OmJ0Fs=-+C2eb9+vvC**zUkC53E11!N7(Cs|Ge2*mz)*
zfz<<R2A7V4k{gsNL8%dx#zEOddbprGM*c74*a{psa6AqPGJg~DG<A8m%5+OO=)=HK
zNI!&ZL4jieCk>n|a9!r8aAyK{HgFq(+YH<@gDEGNnhK`ogQ*Z{M1sl;(pv<TAgExb
zF@_m2pn*BwUEce00`^K!^@D1F`OHCeM^H^L8#$;NOc=(*U`zlGW*36lB$(B~>@b)e
z1>RKPRf0PHs)#KJnzf**gC+)_Fcgn*5_Da6gH}Iig+WUNtvF~UK}!cMgMqZ5m18_6
zXh%UCUB;!LQw};#(5Z0hb|*w9GJ1G}ZYk)N(d!y?-Jm-abf<%ECFoXz?rhL)1l=b3
zE`x4|cj{dobc>)nMh8~VJDmw5k%*4{M=mpVu-67tduu_@3wjGdZ!zdC1-)%SZ+o!O
z!@@CD4C$bkoqZO#@5ePeuG(-;UB#V$&<lf}!d*1(b#ecRd&QtP40=V-8{^(7=v@{p
zyTNh=H!Z<(AD7O!n8SqyhiCc2gI|{}yAW{s@Am^g#F-IiF`RdB$_V^n;I9RPQZOh7
z11A_%f<Z4B_}E>7pu{YCffEESzO5jb<w$SP4uVb)blD*htng&dB?^K#2$CSsK`;t}
zJP3*)uz}hb8O9)0grNgmfvO;if+!B6B#3knnIKAoXcR;?h|59jBB@CbS2>c5evWu0
zi2Wdrf&>H0PN3aDR{~uPbR*Cm_JU}iYtu!bZD2})DF?<4%v4}1fvE<j!Lexcl$d4w
z90f)PCJ#&z7|Q|bv=^jdkS0N@gVY4WRxtE~k&jefK~@Q}nILzAyc*<<Aa4eFC&+D3
z)Plkb3ZEm}7GrLmz;@9A6W|8R_5%z3YG4QW(18sDs{$JZHV*8Lz$Ss!fi;0m13L`t
zD6m;z^T68R(ox7PsEx4fhUH3F9wT2~=(xx+7do}j@j_=Ybe2MATj;byrxQ9|cJ?q~
zSr|G==xC(h3SBpJtD)Nn-6)(2!%8=-1jx*S+$-UX6V7<y%pj~*IS60vhSlY;8i&;+
ztm?3uhSgCxTf$8EaMlZFb?CX6l^S}}p;y7Qz0hl6?pau$4eQOYj{p5|z8^MK*m5}v
z)I#qyZYl7qz8ba$VJiq*FvVdj30pdB84M?ftvqaZ!*(xhW0Ytm?1UIA3OnfG#V|%S
z>^8z~3tih`Hwn8s?4k#x6!ywYqSkZ6-V`(3AaO#^3ww10D}}xLF?@jCf?alT^@6Y$
zqL(V{Md$zudm0@mVXp{#WArA3eK+h^!hSXEdttvG_UFR>eAsV<{W$C=VZXqA1unTl
zKfqN&=*Rfk#?NQyXW{B14z@5Xg<(0|*q~uJ6Na;4=!IdOVGCTVABJfd=Am*z)eDs$
zswh-CRHHEJhEW(sDvaVVO2SBokqM(=7&~F?hOrmM^)Q|b<E1d(7RKAdxWPCG^wq`7
zVH|{U6vjyy>o7K9oQ828#zh$0FzIbvE0vj8PP<6_!%Px-Hq>=?^67r4S3*4q^)S>$
zsBLJf?D;b*ytXn?m{!8H8m0kTovJX6!&HZ<2~!&mE8(yj4jbWcIUMS6Xv0w}93^4a
z3A5!e>xWq!W)O9l*)T7MxfAAIm?vRw!vfc9ZaA)n<3>0xLR$)L9nl;iMx<>gv|V&M
zhITo$eO%LpHVADP+9<SfXp_+D(3;Q=Lpus>7TP?#bfikIDpgdep-OFD1C|D=6sl69
zN?Mi1s&ttuyQ*AK<%TL7RURWrCGtxu$5D=}oEhcRl;bI<uADjL%rm0IX)C9roGyDk
zon_wpIxD=cbOy=^`0Y7LIT4cRDQ5=~<S9oh#~^8)a)!zoA?2KMa^;ql>ne9jxzkL>
z;5O7$O-*SvU02hgs+3iw&Y+b_M^%<prLQVRRYu5sq-H!d(@`^PjAyB~Rkfq4U3R-x
zmsPc|sw=#zt*)x-Kve@(4OLYk@dpxlAVUXoaHy(5LJd_Ns_ID1mes7IX6tIUt!6_t
z8>?BZYE#IbplS^!7xY}^O(}1h>7cwB<;^M&dQ84OQ+b7|JF4!gdPUW%s_v<J9kcsX
zeXQn6YR*-26*X7IoG7*Es>La_IHMLLwSAjvI;uINnl;t*RI{#{%c`kW(<sEEomo?@
zwrX`0hOk;V(}#$~QLPwpGpc1&J5cRNwJ{1?QXNNis_b3vtngBuqs58p7}d#Dx1_pd
z3}>pYtGZRyZL02>jPdEVRkx$MU3N%!`@FjE2C5sXuEH>w>L#kIRoAF)p}J#?6{+r}
z%*u@Pt{kdyF^<B-D!m3{g?b$hud$0C-5$702zb2q6uPigFGjC1I&;zMs(K^U%T%vW
zy)il>)pD0N6U*omIbHQjs$WKrlIpvvUs3%T)vv0)r}}jUBK5oMFYmAL&Z8fwzE*vs
z`i1JRt3GZ`aZlJ`Z@RyXTM^t*D1Qg8^_6dwzovX#kU47LszFl?0$c*(`b-5C6;xGF
z$L~FFV)(0><HU*+iVC#~hiE!)YgDRK&Rmj_tD>5UI=s<|l!_7+X%!h2r79XRwM^Vn
zaiHQ<B`uXiN|%*(ly()8hv}-)4Teymn_n;U4oMGq8q;u#l{QMJN*79xmA1;1lqoCY
zDB~(qQD#P&sxmcYJZ0+2%qg>=%%U<&%4}0+yD|-Bnhee|9c8*4MldV912cg#N|{KR
zSeZl_t&CBoP-b13u`-s~F;f@$%ox>`wmJBKo3=Dm!<HKM)W}!al*%e9n?+<Ll4z=Y
zO63)m&#Jtsikd1sRn%4Ct3s(lt75FyW3|4n#;zJy)VNUNv9dJ=i`lla9Spi+a6{Q;
z^n@w9g8M>+D;FCotCWqDjg{S@Y@)1I)+n1QJ5)APHdofF%gRWmjQqBd<8tH{sWaV5
z<Tj8!EvmMVv<T^hBBlgF>YoTHbt0y8F;Q(HYJ^d<7B#)7Sx2NGV*U_9hk!FggE6f*
z(t@_#2<hAyuH;4Sdem-2?G{I1+N%ihiP}197f~A_2-T?LMV)%oX-1tE!cY)Cf+&!v
zGeE2cVltvmiZBHx8SOTsZY%2cFk;O<o^BL%)2N$cBqLgOqvb}lyd1H^*PzQa>X)Lv
z6ZPGwUy1rv^nOPDR@7gP`u(Wyqu(%MZ=j9XZ{kIMGx9r;-{lU-`&itC7SW&)4SLbQ
z$L$JkIikU(OmP~xQBYxiBVKlQIg}LmxOR*Jh3mqIS9>}N(kK{4L5Az6D0CuT)>NXf
z8ilQB<1zw2_E8w{H<mxT5q>YDFo{AP@po>BUzaGnG*TsIcjlShjZ`I4GdQb8YBo|{
zr0S8Hi`0Ci79!P%R5Ma%MyiFg6~kC~e!;1MM?QPMc*Noe#Ic8CD~ePcZ5%?_cO#WX
zY8a^^QtOc#V@JUTjc_N&Pl9G)iK8fqA{|90icgPXCyL!Do{8dS6t@^G6L+Jy$ID?{
zDZn?5qd19T9mViZqd1S^aTMEVN6Iww$W)C4u?ezrR+twmX+%jgN?K9U<yag#@6mCK
zj=IFheI&0ta2mkMlQ>F}DA7@3qGS{$d6X1UvK}SlD6x@tBJD<cI?|O$&qTT!X)n?X
zkzS1S_DDAw$b+B*-HWsz={V9l(k9Yrr1MA@k+w{JZ;(P8nQPt1RG6*GG$PZAOppDA
z#^*J@!3KyUlSD>G#ze+O$iSWUdF_u=R2oNV5~VsyO*E`W!(KG>qft2;Inl_CMpMzK
z5{;_Ss27c($L&IhvsyIL(a1z28)Z(ExlvY$vTBsoqpTNYag^yOvr*oPEdJV&?I2tt
z!T_dSM#px9E>DYljxe%uWOZasWJmGo<+$X=rAAzK<8mb~H)6+)otfCF#f}#{_1Kw<
zorTzG$4-}Hp3E1l_?0>_Qsu_(>9Om^?o{knVs|EHX4OXQj^imOp6bL?VLYYdDH9`$
z=rls`=8+OHo-y%87CdA|LmDz96pO1Na(>0t7>T!#MJukZ#j{R4TaRax*mGmA5_{Fy
ztHqudd(GH8Gxmzu8#8lx-9>f@=6ytZusSrBxy+G5c+O=`P)>j!$MY_egL2~WVmv>N
z7hEO^Wrj$X=|Y(&(q+<6&iL};rFy*0Wdc!74f5h`^?19BNh9%gFWz2{n>7Tr$4$ho
zb{K@x>a%YQKQgT(ZfOLw#;p`#opIZZ+Z9AZ#_crjRO3#_UA}|=5!ivaqSnUlkD!VQ
zGnrv7La)hK-Cif|bvbn0>+>$Kr!c0Ap;inx#=S8{2jk@u6SO1$F-PVw<kpCnBMcG6
z{ZiadFiwEZbdEW%EXON(?7Oi)75kOgpGBgA*l)ysJN7%V-$kEb>__NvLw{N9XR*H?
z58QZAi3fdj6L83yH_13UO&s8IIf_+3Ryac;=*Guc0LLm0EgTZqv*T!lJqi0k99Qt=
z#c?l=10KzB62~J39b@P_@p*73HrAzBmt*a4FkG+1+K=@h*5g>)*toH&#HJCOW%e=}
z6Q@C(>Nqv=D2hi(oR{L<VRpB?9_P(CZ^wBY=g{joH*sFXxs3}iF6QH+5f{C<@Z(}N
zE|Pd$LDX|>+la-B?K1i=W88CFg|kbHhT9#nO=7EKYhoM=Hs4WpcewbQ-mxRyv17O+
zad#wUN7~|ued_NR_IDKi4x1z;Hz_revYV8r@kD{7jQ_{T7mqyeNXwo$ZsODu$4i_#
z1FoHR;&d3S#1!7h*6k-wkT_xDC}giroFs8H5=ke{C~@7yZ6t0panDSqrjn`oq~ax&
zFsUR-MJJVUGUG9Jn@Iza77%ey%;JZ9d`WdRsfI{_msEEo)g-BEq^L`(X);?*X6wo9
zLNeP)W`krlOlDPrRA@Cfsp-UX6K^W<rW3D{FtJf1@y<v*o7CN;K85VLNxhQPXOKCC
zS=Q=}q<#j6#p*#)50j>oG`*zRPMY{%C(R;hm6BGOiMNq#wS~kOh<rpq76w!?l7<dr
z^k;;;5MwYp;>$H6zmwJwvD!&1Pg;zEWybDKn~@Hk0Kt@qaYP_v!Z1X{LY0zk8DW1(
z*G;+=MCK*k*`!-fx(&v#GhPk7(Q(q%Nw+}Y6f=MKTxRQJv`~XV?g+p{To{90Fu;qE
zy?}R?J;mP;hG-zB0l^1DjKMP;o@2=vctc!#o6#11AA^EPUt_Rp<EI9r9T?TXxCH{u
zFvhUVK|=QFqqiMDMA+qZ;+w=DCVrOqYstV#1{L(lCIc@S_~`CT1_e4Ilc1agP7=6D
zP(gnmJJHc;5|)y%j6RMebd#`xJ_Yuy^S%_9Vz?nJGo87rFqa{3IdDgTy8>Lz_Y>tO
zN+&8!RFSALuG^UJmh+r&b%hvb1fL=%4i`GOg7Fh{FY)RDKk7I-@k^=kD}`U9BpN2s
zD2ehU!hzx9yq3frMwj52=YEf!6<;2LO;E-r+DWvV=t`nz6YV9sp6FJh+l)iOAb>WB
zE)u<-Xq%W)V#<ke65}RjDlwJBR1;H6jF*^tVj78QA~2TGD`thG2Sz6*O^i(tKZ|&l
zG)Qn2pXwwv$*{%nmZ6^v@x$IqMt(9%lB}I%L6Vn~+(~jb$ty`-P4Wh3Fy!4N_mkWt
zd6DGfB)3UXOA0S3>Pa!56n;{yChP0T*iFWjWL#jz10ul_+er{$XqOY)$52LMR}(u(
zY>=Qs!YW)r;CzM+kXW5qlh{#Wv&80!wfgiXp8BIpt}az{si8|_U3T%r5nZn0IU>3|
z#xonV<Kj^Y+No*B(@tGGbK05L&VqKfbEXA?&=DYv&_#smILjP)c2+o+f<AI|2QtGx
z?qhHXgH!1aegjNcZ;)$VI~kI{YiA9~+_htot{s`ywOi5d3=*Adx2fGTwL8{Rbv^Cr
zX}quL%CfHb`~oXlR}8b`&v<%fo1R(LGqIk@IpL$)VTcgMy*KiHvWFg>kxb|r>*@|&
zO>|Ww@ujW~b=B(G5|iuCB9Ek}XX|>lrDtRA9kUuq7`0bMW;f((({=nIA5%jwPV2=P
zy?vW*U|t-iy47^k)6KeWF6gGOn_4#wrhDl#YnXJTTOHk6#;hL9x54}w%p1|IA!bTw
z&MQDHGD6i6wY0)(w@#wFEd;RY?vPI$!m~;kzvwdAIRgz5K8RG2HQn=cufY&(hRQJp
z4iRkZVP*IiuKsp#M`oxKVv<k~;(idIbD3W5>E%c-=ZLgGw1n<s02>4Cc*mGD2DmV0
ztTDKYL0L|b!Mp*EZepAgqlp;v!@VXhDly!J!KzIAYkJ^f6o>go2X%&7VPtHe^}y&s
zp#v9V0O;07$34TZ*mI5U<6H*?`WZJy&&pc0w8BLsLdchK3xS&f-oo<27eAl8K4Ugh
zUY+1d0zdfyFJ|LJ#|3`#^o|{RN1}J&xN`A>fs?pS3hYc;yVx7Fp3%C2FM#uVbcX?7
zY&WijM_T7vTRyu9&wIh65{!%d@yN5TkyIN=k&!voc-qvpnbT%on+B%`Ve$kb-w}0-
zs4=t5u|5;>ni?kwqqWJj$+ao8u^Nwx!d#-Xt<w(U%hEunp-y9+COXx6*x?A_DAJ=u
z=ZXzkb9GVDMOha!8u<|mPZxDv%;}=T@Lq(c6)U>%b+O7WbKKM8`n`xXK4@VWbiW?!
z^>saV^|+zOV{I$i*0pV5o(JOb5gp99HoL5CU)vRoOrp~ry^QD;z)b+ITySo~meAH{
zJJfchZKiE*O0FqYOsQc?V^elbc^VHKGUW!hR(Xsk%HT;bcyNnxT*Jq);HfG#<9NoY
z8)u$#r<^wD0q`jxh^JdNPM-sz&cHZ<aYFV(Y&<Z-I6HVk<-;oQ6be3S!Z;)2WO)38
zam&VajXP!Big9O*TQ%;iaT~^MnyD!>RWnnbnF@I(o<{6*jS~(K0gb+miZPW8S<8{8
z+{}1prfaHI&f==JIU}J8aoJS+9FMK8n(BbPt<{+4awg0*h6!*-NMeg*wPv<!W<xWJ
z(>&6fR!z+_wYu?2#w#1oMaD+s)r{vE56U^?EgG+3JZtJDQ!gVYovBw$y<zHOQ@3WW
zWai39Wo729nJ=07vYB_yyfq81S*Vx=YZhIzSTT#%EV+h>8?4#pnr#)c&6@45*<LZ*
zEhZ0}CjM%s>6vENG?&>)(~SA>F+8mZ5BF%f%mj~gznr@1nO5Dj=1gnev>K+><P5e}
zn-elxUDN8D)_}t-oW+Np9!!?QK8|T@nQ7&wwPsp{X^k<n%d}gVhGg1_!F-|4is=NV
z6PiwhnH#1v;(WMnhjEb%k;lDbi4Pd*xrUL@h;MFiwp_2xDImSB>Ge6P!@yQ_DZv>L
zrTD?b@q}<mMnfXB(ew(051Qo)N60b$vmBY_#4M+#kKa{1V+~Ja!UIfPvoghm_Z#s&
zX2mlr4YP8FS!r@+9;0e7x{e-n+{niKHD-WIYE}xfGDegMAAZ6(4}@zV@B)FHh^6Qo
zf55RO1~j0f3%9?xipL?2!T6zBjm)6MNY4T8iCq&^OfYML8b%@+`^lj==yQykFb<SM
z5g0az4f|5jJBjW*+#cguE6BUt=JXeY=SO`G1xEpTJ=mAF(aV6FeB5B;h8DM)CZ6WZ
zLIjf{?uIuxxRb%XN`jjTT*~vAEs4t<|H-ULYMg+`D?ePW1-w(|RSSG@xqw5uWONz7
ztVX+rzm8R-XN_*)2Z=GNJcGwZ8>2^N<5Xhw7-s=vTux`?w}Q_f@V=i*&Iyibm$3<{
zGHJ}Kn^c=&i&3sa1S}xd74H$AJYcf6$+{*(p_0L)v~oOK#x;0Y0di62P0UW<fL<P(
zyfC>nMadLpQ`Ai1nWAorIa4%D(dMk6!Z*dLDFz(<LoZ_y^Zbgw#`SeGcFnkA#)TP=
zjrEMJ8#`z0ys-<$E*iUJ>^5V!V=kt#ZA>jPm?U8lii;q}KEjs}RblL^!I(ToATi{E
zE>iS};5N?gz$F__;Rc(*rp6A99T}S$oD!`~OKw^YQpZhQ{LQA7Al-O^0@9SHvksrP
z;B8OqF0wtQ_1Uyu<J9nGnx5I@xMeHh_G?ce%Rc9ew_9mDWEW{W#>_qbR<zeK2@SKM
z(rz#9#%Zsd_MEimroBqqtERn1+H0o0`>{)vQOk&`bkpSuLR!=1X1ctbE*EKkg?%%9
zd__*`cTzt<Xb$^d{e)i{=A?3H9P>gDj~wtvDKKg1q@kOJl{BoUVIvKjY1n3{G6%`f
zaf@-RG!4gTh|W_?%<879mnuJ119V}h?3hL`Gj<2dM;+pkXt9&VZW>pZN<40)8{G$K
z+(qDX8uwG)jmK%6q;ZkPHcgx~sieHeYjM6;(n}K`m;Pyz@OLt?eEN`fQ(Z}Qle1}c
zJJr2ZuOJ+TW4{~M2lzR~KA38JRd^a3enQ;TRB#4QO(Qi;Ou)q)N6vjQy_6>rA0x&%
z8gWyOBP2CCH6}G_YVy<+__EXWIK|{hOhmKYVaXkqD#LPRIFlgjJ*Unv8NC^LrD451
ztXGhTb6B4n&f(!m7=^|~y%~1mVF#D?_u=E<IM2i#_A0|(XV~iwd&|S#DnkF!3(kJB
zUd%gyo*wp6#7Pf(mkyVcVc!|{E5m+OW(@lsOx9tXxZg+E6S~%he$1N&U*l*S2JSGZ
za8@zn3G6U(hLJmrD#NHVjJm_fNB`z9igAZAjPx+FxGfweDJ}syqYk^Zog3QaQOO;Z
z8l%#9RCY(@%Bb8JmB)Cr+sJV_N7?C)oN(mCBS()MGjg(#laE|?<TgfA^PG@Z$wo8z
zsM;D;lhJH2npLA&J@Q;+Mjm;UkvAUIT|SKm4<V_KM{|`?vo>n^qgKG1o|Yc9%&3)*
z+Ucm{WB%KyS043Rqh5Q|>y3IoqW?!dJzD9FR{Rly7yT|`;4sdQF$CT)G0Mt}0(TTt
zIAJ>IjRGIT^BY}^82ocFhBs1;k!p{W9YtmIpN?YOsJo-2GD_;4V4d_viH{q_k#<Jf
z9qGzQS4X-r(#6R5qfvE)C994yGa9?2ab;xjhY8v35#}S>E+X_t*nqY_vMVF&kL+M%
zgAoQ`twPu4$i^d^jI176GqQPBYGkExRxV{_7f)@>%8jf%#`E*=(7ViWIlbFyb1oyF
zm&Rw3v1ioLc(7LH<e590O-*G}^=vB4rgS!yWz(Z<I_J{`E48fRWtI7?l4mm+XNOnY
zc*Fvpqrj;H)#a?(&#Eg~)z7M{SvAP2A-h_uaaK*Ts?PWTfMGV<<Y>&S&Spnh%|$MF
zq;Y3{?wXg?>RD|rtIcP%g{-!i)t0i_wyd^2tF^LPJF9iFS}&_DXEmF7E}yo<1lGt^
zjojW|=G8NAF7xIyZz1y<8IuTO@_IM(MwvIxJe$>BKAnkabCDYt32kdx-OK9pS$$hp
zZ!m>My_ePFtZuV8H=CP64$y3_p3Ti?b4%G=Bb#exbKPvNm(49_^DduhGw)^djcjR|
zsl=IBq@FeBvgSh8+?F-BW4I}6YUJi%mho1YwNp$&&N@?Br^2V5biAxn&pIv6wCE&R
zx0UreS+AS*f~=>r<#x8*%a)fJaJL*~eN4qbyrar|o%tpUrm~=t1-&fr5gn3+E}yOw
zHnK3vR4r3prs|oR;{(uQ7muRiQ+MLoEcUXvp2g75XK^cw+nn1G57-4BkF#VdODb8?
z!#z2l!J>P((ZUTzrt{2{GE>GSa%Nmy=4QsrOg%GmnVHYbLS_~-vy_=_nc1G14kwEk
zKQlpQ;>;xY8N&mAvh+-53mA>Fteh3Ktnjj;la1YMJe7^B*|?F7i_BIs>t(i+*=}Z+
zGwWwIKo9}?<}({-R%g~^Hp^_Dm)yKm$xDsAG|tOzUasWjMqVD{*^s&8;yH`C<K<30
zcjj_uK6e&!XEAq{a%Wr42MYFc$IqPs0~4GmcQmgQoGf>W+;#b2B6l`-8@XHHfn@nq
zJ)a8mDV<N{c#>8=y_Q#;yfU9xMtNn8sSY+CE|OQ5*>Qpk4P;)yq(%%a&gy)2l+R{)
z&CNF+b&%H@dClftIrm&7aOYExm|(q;d*j@*dEMo+!kEn&DVrO4eVo^AKIi6hQ#ljS
zHu8BlpKs*NQr@iP&Fy(}nW09_5M!8mQ}Y6+Im(-Pj!Z600utn{QO-06$WsvI9XIbx
z<(&$jrPJ~9PCf7R7^c+i<y}ATb@N_;Sx>p&%YBsxQ+ZIygC0VT^3df&nZnsTY~-qz
zD=$~|T+K1H1IOcgxk~dW&GD?WjUkFW_VT!%$8&i+&)g4jn=^!BKaU4_9ON-N$fok7
zk|(`9@zGg^t{*;mPrJFUpwlGR&0KeK?dN(2ZfkR0<h(7!ZB;KfD><)s(Ql2b%yFK&
zd_q)F%L^|r>Uq(~3qLPb^Rb(ctNFOdZ6&uJGEX42KyEvjV47p<lEoAg%rjWYt&b2b
z1f=9v<rw*~=m_6|F0R~aTpQ*#%Wb|^cGuj-+SD3zS5(?-l@T*o%&N6ny@vezb$5-)
zyT@y-_FBuV^_;byyVjdp>rJoqDr>#QTCa(87i+!lS}$4a>9yW?t!LL(T5CaNEwF2m
zT8pB!cxnyLHA*%fZ)Jit6R(+MEe+SKUzA!!sa=%1MQOPx^@|cTgQ64`rKl*yMJeIE
zJFa0%X;B&#rK~9BMY&v*TSd8DlslYFTVCdL3Pj46{h~Z5%0W>Mi?S-pQBjVIa>84G
zTmqNVqC70hqXNfe8I`OR&P?Id3dbv)g~DkUPN#4pJSwtq@?yp*W@^QZSIpFlnYm(S
zzL*J$89ZNMx~Nr(TD9Oa7;t<euljVsCkj;gTm__NXIA~W!kaI=1!f=gkb(|5$&maV
zDW0c_dWFwkK>GH2y{OL@^@XCoRMfX2`*Sf@DduWS>yBi*^Oa)0R?N>A^9#lNVllr|
z%x^2^k?*onEYymHrD9=wf%KP)m1410EG`y{<6@~&EY*sog<@%2u{18WRf=u3Vtb`%
z;BTgActvBbXv`OlrJ@mX45X11$Y#}QBN<20?iKBz=-@9ZI!Vz<i|#BFmUR81n-;yG
z=naeIfK%L7qQajl{OQ846#fk7F8e-WofuPqn+e1{uTB@MQNgI?>0&Tb40;9PZjthY
zL76@ipD+*&|3rnFDO3w#C51{cV2{C9=0xFWmS4nC5ks^XGboT_AXzDrsL(CWt79Kf
zikoNLZ5AdhOoE%7VpJ(cv&E=hj8=-QQe?A5<`r4J$mWWyQ)F?GnPTI#TjXAmw~D+|
zY#cd7F;f(^qVS5MUK9&Ou~ZacQ6xp7iy|wyjEk|0hp-jnMll}aY0$`OwBCu<yRG$Z
zwBGmEgWh@=t*a8BHmvIF>az7XS&#Geq_>_#>pEIDB|e4NMC&QKk?=U;*7|yPeZ9ZF
z9->nj9ZR^`T3=7cr#Hv$z4<UncWUfTkKM}Hof*5c%sPhkyX)hPr%R5frue)LWcugx
zAj9#L9#1=bd<PQ$PisEnxKd@p=1OZ^i7>Rq9_tG9xS1>Xd=EbFtvQ})F=Blt8qer)
zwKcAG#?|h)x;(D-$JG@^vR8v~H6B-!aTN|D{%YY&Gwbk?0(`m;dcvG>BN?Aj8=ukQ
zvy}0)kTY>4;3?~PuJR0@9I~;yGimh3UW+{yJ1BO_G55JPqvN@YWMh7+${5?2%VoCY
z+ET-o8`hb!PTxA(x(!<~w%V~(ZEMzgCF_-~H;rdwSkJTGiuGda?Xcd+dShEJ*?QU5
zr)|AznS6i2)*H4Cy|r^CJ6E=Ibvs|O^JTkGvI}LqSh9<Fw|1#ym&$f&#%?Ry?Pc3+
z+LmWqb=#V=t$8G|;-q}$lR}6qqGOmQ32Bdb@7qdkYiN-hs6A`jo^97{d(O7!ZM(%e
zolI5ZGb9!x?YQsZ=^1H$kRAooJ(-Avp|Ti($4CNhiEu5zlefzxK*$8cWD1z_<#T)<
z;|$%D$pAR986i14?8bauOnkMyA?C6&X%I5UVCLw?>_-OPBD4@g`W&5bZNFmsv$o%~
z{T8Q|A|{v9*kDRA56!NW?TTwxs&=JeIlZdOxu=*ywBj=yU?sLIJM2ngSFH6NrXtvw
zg~DOdjcFKmHL|O*T}_#yV9-NYza1z$z#wXivsVM30R`*>aBWzzVT<!wF&_faj)*c~
zIA&^9$*QtdEzWaQ5&KOQl-4qc2iXn~bAu=uhP*K5h+!WXT^ZWg<r8UQk2&(=IUCR0
zc)`YtHeO=pq>bTl&V0he3&a|43_^22hGWTv-I3a)Vv||RVMUA`Vn`3O=zNTaG2aq$
zmare6z4M$3fCy#8V<N-}0TCO0;_SW9>BjG8#hNDbGqF#NJztENV8>9(`x5-R@;)5>
zzMdV{?Qo81oR9~Fcceb<811NHM=j3c9dT|J?xQ$NG_p1;*{p0c*Je{TtJrK7HvqVx
zw^_qx9h+I3muz0Pxoh(&n^$Z;i;FE>SJ}K_b7YD_o)zEbW1Cxxs{>oY6V9!5tZiD`
zv9^mZ5)szchKSO#mP1llB`Q5!Y9B6j4wt%zOZ~&8mBS_fa49@oQin^?;Zl6KwBv9|
zA1;~0<;vl5`*68)xZFKl_79hX!{zvJSsyOvhiBE{TH|o-vcukGhwGOe*1f~)>e5bh
zX}5RjvVZ9)yUZ<JRw+L$>D)@4vaviG??@|@9^7@{;QG$>?!L1xx^UOtLkHLUyAQ2g
zbk=&Z@2q_n>^`)9`QKmu*2`as*Rw8v#pQ3~W2EkJ&hCR3+++8JJI~v-Ufs8U*WL>+
zwp3kF@6~vJ&^KPY^rZ)V_Ikg6GyJ`~583@Y5A3|~;Cixa=bnu>YtBEg>)`qO_MCID
z`k;gR_8zHazc}{|C$t%7@7%NdtOHv%flWKn&DgVZ?>QIlJaEC*8{>pG<BD(T+T0os
z*?sPA*;%foX6W4)uA?Im<NYnO4#6I67dbKe#{X%?(Y~9L(2QgGa!z0~@Le3^S>r?<
zup3_rnsJPW>q%>dJ$Lu6J?GSqc*LBbFJ=FM-Fts~YCqz-C(i6ArWpqh9oTtj*Lj!N
zb9Nrud2*Yf_nm$At^@1AKAs~E?mV=6-`@4J_v}1)(DwK2JFB|?l8F!hfBrpdaP~{@
zc<J%@g_UN6`z9K2g>aN6uub2p-|e*9oZx2I1NhzFx-DOrKJd-Eo#<v9+<oEJtvKZa
zzxcx8n>_6Je<>Gkd^h&so_)BAyW;z@{auH6DXb6eyKv{(=UX+g?>^;ot1o=kpB=|$
z40j*o&Us+ho_#ydIm$Np-#zt_d%pLEyC2Ul<+sP`F}Gl&EzetS-1)TQ(u@mt?%sR#
z*8RU5<CL>LyX~?&9G7NbKfGwq&O`eSSX_PL`uxDIvmsnkJ@n8+6YD#|zpwnk+mF*F
z@n)y5czHeEdEn6Qv-j*;-o5A0t^-HfT&2}Fp4evKoVtgXg7|UUq8%r^8T-$*XYbp4
z@X$`&e@z^rSNU>IY%`AeH|1nBW8>HENPEQ<o8@FQ18200e)p@pd2?>^(i6Fhp0n%X
z-5Wpo_;uUA@4%tW+kg#n+?ydk^eek|o?{IT4%^wdi`{6<5nFM>nsFA7)7Jj|2lnw-
zOdr~{|De^o9(<&RoT#mF<!0dIarTi~u(6L`^o!%XZ^)N&*4}-1#MqXro;-D&sAgPo
z#yIASz?0F8W4H)BDa|<gMc@f*#=Z;pUvy}fZSC2A{!V-G+)?(D6V;3h_U%1qAAi&i
z9oV;L&yj!OWj{Sx%|Q2Mt$N`R@0m_iGp=-H-rc$P>|GcqIdYfeNo>a4b)y;c*NtW@
zTsNArcpYhm^^Vw(lemlGNq^UkW*qsDJ(*kMh}+_nQ*Qr_Hyz*GCAr?X(&=lXM`6ER
zKCtgX+ue2UE}YD+(xhvv8F<k9<T>?PYsQuLlatwuVAr{auCxvQ_cHKgHY3@6-uXvp
z$4O}hM+0~7++*W?2d}zs$_6<p&A8&|+31h62?pPAz~E7Xli$xtX-2$r@6J7kE|Fsu
zqoe0sGT8=fk!!0Nc6sN)iT&hZyWV`_Phat_$*r+`l`X;D<63A&aNTG|a&0wZ$3?r(
zzQ7*D82-gqK8MTUdXm1BEBBc%@5E(#y8lYs<fQByS8m2;UH8{!Gc4jMWSgAKW(>H8
zEblw;kevt4vHYFG+j&>C<63LR#_!zk+Ho?Qp)X{I*y=?W;zI0-ZSuR_^rSSy9N4{o
z&n~VXt4BfxJRsBCe(ptk_MGfy?EAmX_+1lD+R5A;*rXX(I+LB0FXi$#BTDPlAA98G
zZ~EBd>YMiaoTz5(-+AbKyB`q?dw1_WZ__pZ?nKUio>+Oy@%^Efk;f}KB(D&T-ii~}
zi~~FO9^830hTZn=vIlpaw-HshMRQKn*4V5W_>J8d6~s90Ra<nznsN1pgHKd5{`mZz
zd(YDs?b?g@p4~^^Pmg;u_Ut=v{dc1|S2{&q>Cm`}a6+1~@1jHKB0jc(;49j5LYjd=
z&tr|Ik*#t<nz0dZw$YGd8W~4ZPHZ#kUTv|q;4OM{UTw?o=;SnGZlUH~>&>Wpb#HFr
z+HXd^Hiu58t@e`>|D|}_y@mQB`Vp=J&DdUFy7rr~IKMQ19r#if=eI4)UkA3vLhZV+
zZ!9j%)vp7)=u!=*(QE%pS*p#~t^@nVQf*sp?mDnF=H|V{Yrh%W=NAw}ajl<4x6d!t
zw*H~NXxCeh@9h$=MSpkM!|M%id%ZTlP~UprI7!V|^y*uFDJQ8JOGi0UPJA=arBcJC
z;I;jwuo+(cC@1rivo+>w3)g|I;ni^FxR!TOd@0`i(zSl1pc(c0!oszEz_J;0-u%|h
zc;XLF^jUPH8QbTNa@lh-nz2yB{nD1Z=xgrywiEoDf~~PoTdHlnHBM49@Edy_*czT!
zyG}G?X>RMQuaono)aR}fTcf^klx~)jvo#hLy{&&!PJA=wY98)j>s#NaoRns4uWdWZ
z0sD@Zp6JsTw#IgE3DKfk-5{LsW-QegYKsf=-qv5r32(->+LE_atIuz{cDKeA%~<lT
z1I^f0o1eQ5d@0*-M}J+|8uj_3bitjJU36Q0vA*?B+evE1(ot>@PEs?DcAs)`nlV?K
zzb+gp^WNOnm&7M$Ys}9brO*5%HDmrL0~sf&8At1MI!Vpgws5VVMYnnL3yX7G-@~4S
zt+CBpn#U;D$^D_<zx&wdi~sKk`ElPG*KFqaac{;&hj#DTedv;_259{62jBF+<JSx$
zS(9vf4H|O%W{#hA(V;{8_S!?c5F>oV8C#of3Mh|XGk&k>{Ku~u`w`1-&)Icw_j!AF
z9pJ>eb9YPl<B>MOg#JV|0|N<Lo+u`oz<-ZlGY<VWvzI+|5w{1Dm|c9-Nrt~|&66MX
zj^ot#b@{U|f5pch{;@}V>`xJS_lC>gc=;>tvGFg`rG4xX5ST5kdd|MHANt!>Px2lw
zId09E2>kP|?y>UQW;|tly^YrWp|tV(;BQ}lfdBcTjk;6uqW4XOHv_ql)Zd%}mPt~7
ziw*6SN&PKFPqO}7i+n7k|7m!!p4;NZcHU9sJAqtC+V3p9i}0=>mr2s^Zg`P*7yc2*
zg{1x-!qbH%knOk^UR)+g|9j&_dl|1A;5CI8mt!HzPj7f{PS!gk_DQl_6)$pDSOd9z
z=JDb}vOP-_u#oiMMghwtso#zl*LPpMSkL`LZh>4#*4M@BMtH5_ML$V7xIxMZUR+4}
zr@{>6dUD~KumHK<vB(bqxsWWsQ+N)@g{1$v6tIvt#EU5kS^plqxR7k;0SZ`1+7D8|
zLbBXL@#1<8<3;<WcyS@Qo`>PZ&-Dnrc#QldUi5n`UbH`6_ymv(N&8>p#d@EN7Z;NH
zrwE@4av|yW3<_8#$@-tQp}m5X{}wOu?}UF3a{2p>_bbTyo{ty#BD}cXf5407ULx{8
zf?PHu{r_phdIh;1Un%m{A?x`UDYq5r_bR-|R|{Vwe68?x!q<abNcz7)_(qTmN&TCI
zZw9%L)W1deR*=giX@A>>_L`C9-;Njg4!pRKl;0_Q7s!RA{@ud&fLyK)S^m8nrLG{q
z-}j45(*Cc)4~U+m{)55~fh_kCkv}TD9OOdM|6{_B3;zvdxlfAxDUiq0=SBX4@QWZ9
zlJ$Iv0+vZq|K$zs6{P$XyvTpYi_2D|-&Z%ht{~;FiF|d)<NLc(ZY#3>@8Ly$U;HP@
z?eJr~xPAUh?326!bU($5<$ospIml&_wEtp5du(LAzY>3v<$o<Q$@Xk0uOV6A_4sxK
z(sX^XA5XH%8{m!n1K|xRVEKK>DsF@~ZkIn4o(fWbQ@&k=^t-v(Zvk?9-&*9`i2o$1
zI!)}i72XbHJ$DfKj>0<$?<~9v$YqjL-Bs*&6W(3?NdEgrB2SX4dx-sXv2RA2N@6GZ
z`R*<9X5{B6!_MEwX^^fJ@t-7BGos&$bgANvoW&dK_b6P2RMqju_AQ9t63BMkm&T)z
zE@z1U{l$MP(xrtrvW+*cw+FI)eW4HH@03-%u^kGe-wrB;lnrE-8-g^AM9x9l*M%0u
z-zg6i?iBqwBJUEO3v$_vT+jKilMfQVNm6xz*e?Vr?-hAJ1uR>UE)NzzlK&pS8`pP8
zc(LdoLL(NE_De)2DIW%@x>R_X@L^P9A-P>20rLF%Xwj1_{}_=;{`;4rf1JpVrxFXv
zfB#D4&B*WJiLjGT5&x%(KS}*FK(6;$AlvgCv6GZv0MhRtK$>2RH@5$!Aj`c>^e-2_
zLikFM<zFlE>x8cdxsd$#4I-2L_l+WxtpCj-9}DUF7QB&fm2xEOdArCY|9ywZB>#P<
z@Li%O`8nSw`u7X}RqUIQrVohU2gQ%%_WKCP`aUl9e-ruBAlvs@;pavFWsnQW@?RDE
z*Fl=T!MCH3F5eWtV<BC?CH^Ga^&OE({`(&yPm-$dik{@Z-xGcxWIaEi@(85qhvN5R
zkn&IXb_8-e{uFlZU;hJg`}_vvdaj2zUgzH!r2bTp`kUd6Ro<L}kpJFNWRm~h8gKNw
zjqo(^#*ps}vfg`${&Zmp<U+FCJ%#rIx&E@)CrOnnepA9}VMRCta+xGmvv?yt;T*{I
z&I=c4#4<_R7ezlws<z>c?YytpHzUiRA@)h$1o|HIv@Z+$AYFaF30Z$2GRf_jfGnp)
zKS`=mu@6DYnaH_t4P?Dz5Pzpwz8!&F&jZE28EHC8?3<D0&xW1rI~SyUp7>9a`{f1D
z^Yc9zq<oS1ku3jEkxA~qmx=yQLDu_l(LYM$M}zn~<uSs?ivDrJ$BX_6BLB7Ui6Gba
z6p#x^`RT%Efi(TC$j=e^xggu|Jds}@@(YD80@?0=68k@k{$;|K3tu68C5XRMUd1;d
z+woelzfSmi;Twc+1o3yun?-($@U2v0A=%EifmFRi_%4w3zn97*kf!&G{|AI068~c%
z*ZW~9_faWFQh&L~B>(-m$Xk&vpAf%EQuQgkvEEOMouvF3;b%pEEM&RQ!JkFGAmvE@
z`z7I5L{IYHuZlcL*7G&dPm-!{h@Pb1H$i?M-x2vg#C}c4dcKeH-2Z+o{HfIYGpU#4
zdVenbh47ak%l{hWLQ=jS->yQs+yHvk^9RBkiXTb+jYKASyx$C@>Xvxpaeo>GA^mSF
zGD-P%;&&&J?=1F7vK@DYp1ix*w<7)SA%2tO`c4=9B&jb!&-UI&I0aHaE&h{a{WGE`
zDbI?$73t@}kM-8Yf09%!;EmsFL;UUwQhz4jjzVt#F8sKjWsvLXgZx|rDL+Zt1JRR|
zL!lBq$$xj?jrD4fen#{o8nH~0_6&NicU}C)Aj?~k9|&?e5#)N$k@`utXP59?;dvn2
zvm4|>^4~ppqyL4%y~2GoVj)@X!6I))mOmhNlKzK4>Ms&r4Dx#FGLXwPA^W%fT*{NI
z|4|_KgU5-Ur2g^3zY;x3zo&>iN%}uk^d#k{37-z~^E^}RB>(+eywUI3qMs!9=jVx@
zq~G&J-iq{lf%r|5`WHgacKn0bN$OuL`hOJppTxcy>Hku(la&8iWRm~BLS&NmSBn1C
zq9@so*9u=R`fEbg{|1z26>r8H<+n(EB-isc(Z5~vlcefhqTh_H@7-c2DZfYfUg7)1
zz7<*S{o?<x;(txZ&+|bkKS}C8Ec&ZMw(BF(u8&H2lKbB$K-T*y;b%ble^%trgFGJo
z9pthZS^jIV)8(6ZWBG3hzYX%d^F5Kj4|16#{r^+!KM?*9WW7HY`6nX(xA3Q;{~5@I
zr2j8O|4Y$tMYiiVQto;bu$)Npb6y|oA#Wh-+lu@=HxxgT@{K`$v{OOWb5pUCwBKCx
zw-EiYkoDYB%H2wMYbj5%-qS>XThVVu`rTgq?jXFQ_#Y3lop+LY?<~BF)KAiWSCJ=4
z{oO=AN$T$oJ-6rSVxJ`S_Y(ai`8n?cJ<FG+{FL~Sv{ytXS<fs;|C-PfJ4t<AI47JJ
zE(jNeOCXm?vivr&Zx=R%_Z9z1(*F$6UmdbsQ_7K)&lKKIcz>~zw6{c_B=v2vcf`IK
z>DLweB)L8NQhr6~gWT=|@h9o0L?&5ZBr-|=9imT!TI`c#IV1YbNP8;wNm4%){UoU$
zK~H94Cn@J3+f|63q<&rGtw@&#ivLdGS;Di0=YU*D*1Jo1uJAmuZ$|o`FZPp3u6MVr
z_gKj7cY*jHPjY|SEA>v2?b`=ExnJxjf~^0+Qa?%gfbgL3knkem#UK}w{tpp8RCtN-
zk3lZSLe_H_{@l)&3I7D-_IZTJe<pmC@Gn4?dyL4B6+T}01dt0!zrPlFlGHy@^w)$e
z|0JpZ$spVNG?AY!d<MwR^(>M9M&xG;pCkIe7d}_`JmK>}E}N0-e}ULZ$}bd|r2Hb0
ze;=}*e~|iKEPRRZAEo}QL)Q0CQtntt|Chp_+wo;mZZp#F<*<{l5Wmex`zyuoUxcp`
z|4GvC)uKNhWPPuZ`bf&J6?u}>zfSD07rp`H{`F?@pCtX?B7ScbJ4yZ9L?$V}U1XB-
zJ4D`$w7*mA$3ptQOZ-X7?-qG8(*7Rtd#~_)!uLzL--p~^J|OjdQ1~I?hoyd!|9%W_
z+`gX>J<0ZcQurz1r^Qav{uz-sBki9R`>~MzpObn&FZ_b=i&D?kA?y2+l$#{=UzU2m
zBKE68mj8Dt|5f4FgkP8XNcw$4_)XDIlJ;+j{o7(EssE1fKZM^E`y}c2J?Qy8|EJhV
z>VF_IN%@B&ll1>F$nm6Kiv52;e*eD_ULQBJ?Dshpr2ox$b1P(hHy8aaL6*A>Nd0Nz
zM^e76@OHx63-183-aCO@Nc!Jdco&fM-W|l>Dfi@?kbd`qOx{~~ACUDqAj?gQz9RCh
z$Tg75W@LSy_|?Uaq<&6hlJY#r^(~5?q<%@{Nm9Q}?AwJ6@gr%!ukZ|EQ|z0O{%69@
z&)E{cNwQrX(UbJ=io6+V?}?qHye#sukn39!|4CB6D)kM-PO|<$<f}uL3#HtNAnRAs
zzDO9$`Zgo|cZgpiev_nKi++;S8|b-xhGM@uWcd-wkr{|X_LOz0hopWi@>ZnZ1H|8g
ztmkZz&k^ndxlEGf&K3P;r2RaxZ${eB7yBfs-wi$aAhB;o+Ak3MB&pvcdXn;m!o9+M
zAh-L2MLq!Xb6*Vdc)A2+fBBz^{ox?}A1V636h0Be-ziVS8_PcnWcg=<^nZ@<xx(jz
zY|jgXFBHB=>@NnnkSza?!j}qP2D1FiK`tcyUMc#25xz?FuMzpRAlvl@(Z5ObZx;RA
zME?$u%OttJcZ%P8K+5k0`FTG8a{V6?`$xrox!6A;_D_lZvts|8@bkhifUM_BB7Yg=
zLUMgy5&Ks`uJ@ZFe_QO|5&j3rg=D$!3cn}%Nz(p((UV;N55)h6!XJtLC&K>*xsYtf
z&q3Dn3(;>yZvS72-y~W7*HX`KgzOw<d#(@Ceglx@Z^TaHtC0Geh<=hRe^b$utmkIJ
zn~R=g`CExRNtVBj=qJf~P7^&z`L-gHly4{UB&ol>*zX{`qwr4RzZF^j&hTTq?keT(
zCgn)l?=Ji!ke{n0@;yN=B>nCsytnAD4q5&_QoamwKbV$!wj#?_#P8~m_1B~v$$C7I
zNy>GQpLap@B+p-GfGmH1klU>-^0LS)AQzJUk;o+LjYTF|zZQ8ja@WqpPO^Lsa=nFc
z4ASo`kPFG{&kIB*Y2OdB+yRjF90a-ki$(uX(O&{`AzAKW!bggpWIcZlayvXm^qY}>
zj}`kQsee55Y{y?oJx>z($sqntc_!b4{P#I{V|$+mviu9gPO=>@23g-rME+-x+yB)f
zzaC_{H-fC^Eg=2hD)x7RTuAEQC49H&HzWPt13O(lh&Q(5qvAhF`dtn^*ZXfE<xhaT
z5BP$}Ule``WIMkC(*JA1Z-QJ%>c0bWecuOJ&wq=3lC0;a(DQrzACU5|L9YKdLT)6=
z*W<<#!gR`wA#**aiv6Y{-$Hn6(ccE-Lb9IIgtrymPI!Ca9Y8K5{qHEeljyf1Kj-P<
zUjo@qN6L|us~|t`9LW0TL4J-Uke`3M$Y+4`Yk~A@3p*hFdLl1_+|H{Y7m~-97P}Gs
zB-xHN(UYvd5SgUB4pMbC-nhP9AV1%QAnkiWw(lUw_FpW#1f>6Aksk))@035s8_PY4
zf{=cHAu`F&@i>t6J`tq<lf+JPJx>)rL-;phC+YvUAnW}*(UUCqT#-r2FA$mJ`d%XX
zmx_Lp++P0zJ?nk7*h$K-1-ahW312VzH;ViwkPAuww}}3&!ncW?WIb;ezC-v<;k$(I
z23gO0MSh>~{i6SX$R8AbNc0~O`J*5glIy!%WRmj7gdYdFo==GUNs!A{r2nVk$L;$$
z@h4gD=SBY|k-sc<lJ>8Ne09k6eO1bBMb`f{@grI88zA-H6#3g?C#nAqxN-gx`9H-@
z(*6VC4~0JxJ4ySGg+CGgm+-%Z{}1G{8Cm|PVkas8OysLWmixJsn<VwW5d9>n|E1_j
zZl_;~JW2Zf8hY|KAdjCv;K@PAayNp^?RTo^w<7DgsrXHj`kO&d-dyUtwaB*-o(6K6
zB-?XG(NB`~+zEQ_pLYe>zI%utN&D%-lJK5lpCrrQNA#0q`LdLA#7<K0icC_T0=b<k
zAlp$B`y}b-LC?>#DE`}o4e=-GcVCcx_ZR(Uq+d(?+u}!3-w}C|)OSTcN$Pu|C)s`<
z<mVZHv<G4*DThKOjKqE{WcgUi?T~Vlq&*Qm$#xj=Pes2OS#BtPBak@;&Ieg!zwn_T
z4}m`cxqbf(<nQa_MSha-=^)qpOpyM64{{;7p63dmCwh|pFA)6;ML$WFdy(iTN!81w
z+{;1Auj8AL`{x@Wvz_k(S<kzL?-9ON_&$*S9}xM2AeTw9{D;KvqvChD_>r`KOzfW!
z`IBNNY5$b))1u#u^!tq1Ny?uUeopjTk^Y|-Ka$(`iz07Dmiv<U9SiCIW%1vN^!p0@
z$bSd9oxd*SNXp+5nWW#hMV=(}-x2+0r2Rjn{C7cq?(d8LW~BdrihVQE{sXa-lz%8P
zN%=>@AB%o7((fl?C)uw57I~7?{}g)q|4jIE@tY*=zkr_p|0DKE(*7$c|7+oI#BY-H
z+c-F`Lh7%FgND3551yltem8(0+jB$lpCs)!68$8pzcKXW9|~_Ge#b(VJ5|cvRCqHf
zPttyKkvAjlw-7r?`IaJ+ly4=xweU7#Cuu)Tcw5nLM*7`O{BAFPzYkg89i*OXLVizo
zlJdtwUibeI{8{dFDM#|(DbZIzwrhq;A^m4XUjuoZ&xxI+yeRf1(UUB<U1XBKt4)yg
zpDDbb*h$vY2HDOPkmXlF`VYjeKrSTpkuVlLN&OCx>q|u*f~+qSt_jycE+p*_0NIY6
zAnQ3xcs9s|r2ZV?F42=Lf1c>i7w#55NO%Frg=9T@gcl0;3ik>3gIq}ZKUn0gNWTN(
zM^Zj0JS4mbWP2YXyaePjNtXL#(H{%xe^~q{N&ThJla~n}CVrEo{ZGXHr^1Je-z2$z
zJQ{j_zmF9@LHJ~lpX;e2KU3tt5k6b=&jq<0PqO{bll8nn>OU5;+za8)>#={7a^$Iy
zUoA38zt@ON-VE|PME_3FPm=!c68pPBZkP9p-)5x$`^5kK;x|d!{}p;}_YaHTN5pS4
z((mK2^LYBC_>rvVQ^HS+o@DzzD>6yH&j~*-dXoAt2)`)$Nz(o$=(+x{f~@!J;{Ofd
zH-+C4ejDU6Nq!&Sho0qsAa;`V|4?L-@{fc+7XAce`JaJYCQ1LFi{CGVzZ5@`>-&|+
zTaoqrTKqS57RZ~C?YJR!Cf0jnkmdeRcoXhqSV-zm72Z_zn~{Du6Th2-toN4UKS}!C
z3VMDIw-r0d_S{M2t;l-s0zcMsSMi@D^>>4w^`8#X|DNJEN&4MO{2Y+7EB@0WSA;Vl
zmr2sUD)w2ilhoHhuD34ooY=P_%gw`&$HkJA+a~2m+P8~5N$MM7zc0x3Hbp*D{7L%V
zPk4XPPm=5Hh`kF^?u#GE@+%^flzri<=t=4aB9oK@VJK8$pCtVv=(*lR{It*rQ;-Wu
z|Dnhv<&iKG<{-C6A@VxNWs)p67X4<V{Q*+miXTb+14TX=<mWg`)_XQcd6%qzlGL9k
z`pw98pD*^U$luew@Z<OKVDTrZe+bC+Tq1gs_5QKwFBLsW{beGPJT4zDGD-U*gpU+G
zN&TORycuc#bJ*FQ$AWCn<E7kRiTu~XCkdY-_NNM;CVaZ^86eL?&jwlFbA-<oK40uF
z2Dy-I$4kWiPokeB?JpJmBw7E<r2NZ;uMoac_%Fg&f%Jck$gdT?PWXD^8$d23*ZW4{
zn}lx``>~Ma-y;4b<+p-t$J<4Ihu9}czjum$Gt&MpDgSQadxY<ma+{Im-Y0(V7r)KO
z{q=+5_aWhjg&z@q6r|tBL4KZ3fLutf=aV8&lKM}H|7XPSv*I^N`h8CPz94>I6n;tg
zWsu7x*^Yk~J;{2$Dl$p=Yr?M!zae&#<-aBRZ-eX-_@T%@0{MCW3#8xw1KEG~E3y9?
z<Z>)zJANbnBxNQ>W&Jk*d3@g(<U-Q^hr*i(PZi!2<Z>)zJvWnbHy7SQ%9FI;Qg|zn
zpYt|ipCtWG6Z`GNK1tedFZMf%{aDEL-5LHYe;46hrJlP>JtY0^A@<XSCE-1V_X4?)
ztnc0;Pm=okh`kK5URV4kNxv!an-=?Kq+bPget$LbyE<ffPu5cx&I#v*3m}(CvfWE!
z-v)9&y06G*fV7_pa@mUXyTAC6Y)?zr7Ir}T^+a9<xlEGf`l2V<o>h^Lg{(gi|INty
zL)b|rekA2cWRmMmK-Oo(o{F8M|4`)3Nc%|alcYYA^0{zL{3c1i0(!RN0b(bqx55XC
zek;;{r}&>G{v`Ehi@X_WKS%s_f!q$~iT`G#|M_AkDeo3}lGHy)>=%fgq<)XcB;^Z5
zz9wY(y;9FU;eM%alJtA9=t;^4L?$U86q)4rak1zhB7CUWC&}xW%f$XL;h%sk{|J#E
z334I1o<9@*x#%ZJ`=iAEXt9&j|Ap`|q9>{UOOZDtzt6|P&h|V({79aEo-8uSdY&RO
z$#PE<{nJH$rpV6%xsdexTaiiD^K6kPN&Vl6ev;Hb2YS}?Tq*xN@gr$}zVHRY7mA&v
z{Y4^gM%w>D?3<C-wf`)3lI?i8$Rx|ZLge3vtml<d&m_6NS3ys{M(iZz*MThedeI*X
z>Hh{P_eSBHgm0F5Ncz1+<VjNhR?$zA`nQRmr2KY~Ny_gKzEk)vv2RB1kM9+~_kol@
zApVnNy&n|)B)NS)3_bZ#u}_laE{C4{nDFD`H%XTNgy<*9a-WoPpA!3KWckmCeUdEy
zS<z3D`p-$dpBH{X{7BlrDDu@I%Y8}8e_6_t)PDu!_Whd3Ul;pUWVvsM|2M^dD{?!0
zTl~KRQvR-#n<VS~p6E%o>jxqq53>FrqCWCR!XHchB=tW5dH(yU=(i$2$Ir!YlC0+!
zQr|Cy|0DdB_-{tm_ZzVv53(H_ozX(d*JJ1O5lH*>#ePl5@;8w3$3lMI8;U>4?S2!G
z^_(iasn{n;`^})|{&7pOPm=ZCTJ$9SZzDVnWVzdmd<WqjK`tcy?<Bml@Gio;3hySo
zJIG}-vYtN@J4yK-!qY*v^PVE#3*@pHS?=CqKNfO3y5fH_$#zc3dP&ODAlox5oD(}q
z`@G1Tk=tic{FXq<+r^)xd|$DjA^K}VwxcQKw<7({6hD&k{Y0K5_4gM&Nx3C#i=L#u
zBl0Av?~49dNdKPrUlX$YvXq}B^?lJ#lH1)E{jrestcw36sUL`blGF##^Eiz_%CY!O
zlKLH@-;A^;QeF#<_>;7!B5y_d4aM(xkoAqEzDZJ_iT>)4<#H)EN$S@`KS}Bf(QiiD
z*TudWX&;N7r2GJpNy=9EK;cfY9}lwrv!vd$h35!&$$BP9|8t?|etW+7?H0dD((gf{
zpCt7cNVz>?C#k<sxEEyo`^8Su{$P<ON&Nw_9~3*u@)wEzV&Ox?{`-*Yd#KcRiPX0h
z>Ho*#H%aObL(lu#Kb86(F7l(qe>1Y&qs9L(#BVFo|1si6QvOSkw<7%>D}Kj9`ae$m
zNy?8GndJBJ1d%r*{r_70o&-{UviNUB`aMPbCQ1EMMSpe3a!-@>KVA3?sfVQfng5@?
zHxIXRY~#P}-5`}T8VYR<BtwRTOr`Q9q$pF!JY+2MlwFEs&J;qTL6IUvC_;wJ3YkiV
zjFA))@Aq@w-(#)oajbV8J9>V{?>&z9`eS|8IiJ^X-|Jp$=Xoqc<l8N;PLlTy=Y_~4
z%sWM%1Maar%Dh)(6e98W*>AM{iV^=Y_A5l}V{I=+_WgeQLE^?+PKkbk^(oOm;J62E
zuLy~o=(vX*mlFFV>ml-F%d3NY-cE7eN1eAKq>sn!Ux?Trw>>5Lsn$c}CoC5t_9tzJ
z$kRmj>uKwY5&vf#H{JdzvCpvmS=$SddNXaW2+8}r{UP!TB6((6e#v%-{bkF=h~F#r
zONstf>nlS1XWJjr*Xx#xk+?VH2j4W`a$HLCy<_{kwnO6PST08V=Gy-~`$P2aTZYIV
zSWbz4p7klwf9SZ6Y_AB3``G@4i2W1WR|m=Wsq>{oKi_(YyuflX;`f>T7uvrN@%!9%
zi2Q|Ri2S8xi2Rl1e?#&ta-NjvzqTGCe`C22Id|XL?|b_};(o9U@%zy-#Qu}ze?sDy
zIM2`KQu7z*O^N?9>kAS4a@&g$zh7PFH}iMLLF_9mr$qmU?JI3BL_Qz>>;GWWGVvvA
zBNQg~QrTf8v$FjwLgK2}AI9%Qme&%oSGNq2Yl!UY+SV5%{<Z8^5#qm&{ULH~k$QD4
z*RvgBuWvT69-?2@av_p$efw=-ze2>Xk^MFlS)VfdL+ni~SA_J{RQ|A;<BAc#=Js0^
z61S=2Ao6CGA#w|IbL)!{|CaXqzlrp<h5IT-{I;}TMaaHxEq}O;Nc^@U*J*3#f#`Rz
zoDzK-+jlf~vR@%mr>*U)gVf(e`RI3dk^JpM^zB^-BJXLr5V3c#9g?S$Wr%$*>pNTj
zPe|S_&bN>AK=SWv86xjz86tPJT!`5Bw;dvPvs{SSyW8&o^FaF-BmMWZUm=qBAlo7G
z!Ip~=zeDT?kq;HA*UR$Z<`E(T;&-Iwl<0fgew6JH{n6$z)~BT2an?ipkGEWm_?=+C
zV#M!6=RL{(#YjJW?3a=}r#R24wihG*r`hkHkoeObzbYi|49BfblJ`vKO^Lp*^{a!d
z|5?rl$$z%xl<3c~z7VmWYkMJLKhOEkx8MJQ)VaX<ixIyI?YAl<?jpyfM1QgM5cv|z
zDbe@0z9Pi`Qs*mo97I3BGDIF|IVJkbtS?0DgB(BDyxji9$hjF}zhWf+mCifV{wc9v
zWj#c`+A^fhwdVD<-(Wk$f0%ir^$`6{=FQd@BKdE%9TGR(@;@Q@Zg>3ulJt3p`yFB4
z>AHo8|6TUG+kVA}|492)gv@)C{fm*j_u3C4-)A```Nr5j)*NTvZ;lrkkUSGCSA_UK
zVE;nI{-EuJh<&2%#faZS_Djk9CQFa~VcS!pf5du-JjHS$Vt>?ji2RuOxb>@(<e%#N
zPnb`d(_C+Ll6p@$FGPOYe8&2e*r!{c68#M6S^wu8KhyFHmR}SZ5dT@0ixIz<><5uw
zwp@tVU$H$Uea&|KYv$|rgT%dIzG?kGA$i_%d?8|g+xChO|99*Uk>54vSf3L6T<a@B
z{NJ-bM1J4=z?^4(XnrIz3X%AaZLbLVd){aEPl^63>sN);U*x!y=)bnU5V3z_J4F80
z@~V)y?;Mws^Y?@EF19@--$R#3kKa<;A@VZIko?QdUv2;0@(Pgw@%zJaO7tsj|I_@-
zeh~W_wKA5NrDi3wvROrBK;o)ePRTyjke>O~GV9n6GQWBv@%7CHwnOymT26_6J?m3a
zr=jB-nH!oLnH!6Yl;mq{eM<V<#Boh*Pf1);+nd?`Z^-XwEu3$2=Yz~^OUuPbKU>*v
zb&z~pD<5p-yv0cW+uOgj{UPx?m~E_I9VG9L%EvzKEb{NB_i%oQzP(6aoy@&$hs1X=
z_p!bZ@!Qva`-$vFH_P1}R}m7|!~Vrcy`J`i$Onnc=Md{tVn5V+h<uo3NZjF;i;=h^
z9Dk(!Q)2IJ`%$*1q|Pz6A8Y%nko?Cvt`M;wZ+kJ~cY^&=qCe64iV**k>|YV$f3p26
zLj3#K|DTZfQxp$Rb)J;$$LZD=Bk^b04-(hcJWJ&IInQ>8e39j1#P3r1aa|4;`8*gZ
zavfY_UN15rb%%-g-D*9gzu}f4|9#^~>+dl~nfIFanWIGpB>z~Ez9v|H(00gw7dgfD
zskT39J7gX+ET?2XGp&dCy)4rAE7lhx_E+sUTcp3&%{Q%o%Y0j;&K!{eiJxmZCF}R0
z^^m+DTP{ZYK9wJN=ZmcKBFkSp9x}hhmLYvD5sCZRdWgJKB=0g2zu(M1Y+os|A0_K#
z`(9OKRD|STQ~t!)a2!Nm(=y~gSzT9Te>W0&<Y{R;Bz|iVznw(>r#9M)^xZ)uUq|cr
z6B#M7ceTD4`6u>=+7FVymq`5KB7Gbsvc4yX42b<i%Mia)tUpyG&*|ow*7p@z@3Tcl
zA!0wzcF6kl7wPj#>mloMqve#;y;*wty+h>OjkI4%{6<O7Jnl8`vtKdN-&pw(f4}`9
z=W??3kBF?t<F-TUOtlP=pAhMDy5$)nd0w*qb?e^{S%<mSe<U*hFGU8VpG6}5eJhgZ
zXOTKftzRxOQWE!@^z^&Zerwdu;!4a?5x>eJ`Pa0*nz@$9{?r#)?+wh2Mf^7r8IU|p
z&E_KgZEAZ;^qbk<Lgf5!D-yrG{UQ2Y%}yfgvcHI5ck@7zd_625B+}QRBBK!5m)^ER
z<l{y1oFJ0_B(smmfap&(Pq!YjE@z3<J=gL@BL0_%3`qX|mQ$j?)cT4L|8n_Lcd+9M
z5x>i2hgaCIBBbvt9XHf*#fbma@?#x`IqpW0^|-~nO=LjyBP^$+&pV~(e2*59$CzX7
z57CdaToKaWc>AvksWZWG5cvV~L35(*DY@Prw*3*2`j6TVGXJU8KPi%Tn(ZmkKV|#V
z<}>z#*r!`AMC>zchse*0)PLUkijcS$><^J&6q(mcBJ-VX`E~P65x=+0x6OBKpJTpf
z{ri^ZiJYVPB7H6t$^VteNJ;&#rKip}BJy|kD@6RhxBn0JgXkAqhR8o!PKo{}+n1O>
z+Ye%2YW`w9M8C{jZhbN0|Ev895&Lh>_q)iv|FA#ge!V9D<GX(&ers7@h~%wqdrIPJ
zO3!s!U&OD0h<-hD1Car-H?$0q8<`u5#BFSriR^DPk&zO83+o~BmLlh(mG#9)-EHJY
zz3uD|IbS<jPRZxXp4L}{<nJJV`q|6eTSVW*+{gN^B7Jldseh2|2aAl9%(IvMk1%_S
z)H%jH-ue^GlSS(Gv3#n?fW)0)_7m|t%ksG*15)=w%M~GcE|NdI*l~r3{Sw<#qVMnc
zOU-ioLF@x8L*#+xW#%Aru*g^ylJ9cI6(j31)OD`1e@gOPZGB4g*EsH4+Y1rD>+E;E
z{r*pq`Zu_4Az~k9drI^-y6>B8hdj64De_!<pY5Y<hs2E$IhXfapA!3c>r-<7dD!}5
zWWOGhA9)@(r#dbreosjcXV~vq^Er|0^?8v2$@hZ!qV*8{EX#$+JYSKWI<K2=JMJBk
zI&(w@#P0)<`7RL2|C#M6(Jz#q`^ncLdA_wDB+p`z`b$Lof3_Xs|BGdayxjcF`jo`~
zE<Jhw6p{ZD+2^Wt^<tu5(=w!fb#rZz=hnLBde%1-8Ib%NTTV&d#?}`jer5K9)Nd-H
zZ)SZ;^v$K`+-_~|D6;;0i1gDzWS#dG8IZUxmLYy!Eko9&r$}5ck^IMr<UQ5)vqbWo
zE#h~fS#Dk~GK!IX8)Cnd=!Z&A{;NdR{d&tch}drw84&qq%f(2(TkHo}-`gxh?6-^L
z8)5lQ%XgdihzyATD3SHLUu1nIi{zamlIJmz0g<O#PKp0C>mhlc646gLpR*lO=M@pZ
zS4Hx^X1-zln<4|^_m=s#^@T{iIkrRM=9=$W57EDGeqhcM>Ek2IAB&8X?Bi#)FSLDC
zNZrpJSBTiZkexohvR_K<i=>BNo8Q<E68D`*pFdd-S(l}@FB6GhE^^(huzscWe~Ro+
z)q0uVnj(HREY~#G78xn2Q``1BW?d0|1N*0>k9F<0p6!K5d_&op--h;s^i^hk6SJAf
zdEUbER%R=adfSMMLZr_2_G@kKV73vd*Vghb=C0;$=I$b+80ljV`=vzRPI~6Kr}K3X
zsrw)M7b0<;?Z3DER)xfOaa=Ltw~y=WYkx?;U9I2W>}LC_ko?^p2ayl3oRWR&X?-CQ
zcaZH6|3fW9J`au&@jKQ$&UT3Yc+0DU<U7InAnSFKWr+P`%PG<KvHz(e^EktDU;9JO
z$2pcE`OmZbzaV{`ul)3Lk?TP28<&aPe}-5Ok*~1~iN98)&+A3{zsdTWMFzz07Lhu)
zSr4gmk7bD8D9aG}Uh_V4wC#}3`w1d-AF>|O&t%If$@j4IoSSL3L;9F$zG^+hf42FW
zNS(LLIU)nHF7qry;y$!ojI75e@}uAR_Af^6hhNz*CG{3L&o?6Sx8`^DPl^3|>sN*J
z`=jF^eJ>H2|8nag`?At9q~4#FQ=<RNdWgJ6eO@#Q5POOAu+;XHtj}7uSGOI~XHDzZ
z7U{RH$bPPCHWV3<{Ef^Ft%vA0vJ8<oHXB=Ci1?M+4v{yp3|X(utZ!j?YqPaTzdPIB
z*4)M1)!a>F6e96^+76lbUX}}yecoGk{u95RBJv?3d5#cS&)y=x_na&;pFSe<IMeoi
zB5~)K=UacFh+nx#+yIdQ>1(j%l=xp|eM<bVlb(KVFmJRUr2Z||54U`W?U20pSbv{6
zR-~_SA|oYr#@jx@e87GwiGNUf{2#L4B$0j|u|K5$$IWRX{!iNuS?}jW^34>9f6?+R
z5&xIXS4GbM+ai8*L<S^&uH{0+{{#EYGe5LHWStgRhUELqTxdN+|G8y|{Dt|Y`IYU(
zNZca(LFBK^Z$#$(o#pRE21LKu`X8-_#Q$U>)_^i;;ZG9KYQDg~&erAv^wm+7BYH
z(IBf|A~GsM`m5}?DrQx4P3QUl5?TLh?z<4NuVp(#u5Q+_9-^-)vaYqPFGTFM?N`@v
z^(;3Kxo<TVIoHk2Ekv%zRw8w_F}D@jkJcjoZLQx$q|R=ZcNg(%Z+!=mk&?U}ZQslG
zl;q#nddRuzF7mu`m`Hptk-A5SjFjkm+kUiptVmxcSUyQ)K=Pezxe&4UvEM27gXm8+
zPqV%d@jKmqXP9S-%;y5frNsY2>Dh<=BJ(L1>Hjk8uMk<MYeWWQorYP4_}^?9A`chI
zce_ZPJIoQb-!0PDIFXT(_zCuZz<f|dKgsfBk^GNXo+2_J`o}Co`kiL|)7C@g{j5md
z=SA-4uZqOKX1-y*WxgXaQZm1HZJ%qtFOvTw5&OsHC+4RjqZsMyGy6gPQ=m0|6q(O2
zBG1t)Mf$G1ZZ?lKMfzV$q;7Q)zq*#|i42InzKH+2*8dZ7J~mW5eQn}=O`R_#^_p2<
zjP$#u{Z@rMKkcMA`q^D%T{?-ZV`q{3&;cR?;&+H;h<v0-o<1URr<rGp49GnCn*FSY
ztmE0%pJSdYat<yO$#;p!NJ+l_)<fh=Ekp7S5Xm#x@)aWahKlrkz2zH421I|GNZg$w
z>v5k*{?Q@>B9Af0S`UevAQJbmNWQ5e{!fV1e@bLP`g}&@9L%&HqJKdo?nRM2vn{_S
zlK)-Hb3_It&pgYJdLN7A`NaB^^!u6g#C>J|Z|wiANS!6t{~|IV{j9JIsq?4hl+0s|
z^|JfcS|W97iTKwMso%i*h9Z47GB>n-6OlYkMdCKKeKV1f5_?PQQ<8TJ>mhYpiCi~3
zTHZy(Z&!0Skpa=~Zth{W6N&FEa$oK)vR;RX^wCQs?r@O-@jKo!WIg&=PD$J;(lhVV
zY=`vSSH#}W^0}7J6RCTl<%>iH#Q$R3FSQ<`FSneMJeS%3a*;l-vV5&bU)PJw_g0ZS
z!>zwvWL-vy^gBjm6e4kBWvAYF`=vzxp!C#x$aYBFBy+O)u<a>{e^h$nAGaM6Kh-j%
z&!<HEo)L+A*78itFNzFE{4C25`6csZ^A(YNvqc8P?=|yvkveaQ)SqMhT#>l<MMg1_
z?*sXf=Of2|?6_hi?i2e#<WDU_<oP0feQqwYy&@#;JIDPfa_)Z<>GKcgfz(-P{Tl0M
z^DY(ft0Hpls#{;v`gKI|*R#B?<%S~Xu8HMMMb@!}$S6kcXItAZCGp!h&vv$_r2p-$
zPl>*@^(nbtcCmhSl0J7+Ue;?*k@<8Mskgu7o+9gdi1oci{Erh^&y&nkMf}b%`-$Ya
zK*YblNd7@0aYJmsPGsKKi|oU2>+d$lh{R72$v@HhM@8~HVf!;8^O+$sApSE&@;`6;
zi<W1J42b<D^JVK(Vt++?>b-8iH$?tD)jW~9AK5-%WE3KK7TBH={b$m{g(7`_DY9PQ
z*&ky6-u%J(l-L*B{-gPm$oeg_e@f=_yY-N|D=kCv{b?B@|0VK#x7G&PylaSDr}ahZ
zY#`EiW06sa)NN}279x2!w?9N~X?rW{Q)1u7`eI~0+slvscC~*>?0ZPh{<Rm8JDUFy
zIlo;+`aQt<o|X?4iR&fO&*A0~=8+;JCH_ZQUyS5C+Ifx<>HieVr;7A>hRA^UpJ{n@
zl79OsFZ+6)$i80UI)z9d{bh%jn&l#S23o#MWI+4|iLCDs>mm9pMdGfq9-_b6yvFwH
zY=`*YVEZuZA^C5y{#NTD`G#AD$hTRB$hV8+9br90f2VnudAIGwNZd&Ik#Cga?iJb3
zF_y>Le!s|o#ErKMiGRSHXni3P_mJ%nd6GHV`c)zE4?C_H@q5H|rkIbKk2xOV_qaJ#
z<eWV%GQa5}1F}BPnJ?Hr%Y0cR&nuQ^i;QCAbLL(96(afO$WGn&?U#~1Kb9VTCgQ(P
z<U0M%{wc|`*m}r${Mm9!@+`GJCHh~a=f3-wh`dHaUNl7VRkoax>$SS{%(t#cTs@KX
zZ6MOu`XVDG_J;OvWIM!vW6O|yjjeBDJw)HsY-T+~-`w0(Wc{`f>2G`MTbnzGjFiN;
zvAz)Ldnd>5Y(Gd`TaoqaXt|T^g~&WR+rBEK&fbnoiN1^U^tZ3=5dD54``6w2l-Lij
z-+?0Qb%;oRy+raIVf&FH0}|KUGQ{s_k@yot;!hUw>mxE!;&+DaXWIWP%V*mk;&*{%
zNS%vB@?LCxG2(ZL{OG6L{t*2D%Mf{>d6_v#q~7J0uMim!{~@*y75N;$L8RYXtRF7o
zcbiE45ti>1@f#(Q|31t2+dkf$VEcoXCz=nLlg!EH!y@^nSbo%e%zWIODl!U@exHz?
zJk#v|l=-yv(?tfve}?(2`J6e^d|qTg^1f&p5<g31J!aefy6q7A8<rvQZ&`+{>${dy
z5;w>G?}^-}KDPX+{ULtyMXry}Eq`G<#Qvp7{CAeW7peQB$ViF*Pu4^HmRg3`e-Sw^
ze^_2=xpJe-UPWX;{Hu!eS6w9E+O`)W{<UmR$-32+9(x_zA#z>I5V@Ynd^fNj65q(&
zNMyg7i1gE3q`%EAZz<xxwMf2J)^BHhYmxcyA`-WU$bk5_GuxYciqz?3b`}|sI$g|t
zt%u~<&+KaMZ+jt<r@QSC`9P7+jb7G6^ha2R$h}4C9c3PE9%CMB9%mjeG9Ym$m?xSi
ziRA4gvOcGa42b_3mLc+)BKvWU^^o}UMAq#>kv#n^53v0*k@!I(@mGqBl-P&b?<(_Z
z`=!M18tJKfo&B!2AH;rx<%$shVUD}eyvgw?@w?gfTSVq@hvgCGo#tKU-6A6;c}CiP
zk2%V`*St?;K;}6{<T`o4_6IFLVtI=BsK|iSdCYv=`jptGS`W$pjQyvZGi-<K?@Y^u
z$oYL$cJ}`b`=w;P-nIRG+aY~?ATrMnEq`QwEK+yANZ!w_|HAyz{K{Npel0Q}`M(j#
z|DDMF;Ah(*`d_U7L&Sfj`KQPzMEw4eoxiWGy<xU~^+ZN75?5b-)LmC(T^czKvW`tf
z{F<4~%}veCL`F*1XA9|hF5JO(NS-z#_O_OHF?SUig-G0P_S@au!)#}^7nw%~vy;ex
z#P22YJz{^6eLO&<kAp<!d8CM6Z}TYgXc7P8MEp+@87Wz})2xToIoJLdiu84{^@By~
zTyFbN+piYs<0jj07O6X2WI(QqyKKMP94S(Nw8((uA7dFJk2S|xUyS(QFF)!(AX4{X
zk$FrJ>GLU(|DA=GMF!;Wj&nrj|B3aGe4mQMFR(r(_Rs9M&~_N#`$YO!Y%Z}KVqal-
zb&`3kxshH>`l}`xu4R28GQS$K({C*ixwicw@pUYxL|<2W*0rJSDX}-QJ|+8JW<A7j
z6Op=2txw7Qrls|e^SrfXNPH`iysfP-M(VVYAM@SW{*Zmy(=sG)2a)<+tWQaveXUQ4
zzN_^q>F+@4;UOaRdx`vRe1gb%Jyj(Bbdgbr*w2)m{`#8zMDm?u`CO4Y7l{nW{07-R
zSR~ICB7Rp}e~rk1<iFOuPQ>pf%QuS*NS<3o`oB}eZ<I*<z2<%9XmgA?)*L62XT14<
z$b23W84&wqk-QIEUx=KK$Lv4V{!fVLpSFIw`K-u*<bBTi=SAwzvi>EJ_}SLKAu>`D
z|EBa@5ATb}ABe2SCnEV4hzyARGjpN&x$P;5`^xr3=GXRv<oQO#?|bW0()Ul+L;ROm
zhQ$418Dd{%86q#YyecH`?~Y&L_>}DDO56Vwk=NKbt5;%{ii~2Uze@I79i)#c&i8Lf
zUsau_B4oYRvOgrgx@Cx5L!`g8Mb1@Sb3Kt!h+LN&*$$DLisWl1az0wvUJ(+vx#L@k
z^wG-k5c@VFeQsyA7V+EJ_O>FU7+H_q?7zFYhuO~YDXH7u`jqJRbX*79i;?yCkNoI=
zZ;|!dU*x(zSj65-q`xCY^7l565*ZM`<3!^7h~zs}#Q!wg`-+T0B<?)NU0_}$Qn$Z3
zKx9Dj4m2;b9-<#4QvY)6A^IyUL*yZrA^t<Hzsh<@{cEki&ia(thglEFd!u=iNZwm4
z-)asQ87Yao&H9w+Z?`@r`a7I&gzb=h7-_j8B+ot0GfJf2`^+)UQxTG9to%9m4~WPQ
zniI{3M8-cM`6f9Y(&xh>^L@;ENd0M+A<qHNi|o@Z>mm9#MV@!wvmPRUV!0wD?*jW5
zBKFT^r~fZR`u$p@|8LFjMf`rWyhLO`{Fa)_MDi>*f3^O1^AC}c691L<|I>EJ_r7Y4
zvwmud3`o6NBI{Vs_Vp}pD3Wg@b7PV5Z%Dqz%EP)ebv{TR%|+Iwg-AbJi9A2<WO+9c
zzdb}oA>!Y~c1WGBBKv=^Nc^E9eH>={ks<?fUpme*<b0eWa(_I_yvTM)o{KF*^7ps?
zQjvUvEnjY4Au_+KZ678wAoIUj<adL6Me^S#l4ra~+(RODCX3APQIWW*<}}-%7Kwkx
z`e)7OMdDr($@{wHw{4$ezHfdgQtxAtQHa<-wci4Bp-8`r90#fQgGj$WSr6%RiTShj
z5dBh-eOO^VWIrmEW&2uFM6NH=$GReM>xtB9C^8`ZG#2sOL}VSBiwsDATZqJODKaWT
z{I@F0@@*?JAoW{|42a(jA_F3~5g94b?^u?d|2;+I4k9BZ`S&Wz>g+Az*TwRFmb;q!
zo88RrBI|IVNS=d421I{|$beiwM~mb?Mr0jN6d4ftRFSyTMFzzG4D(EpkrI2~GPJ9Q
z<m*?K)ji8RTV#IcSw3Inyp&rWU=9=+DVfipvMk?V5%~)9O6!N3SD9CfoTFiuZxpG2
zi%9%%kv?xTZ#VA{84&-w%zH$}J!QQ9Gjh&Hml6HHA@dniM)ryk|FLCwXG9)XhV<`9
zp8LzlQkdAsmu2hwpvXwcI!!J^n-Twq%d-7?#GGP2Dl#B`kBN*z#Qu00)`}7TsbyK8
z(?kZOzv&|9>jjaK68p<#SwF9c49MsIyCUaruE_QLspSPC^*$3Bkn7_ck@3GIb-yj6
zN=D@G%Ch^(Pa^aAMMS?$WZr+6rJH2)sccp?tBDNAeQ9mWutah#k#(qTxsJ$ye9o+I
z{RSfIu!%??%|+^OAu=F&wz7VEk?XdN^*f2g?P_^<kx>zr$Zrq%!*(Kd_7oWqxueKF
z?k7^Wo9z($0hVEj<O3}iBK98k>uEoT{vh*U^APh;kvhFBA1=~IZ}S+D0ZYVVMf{Jq
z9wMJ;8OG;`SR(yt=IJ6MCH6C<Xa47ltn<Yp`TC0tSR!6(c~wYUx#Ln&XOQ&Ny~g%J
z#P0^#>E}kVM7+to*}O$$q-6bWlOB$+9g=UP$oU&%eIZhBob9kg^8F&~I#I+vNo1r%
z|FHBW;v=?0<SFK(BK4<PeoCaz=^_J`h%+oh{9m-362Dp2r$qmf^zdc#74ubdwn*JK
z%(q1bL_f!TUu0hMY=<S1KQuoQ@&DBFe39`_SfaQEil^Q}k-T48UL-P7mPr0udiag)
zkbVBaTq2f8{#j)Ieia#zI=@>^N!^v!!xG7VirmMlHp%i=7tz-+Ynp4Dwaj(Q+9D$*
zb?Ql9DmJj+y7q(2qoMVU%ni+rY~MsIm0wfqn~4mFepAbkemApzb8`!k`}KAr_1cK^
zwX^NJn7f&Khzv-+c4m8#xQ-%uI$Pey^8O<J-K;-QWIqnE+{^aE%_BrcO7b6NJuH=c
zjO9Y4j}v64?uq6}=E;sLM(UkvzlxALr`bOxdCqj*zGgoW{W<oBrIODR$#cGWfr#J5
zA_HQ-L}cFvSsrX&E;1l-S6V;Ryvq7(Eng=xAnSFb?YG!|t2ta`z*6yc5&yfauL$Y;
zZu=J_ek0`v@3DU&GQaz5FGS);JI@&Vtxl4Etn(Hk_HnY8isS7E(NC}pksq*}vQ+ws
z)~Bo?{S(r2KYG@7$bP&amWs1PuCLi515)QT%PG;nZaw64`8|<*ABxyN5?Sw0Ezh^S
z(EQT+uS5o<zr~gzd49B9h}eI!Jtgy4W<BKd@DGvx)@Yi|vx-Q3RgvprZOgU9N|Nh{
z42WM{vz|!44Qz*{k{elu_-`my5*u4CGdB_Gqq#`jW+L-$WqDhX`0dQ?MFvFQ+T1~`
zBzY&xJBtj6Ut5uV=^#?4lStqDhzy8+KN0_~BK`;44$&WC8RCDKdARLIh?S&2$~@Nk
z<3t7|&+(Rvk?%`q*bi2c+)t$M^DSQ_;x|}iK=NNMvX0kUzD}$pd6>vZ$@<@7`)#&E
z^4u=64)<CQnfGYRtCQ3l<GjU4{c-YR9VUvE#K|K1M??lB-xSL!(LZYYV<K@=Mb>ME
zSV^2|{R<-3!)%dtdei#1&38oxtR%iClK*}41CasI&l9QlkyuIm)cX160`oJG0V|1L
zn2XGBMMg^cTw*<>&Qi;e`oEaVZ2wiPB>f8WPm%pzvso6umRUn&K=RbI{7*<<wH%)k
zeQoKP$9lG>tSo&a>FIZ4`!%;;A>!9UcKY31WPi66(YLmK2a%DI&xdx_SA^v2;CvlL
z)^9(@!^)DoTdoM{_W=7BBY6*!AN3Cvi91YW9S^sBgvfyCds}~$^(l!z+WM5l9V<O`
zPOu$TmfT0=Iy_S(zORV?S(eW>&oR#x$#a3o=Td*m14QC3vwnz3zAJ6NT4euj61m^q
zB{Cp>cZ<Z|BUTnii}W=?#QuQD_$MU(LC2@0-b2<y=Jl|M|05#yM=d`lG9dQHEvH03
z)%GVu@;xOo@8>Pg63P3r`Kri(#LpJFe%=w8{|A=m+rB_#K=OYslJ86FVP(nRh^+Hc
zk$fve^8aD36d92Czs!>6nO~_$+*)RBk&!a4bL-bz1yXMV`BxD)7U`?8<012IE>;m+
zTHZ!v6e4~*$WGoJMb@RQNPq1`^6zPO5E+GtUq{)A|BwBO5x*|>gT(J+86xj%8PZ=@
zv9fr8NPJJ*A^rzhhR6q7PD!3atxt*nVUFu%drHpXvC^}?C)*C`=QPW(isXKlQ_}A_
z))yjqF0ef%eO)L$@t4@1lDGlZL*&8sA0iTerC3G0R%8?-@x$x~xei8%Rm6Kl=0Dc*
z10v_-5s?8|r)jp&5Lxf%MEaO1G76ErFUwB-*X#$G_nRW|Z&?rVe@~>p1-5@CQtxxi
zUx<uCB<~{GsrQ|Tyx9J*isYpt^IKs(q>n$uDq_i|nOtgC66t4ck$km9`l%~2AaZ??
zJPoXetlLJGA%2a`O+@N#YI{oZY-W8SlDC!ZkiNDPnRgqJIy>18$+xrZyII~{tSWs+
z%bi5}>0)^wb6;~mk^J2)A0SevhxLb8f2hcS_#Ywi{Be?qe;?Zmk$veaJM+6hWL_8B
zzYxh^E<5uZAnN~$Y>n$h^4uzt=WdZaqeMnZ{O*^Yc~2DSZ<75Wc_&+j<e6gmpOAXf
z6i>YwBKe=S{Gv$zbF7~$G9Z1uZ~X_>L*nO&RmJ%t{VlZqbMp(4_^-sO;`bu;mzc{%
zp6}M!EX!A7mWqs&<ga3TRgt)L><8(mwpmA{-g>q};v0(8Z)`Rb>2FiZTZrV@Qe;5n
ztwj7<S>8q@-}WK{BDb-9N9zj_zqYcIZ&!17`>zht?;gs>`RpK4Z!dFi5qlSr`umCu
zh}_lo{jE=ly}SJmv|kU)2bqV6jFiM3X8YkH^^Xwg?`X@%i1c~9<rB;kMFu49B=cmm
zk9ms7fcTwio@SmdQm>!Lfb?~??dMoui1?jn`~L}2?*jLAq3aeR^SsD*NZcik>u){8
z?^4UFljJRT-v6Ikj0&y?>o&ma29XC^hRBy$hMe0gL_SZh6S=S5Z2fKKog#5}iHubt
z=Vqki3X$vbUfU}|@{P7XL>_Ayl5f0aNZ%90n&RW)n&LAe17e?U86wZHoRYjVZGT>5
z-CnX^O78!!+3$7RQxf;K^yHgkzqumwc;B37ek3v=c^6oQ<oQCR-!ILtY=`I<Sx!lw
z@1=)J?Dw<%3XyZMLU#K7({YtrWce$bRm`d)ers8-E;1nf)Udsl$b4&Ct|ziTjm5Ra
zEv;`Q;<v5HD8_2C?`%Ivy|yC#>}EYA-yS0K?<mIaKjNB_dx#83+`%ILhguKOA8z&*
zsdJQhw8((O9cvllcbsL2e7wl@c8cXwMdD5usnb`a&vQivB<?&BzY8p1XnP^zcd`90
z5y@L_US|Cukx`7)9c=&0%_|%Su@5n?G>6&_v0o)p|60q}iLCoAmTwj5_coCMiMw5_
zCXNz0_hYOdD>5MZ3D!SgK4?xfA2KJIlSTTOBC^g;SpTF*A5WRjSU+84K=$ngk@%NH
z>dzMG_cf9DH!Qzt`E8K_@q5SmIU@Dv*$%OPXntgVY&*pMiRD7X{;BOL(a(4M0+Bu!
ziqv0Z|CHFjvEO$h@!y+2*dL-_Y#Ac|X#OOU=V#mh3CXwA@sM?2ZW)sQHxa+z%@rd1
z@|Wc`HkZH<iLYcC5?9%*BC;OUMdE9T3`krZ%PEPkD?O}d*0&!--@shgT+dwJ+`w#T
zHWC?-_zf*X<c&n;-Pm%Oxry0CWE3LtO>KwB&CKQ^^K4;mAyQ{6k&%+Pt)=IBXl;8+
z;@Vgbx!!jZ`Q70^BJrJVhsb-2tbbRL_-^I_BKvWW?Y%@sO5zW<z9Pi`2>V0o^%kjr
zoJhZ?h}1p9_P!!<{X_;tK1*cX&b1zr?|hMbmsk(+?{EEOB6Ws}#9t#aQj+I7>ER6`
zzdMW&*AhotKUO5qM3Fp`MFvEkV*504Ey>S|<a<%1zgI=_&K1e?vB-e<eQFuf|9s0S
zsk2ae;=d7*zqkJn=3<cn$@`Ot-%{IuF_&5Yo5=dF6gg*=T4tXIHALcSiS)gJNM8-j
zMj`_e-`Fz5uS{g#&8<&~eN+2wCQ^4Rb9?Jsi`AsxRb>1x$vpRP9Y}xeMD!i3hv++s
ztm|H8XWLT}w~zIZxP8t2MAqd1%Lkf0MDiXaG9Z4vY(Lz3NdDepb@62DQ<CQt>EpT*
zk^73ooh@>HE)vOKZvAB<eGU@YkD=!EBJsmS1|-kTBL25npA!G!)<gVmvz!vYJESMy
zT_XEEMx^iY_OA%ZKf(TmNdAe=`>@EoAGd!Y($^ER!)YSx@vP;UBKcl4UltjVK3=i@
zHS<l|3z58Y9rvEdzI-B5cfR?VNWWi+47ir~qh&~*Wg_*LTMv<c73p_{^^p84Me0@F
zLNBXG_NSWS$WvV;|Jovb)^)y=^ij`xNL&NU5P3ZjzxB-x%!VQZ;<uso8(9yD+t_kS
z`e`aX{cS2DZ((jDl5aba`LwaTlgNO??PC3IB6W7R9g?qu^&Lg>?IjYoxAk4jeMCkv
zl7C<OLFE0!>f*tc4-u(znB`s~1LA+U?MIqNiS&8A<r78xPO{ubWI*(%n5UYjnWu})
ztFL*sd7ka(n-_?j$4f;9ByYJnz<P*&pvZg%nU~uR(GRf<sehG7{u?acWIM!ut9iRf
z{yRkS+$l03>oLmqd#z82eVp{fjkn(f`$6(PU_NM0v^^z$57|D+oNPZx{zt5zVtp|(
z?<egCneWpg=l5BWd^1J*f64MIA|oaCH>^)d{9Dq~_uC?Q=h_cqf8R31?*nt5`JwG8
zsW)GG@_lYQB>oG_tAphG()l3rSC%32BFlw{{cGDH>-(K$i2ZxZDOs1r(lgJcwpWDY
z`^El{Jj+G&zgnLX{cqB<jw{WQEwj2+%^D(k))x6ZZ6Ff2uDPDbfcR}7;<t&&`P$6f
zN@V`qhzyATb|U_5Mf}@|<ZmzH-_iOmBA+kaMf?vH8IU}^MB<OO9uj|?<zgiM1o=_-
zL=pL9%Y8)Z_7(Y@yijC7{4WyOr*g{!Y=`8zOeF4Vk$$cd$#;uLzaz|1wvQIcH(q2w
z_Gh9<zDXi=Cz}t83`oAI)<0oBX-*Rvkoc#}r_E<<ht!{G8B%|iWk~!>*3Y&+CG&j4
zddPmgC!(Jx(&v2JA@)Tg>$2E-NSz-oL*$<<L;7518Iu1uk#$;OuCzTRdH$51d6jOZ
z7ZZIY$*{6n#jGmgU)^#Iv!=+X2$^Sn`7{59j)U~Qk!6Uzsr8#$-cn?~+gjgRWS(tA
z2Bg2XmQ#{{H|b$JvxB*pxwpu3!GR(JvJO4Wp0@Y49a8^jk-Eo=^nHrRc|TiZKQ9s)
zDam`W{RZ2flDt<)kKYj6A^ESh4Dq{Oq|U9@L*j;;w~5SqgylO$`n^Xa-&pI%iR2qE
zl5euefcQTulJ_y|Q=)&|dWbyLGGyPLw*DD&x=6ieEk7qRAn`LTL*(bp7p#ZqUo>Y~
zpA!2^)<fi%Mf!PNWL|IE4za%@($^fzb8Uy%-xG=dNMxNBimc<;BJ2E}NZudKrPePO
ziT_n(K;+-d->rw_St*jY($<+@WfA?FmaB>MS>3E@{n{e+>sVh;<hpAl62GxX|4nUg
zF4D)Qwr?&nAo`Z(79#bwvK^9VJCVFQi1@d$9U|{261TJU5Pe&bK6f|ww7!GLfcSN^
zy|c)AbhUnevzy3(_;)uCFb_0)i1cxg$biU)SWd}&dPxtDv>lSaw|SI#w0Vq3zT-s3
z|AN#%Uis<kB-erHPqv&AeIMITF;BH0#D1E2y7iEGoN0Yu>mmOA%(Kk1&2!9iMFzzG
zJo9|(Q!<Z>tcTRS#Qy!Q|0g8x0LMe*LC!nadPsklTZZ^uX&GW4Y8hg`#xlfyt>x88
z*5P{R{Wqk)8=MCs53`&S{f)NYBywHfCNkd<BG>6i%lC>5NS~uE7b1Db*j|X#nIJoU
z57-Y<_aPDc6x$yax!*oz&J^iumdNMrn<9DMGT%1e5y>}KWI*H(EEghq=E=_eX1@I(
z`UN8MUT8fe-<P(3WqlzMzsPpT{Jytbi1_~?JM&y3BL8eI6<LoJB6-$mmDx)~{HuuQ
ztBVZC^G$t`xJDwM3r%e=MCRGtc1YZomQynSR@SGa-)*ditY;gMzIL%5QfD_2|J`lx
zAkyDHBKf<D_;nNU>n<`<;@3lZ&chKR^^Ub4#D0QE{D~s-Io<LZA_L;r&-$}O`nbsQ
z#UgnJT0c}|y{{D+ko?z~*NgN$OeAi&NWPIG>o8iZDNYvgdqiZUB>&^m)7O)>L;Rnz
zoRa!aOAnthr<*hE53xUMxe%%Qg6%1(JInf2A^pGNxI(0#S7nE@Mb_snk+^pq2idm|
zMb>el^^o=b+Hy+rePcaD{?;-?{!XOMV(TIE{K@>;{6)lnndM(C|7QL!(pTv=*}14H
z(pOE90a@4DB6;eFoY#gTeq|y9B5x|v*XAPqY+-q8b6b&oJJ`OX<y}Pb?`r+-W;>Aq
zv9~w(6j|^8*bcFGHutt3Qg1)&yNdMH&31^rhh<2fo|Yl=Im9x=eyC;0dK_c@vDT-=
z|2W%^H%}1h<0ShRB6UxZ9iHyEGwh#|{AWr}zJBJ}B5~*0A0l6988YwwmLYjA70ENe
zyv+8L_zki?C2^Nq5AnOgGDIF?xgsRrQ2RsTuM*k!>&=_3zs0=O9B$rb-frGuju1Jo
zqb%Pml6Rc-6Ghf*vdB6;D$>v6<`W|O{+!5w_`N8SceY6U8zOOU**-_)diz*pKA(vA
ze=bu0YmvN5MDi^Y8L);}vTb%hsV0)Awz<B@_&21V4V)(>`i9mQBKAhM7b5GhvF!LY
z7Ll9Se^p3)Q|D>s_>|b2TMwzz!v33EpOSTNB|Yoj+IEP%v*ncJZ!10AMPy%hvtKdN
zM?3pL;@exU2=U)j{?zMe?&WyM=gWQ~eqGJ|MdsI2B<^7IP!a#5L<S`8X!98B3z75I
z$9Bm1JY6LJnU>EMneTZbBPIF^?SHXI-An9WZh3$?P-H;j28-0YQe=OwwH;FbdJ(@H
zMe^QcdrI^-Tc47+;nI`mcH2{;A0a({ciRr>=U&T&i2oSdA?yC2NdFIuT<6n7)^(<c
z|BE66BEKxs_bb*HBYEF&+}rj~NuGDChs<k^NM9d`*gv*CCHhaShsd8=hRE|RL*xaP
zA?x&o^<Rqg`K|2`|L;Wl`N4XKezC|pEU_M<|Jht>Jw*SDi2twFL+Y)x45{;%Wk}o_
z+wr1N0pedGKUgYKuZl?CHLb5^eGQTMuVcBc$biJxvz(GV4XjUz|GL(v#D9J3A^kM6
z46$z{a^5zzeRJC({##m3N!-@fr$oPv^z^f>?S+VaJNs>Kzd~excCsCkzpc5e_5XzA
z+1+{CIS)j?r)7xTQKa8a=3cg^#ILjU5dXc+E+Y1QMb>kF>${oV%>zXIdWf8hL##hk
zB<^s_M~L|KwtSR%w20qvA_LOL@#YC4aVLrRonrl|=4s~XA_EeChUG%Uex~gZxvxl_
zv#d{v{cPLMwH=~A&pcmb-!Bol{|qv(5*d*E*NDVjYkf-QcZ2nXNZc^l@w-_h->oA0
z;g)X~xi0P%8IXR)i|o%tk+?}Bc^(lNkUUdFp3A0N57EyspSAvfL4F^3)pcgO4y68T
z=IiDgwnOIgmSu?l+t$BpeK8V0*M6%)>b&PTi2S}tzIoPHg!q4Ce@LE>%}+$^^DQqB
z*|*QF|I+%e%tayt^1Sqe^^2{C<p0q!B>xg~sqMem4)I@Rxft<VZoic1f3?0M#Q!(f
z{oQd8{R+z!A?x&~{UPyxS*{52Ut@b-Gzt)ViS?_4<STVPh+N6?>Lht9JMXHHxGL_e
zs>r#iCbG^oobP{0>eqDrwOtRQuO-q)9qU(x#MgCPO7!)dzrO7$u{W^(|0JoquIv8a
zK>Arv{qXtH$bF^coNQ!0<a4r#<wC^2ne60kZf+{FznfcbDKa4bTiV{rdPw{>mLc-C
z=62Tq8<KB(=lM5eUTvJG5UI1H?J3dkWIaUQ*=#G4cQ@N1>$HdUdzzhWPl?}uq$giz
z+gB&aySMW~<Sv#W@;;VVg~aXa`28GTjQDr8A4J~YGDPlX86tPLT!@_ip0cxk2ivb0
z@jKLhkmt0cMCu$ZVn5bA-ue?n_Vr}zPqBQu?Ppl-XP#sIx#oH1`68nj>Ei<VQSV~M
zUE(;1y}!uye3|7zwihD)gKdYb*Oitb_Mw&`@>S;5<~8QEBBL0IzfOMiKg|9qvEL{?
z{SOzBZ?j(^V!z#Xh<u0TV#IHR{qGdn*Sjr`6zT6?kx_`m-)H~P<`~D_@3=xFZoKU&
z(ND0x80qst`xPR2CfW{>AF>RQCs{5;?2~2Z+)uS1r0$a<_G#At6SA)}6wmt3bRJ0F
z=dGV5QtxGv=jAtSe^X>Y{NJ(+k>9qwI>>b}*ZERX=RNCJ2bt#w?th;1Lh5{IIVJj!
zZ2wqf9p_tKV18yUG(Q)a&mxia|6XLIM8CxTKidwex72b<^uO4?Ok^Iv+7BZCZn+SN
zUm-jE!(3_p>9~~m{bfBwUZZu!5|Mh9L<YoO*{mWm|FuNctET1Jmg|}8io~sFu5W!K
zkpZc*k;wJYOvJyrxv5B<mLluf%KB|Y>TYkjwYh`HfaGao?r830?kqAOer?TN%w5gh
z%-ux>q~CVdx3|6`B(8(~A$2>7==U-^+YZrpu?+Fw$1-F+x{3H5V0}vb540ZQ*V8gY
zKG;0e`jo^UW<5miWf>wLE|TX+%e`%f*pD)gHjlBrA|(D;`IG;6k#lge^Yjtf|I<X)
zp`Yb*MEougiNDaiNF>iCmivpu4=@Lb#0{~0mF4R!4-?6Eqj{4^zFREcDiVK(<q_tc
z=3VC9A|oYvM_ONu_}$|?qs)8F`^?ei7?F{Z{>DqsydShZCBLUVW<8|d<CY=weZu-D
z&1vRS=F{dg=5%w0$bjU1R^;y|FIx}E|Ele?&DYG=MMg2w|9kQy&j*g1=QzmxKe2tj
z?F-D$%!MKY(&y)vA@UdIm(~{|eqY&dk^Kr0zprgCM*P09A4LAv{LcK|c8L84%f*P_
zV*8~;|D*MV$oej^y%@>!v;5#v^B2dZ#J<e>l<1dR50QVhT!@_O6}Cg}3v29<#g&K*
zh+k#PDal{OddNK2v<$IVGuJY!+ny4?8qyQLw(SsoEz1yj9n1fQ<f*MZ)T`@!ka;$+
zoD%<atuIFW){`Gx-`v1)ka~?QL;N;0H!?T2y%>pWY(I!xX1NftZz4POo7%sb{R<KQ
z=Jwmve#MCYX7+=~Ei6OUbqmW7`<9jq5&KrQL*%V3r$pb%_HE2<&F$>JDx}XH9Je}1
ze{GbH`)FHpcjt%b_Z0E#AX0xX+Y1rD|JV+ZJDYop?C(CdL*)HL@^%yP?{0fa{0_9e
zhuPCS$o?tuKSX+Xn0dJUijn$9$PXTA|CHoA%J!p0<YO%#XZZw?k&=8T+V3RuWcxwv
zeJmFu_ET(!)IH5{r<-Tko|3pTZSQOLvtLTq{~YTf`+k9Wi8(+d-$0S^Z%Dnt&I8GN
zxk%limanoMGT&<~L%#3cXgMW*H%rg=ju9ex@3bF8zRNPif22r1_gWrpJLL0byk&_0
z10s1IwEU3mDVfJ3_M2imB<@iW`xDj|B6X(O4vBlpeA;@5{u#><dAenYJVT`3^VUQ3
zFIa}iFPgJN<~`f;Yv$|b8|IrL>-LVwetcm4Jdt{zh~!x);`gP<ycUTJ7=PyzslV9z
zLd5=~{eCi+m_Li;`AuX%{C~F$>E}<8K1$kTeN`3lTT|q`)DjsG{W@lC>r?Xg-iFdM
zzs9yh;>twoG_~AJq`#Jyx3Ije<?TfB?J6=LdE1Nh-O2ittmA*IPf7jGj@ws6-cQ7T
zf9nqv87cAWAw7K_CQ`q*{UGvjBK@3XeKF#Hn&bM3)IZzt=Zn<4z`RgoK=c=x7h7M5
z<m)dx>pZ|5WPeEgE3Lm;#Q$2`3z0lG$j<z37TJ%x?GIU((UuF5zQ@=Ok;gjjLF-fU
zJ?T;D*A|}>>35b$AFtTI5b5Jh+3E9L$9*7DZ=T3GULca^bL+n_zZ6-AMIw2A5b^s-
zB;PM017cqxlJ^gDrAYi=B6X_ns27twYe~jlL&RQJB+vRH_C~gEB$9Vyk?~JRd}GD4
zj+;5}<|1{rbY951ZY$Eqjw1g)WKY{6_Wy|F>ul~Vl4oC$k&^j!wH}i1P!WAEk$E2@
z5`VnNfY?tIsduVKKm9D9XI?C_59K2L3=kO*{a}&2L#&6K(_te1H(Fna*l&`Z{_YUz
zW2EJ==0uS?4~z6WMPxwY9~Fsv((*LhA@-+4;-0ZQ-F8Ud&sjgyeBO3QU$0t*<atfx
z`j{uO&!36ZStv3fdA~Hj7K!`Tc8L7F<%$sh#quZb67v`5SuRrlSIfVPjACTH{<0rr
z->d9IR7UDlvkbA<vJ6?51|oSHTAz}2X<|JjzJ<B9_5X&<x0UmxWSzE^9{YB-r^Mdc
z_8n}m2+7yRaXXqjiR5eRJP^NKEEgkwyV`G6xy=SdXKP<u9J-EPI_2N3FR>hPd>y@1
z#@aJ)Mb|DTHV79YN*m~<J<gSLVU_DktX4}e-H51-p#)uRbd}lsGIly%$~Vxv@w}AA
zOFg~oj!P-;O6a+SpUpCtoLG%Zy9(#237NYS+me?u{u{Dtxvtt#sFZU#lAFs&?r<ZC
z=!tW+T)rq?_TRcPwyA_OSi(Ho5mu7#o(^i$L1`G6TP#@p+<fcMMyzBB&RxhdiimMU
z^q^X_R!VE7v{s5=U0kBcDu3%BCdf<qe=klnUQTP}%(uL*UP_pBIho7JT%Mcicnsq)
zjK?sZuxf-=W9iD`959(q%%cjO*38YL3UzDeBWSlX?N-OH8{#JU?&Cy6yX5PEbx<r$
zK7wik*V9WotYy(SE*){HM$%Zn%CT0|EsYJ0q})woKa#mcr&KG62&P)bRLhuZS-x?$
zqCV+s=hkHi*6d*ZZR!4&h0MvxAxqC_JT3?_mokOgtW{hKrCcA`6@W`M#M=19yUO$&
z*Vf2*7tZl6H~reY%g$eHJn17MF4%4)9fhF|v0*5~B{r`lS3It$u*RA;$fYld4WMN7
z+zv60e8<@C=$Ur6AZa(kjv#4A3@tE}qbtF=0WXz#X_sqq2>BZ1u8&xpaPCCdfz-`5
zIoIjcWFAlEXUNhMzn)lopo<$+Hhm0n_lXrMk1%u{@f$^KN5a}8R>NA3SebV%W0tYb
z`4@C`2#Zx~9|j_NqN|OyQ!Ghzg;*u;oO{<PrkV?jRJ9S?@e=R4#k*Y6$XuCx-Fayr
z^TpbZPMh$q0lj3c@vbL^YB-limgtnQIBSurGLybMUZPrD6-2}hDt7;XSQ1>qc?hXu
zQ=-ZGQHY4RuSKCc)aaJ0_AJ&oDortbMS|$Q67S+TMlabXN$Bv4^qnFz;_taaak)g@
z26WYg#^W-LBN|~PWa&;==dh9`N?uBq=w%d!_-LDebGDaEzkcoz=C8q5aEa5@1Z#YZ
zW&N=@)k)upd{Ltw&h>M3dqxB<mGiMQ{wFT6m$B#B@y@x#m0TB>+SG_^HPU~~6uM!E
zPtL<43t_Qk9hr38`QyA~*EzcG_(d;qake8%G#OWN_>IJ+KGjNN*-2W3PGeW2cn@5n
zp}6wmF!YR_iS&fURaO^W_EC;5*0esUTH+VidA5l7#ks1+s>MZDj^7U<(s&$@_OX7%
z#wN#Z#U^JrJe=e3x2NY=gP2x?MMQ{EXaY$iL0lg(w^;jVJT9hK>^NZ&+nuCwSBXQ@
zo&LtrP+Y_@t-E6S7~<NBRg2U9JAUo*UEvqK#A(ltK+HGh#=DqN++(}5IPD`MdgiVY
z_rzG(9waS^^<(<68X?A=Fm^BwT%7joBZgi=j7i638t0&A&Jd3&v&i@VdR@i?Oa9_4
zp|wk+dm4&q)goVZ+OfvNUEE{ire8bG1}-s=_>ha+b)2wxSd9H>5I3Gk#k#~<i+MzA
z`Hjy`M4Xr0m6B~9VU_6MSzew&92g%vv66_{(?hOC9KCX!ug1DA(~1wstZ@wacW;F?
zQbk>|#6gb(vXX|{W9>m$B_>^kh<Id*U5J<UvBnvi5>vn>Ziiuwlh`hL#xFYzv=*xt
zcY*Fi#2R$Q5FcJ~yyKdlPSRNW{1q^rh$tC{HFmHC-ACgg&LLHPVdF@BN?3dRVv`>r
zbF>yK6s>hf*PV9bVv6G(&ckAvND!AsT<j4W8ycr6?t~#Wz%Tn8q*_dM8#2dA#@UEP
zjLUf-hIkiK&AwsebaAe-O9NdAM>N*u$q<=kd^d=s^{JNK@aP~u&!dfQ7-Dtf`e;ws
zLy>@&cJY#r2oab3EaR}ov|=*RX<QY_OH4YKsvGZO5#zLnUotnWA;$e6I>=^8mZ*`P
zgIvBkG&DYTfrAu>E4$PDx62@`Wn_tbu`>~y-L7-9%x(bDB)a(Y>qd=Ozj3^)9wj3m
zFHP_Zm+_G$wl<Cmegk>=+ts)NYivMVIPIy{gX<`Cai@;Q#yDY5Qse2U5%-O_84*z>
zZjl(`l#HTKT<6j0c-rWM?tT*dMMNCJKcYztWl@rso-~wwb&D&W8gb;~&J~w<TzPRo
z-Xz~Gxh%KDT9Ke5hV1f-;=II}%=R}|C_cdBX)ZQ2KDWwoj!P|GqFQvHJ;dY|ERJ$)
za&~9M&?7ovMgyZOx{r%JyQQIvh;bM~7kAM*l#C^gNBekqj}3^`2t!<h@ktUr$5Te6
zdWO1j^LUZ;aYv5_((sFAienaq;z-5)C4c{mjrxWKi+f1}dWj<+VX<oQ1Ru|Tw_)ub
zyU#4+lO(?%W$4C}D*J52xpABlbn(q0uDAFK5GOINkJ;=;Y(PuaFP1&3#m+>h%j3{6
z!Hy)2V;0wb-2UUzjYo|*tR15b)+N^dJkrOVp$Cn}ag6ngg^lBwtusyj75hePtY6%b
zT2MTWRC$~nT&ggwn9Lv=ipP}Lk0*(Z<Gop=;w5f<@e;E<fI{*35&IS&)?tm?VVzv#
z2T(0LghZ5+Dn8F+Q)2z%jE`i#EpU!Q6E}eP-6#KfR7Py}X}}s(qed)o>~#JiHN+~+
zC>~g%P<3KsC1ZjuqK#-W{Nfy-i%z3tEOvgr@o8TPzaJtMVaYdK=C(h&cxH`FiHK&*
zD0Vu#m`D(YvRI7}X(PKJ5aam7UdGN$q*|6Q-cciNv^7aklZd#i;zKxgCR@{JGSc%d
zCKC&reL|p%k`We@junq4<LZf3yOJ8Q?6GQFa)x5NXOJKs1M)jq9yba~PQn_K&Tduc
zMv^M-&ygzD;CcMwBOz8Yx-Uo9o`?y&i)wK##CM$hLPXDTL}ORurjS`<(kxSFN|ul%
zW*MvYKyKCIh8MFeCt_S&wOB*UfA-|V_KnPO&BQyLBf$`KacJUx8c(!wgNh}NmzYsx
z&R(Lc+)H*>iQ5B>N4qhZak#{1e5B8+QMZf^;>%ArTp~eOLyTv=?C|HT;Sx!!N1-TA
zmTaA)5XG~HZCv8KMAEo=VxwZ&qrX`3Sl#f84}#e9*pCQ{uMc63^Bfmb_7aOt$#_5u
zmv|S;6c4j;V&i*C=(c2uvsn@w@8ZP9nT+|yGR5@cj7QSA`DX8^mVHXb%*hf{h`RBv
zJqtSnYvgMmH(FL`c$CB%x2k#c67O2#obToBh$Z4p-i6Cj#Qe*fq>5FG<&0%|8(sE}
zu(%A~CM*o$9CHi9G7QUdej$cmOu7NqNxW=HyAd&jcafz8Lu^VXZcDLN-Oy#vaIyPL
zI<AFiC_akf!J!F$Wz>z&sMw=$iI1YV6~wc0tVVnW#8Y@&@$qOC@3Q+THA<Oum3T+e
zu*NQQL~IZf<fS8b$JoKJ=3OFo96iUr#mO!uBD#tjS<E-Pr7_=lLg`FI_N*K~i1T}F
z%lKWMpLSdF({9W7F_=}0AML`qef(aWTfg`qjnf_<;Mt@}HHs{w$TBLj5D~Y?0dx@E
zcZ=V5qf`DE5s$y|RV(`g1rarJCt`2JcJaG13F6r-ZimBSg=jLfGOfzF#%DxC6e7Vm
zVzVPaSQYA)hLv~O*5;g}m&!S7e9UICk(31SS(~*RemO(C5OK+NIu`bC)+m|pFPvj5
z;vLrVs1Yvwk{_pd%UqT!ku>~5$6Cc!#O`N%&ZOf{0h^F2PG>x!#0@2md?_w1Sie1T
zi<6x&@<o<1)-+<T!fy_#;+Vx{5XUUmCH{sH#Unu+hPaEC#KpwjB767WVku%ZYQ!ZO
z{YBDAbP%Z;#GeN^SIJ+T;_gGwam~b5#Pt?)>zV&6PwxE1CtduCGlMDACh5Si#@|U;
zh@o-d_&Z5_&D|jC#>ojoJfW}#*$KliF&9xSmnyp;A~tln4$5*aC3yoguN`8{h<q`H
z?E5O6wj<S0rdpf7)MQ!WFDv;h@h)!-k^bTVBWt&SHU7kuH6G4UgLh40T5(zNQak?C
zgCQ>B`14AZG~Wh`82eV9?<&_pd*;z84EQA_N#8Lak$;!_YgKk4VwrNQQ5JtcLRTmL
z6cu4al*T*iX3IqSzM;c8?pLw8@##DRYh0o6r>!Q;GXCK4_v4uFip-N}Bb$C0$kINy
zIB~clU;Ke=JZoAf*Hv88u}sxDO$~B}?0XbhqP4k9F#C&QzIgr}CA-Di({ntfWou7s
z*>dtOuHW)}s@Pv*d*)NcyJ!->7A$8AlEzITzfti?7r$M%BXfM|>c+eHSR9pG$@o~z
zzLznXksPFW>}gMp>=S~m$iB5<jjw(22pfwNSI^EYPAtSqYGluoyo;YPM#f>_a*2Cl
ze5H*i<mI^ZAT}O42GUjB9->;TM$7y~jC1yFkxrXnsKUFf4QfQhw3q_jXE&ML{pw&k
zh=}-;TK-$$M>xklF?;yLFSaMXrfopoD!FwD!}Ynz#25GYXd4y%g&~(<B2BhqTF-`+
zCgbRpq5CUWHy(h_$2souYezL)vaXnAJX6Pg{M$%HY<xdIBG!P21{mV+LVv}UVTgzA
zc+`k$`NI(N2x|lC#^1bNB|-EugnY5d@r^Xz#ZJdvxf`v;JtDR-j!$fD>|pkB!m^js
zM#tP*#jiNYS8n>teglZr$gNPMpMy2-YmqO~$NokKan`y-1l`x?B@RveF(N+wqFQ{T
zk0y`IrH?Bh{t}%36pBrW)rjY&>_=Xv5Ff{pG}b<T9*bXuH;4@&X`I;jq7zLHWisJB
zjd!srajvrdVyet7J}+ZGVjH8Y7BQLVl;SN}zx<Ex@uV7i`6<P-%%qAhI`REC_Gnlf
zhWwhwUXrw19E;pb58lNeS|TjAA|8L^xA2%k{O-6rvGFA&{$SaWe9?G3F2ofT8(&5*
zWjMzLkv&_WiyJ_EF^M~M{+<)RSVy7w!8tanFLTRoJWL_`K0~MRyH70BGZ<Fl7c+`0
zU_vZiOd3OMV`P4bb|dpA7~=CIwlTIWI}G$3S3vd$BeGP*IWE6goOp1EQ<r_(6OrvI
z@8V#_71{$a&Ow~hI4b%3P?Ow>#}WNDTBGs!q`N3rGNzyX;{vXpaDFs5kNo1qmGT2e
zC4Tjb`L4h(F4(!`%dU8IapR85`3RDRbN(l|_}M1bG*&2T^kDPiF)#mDkB%{O43lU)
zF3$Mrpc}gQg(}W-+=t>vg!lm>`vrsP$NeB?6pwGQGx68W29cRg<6Ol@Li`=}&)8Zf
z6UDPXDpM_{8W%*i#H5dDMbFtXp^NQ_pZ;RLv4e5@${MF+{Q4Gu`fWi(9abp&6_&#g
zyBfzfd(_CSOaAdUyLy;H%q@05KCR-=EY5FjG#U5A*agyOAIyl^jg~C&uPox{-Y6NL
zR`F4p-FIWH;)TU&Ktu7go_CJzj-Opam#s!b;GBJ05u5#U4h-FS$u8&ci*to@_Ins&
zHZ3NT?OQAaQ;4qOP8f?_j^Uoj!b|=KjriCJYdi%-mbfb7D%+e?D~N~{FX3Gr$2c1u
zshj;>7r*Ql#JeW=h1ib0j2dwg<4nfGPWDe<=xQYM7{xAxb2pMk_t8N-=EXUPugLM}
zoSkuEheSiMF8`e+I|o?fOHG`sIEk^L`CCT(1{Ej!#^^L6;vA5EYMex76jx(4tog5G
z@!LZ@mPPuwWt7A>SHkigbfUHFoaPNAT_>iHs}aAb$IRpS#8-uAtraeDUZUNo7K;-f
zZOvj0NL7QRb$AyW8lMZ@m`r48NtU<`cf=*W`Ntl`EjRY)&FGJOu~Bhy;u~%}B4)c!
z?7_*#|w^<!LIN4cLs#_<e1E*_5g#<D}Mjks}Vw=ceMw4~?wM<y}B_^LWI!s79q
zYH?eNt2Y0y{@Uj<$DbwQGH93k$}@*E6t|#`G+7p{MFjcE;)@P-<D)IU9yZBaF+_sw
z`pqRMiLVMdzxYx6=ePn`jrRF(2|1V8!5>4+O(woj#zD%y&QYz~-@ZBI5@gO%gO_Ym
z*rV*)qHf$ChQt++yOG7&Sd3q++E2vBxr(d1PVN_xYW(^!K6hf{*(~nf*W@y1w_L>d
z<`AE1%OfK0jx1+9*TtvH(-ewR5*J<E8Oq~;L@zkUMukiEojuMAjmJFVzL9<5AfgUy
zKQMk>V9T0B-B_G>iG|3mRXji@{|=Pc?CV#!L>oEhG5;M~Jx)%}S{Ch6H~W?v9fURi
z5-xwM!7|0=7q`-Eg87J;YF><Y{HBn7-={|Mt43JKm+9i&-=_b!RQdGzcYkvsOZLeb
z`ItidE*f{0X#6R3@emW2WNcJ?ym!YCx8Wg?G<sn}<IWI&O^YYhH#q`ve~!CtTmduq
zRLkxqxp!s6#=nk<hN|T9#h+0lLEMq1&}k<YXA=%wH#TY{*GzUw;wW=%W&Z{@vT!ja
z|G<{mYJ|nbl>M?ug7~n>ek7vjc#>&PS52_)87{dd<2s5PN__Ce6GeF}A~T9l#JD)i
z2#dRISmVJf?zUIr7c+_<YD<Z&P5O~BEyV1`9ftf%bbuIl$G^Qq&vB{6yV#?s7Pp6J
zGVby*t);Y<U3vNRTzQ*NqYPc#3F8v48Jmn@Dq+uJ?Gd{86%50W*y;biCuU3Xw>x3f
zC1Mg;Dn|rjoQ?g^b<9VEHG0YGNDvRCv0&v9i%Xo6xNpP-8%H^AU$GyR^H+SF67F@Q
za>p$Dq>IfXeWdCXH|gB1ba|osWbUpPziPx??@kt?Jq>05<T`)1$X|$9<M_l&{G;@;
z+<A_JSvjiZmVGJLTg;<&%p+1or{R+G%l_n&6Jz7^w~Tf<Lz0j6`;-eJ`-Lx`FaIt+
z7w8}!l;YI2N0<HP#$>V@(N+F;QCx<QK2|sDKA#lB^tcQnK|TwHX5klW@V5kr@nDlp
z=5LLYCDuOsGs6GV*!{<Pp51qSXD7RKGO>y`-5Uah_U^Ey!$J*CRZ$`}%ysXCRUq_k
zm~9BfIF*uywzR0oE0k8QFO!WmOsXi2S{hJ8Mw5h0H#AsP1T-O8o7AcmwS}N;6BV{@
zE25;E#we;y8XMc~^YNC>`Oa6f#q-B^KA-P7=Y8HkUhg00e9rfC<CzX7_)_AUa&{3j
z4@XpwGyHUJqJs$B?TrX1{RlIyH2~o};6abB;srhVSB-Q)Z!2QC&{W(F6O2Z{O{$4*
z!sH(7%0%mGhEd%fTNfc0xNvp6XNf>_*sT~U-Ll{uSToT^-D3Q5GXw(W%?)l7X0fKC
zFu7XvR;_mKChm4Cd<m*PL`;$JpNXgqzt2RO0qhXmj>$W<DoG|E_1&Ak%iO%oZU9q=
zG<)rblp<R2kcEtJ``05N1x`{b4juevg}H%{UYb|Xe|wYf)rlvgkO*rq^hP7t!-(_5
zw=?@fp@6l%w=Nbn;<$LKR+jFBn-4YYtiVmS>&>guGK?=+<*SPk(9hKq#`du_ZktN7
zzEJ~bzuDE%hpi#D6mwV*lb@)w2BLbXOt$@D-ih8Z{s$I7UIZx^kBSnTZ>i=^(#D6j
z0lMY2wu@%Jn0K=|MRawNIzewv6VxzuPIEA_RR;kn=hcRxH^05f$n%S(>^o;K>+%xA
zlNSgv9#<g6f~;Uu=f%R|ot1GMPP1C1jmf6lkD_O99jTjfK;tSNctL(}9|!t9367h}
zV(psmBKl?@0-5#EX<hacjtL58TVwC9u^*T2(%%@bD_Vn+<KXsay4w+WLd#NwJ=J+|
z)4W&ZH<Oz;bwcM%sl-l`fNpXPVkf|PvOE%PMMMSuHXCz=jb<};9#VHZm3~(;-97zC
z-pAxY-(fct&!e(E7W8CE!`#+-tR^E?xE`(;0QXvh*;QA4ShMf*C4;rdE4#%+XA&UW
zW{oEZH`BpSRnCa$0+b)i5rz_P)a5sAb0eB;AS&h<VIn&TEJLw=#yG!@%R>)-Ft|Ze
z;AS#q+1D?l*o9?)%f92Lf)*cCY|DtUh9L8|Q^XE3HYE`F9joDBJFqHJO}IB<PGWMF
z1&x@E6t0ob&unwY5RE^b6%^_R7Hq80H+b2Aixl6Q&da_}ZYml1vVK!W?XeNPxp9R*
zrdXDFElACj5qPbSS;OkAl?aa#9bS-vVpj`X<Ci8+=rdR7p|V5}co>BNC}+mv8RhN!
z#l-pT=4SFyu4b(7HqT~Ttcl)mI-Rvf?=%qxDsejJOET@%9=u-7@P26vz;@&$|MM^j
zgti#SL81>O`EQBxCCT?yZop@Na0eb&RPvp5{@ah++@r~+oT@a#pNe5|e?sWSI%7@k
zvlmn{1dz+Eu+>?Z8{d!o#0_dsYo|ve+=8Af`g654?iQy@yC3X!=Fs}Mk4<0`btRY|
zjo4ilytnZH=>jGV)0x`M5icSQWnb1$netVcY%IuH;qR;~=huvwR%%*QrV3h|er5J}
z7}1N>Z_pGen`f#jK7CL+*LkeEA&>jSzXHHp2X#I`d$%VPvAb-^T_F!8@wcM|v8)?+
zqB@|w=|$jt-=cEFhSnN(>HOFYt4$>WQ_K5~68DEGvx1!Re78zO4;j?*<2@SE?qBUn
z#XYOuW$uGOYocXoP&qf{=4xMQII@S?78UW}SYdUc*yCVsSe#CMuqS$}@1Eqk(R1Y|
z+n0|HlL+R!&06&6UO{zU?7Z}m<WpdTtApS4H`XpEV0r@cWZ#`-07-LnA7z#K?OK%v
zlI916YaeB2^WI!@T-JHC!6;Fg47<ZRv5u4-Vm+~&ml42tq(<#gc?EwARcDBpiE1_P
zftPL7x!Q)`g%^!$HPO+C)i81L=ys1dlH-U<{m%D9*yuYaQ6&c$hx;F{fmoESIJuG;
zUy8t~Df<}6Q`iorH9+FiM+_Nu#Fg74t_nN{fP2ef#vaX)0VV)+H7kcbCDz<NDn0>c
zJHKJyk$owG4z?m>$<w-gxh`StiN~;ekkHHqLsW+=lOKiG%~jFtdi^dRC;5@QhSl$}
zZf~=Jf9Tik+Xc%_MOk~qixI{ev2JIbcQjkDP@MSTvhtVL+JK$ciUg2Jl|f$od+AI5
zzbjrQzgc9;jY<ezretq64;^{@6>1kyC?|R#`h9)pT?e9oJ&K>LvqKlE+^L^ei8D+_
zKsLR$=*>bq1KjI7^n7P89QaN7VyzAbfyziu)IOvH9z{xHiSWRtY~+uHfsN$&PtDsc
zIljR}tgw_#R|<M19S;6b^FCP@AKw=NY+(0lxAO!)dPaDdh;dJXDdKxfgai6(*bhXM
zQRaDVt+R(hfAxDQsMr*v7vurhCdU~UkH4q+!p0w82$N1@I1B|5`@!&RSv2K5RlpT=
z_MJ$>Ly2JQnN?ei&o&UF#Ob&8omHU6KUYcwtbodXx(*xUka6r)ePy$$uRNPL>2@}C
zQjYcD3w?)|<B#Tjz7G#FY^ORJA<5azv%~jnoZe1A^%}$u{^DN6_(RS1$tGe|L5sPq
zFdS$SpfuQW{Kj-8d7ZC3__pTI`ty-O`E>QO>4f~9X0!TyvVKtKFp8(IKhp@Jjys2J
zkYWm=-fbd_m4CI)0HVPCkw<l<fxD2`b2?|*W_zs8*6i$Iuhi=#Fhtawaq57rE-w|D
zZ5v)MxZSVyqWu?-gjnB03%11H!EaC@tPx+_yax|0vLNxTw|;-Ea8Cy`B$Ev4>MVjd
z?{mSbzH`-BLeEpY8h&RDz<|`O)na3acJ)ohrsq30lY+%XWAD@;7&&V8r?H0n)w(y0
z;R+(Cc*l~lW$!7v(XbCR<^8T^n+4G(0yvRXfci*XoaX!4j4V)~3|AYSgQ2>}2JgdK
zb7&ZwC)=xHM6%r!C~{}MPxrjhVFkH@rA00aos-Oc8c}CbIq^Odcp}_{jVr~Rfk12w
zsB^!4^5Wo7Yh9jSk}EI}b%^q6VPxKH6sH_#(<A=psE&W^E#MaTmr?!`Q3gV1F9jJ^
z1!ce6o~72(#!15*DHUGX{FhQJ@1gsPX1lVZE#HqnMsd0gBE&HBvVtH8)NhxfSKxWD
z47E-BT8s--bhQawGz=_WZ5Xbkgs+Hk@sG*0UW=>q8lY)Tc}JN~j}LO^`Wg1Q<<sV+
zF!pft>N9m_TB3r*xRp9*VPkYfz?Fseux&xH&O|57D4lfWP>m5g4pH3!<;AnJLiPk%
zYXTk{#R27fJE0?gLie=GA(j>Xa^DdKUVeVtJpEDU=ely*3pQlLNo`Sic0qvgI1|~P
zAzNdwuF$fRusCk3^Yx~r2uFOMTdI@5{}S#aH5s<q)IM{ZvQ`dp5tZ}$Irh6V_NXZv
z&5w!`pH<&j6Zs7^)Bf*igF1;o?JrVY1U`|xa~ySP@^0UKZs|bkVBg=kH5rq0UMg+e
zg%^5R8WL?iAQLff)w<K?t`9sPWa-V-WmME$*xiRH{hsbpp|rF%88L>=W8*(xOggVD
zFZ^n<X%%EVgnK#<*b8kFS>lQO5OK3P)Wt^l4yRAW-vpj?f8WYsDQtU%6zFj#^6X)M
z@(Ua01l`Mzj(syug&PNQPN;Li`#sIUj{p<Kx980q3k+P`qei-)XqI<hW38p89nD6&
zae#4_@iY?S+ju9bP6)r7+C3$^9?@ULZmw}X41Fe>Ka3t1dZ$(&<fy^(UonxP#yP1y
zdZ!7a8j;%UpH5DWvTVwrC-qae;}n&*H4(`zTRisUji&T#wmsrg$#Kz?|0u2$RBpN7
zNv%9|c9WCsD8LhNtW{^oQ6Wzyy+<Iw3?QKJk0(|>e@O;Qz`wOR@00VabykB&1LI@C
z<^@qaQ-rJmFV8pS=$Ur=7_>K$h@{nbuGsfghDwt&wyU#{bJj2qRoKnO2W8)>)pwHN
zuOzwK&&Gk5_UX4@Q1B2U`VipRm%mL_&_vsUy4;DBA{vu^O(er|wJFdV_J1-(olm+Q
z4_ItiO-P^iq-XL*nD<>Vju&y`l{79h8SxEKfv}4hH>K=MCC~QVye~2;-g)-mF;$f*
zCt5t3b(xWOySj*bYq0V79PBd6UcE7pkH6zkfQ|Zz4BUA{z;g|=m|FV`xc|m<Fza|k
zyB(+NmYxq5U9q+U-b|D?UhY)XZ0h?#*a{vJ%dI3AJU&)G6^g((%cZ>y*+d7gVth1w
z9j!kAPT;0LdiD|y4ZNE~T!TT+(uUSacMAt^#MMc&ozrA&d_3!avIcuHhFJ{QY!F{i
zcBsMCJ@gte4P9-3sPx}h6@^6{WN)BbC?b!*{n)t5JdxT^_QDrd5EMvm#kVnje+^?~
zCCS6+5#e4}){VwqNz}0$o|g!B>i5AuzNinI#%HzOTxFRh=w7G1Z^bou5iviu&A6*@
zF(;yao}(WQ#v$UKt0}j*A9>aCehIf%i?ZpFSKN*=BlGjw62N#iqBV&4I)SW)SqV3<
z-2i*=tZ+3_mwn8yjk}^3I=#^dq+UTq+^x&?CKA0*yTyxy`=!|MU26Xa-EwiJ@I=gl
zl9L>X`cYPG*xy+?z1^)+_cEt@A<BQHex6O-*V-I3L=jdF@6x4KaSilTE#E|rKIBGA
zJUG4VJMqN`A4tu+9hN4oH0PiuFs!|nKiHH)%lrhg5pR!YJI6ps<1#xpkRi^J=zodR
zpD-degnF*QzL#rqsg0f~k<$UvMUyM(0C;cAOel!xoG$9>fcyCSs;qhu8#|6a+=^Xw
ziQ=6Zm8*V8B(VrB0PO9QcMCEvNAz)F9e)~rG@f5dwsH1`-CF7-kt1me47()kF~c^0
z4ST)Ww$^NrU=3v_O++}_z&QOeG9+3HS>-E=ZecBq1OHxi3~X2O(pd=dbHamuB1o*M
z`u?rxIgw%&^V3<u<RI{oxQ7Gf;C^L=w`<>t%GdI!7~;Q|5wQ3+`2?fYI=c`}Dds#8
zd7ycS4F)|r{}OrN@<C|zA2ppVVc?1tet~kLY#WF$u%Usq0B&|Z69F5f+(it94I<)d
z;z2|K<g=`hrGN8ANu1iLWZKz8O&ih}!<%SlPX6)babP2g>aZ~ZG|IM9k0y7PQ&jOQ
zU9-*O3tT1sq$i5-v8*a<T-HyOL1Et!;{xsJT3pC^qR%Jd?<C17alCe8d>P=}yb-XZ
zu$LwZKrEAix0ew-U*&PHmFB%w$(%L^<yZyUiJ8@6v&s8j=8ESBNn{D|MDr>{ynN<C
z?AkRDS)~a{&hF}tL-b4$1F7iI<lrSLI5;&?8oKX0<kKGjtJVRDa)w@ZBl$eoY=qek
z-f0&BWAzhCS^7quF@B9Lll(v12wQez8Ck>I_ikd18JWAQ>wTy^?gU`IdJ+NK{==>j
z`vXeqLL!P_k3lTTix)uhIO%%B7{DPm-!adqGZB8OImT{X{l(^ZqG1=&gAF>c!cb}`
zQs-FXa=O_@iyp-y?*l2Cd=8V5*B~Q8BIL|Y{({*cjQA1VVVH0IO#E^75vG0?Y#jT%
zR)FK2Z$y+kPXvjLsf<u5m(jRK0TS+qYfvrOZzBHhrVS&IZ^}RGwyOtZi&Oit*tO4Z
zT_N6M*Pj@8C5E8n<Fv7MCjL=#u$F@FB=XXyy4a@Eey-uiXb1ig%=}=V4p6!6&oRUm
z|9E!AgBKI}G{*0N)LMgCZ8MLjsz<?XFG6<8@-xQMCf^TLc0q8z$tSUNWx?w=PAam4
zhc6KT!chsu<{Cl<3y#TPJjS2s<KQ(11-ENa0Z|_Ha$Us4lP2el(hl#oe{kSEO({`b
z&}3uhwHR4z&<QHto~1BQLC-Fo5c!r5j7&7HyNrC>W`DDJHO>5`hy^4}o@X9oCzO&X
zy<Elyh0Shb+(X^J08+`z^B86C^^G?&$^wH|p-ktfh@FOkDE)cralK))RE=jfjiQYy
z9Qsye2o%7o6<2>Fv3Bb&_;}u5h_Y9qcn7xXL9saIN$8J9Ko#^WAP}b#A2;=50*udN
zqDF|oKaq-|^W58i9GA_ExVoRNDAr%RV9?J-UTo4uMJu4Fsz`K;72?_MjL>4GIcsrK
zD)RS~jo=jus`oAtsQsxJvZA7Fg+K0pUs9UwYF8qN6*z3LzGVS6EO{oYEnNNHI2c^T
z?m((5PI4Jn(jwK(k&L|<UCCe8kj6}Dy(RE`UHTJgc0r(@5(iPS^NHkT^V3FfM8XKK
zv$%~b$WgcZj;;WXOM(h{Rj+sB9ve8IN&JEBA5US!b5mmD*}l7I*x$x@Q+kiEEf8hE
z7u9f4KV=FIR5#fD?X{kSL{o}LTQ3iy_ve>$YW=!A#j^QiC3@MwY;Q(SzxOZueofz5
z50dw41YA9+K_ZneB^L7j$;B1__?k%3hGFcBNm`rRM(|pwyUaX<rkuOA@<Hobg&YGI
zq2@C07f};!EFjr42L?W)xMNUmx|8*tM*JTsj3!l8;P`wESBZ*pA9Dm{@%#hkUFfvq
z@PWRw!}oLqemTlNlg3Xso{N|!n)DQ9_Wird8)q{q0vr(tA!oM2A^-<stY`+=gV=`M
zj7e)#Vp02K-C8NuFfU1($1ucN=|t+aV>ru^<Zuw2?hiJrlkG}EV~AIbmu-2QlBgoX
zSFO(Xn$mYW4$T5JtSR^qopn@d0@0oF0eJ7>$$6Fqm{j(F-oqw3i;dIdz^<TrJzl0D
zjl5zOa!%sl8JVdOzog#^;qTb-G*f^S$q%;v=p`y3s&&eb*!b0m{<&H`MmUT7yd%Kl
z{<?tZNmu_@+;2P+e`;1`KwXY2#zu%puSD6J_^c@>hQ-?C%ebcqk=3)EZ*Ig{l+SCW
z$@}!j0~SDD4y+U=FMt$hH!l?%d)d72!bY6lqti&KWcWE;-F!=7B4D5E=6+7scT&vO
zu3f7QpZCQ*>w7DbP@s+lsoDQaEI^+nV?<BaccM2D##)d1`Cq6Xubmx3woJesC*>5T
z#nrXobH9Es1)uv>0QV71@)y=QkMX1Qd@Y6!&>ng502ui1?@qB>=w09M&@w)w->S5f
zmt>pYzctZ&7pco7Dp<UbsIzqtaxN3We$JlbWN~WkJR;QZJf1(mbxFRA{OSUda};xk
zqs|{{wyC~iXtqWA?z}msTtF)Og5GPj;@~%hNdbULym9g1cchOVEpeJ<oPBW*4Cb(*
zcHKmgKk^>S5mIsu@mWoSfTCmACBlju2c=)4+6mmY+>R~$zhgP)>|i9D0B>)E6>va&
z9LSspV6f;)<9|3(N1avDDh0*PFuo8rsgn48Cm6{jvvt~cD#6<cr~=?FmH=|CtZPaI
zq-1Rl!RSbYKd}rTu30gkHaUpis}-)i3hHz-xrPZm)GnO}OXo@cY?2Eye<{G-Z^TJF
z^8n%DU{Ajh*2EYeS6hzwiH2$TZHqOpmsTVFROZ1w_)K_>(=ku;ersK(%7`mf(2a#%
z@f2%QmT|ROCyRHb6jHs;a(J~;tm;sh<1_jRMwIw{2*d^srk2_%I)?N+e%_PNvwBY$
z><aMz&C<pX$P3+y#T<e+&Jtktvw}RUGbX>g1_s~;uzNcV+2X>uI_zqUU&Oc{nK7+|
z>%Ef9{mt3K(wqqFCW0vZIg>c_(Ww0W=+S}Asr!2`d~MzDEu@3st6!dc@T*HBGL-ec
zqW8n`VwaPYA5O~KeJ9Pnn)v#?w)A58@O)i1B5bwuL4dncXC;Zsa?qYVwv%|yc^(z=
zDn3t>_h(HM_vSdNJ0+2CPVJ+s<y(8hbjRX}Yl$$d5o<viq-<cTR1Z{FKfjv(Yjqyq
zii!|4l~EjQqFi@xPSKhZE;6De04kSclhsycntg^f(VdlNRHc)SV@Pd#5g6Emn&!L4
z&ZF9hUZK<2<z+@3?~liZO=w0QJg0l_Fje6RK^mkdyWQMn*Bd0#1(SQ*>p4bALr*u6
zoF~xo8Rq5{xPnA4gWTC!1@IoaN?vuHvDW+ZHt}V~8J5-r?s;Ws9Ec1|J2^gjB0t9#
zrTlHr(>0cWck3s8a!w{Q?DeL!qJnH<U17IAjze?moOMOKTqcKLX`AH*ddW~HwUSN4
z-i4i2OlG7+%U1>!8GaFY8iI|es7j?Vo$*YBkJtKLiw&{F(!=Bu^DGogC)TyfbA_-f
z7fPs!hZ6M%lgO|mM`XO8z*=y;Ap{Y%Dsui@Y`k2FfY6M90C2tU08UQ+*`feEe|00Q
zSMMhI#HOy^X`&cEt;^kIOJ{}746C2BTT_FiQzFcUX{|pN6?>7FjrjJyV_(kxWkFdC
zv&`V<RJRc;GH`ZlCS(6vRAfmSKiJvfrcFc#LL{8dA|xHiAFuG_Si`PIYIX;2)IheM
ztg~8*+P{`I`i@RzSc-kH`i-^X;C@tgS#d`f1ZtJgc6rXv`OJ=bv%yZq#wIWEMF#E%
zyFC8MoP^y*?OEZeJtAIY$<gU<SmXtG9)K%(VFE3l>xZ%_wI1woCy{hwEH)f7fIoW_
zn8f(_>gIqI_<nP2{v72pj3S<@^?tmZH`}Xg7@|8xPeiYu!V6d4=ZojpmL|V;p-zQ7
zVO_X+l#ScOU$e={pI+vcFL@Lykh3$1FpNK9AvOEJYofh=<B}Ub0P^(8Gsq8U;?HB9
zu~xSV0$t>f$vvs*vJbM|jAzC6lU>0s)>zf*<Qs`v>(k4izv=V%kwBW<qDm9r6jzS;
zf`Fa;6A*2V*<op-gLRbENyAil{Ox{}M@rn+Isy{G$K!^Pqo8HI`d|eu2}sQOXk8TQ
z7=L^6zV(89Z^D2rt_Xvx#@-DePpCYYwgxrBNh3}g!L%aR_E4q9zZWlZ-gAg1rIVSv
zWIB1&3Y5dkz{jo5V}->UBMWYKV+g_|Yp+?m+{^knux|!opCm$Tj9UR=a=eioC$!5@
zeJ5}aL#qK-vMz)j>?5TsY2xc6@D*`o?8mmHA7{6|lLI^nfnvC_0wOd3i=FgxKh|8`
zXrg<uHo-DstcAwoqx>bwE8hP?40+69CH(S|2+t~7^G-0z<{s9`Td?-M3v0i=u?#Rz
zs$=7|xMC>EnLvRxr0N2lmH^VRKR|DCb;%{M>U^=pnkVxCWE)hK!RE|!vk{Xa((~k6
z-}RxK!RgOOR4$`pA1A5A9K2_qJi>FG=jm4>*!#NdyKhUyYO`%9Vx2bZoL=;cvUw#c
z`tyPNOM2l^l_`Kp=sW3PQhrx{&VaS6%_|^(K?W7(1`!dso~ZsrtAb!a(+H%TTavsE
z(n+mi#CON|;J)w7cCgHi^1OF2vF;}pl)TdI9Jh;8-XlHDF4OXbZ&({qvdd0^?^z1N
z70U=2u(1#FZK7mzxLJaHU-0%f%}`b|%#wTIbSjD^*0VgoZ3dX_w&a*{zXq-*jL0~5
zh@Zwqn$10+PxP+KFGXdxcDq$K{nqsI(@SBqp;5o19iX!Xffr5Guvhx{L}SOGEBIdY
zol`@+?B?22^&9fj$bWBL=Cwdk`Ea5tAdynMqk<vpnc0whtDQThxJWthNAedDjr`}1
z_NnVPxx!WbtdlJUjvtAm63+se$ja5sG4Z}u4mJlhyH&>j&BlsrZXb4Q-AxC(mY13I
zdl29_gfhhomG2+zPL~YLHnkZ|CZz(ystDFR4xT^Z-n^<Wfk#jnoyMtbZ*SxqqvA-o
z%?n%!v_NC*6Gqfet7@Gu{GQlQs8i0~!sOU?r|l1&H1?-s$P$3zZO6O-#Y=UwW!87r
z&N3K{@AB6p2s+ROd2%>+EmTbxJU69G_sMQgyJb9T7xn0r_{?)hzwAL&0cFa(#`=y<
zZ=l@Ac^_wepbY%yQJF3T>_hmP8YFEDlj+Yc6UV;D%cMJ<uQ76z9eviovCsC7VH5Fv
z2AGF0eRtU~>Llv%!<QIhTAREFyKh1`?wcGYb(vx|$9YN4s|yTaX!e_mtUJAbaa}xL
zxSzZexgwzEaxw|7{)r|Mnu+kIwmKW8M3mnymo_G}<Pdl341a<z01H7AkZV<6S&p#V
z97|dDy%4^*U$gzFt4Fo_X7kdOABCNIF-L0RZ~LJrU0L!7^$s;KnI#?o3%)98YZ9SP
zs=QU8Jz%maN30mq8TUK)z#6UDt$-nc%nP3_1|~8j7Vbf6Fqvf@YUt`(9~SAVf_Dq`
z+0r^xnerP-X#0qfU6Oq)#p(Q6g2paR{V^i^!Ny63clFexR~V-uX$<2(5ksziUG#8&
z6Q}9r;J3|v_pW6gXnHfTzEftefl&mbchvcyBqn8@bupHAL(aj`_M<r(w#)8we&i!k
zQ}Bg4S7)vwz{qEHQ4cZ27k2wS2?!fng&UFKqOlrob>-v*PAz3TwAK0MT8P1^@Otpm
z?k4{&+0wi}7x!ZO4=v@)kBhhgcB#|tqi!I`SjrGfqJl6r(ziDZ&q6aK{{ANa6nfcQ
z-WP2xOBFHJZ!gnnia1-v1j-{INTuTA!;DeBSunbmOZtJI;uDa@Q<4ZA_eKxLvE}1q
zHZz-JR`1x2p(iuXX}GsDMr)$kwHhQP_Ta=$^!Qr_77cHGd@q$a8pd5D(zjm!Hu>lC
zKsx2H!bll^o+V5Xm@tEq8ty-CNt>Dk+JorbFsXyk)@3L4GtqrSo7bP@KF~NbQo9IJ
zm$$5jy?lY6Pc>ZUU+P1goFX-PlZmqojSv2sLr&ozxSAJ$rsC@Xj?Zw!IYAQ7`t6LP
zE^%7K9A{1bYy>#xZPo&_*cf;x7T7+KdBDPQ-8%VtD*iX9sbSzK2MAwNqibEfD9a>1
zO8lJ-6Zzq9z#3QIzg;7){>tj7_8BOnT<c7f4h8}dV9RT9|FB!9qiN&ai<G!Fu7Okp
z1jftNW$G}ub#ZRkW~;C?8+5I4uB3?fq_yFo5d?gk4#rm-;UU+ISW6EURO?MXs^9<C
z<kBL$?GNlKT<5zb`Mrd(i2K#Jl2iB~{ZYw8!etqy+S8QSl<1Aa#@dzj0N~yG0l?nb
ziONN$`0Yt9zY%M;A=z@RkA7E`jJFivUUJ=!*+iOANyg)$=Znc*epoSE>rUqwmnLT3
zWkH3KC`Eh_IKWGy9!#13z=DUWcv!(=^qUt$@6H$47ekXn7Y@d|>uh!WnNpU>`bU-=
zn*)9%O&qWhSL|Uoh4FaQq=~@bN_YSZRu$4rxeu}ZI7K`vrGoUictP}d6R^&MKVFd>
z57LXyR)*4|{2?|eInDb9D>O_fkn^lMM&9ioO+FbX(0~9rj5s(1BPA>sznzLz*P~)y
z_Gp0LF-(E+cjLw0(n(hW-UQW0v;D5BURt2Gu|=_b0dhnJLE$33T%wY4{-guX%f4Wt
z^pn*8;+DK&*4Ps~9<|IQ*ObUL_S)|oh?*i~reTP(o7`~|VEwP6w>8OoET(Y(7nfJ?
zo_EL9<dlZ_0i6ge!ZEkKAR%(5Oo^h(Q;nFnqgtuQ+N?F|g35W{O?{fs&vy$x+=XJl
ztj-62f!K(c1o>G=BqdF{oehl`7j7;QXqYRt&t<KQOq~~WpPH1R&(lHQO-!r-XlDQF
zQ8aOTinyD+KgPaY5l}L`9|w536J-s1$#}XnKC{K?K}txr?X^F<-~+Mr{LjYe*eFu=
zRbGh7^==*8=MWdkA^uI^*0~S6Ooly`_sH0+&xz$NflLUZHOq&y=Ab&?5!GJ7jl`b^
z!?7j~Sx51Blb<D)hHXYWHZ*C`uMC)BO*EAZ<aB$mEtd#*@r>RA%ddk^>vy!bbSoW*
z@_0Fq4ZiAdhVmT#tmMc$m3hN9lx+)7QUb0%?A?56Q=Y|;5^NTuX@f+-r68qU;fB-Q
zkG#%KN^r^V7CdS=$>y@|ROWqW2|8J5VV77ZG#*1#_jH||q8>!7=m?IVj<rAcXqW@N
ziyHoUvc<+*CX9n0a3=Y^Bv(NG+s5`CC4Qt^yx^I61bv5K)Vt@8VMF*A$JJ7hcx_Q;
z0I}0|&Vt=oC4|?LDb$gt`#Xun@XE_Cjolr+LGN>Q;jk>j6dJB4%(pbTu|K`onCA<%
zo~CS3`3s`*oUo*!TTX)1{l5D%O-{gnGg22QGi_IAKC&gx^Xp}=hTAbh<mYq`>nuR=
zbMZxbKWQR!406oTcYI41o!u(2CD+rsaD&;gS+^T-6T`xWy#Gc?_;kgl?Q{nPOH>{v
zG}$JLC+|L7Fef<EnxOjKkR8vU{Cgq~2RkkP(beTBLqPHE)Q*E^qA~{UJ0S=he@|Rp
z)++D&0n_s<y_1Mz%Nkxh%J(0{%hVCE%SNc!yO}(ga;`vF<T<P;_2b=M)SwKVeG6cy
zeijN2j$yCr=4hd9tn&w#L_i?QH`6H&Kz^Q^CiKBC0OB4_85uAs$1UA|Q;K~tdXj@K
zcm^6SB)y;3<%#G`l#dr_M^II7T*lf&<=8k!H0A6iqv9Py_v<V<>}Mh`#|yySvPx(U
zoGRabF?zB%mAtqfj8LM#8oz-l_zut)Hb_bQU!=quWK$5mOr1x|Ad_<|;2J#RDqe6(
z7`hkg$7`39z{|a2@x&$~q>>vPB2qsYFPqb^U0v>AD2=o4e^kGdK*%{?Uj4-Ew_eZa
zhg1aUU^adq6NXVHrG6m5=A;~*Zbyu$>QL+Ru|`+#T~nSV;PZ>qYoEvBaoJpjcN~Y<
zZ3QE#jg98i?wVH(O!qcDrv`@Ixc4Yt1*#x*j98+A#M5Ab^4+dD1Ov?WaRg94s7vB`
zcDv(%HgGi$=KAjVWtxrH84-DbZ09IA{gycR#3Cvm@p%4*({DgNdK2oQsqc3Er(q1g
zwHsxihT`o3Ppz1!$N~lm<K2p#+s!eX?oDLz1JA%fIhH)*ICYt1v+zQ;7r-$#4qhjl
zX*ceXnhJU0{vglP8Tc(~V9kyz(0uGsoKBov$E*KQ^S%~q>crR6izhmkBO;d^bt@{A
z{ZWc-*#EjDy4<>=*q9uw)mbhZ*KpfdseF{(ccadR%`&L(6o$ll&!g)P6~NYps7xCZ
z!{SPpgOyit=#`?+wI)}Z>Y`Nh)Pss|ELgCN#Am0&0pN+=bS0g1<zVIUZ(E}B*!Qz0
z;LUh3`Tw|<9x#d2)}+ZP(c`3(7?*!%je`^eU?cOKH1FH$XC=ag?(X0g&!n`6!i9ci
zVS{-TmFH3UR8+o)wYn9vUq_%39yU3aA^|bujarHICgVlGuZeV-Erv+Z_|JCrur6GK
z=kfgy)S#q$hHPuZ$GV+@rSCwL2;<$0Q_FRJPNG2xQt?NB)7s>;aan`Ttglj4s6U^Z
ze(QROv>7_so{h4S#47MkY<Rw9mHMagOa%Nj<z8EX_mq;$d+h&#-u>*m6D}oEyDJZ4
z)!s*Mz&y$oPm?@SxPOYM<sLbHZ-EHXJH?Du@!ZF_2H6rpg)EFvC!w)6DZhU5B_OGr
zEUNb~t6myc`|Lsq+KK-l##NwKsW_s@izV;Oh4xtu?9LgH`iaznmgT=??jL3aK9kcu
zNRExpdDrCHtHPQ}25YQvLX#n4zYu*+1PH#7R~1-B`f&{1TNs1)EOYC}e}y?|QS>3S
z)!nRmlS3COhGKb!sQCLUb%8ZZGCawRnD83AQ^-+RBh**VqytOK59iJ@J0st$%dp$n
zv)H{>@Ktd?>{eKE>+Fa4d^2^D_hgl;cO!K{YYn?$@}<O^eW$FjdnANU{0pK&fhaXd
zj2EYYux9)Hs)-WI34>1~pICy&QErq-MMp7*vP}t6;=wsx#gzaB-s0NB8h$*k414S6
zBuh$%;&7@)XGXAfJDyT!Rm(e5iF~APda+xNIQ=R`g!>Jv3l0RlX%LHRg?12drTowt
zO1R%A>o>)5)-oIIzL@H)_-Mnq$XoF@I<51;nlmb*{DFOO0AZ;KXe`|z>^-$2PY2JX
z&gYtv+5v75y8+VV$kPTnrs5_-tq0hMzbQ>4U@2(C0pO^qejjV>g#S9z%O8y>o!Yu*
zqp?TAV9f}N8j8JWtcA9DcQo5iu=d`=&}$BQ`L5V_N4F=%jVpbK#UduP;{`f}5lDQ>
zbFf_nNlKBz6)<4PFme2LOsW7N?3pANX~Z0+m>|9SaWD}?!29V&STwvp{b`_G&TbBF
z`fFq3vJYWt0#9QFZoVqRl1-k$r0r<3oreg@xgHh2JV5z0)@agfGMGIDoH}9P5%F(p
zwuj=zBK4rLq{P)PMENweI}9b(P3hMNSy#5wg;fP8?9%R+)eo+wqKMx7n6I1yIXwQn
zIgl4OWUy;XIg%l+ka}kg?`?$Hq?3_6i9|;(NdyH3J&P~$jC^jyM6(QqUGa~hnA=Mk
z-TDO?`wDKSi3QZ6z*!6(ymhTx!)y^EiXjBPxz@iHr}L;RIe3(ERhNH&p&H=eUPiHy
zIO#i(I%lYEt*z8Ug2L!D^553Sr7m3owcHKV_xkRk9HnIHmKky2V`5^wjL_|u9Q+d9
zgUE)7DrZr-m2Bjl48~VE-AV*M8RN3)NuAZFtokDmfdHPnWt5LEbP9}nZ_{?5XPV&c
zNWly699IB$_GoGwu4<i_lWg$!Vps5Y5HaHg90h#_RX`p*QN*N5W&!=-63_@+Qj?DB
zEEGJa^HRx`M(}{AR(3v($-Ozs<E=Eo_8^Wo`BcN`Ed$W082j>43EAubVGWwZ1F@6M
zv2i<2C$=xK{z2cd?_T`ur<=|WvI20axkRqrAZ${x%)Tp`&po=-k|ifvP*@UuRfv!h
z@Mk|(L7<T1s~4-YE`rhgei{hZfdhMe`xl4GRd8e@J_scOo~0X&TUS6VPi96Xrz<`P
z7W-twSMQSz+0t^D+V5r$;@?ZGTYD7~4$zy;^jar5fK-Zj#Z_Qc8f*icE;Mb`VV5_K
zrDFq>Cpu<Y)|$=C^mpDIvl{@3%|;tH*Iwz->mT?L-Lrz(ru;ylIsDM%chN!gJTDX5
z4+2FEw?TeQIY;4Q=)X?haot)yw_;z25Sy+vk-cHKw|m5@68*u!4s%5G$C4Mu8UDox
zy!G2E(ykLxh)SdwQaj?sME>fL(*BK5cYkpO_g}MAyhQ*m&uqS8VH`@VdoCj&8A_?=
z5fI1!MV%AoIFQBZ7&%?pY+&RsPG~p~*2(cq<gvEbJtqj%S;GD9y1?Xz*ZOxHG4d~E
znQ@%tJqV2Ok_XJTdYryyL?VnWqdXOgs|bjCA51pSnD<guB9PjqoT)SAOl2PV$Kf7p
z5McrH-X`L;N>c_M3r2w2e?Tz8kacYh`~wH`UZGkcN80|@*gdJAsQh%d(Dc$eTsk==
zor3)4idC)NZR81{Bm#>cS_;FOW0E=xk%+1V1X?GPW>wjeLLMafS1c-Ck$*onKE3S5
z%UI!aF+L8lEPcGwk@2SUCXnO@r<$Uo79b40*y)gF1=ZUz<W_o8mQjhAs9sZu%nCeq
z{lMUdqfAs4D24gS=e6u{j*UA1X~wgx>O0R)6b{}mdeY>$LgiQL@?edXShPOhY!{2l
zUYGq#HF)?Z=FrCd2>f!B|4frR?le1rsY$dQcI(GE1$Y|zGtFTwE*0%VNgr$M$(rqI
zlriMl#6_h3!-Vn7fmM&dbvuSU@b!v>7b=Cs-S^RCE_5X-%8yxXH^OQM+V|EADyBRL
zBT*4NeiV~pQ&${JV%BP7_2+fw;>ocW+Ak!+S@Rxj;>Xp&(~%;Q0*J!6u6pfkMFGh9
zmN>xpM^fF4SW^DgCQ|N;-rQ7N<0LzH&>BsGDF3itAdH{u8TM>U+RCxs5>HuLK2kOg
zfGe^0-!;bu(CpX=O;GIpFTC4t6!qf^UPHqGTwKvR3+R-i@Mo<F0I~L|<h9xXq=`9c
zW4f#d86U0R=0#b-?WG-5uyK)21@7@dHfNb;8~tTQVZ?0AbM&Y4oZhDUG2S8|MljjU
z9D)u+$K${2Q7hPT$*WEIKR_KIfmsZ_P=jE3dquPJHTDPd1!J-6m&(!!4rte!^O@-F
zKSY6^grUHleoZ8AZ5_NuIn7JiZ*;3%6@AX)@4XH4(^`dtS5Z7Ka@G3d2{ToA{jM!M
z+mtoi!zIEt-YzBiG+_Yv^@v`LC_g`sAsbd0QuR$ex%%-V+2Dzzo%}E)NFUfT#`)mg
z=Gf_dzDtB3TU}U{HaOz;vc1asSp0cU)*Zw422Ob4uvp{H4i*uWn@R)|(wg@_h?F|#
zHzeDq;}4#Xf+gcw-n>6$k0&>|O%8B;bB*<v;9iXKexNYB0nLGwkhbR_%y=Fu&A~}H
zlK?8^J6JB#9H=Qd6nxY<;im5<Iar2wdl^G}O^n{G_-cK5O({U#Pqx{TPp76F02f6e
zLe>2Ch$>08E&paBSScNp#7TZ!i}u|?!U#FLM0_JECVJ*EHP^pWD-e!{k}aNDU$mI*
ztQzL9M^b)4PA65qp0hh5jq#B!pON>sWdvU%=2yD$JkHtG)S_M4a`Y#b1qv!rSRen<
ziP)H5!Zl1CaT4P!J$_l2sS0YChvQxS`<X&NehH3V2M8)B(K}fAul<)h(PRCs2>U~p
z+?G`bTd^rj9cB~ov40dpB8?$AfKQKWG*<B7e$o!t&_&8usXkS|36Y_W0Y0cc==QAE
zSx2p<?`g7qwubSP6|?8?>!W|D&+}=^paV0;?nfBvF=$f36JaX&l?S=Y4k#kS(MiRF
z#aZSNy?v)BVi{53;aVcUy4O8yv@3uWzTPhiq!LrUzMXRcnEyVk-&5IFm+|<;J^TLu
za}O_KtToB&X~@HkF+h}0ntTU5zdKgYKeKS9@;GIl^n5iAE@RD-?CAG?<7$m%9ywIZ
z@tpWzEd&S4=67w;Ji2M(H1lODxl^kEY6IuhUOf8h&I=1j_fj#x6eq!v_~UPhPY!AE
zOJF=<?smnr{}0Q={T$P1#CAGaQvHVj@>h#AFVs&-qQoZo#7_Glwcd}iZ2wQCak3r%
zc9>3M=<|L~*9dWPZw<jDSlo@&jbwXfgD_rxcuC2Eld*%NXJ{%*O8mn5c~A-rpUhe(
z5tyBA#_M(>e_C@Ca{!kqL;)CAvWpDzwEBt;<ak8yM&SJseIlYa$QJhq2wIi^DkA|v
z5hLXb>-<AmhG*tM`7)wFcuC4Wgr(D_^p<)6(fPmVJ84J!`<i{*)xQ>3>J7_({U&3_
zsVmTXvd$o|LHJT)O-fi)Y_re{qEiOcO6V`bc$8o0J5$oGC@L}-aT#TGv0pcc&nQz9
zgPL5VHXBlWf%WC6|HN!fG~_W!VO`BuNw>uEVGg$3&-d-V5&AbSwcksG{kLFWTf*>^
zTlxjj%N6Poonkvp{+XbxD0*oHOBqavB+NTe33Yx$G<mvV51ag?hS?b-LQ%yMo{^_2
zK!C%Vt(b$#`wL6+=KrWj(O%0UimLTKwzv`*q(yOKZ`u)6*=QI6t=dlG59GhxynQ_A
zyPK(rhl)AYWhPg2e5NVwy+Ymdtd*{2H!noo>vqgdyIitivqM!EK^2h90Zo3uiQT7D
ziER<{+DL(yIRZ*yeH`V0cfpI+6`C!U_t~=ZrRUTA03e3skJ~YhgUNX~^&O%qM+ru2
z4{G2&FE#IcG~#+6J9=vv@2TNGZrE$lqeO^3kMf9G!>t9MSKlg?ADwJCNae<oZ6~kw
zmn<fOE#-I|caS+r8I$*85(KO;6wD`3LI;m#mx1F(Vq-<egH4!-4%#Cyfueo~hrS~Z
z_k2d&7}jzeWfaRyo{7pgV`CMhZ_5fSXy50g1o>>WH(`W-I}_V~g&Px1otXIJAKW>f
z*;gIQD|T)B9nEIbi3RCuEM0^01H6B2dJ*N>S9YN=0R=3$?n&C1lL*NxP0U}JjUK=a
z2QkTK$T7dTrifG`<SG~Z=~~6H=rj9Qk)p{z5EUB>bAniuN?u81zd!7E)lZz{p*vNc
zHJhIjUoKr)A9>JXljB2;eQG_2y;ypY)aRMkg2jCd36XuLXrnc5?@AK>1GWC;boI98
z6@tWK^`zuLorGpQs<XXFVc!L2jdf;+joy}xbbjj5pg_+|0Txd;?}yl<E3<*rS>L%G
zpNYxKx~Lv-fPn1$t%-`4AF1CD_8~#uMj5HbDje>{8Y!pPfsI;UiOQ@8yP681&LS85
z0jnLahY8KJm=v0yYF>^=yZaE#8UB#3%LijfK{eWK4n&7uw@`djzb4vA<U;32&9ln7
z*m+0Fch;tRa;ype{L-<M^9Tq*TJd)af9r#uSx{#Wpzw?<;2$QB?8^XgH9F|_GNQvq
z!dwsNf(JADMcS;v>U<+pd=vKzQg~KnD5JzHT*2haEAF1I&O5>h0P5Hs;fC@3POPSU
zvnlrh?+L0k%=_kI*Xv*eBLV87*>EpetIdY1M7`+a_G79bVD-oVq*Tq%N1l}Ph&^5|
zGcx^w1?v;$T{T=RHND(Ki2f8fM#>suLQAdGbk+zd|KQL?ozEL)&4hzpjXYgdr9q5U
zuc0i)4s0Xv5%_B&{ZzSAN@8tsb#obNzjEQoP#;USF;v5pGyDx}`$_PwD7IB+ZEB8k
z)WN&f%2p)*a17Z)Ie7g|dilu()>}*gdZ^U!^djKM4U-LPWl5Wg`1|2HgXn*|<PZ<|
z+nkcG3p3jB8vCYa^y~<~60T0-3goSi-tr{~*J9T*s;x;JyoceVBHitv@b+TeptFgr
z42|`m50J!{@%RXwHUhFiN>l+FV3lz+{7|w<CjvacUHUFO`;M9pbWg>xq4WKvXQ9f=
zyB)AVejZ=cMIQWHsd(a5OqvK7Y&u=07q!ZMJhe6CIRr!;)IKq(yaYIjW&1STAEmK3
zlWihgB9MG`rx(y26qd$tiZ!0>;Sg;oqlS%metO|v7ScsJVb!DUEK$lY<(0b6rHF&m
zl}SW(`dx`U)X8eIcLODT%s~QO7DS!Z7(b8l_4+Z?sbO(CKjLZ_-~ZQ}(w}gW-VZ9(
zI$OHEk8eG?gYno0X|ZwEM02V=6*G$Z3L6gsCcTJAJ8rQ4*T*x}i53T;rG~x9;g4KN
zV=pRt6JfGd2orp-K5jd!$(4C;y<OBqmS3MqLkwzhd(v1MVukU>I)5;E#m<9dvk<W(
z;hY1A(;aD$ry<#e>WKA3l=mKw0_bWC@fpsJGKN0Of-8J^nl?{7MT_`T4XYS=yz=dz
zY=j6rbuqg&uiEysR{k+=wWPFZ_pSAtWo}&kyWL)^%S);KbZ@}cAlH%&vc1Mfj(VB#
zVv^5%fEigZAPHY$-M6#bvWM^P)R`4{C{Ot^KHZc%ZrB{)t2hv@=Lvpci6=uMhzcH}
z9%rYQImEW*E%Gx3VN~2o=htKWgbuR)9l9q6O?Io`+ZE$U4NuhBFZI~c_LwPcjl+Q~
zuJxk{{r8*W2_JFgF9`s@v#{W!^BgN{gs0adJ`|fb0+!bYd!W&~r=hEFi+jGhL#~V{
zn15_N!u}ogqd#8ze@8&Eqn5|%Y<@3#zb_^7`12-`U2NpR!ME2>&Qr6}+T^l?vECOW
zJj9&80u<`5hq~)+OkZCW#k0N6<Q`6!xmin@?CPh&l@NZfD|<Y+zq3#Wa9>1qaX`f<
zhQ&QOFn*fk7Qq}$0`eVt4;wqTjRl{7xe<bgWe|@N>OBBZwVZ|=rWJ>dj%nD`xgQ%g
zA1|YaUHJoRR(y!wi-XD2ae!y>VDFK=9s#RzYtzXjbygIOfZdYl`8vZDVLsliNTc)h
z%y_FQ|2agv;yB6=*#MJ&UMmsMqtia__F)U@8btJQvo2=4)0J4q_X%^qF7_-{Jk~BM
z>602@h`))V7O1`x<|M<TA2#d+&PKGc&w0#<C{Hd6Aiz3{q#S$4Y_koXiO>gAaB)T^
zf@*HB0-wnG8mxdo>Ps6tB^^>!7aP{`poi1(8J14>>OYl?@F1$lBtqoJb4i+fJyL?;
zuc&g57K#fwAFbc{;y|s3KhZ*B;Uu{K@;VEz^Oo1BaAFg^QEf5rCQTTN2AHIcztbGE
zqg1N`?H7~iA{E29hh{tIb8Ku31O(s4&hA2E-&+uz@*`fnU{f%M*rUxjzBPZrFDk0*
zje}kFZ4M@EVtmCqig+zWa4^Y>ShlGq<u?KxCjm8^dSkK>dT{la>V`o@TafoE8H^*K
zeipd@UXt@84Egh8&pgWH=Yp~T@Xn?C!Fab$^g#AS!rbn|76WB_7&|W6QsJ>L(rvcq
zQ@dyWB;O0glg#4@J`BynrWEmmO{DexD}&j6`?>6^OnxySUFjLd(cdP2?4!Xr_)L@!
z_70c7jV<Hl-H$?L-lirw)OvsMqDr&6yNu`%*kjo($*(uB&Nr5Q5Af1=Hxg9}oCplS
zO=%m0YdF?@TIaiXS*t8=e~Y1N@Z68NU+g<Jk_`kw&*}IjPku3a92-OP2-dI~uErIY
zlqQ7mgZ<(Jd^Oo_uC2}kdP@<vBlSY6o0=h%j2w|aKayh+ecb!Bes4rcbn7v7=dr7$
z4UqD)G!Vs~EVRoHFvA+X2;8oIq}kZxA}J3xVABxo8YUysxMax7tkqbChfc+vY{!q?
zmx^}?^7!ZHHnM>sUXYqU%dsEZErh+1o@I@Tlr0=OZF6Tw+vM=bd17(>{!o;uNgM)|
z2Q|E}1|o<r=Zn){ZX(r<Sj?T>n)2;Omg)B3S4>BNtlO&<a#t(l1R2@Nb_|BRt<mvn
zRmON(rnTX+m*zweUMD9n_j|zoLf!W-XIBNi81xRnkbtVWn{~Ei^hdbN0RsCEI0(Lc
z=KX<zh7SJJY#N&7Yn&qLXFS7XtX)zQ*7oXYZA#pW7r$;etqb4Ze+ch$oAGQkS*;PU
zd(QfaV4I58l$U#lN;PMlr_QlRvI#S5)5zO;IiO6AfOPc47+yxyc$vVnocL~BQIq-w
zJrB&4bYMnxJEb}-!~Kp9&5J%Bv}{)!qokpcBem~3XF_QlSdNg**!gJKZ95%4x&=5Z
zf|E$yp!=>wJqc3WIQq4Ox^6bHLKrLXitDLFRnJrCYCM$Pw(Bln!rIoA{I*}>ejYEh
zMg(=lm-L<Dcr@89P%FGn62UOy3su<I4~8YuoAt9uI!|FZ(AlGX5#&ZO=SH~tdjaHH
zFtbFlSW#4dalT+8KF9!UDJjt|QwTvP5irMCzp;uex;jZERyKT9=Lu>2Eh&Fpm_+_<
z-6}{`bNY>|)o{^>fk0A<#V36yT=R1Y{1RnBx_)D=m=uhJB?}HQ@eJDQrwW(5XMx&x
z@(jl5K-B^fAB~<gYa4QO)d*@=Cz6QU(*Y#DBco8Uv@;8~G&H+H5dem9wK_}NtSb6^
zI7JB36VbbUp?+hdZtwSTBmQXPVYhd?wOFw*v3wZu%>nIQ%~6%uAQ6Kq7U_8wQ?+i0
zDurwzfr{6WAMGREXOBM4kCI5FuEihAoY!UKILa;>j#7;8HqYH!#eCm-*fa@}HtNSe
zMr+Nh^0j?uY5AajPoWngs0qS$?3^n>KZOa%ocPp;IH0Eew8|3x=)HOGG6#0)nL1aZ
zQo-*Z98Gmo7n_v)=+hAZkU!GAw<D!i5ixB}4W}pz^^3f}$g%UsBzgoQYDI9;mDni>
zo9HP3NkenMu`U;nt}nl;?-<|<7d;r_Q4<g9BB+82dsuZ`MxI*{f#XOl*F^N@{o|%Y
zdC7Z2>fKovfAv!wi>yyBFc9V<H?XU=9hOc%)G)O-hW2S|iWMzpQ>RcbuF19km_lTQ
zAii%oN1fl3-9$5f^lawYRe?WYC-Pgm9ml_jN@$3B_$)^3M~(J3W|fex_^%(L-cMHt
ze|z3LgHy?N=cB*mBOWN!eXx;msKLD6VakQ9A>NY}b_IVD`J;F*HhprLk*KOFHvf30
z0N{Cqvo!v4DA5H<7@0Z;?fkOINiKxrPe7U9qC^1dBzn(WXEocfA2)BQNKoKb=8$zi
zTf<2_54eCo2YbhQjgLI@W>mPrch*^@f9}y`b7%#^gTI!sQ1UXg$0XGK&yf=J@q8A6
z!BFrSp_^j_taV=C8=Ax>Ka6t{=6Y6mPn~5Ku$2e*k!DT@aCqK`piX)gjBF&c^yA*B
zKCwxS`MBDapT~yDAJBNh@Kr(+kRwXRR?HYuVCZ+VVf^=r71xgI`{t0lPtDP-06>Yn
zXMUcO*3`D+WvOYe-?k0_PO@g&_ivsv-mIOM-DXE5Qonf{DyRU6bq`?jicxk<UWn9P
z@v^Zr##1MA6B>3WQ8mXrV!}q9xhXZN(w^J#FHuL1hI#wwV5o)<Qd8_glSI!(vm$7O
z#KH;`Yx4p+|7P<VwzrTzpC>x;>}d<5NGx{CR7U<9*+Ct#S5#?ZHiOeCdz^PQ$)+nn
zx}QWR-G1GYO&Mhy!{ZuRXc9ppjL@0UcLIPSpEFHm9w|_<?!Ex@HRZKh`6_KV-XPs=
zWUDr}&Dm%`4T<Ub0DtJ!pvsuiB^x<mupb9M?o0x3VPj9OzM5msNqLdboatbda8#gW
zqtm$ZQ{t;#sjBdzJUZ(;g|IwxmP%&c<^cKqlT?+F5Kf5~$>s`I2<+1IJn?1SDLUfi
zVIHrJy(qq1HaT8y)iAYf4S1$!i1k7IqmM~#D`En*^M4}dR9`X4#eZ`W0qd_c?^N(R
z1jR0{?$=7Y!X`v;H4>fI3KoNdZq?6Idv|-cP%uw|BTtEcW0Aj77bcrINS@FdJjG)p
z{=2tlXGN9GA>tRJ@^ZJTK9S!ec3aQ;hn~Mawa)@N@r?z3o_Daej}%8#8SO2LUye14
z%%6z>_Y?W8IUi!E9B-v$gXGWEkJC{JdrUo4=QEp>X@hqEVBa16h~F<6aQa{B$4wV3
zzHUY39j}~!A^Da1ZCr^ov*pMXi?ttF^nNZ5Zlxyu&g(K5Z}O8S_iRU2<LpldDW@ji
ztTX#kgqWiM7j4Inq5?0F^SPsV5CML)Q#qG>Pg24?MW{TUju)+e70*R3lRFf`9+1e4
z?EAUIy5s?UT&2#&&dSwoAA{YlCNf0%AM-Zxx>#mRFkf0bc>Ham=DqAnRGw&&DK^56
zH^u4<)QUh`r%HCS(Z<e$;^fYQH%OFgNVC?zC*6Xrtm{Ow1TIe_C6mnNKm=|!0)U-J
z`J|Yg97>Jx+L&b3;}vsy;b00)@}4B`vn(Cz#1{^$AB|64+`KGk4ov#Z*~5t1g}^gS
zN_Qj@C=`pMh({gFL2H{Fr-{ImXMdbS^xd5Eh&3T-s4RE!qJ;@sgbq@lck-(9QGG>d
zzQ2!X-HMHU?#50Q@lPNpz0iTjg-R=Ee`l>o9UO8`&o|eWp5sKj(42Z`S%IF_L$(N|
zlktFXz_&nZ7QAMw!IJ@M_d_wqFZlkbE;n_>P}U0vRYAhoCYZxoNiI76G1^^fKU?+y
zW#?ljr^JE$0e?yy?rcgq)V!k4Q)vTe@Mrk|i2ye{V}a|9Fh0SJ<k{^G)bWMBLmuE@
z7rnC@TzSfUwGsc_n$l8m)?kEg4hbAXr+qgHO9%J5#eu?$6{y%Ij32y#tyh=ah(Gau
zo<l@=lcG+6*1nn7IKIkwu?AOsK}`sHr&{~K0kLM|L+(ke=VEdWXJn((%%h+}Zz4!t
zI4qu3hm$LkZOR44V{=zE{INc+_no};p{^|FY^R#zr(@0JsEq&C?|Q89fC(er8aviU
zw6Si*PNKR!U#(M9Hx?{xO3ZR8#hOxqrga_%Cq4@vRH`{02+);a=|sTmQ8|eX6OqU+
z)1;0R0m?*>)P;=qWi8wJ=V|C9b~Q+gHp!n^>k5Zo;cEY(J}K}|HLv4wlE_>CmBos-
zy~R(bO23N;*xwyQbSpEdS}xa4!sq+Ps|1;|WZP?Te<XH4(P5~^#YyfwYRX#HM*;ww
z;|ixY0dCosEmaRjFudUQ<@I|Q#>LZp!ob@3^}LMMZgZUI{c#S_)xEBsIa+Y!8V4m<
z<R|ZQ*m%C#2VaF^?)M$hlVZlIsaQ!ea5QD#DXlCs2+dbPdAy=71KZeTw}CuPYlS9@
zl?jJ5v|1f`kXPwSULZaZ1_EouEDPv4ct5$2c=UTeE40}M*}*a@Q^er851w+4NpsYh
z&&)byG08F>mT<6XZJ7FWf?!Y<9gntSv_2F0$%74(YY$eCwH_!I%<4CkYyGW#{Mi~f
zgk-37R?M~TO+f6Pq$cUzM4n|`4><=rniAiNDqPnBm0pBWMz(gIsMkc-W9<T@vN>~q
zQa}Nkt|XB!?uvFn>W5<Jq_N<`7ufBUhPnN=MwoIC)*KLv!1Y?~A82I-?uS3;N(Z|D
z(uctKOjGjTfp@oDQl)B(i`xr~a@5r(68Yy@$L0#<2pC4n8*wkb9OJ%*U62m4i5da8
zKga=nHz}Ii`>3<007^f?;8}kA(RkUb?fKPURQTIyH^v3k>DR5L%<MhXc^)z>KN1gQ
zmuJF;qX)#=XNo@AFl;c?ncHA7@XjwmdFn}1E}?17uz6oq!}zKZJ4yjC{K-rtXB1{+
zZb|7Q1Z+ya04k~$mYI`IwStPLptl>bi@^Jfdvys>RRwor?X=FGdErk$dAje$?dx)%
z8|)p!jN(@~V(gu6H#R<sUf=!0=7k6wJ%21Ff#F772t&oYoKaQXYuFSb_1iDAIax0d
zv?fs=u{nj|g{!^L<X_utT*wmsspRnb*Xy;?74WO(rmkyV;r}dK@-iBtKMkZr6!#Yd
zG%WIOM%nH@{fS|=sh+QuVj=cMBL;Ad__A)#9(`zLRSrYk<%4x0B{?n^0p%d@Kizkd
zwS?<CV;?lf?LOuQt6gc8sIc(?W5r!+stYNt?%))u-%`VA{e*g&{L})b&KD7QfL`9M
zpSo^@(NGtGmRk|)48oYZE=Ym*XS;Q%5UD>B2gLW?MYpJ65}L-+4U;u6J_nimPUPRE
zA8&-U^SE+u9jENeS|5aGW1G!GLfg?a1Q9q8MK3ssNE1D410@cud5=10zf0aUL`sjC
z1bvqIOgbGl(DPyOy>H+0N*t;vnmJ;_Q#Y}Alc)BVP^rs5M82yx`}p`Ga3V<eozSPm
zLB*+4sCx<(LZi|MS?DA=M3s5ms>|a(;`ufzU7b8mwi+gi5-|PI(8SaDyX*tnzo1r;
z=DV^R6$>&<AwIhu9~6uhiYMnqV9I2M@5A1gObi>ux=2kAoA=H_2_|7f+PQ*MJcAjo
z296Cojq;8vLWcTJ_ETvQi_;-$J^1!zw<|P`jXuoAndA?#ebW0r2NjB0aGrhch6o&t
zpyEFG^mjJQV*aA<@aMGz!tmP~D_9aik@ZF+rl#RIi8`&UMPXgd0y?7XL7kVil6Nw!
z5fPA1PMb}Y%e0hetnK52?`Stt*swhO5KOWMFY{qG4Y=xxo~e$2$culUic@=i@F(2h
zKz*;@1g&^}7(I@?qpb=Lf%%Su)lX;{qFl&C7)lQC0l3TMs{<tcQprQG=n8VKG?vDR
zAXf;7RvlAMHg<1JOL9+Vty@j0DzIXq(;x0T%k`_3)AkfJT(7|n{o|g%*x5nPmB}Z#
zReg=MR(a0m*07Gd+jR2m#Fl{DlV&?NrJ%5GednE&2k$wq6;Z8RO$0vBQ1Xs)y86mK
zKGet8_wfRVrpeifSrwyv#Fl2eU+0Tvvuv@Rw%a4nLdk4k$1{8U_jQJsgP&*uW~4sR
zM22}WrCj@~z}hf6;Kc8$^DBLPLmzKz7};hgFvY_C6vq^nM6-_SRx~q_DoE8V@u@{m
zB6_gLNE>Ts;@ztvvs<R}U}>zq0=s;5oz;+{2|Xj?VCt}B!^=FlZV`8-V=4R9^0>N>
zgSgtuf-7*J&xqb8mwV9TWp>IDV6y!hkgraiQ%vAAhwz|kFAJp4qr=F<Y06ei9Pg=W
zNcUIb^w+U8ZzN^k!TlY$>J~6@zZt)ksLJW{C9kApf5>{_IYKX$9Q^6uC{u)mj%-Ek
zcWaoX??RJ?iKk8UG3;hC>Ex^X4){U&WJRC(nP9Eg8X>iLhbcFuY=ytmXcsXlJ2SjE
z#1u4nMufnig#Bo@@TYZ_01InJ#j%t%p)}c^i5F++XHz>VO^#g^qj(^c5^zFBnk2%0
zGP>V6Fj#wZBX8tQ{J-0jCv3UkLqN**m=X^5-&}h=fC!cnnuQ5B-FluMK#0%#BwNFT
zYvh@17Ap&D%x!ryC2^X39vGIh(}v#|zm{?=QG|>K^^-7eC6+%M=LyG-r|jWzF#C#4
zWSfZ2jQP=JjO+5Lh*CS&#&PN=qQ4W?`k2?9;h<X>KM2xsFKk*|jQBC>O&8~Z)nfS#
z7(y=&AaQ~M@ws2E*2y&VI-V(FMX9%bu`&BL!YXw<DmH%XQEYHsK<9%$)07^O_=^Y!
zFl#J0B6aXqhUU0i>-)QM43?p|{f?qm4$3W0+<B<Uzr86v*klF!v2TBLk@-KRY<9c#
zXv1D!vzC1MP?w^zT6EUp8<;Uk-W{rFS4Cm<`SQ{|u2f6vab30!WTA)46k_o!Ij0=;
z>3CK=3E|h$(8r=|C35h1B~P(+uk_<79Fdp(t(3F<!<7EAlD!t8d5jlm)Z-L;5z(XH
zzl{r_mkwrsD6al`T%Bm(><qfnF?yn&051hfy}<fp@^aG`A|<%(aAPdJeE(W+fzJ=6
zoOxo`M8x8BGosc#_8)VuGy^;z+6AcnV*)x*-B1&#JnkqV)KQ`}6Nutv0tH@@jdssc
z)6H(D?54GqXBdwk{lOY&LqEtN5p2-V2cj(8D5oVpr70W;$o}e}=ySpCBMOe#m{ZxF
zJ|zC{m39L9eqG@++45aYWSNWcvq+ss$`-3&WMiLYW-Xb3`_Z%~5_7U9Yh4mw@!q_o
z6y}ts_#?ud`pHL1aiN(o=9SQdf9Ba`t+*k8T0QB9HLu=9?<Ab=vgdd=G@-Ar$<J`w
zcUMxx@iJq{AzNXb4o)JVM4Z*}CQ!q$`<7$##)H+#=fS}y-&o?$9FGRPn>Hm>kx=J0
v@?LE*3hVYV`wCUcl&Jex@UN+V|MdL#YyOpQnTwZxYX1M_cQog(`8EG9pz4zs
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/dfa13618-942f-47e9-9248-324ed8e367bd.vsidx" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/dfa13618-942f-47e9-9248-324ed8e367bd.vsidx"
deleted file mode 100755
index ab3f77af72613da92a296d450229793258fa3319..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 13898
zcmZ|V2bdeh*$415y@SD~cLUyjJA0><bXdpD>Gn=*X;*9Q-f<_LqJVM0p_pJpLVEAL
zhcwcA0)+Gg(i<sG59y@$N_zWd|NeiT=evhbk37HGdEa?wW>ro+56!uSgT}_j&J0zf
z9%a9LtW*CEIRf3W0~P0vMw`@QCfOuAo0^?XS#MJzHao^<+1V`9%|)zw#^%B>T*GJ^
zhU%;sM&B?^!<;I|+=}704R_gaR}5hr!Z(C40?VkIhU^$JHDc3<T_cW-wrR9oqb-bf
zV00p*6B~(TB+|&bMjjc3FbdW7jAhGM$&8i4=x0X1Fw2Kyn&YN9Wm<-5S*F!At;Do)
z(>6@UHC<_r8|Jt$#}l*BFuOg=Ff2o;(yEYU1(xqwBD4bA3S3K8Em^agU8^muc3>rz
zmAF<Stvt5!!YZV-Y+K8jwPIU+Ro<|TzAc8f3~gDnWyhWs_H4zDExWCrft_{jys!&t
z7m>Z3*~^8!V%dH5%<O*Q7=_~+jw>9u;`ojuhK}qwGIwO*G`r5cbn@6Kq;s+3EIZDM
zdKOOKar%Y2N3OBr3bULlLRSpkAasM8JK?(%6?f8iCo68ncPo>w6t1kgv%+l(x2>Lm
z+its^*e!&+oVm+|yOO#6%=3M3!trF&lbI)TPZpkB@|qp5+4bh7mr5^-ysYcxv6pZ2
z3h5Q`xZ#c)!MM>LH~QnkA5Y|X+VxG#H>Z5F>sy9z3Ey#j$M@Y?Uo9}6_~nIq1ioMK
z{h=RJ{7TJFOh57bWZ(~ce<1v!=?^Von(EaO=9I96dKrY32wMs}7Is_Mxv&f2_`(sw
zk;1D8Pql^cO@Sqa@2c$xe@gfb;U@w`g)l|M5n)w?by2N~nkO0^G2IZ-6w(&5A%><<
z>kieuoD#z=VmK{Ed0@DKX$Gbhm{Wlz0!s$vdm;~P8Q9B#BLlY+2s02~P%(pw7li8R
z2Vof0M9@&rcF^bq(w%VJ39mNc`xC;P2*Qbp$%#pSqT)?dDwBpgY2=fh8t^Cm;iS4H
zrd2VgD&Azpt5keP-Kol?>NyqlsMSN$3{5LE$3v?bT6t&{q1_K%BXpagHyL^|^!?Bu
zh7(>`@xrhgR-Ldq4C}QpF~h_QlR-G}!+{6~aX4sK&2ja1ui8e{o~_!;RmZQo%T-}j
zg;Nd8YG73Zr#k6Z%g3-f@T&t+D?c7d&6PE;QWI89I5pwcM5Pv(wZN(cPAzb2RkK#L
zYgM;a_iI}^wdqc6;MWGCZp*q~uM4v-th%u4!l?^U56pUC)dRa8IQ2l(Yt?#fOT9K-
zANciwnA+@Y@y#urxM7$Le@eZgjfQ%PhU_%jcBAb!+U<sl7I9<XHU`6K-<)pv)9qlo
z6VD{EG)(p3m*$kTV`*p7?MSaFg)b|ntT-|>WoXGzb<~YZWH^vjv-|?6I<hKcHIUVT
zteNV-vgXM0%tF=zSsTdmB#x{LSr25rChG$^wOP*Qve}h!Dm$4>BK5^4Q%9zOOhcK@
z$#g+xnapz8D`XzaJeP%(MN1a3Tw0K$rCH(6R?OLoGuw-1b7MAln!;~ZoMuZl+eWi3
znjP8fber9z*-e|>yxCoB_PTRMea?_`Mm%S<=c4(@5bAp~GN&T1sXnohY)5mAs3W7!
zeAJ1fP97y$lp0Yw7iA*qO-8->s5gpoGs>MP_o93%%8RI2j7BmVjppU-e785>>&}mw
z3x>H684I1L<;j-UY<cr7FK>D307griEh$>E(vo3IR$H>xlJ%CHYRR-EN3FQi>Za=3
zGqy$Sw&SLH_Tt!$J5ij7I0@oJ#);}@vHDYxnsMsHsT-%2I1S^p9;b~sor}{VP8Z{J
z6fZ?_KZ=KDJlqlwr?(ERwqdrdylsoNE6VTe@;kz72d4U<sxRPnFr_|(?S|iOs5Wk^
zx65?9-8S28TfK4G?O}UpwueqfW*wP#qO=p`oq4Ad&v)WP-Rw@*D({(TChCuel_ai_
zxb4J~iI*k9RNqR8Xe2@=ftv)4B#`A%WG9hD5_w59O5$dci6je>Y$D03Nmfg;dXjaM
zY%v+A;~J^qmTwayON}Bmt<)SZ-!jg8>NZm^O#>qh%`~*q&`s;LR3@n`(m5lYn@$(Z
zbRkL?I%&6;c8fGO(n39>@;()Lx)`OSQMcLd#;IDYn|j@}=w?PYbGz9}w`X*FMK?FP
zx!uj3ZtiyTN;eO?dA*xAx{GOM#F=SkrkhztW;HV_&#WR-fA;Lz%q_Bge>297OqiLN
z&H^(FL^fe&6C$gsXOPuIRtvJa$m)T5w`EZ;Ysst|XWcC8_OdL>dU2LV*<z8c6x(|0
zFM83FX)lt!sOZg)dU4w8CcSRfD~w)Y_6n!BsNSUNh-Plc+$eI>%*{q__j4!Dog#Nv
z)L*PTwDZu->$QApa~@6Q(Oj<H?s1yO{d`+5SD(2;y|ugfU~7RND}_-M#<KeLp#Iht
z&T@f2u7wyCvQx;kP@mE$DWa^1R7Xu66)mSI-|pQaZWeJ;-sX5o-BWd6ip(y0i)vd%
z?iKl<SXBEp7gJ@rSVT*c#**wUMZKj3<KkAlY%MR_E6dKx@_4$Ond+y+ayG7(UsgB6
zT3NCCD^7o9JY7*gG1QGNx7AuHvcA>tI~VuI(|(lqGpn!uXsh1>MK*B7zzqiO#K4^#
z_``u14q|%{yMs6$#KXbX;h=2~+U}q|9CXx&#vGdV(3~1t>ThhWer*i5`05*Tbneth
zc1F$aD3PO7J@ZjsjFu)xBlXiS8I96i`+f|LU8!tNtuF4~aoz6KOIGK0ZNL8N9oOvH
zy}Gb-PwV<iS9`lI-F4;8J*#_f-h0R1Th+gt_TIAh&cF{Q&c1Bt?kmsUdG#fi?^w-u
zZQs7*y47UYo}Ig{S>3+t>T9pxvtuN$y7r1oMmJ1)*WP%~z3VTyt9kdj+fUuM?qOWM
z{9*dP{|;ck{DE4Il|Nof52lCELupx~j2}+R-?!!Uk@P5fG(CnMOOK=L>GAXgx`A$_
zC(@JX$@COj9<5yeG<rHcgPuvxqG!`{=(%(gJ&&GGFQA*LK}~8=n>y5`9v!DX6*Qm|
zbdpwR`QH@f`Kq)=>vW1<NVm`iou(Jj87k>4ZPGa!(RsQ+TQsIyX`6OvLQ~qM8Qn&E
zG^Yh!q)YT-x=dGSpAP6M9n$;J5xqaXgkDOw)63`%`T%-4y@Kwf52WQsx_q6kqF2*v
zX!+4E<JZy$(d+1Lx`$p*Z=esR51}{Ghth}9hto&UN76^pN7Kj9$I{2q$I~a!C(<X;
zC)20Ur_!g<r_*Q9XVPcUXVd4<=hElV=hGL^7t$Be7t@!}o9IjF%jnDLE9fif&Gc3D
z)$}#=we)rL_4Ez&jr2|Q&Gaqwt@Lg5?erb=7J4hajlPrKPTxh}P2WS`OW#M|Pd`9E
zNIyjHpm)*_(~r=P(vQ)P(@)S((ofM()6dY)(!1#A=;!Gd=ojf;`X%~h`W5<B`ZfA>
z`VIO`dN=(R{Wkp${Vx3;{XYExy@&pg{)qmV{)GON{*3;d{(}CJ{)+yZ{)YaR{*L~h
z{(=6H{)yg8|4jcv|4RQx|4#ow|4IKv|4six|4Yj+A@v+vM-QL}(u3&1^bmR|J&Ybs
zkDy1=qv+A}7<w!{j;^Q2(-Y_hx{;npPogK&Q|PJmG<rHcgPuvxqG!`{=(%(gJ&&GG
zFQA*LK}~8=n>y5`9v!DX6*Qm|bdpwRNUO9)>vW1<NVm`iou(Jj87k>4ZPGa!(RsQ+
zTQsIyX`6OvLQ~qM8Qn&EG^Yh!q)YT-x=dGSpAP6M9n$;J5xqaXgkDOw)63`%`T%-4
zy@Kwf52RPptLWAA8oG;KOCLn9qr2%IdOf{?KA1j)-bf!xA4VTeA3+~UA4MNcA44BY
zA4eZgpFp2TpG2QbpF*EXpGKcfpFy8VpGBWdpF^KZpGTihUqD|-UqoL_UqWx9FQqS|
zFQ>1ducSBASJ79~*U;C}*U{J0H_$iIH_<oKx6rrJx6!xLchFntt@JkfPI^0i7kxK<
z4}C9vAALXl0R15S5WR!mNk2?KLO)7BMn6tJK|e`9ML$hHLqALJqMxImr(d97q<iU?
z=$GkN=vV32=-25t=r`%z^jq}X^gHys^n3LC^au1F`a}97`eXVN`cwKd`g8gV`b+vN
z`fK_d`dj)t`g{5Z`bYXFdN2Jm{R{mo{Tux|{RjOg{TKZ={SW;w9Xo)3|LFnrKza~8
zm>xn8rH9eO=@IludK5jH9z&0%$I<olczOceKsVA8=}GitdI~+2o<>inXV5e0S@djr
z4n3D{qUX``=>>E%HK<7~YEy^0)T86nr-BA_f=<#34QZ9uXq`^c3+WcxpwsjsIzuI$
zrA<0VBRWqPXp6>lD{a#bO=wEHG^5*SkLI+Xi*$)zOqb~j?b88Wr9*l@I->Wdm(WY;
zc6u4zK_5Ubr&rLO^nvtBdKJBzUPE`$Yw3gNb#yn~L$9Yd&<E3p&>QJP>BH#5=_BYP
z>7(eQ>0{_)>Er0*=@aM^>67S_=~L)a>C@=b=`-jv>9gpw>2v6F>GSCG=?mx!>5J%#
z=}YKM^riG=^yTyw^p*5x`YQTr`WpIL`a1f0`Ud(&`X>5j`WE_D`ZoG@`VM*vy_McZ
z-$`$$@1pOf@1gId@1yUhAD|zkAEI~AJL!k%N9afC$LPoDC+H{Xr|75YXXt0?UG#JG
z^Yjb!i*zsj68$p$3jHem8vQ!`2K^?zn|_Oan|_CWmwu0apZ<W}Lw`trM1M?wLVrqs
zMt@F!L4QeqMSo3yLw`$uM}JTMK>tYpMDL}4rhlP-rGKM;r~jb;r2nG-rvIV;rRD#L
zqJ9dDt)mCf1L;BZV0s8WlpaP8r$^8u=~47(dJH|59!J;H<LL=>1KmhZq$kmn=_&M7
zdKx{Qo<Yx~XVJ6iIrLn*iJnK#rx(!8)SxD{s7)Q}Qjd;Pp9&h#2|7tDG^ABpqjfq(
zFQi*&gHF?n=nR!~mNw}ejp#gGpe-8Ht+Y)$G@&W&(u{7SJ(|;kF4EhMQ*T!FICvjp
zGo?}4ARx=$hO*gMHpTwlL1hmO0=lQCVNB0OK#$Fo1s2N&fi(@LA-Vi8?aM<CV>A25
zaEZAng$BFdR;D0;Qka1T+eIGg=`nq_OikUlB<A8CU<NKx06mmPvR(-H5y_Z_WUQf2
zj$|}QMhdE68ZJ>DGteNQ8(lz;p@)Dz5qfA4z#4kvn1L}wi!t@UhA^h<>06As8_T?z
z|Jz_pN6}l8q7Q@B{@-TJv^AHgjQyd&-2QDKfHA!gUSEu@se)4?4?WDqtA!p;u3tQy
z4OMgxZ!TRF0hGdAB%{Hlo>r9G#h7lgatwRH;^<*<6j-wjr06_-j+wFw8jR^Tzg}`(
znW9%)a~YLUT^ZZJ4ra=LJ}>Icm18&p0_dS0dipRkWgaedJrva`*e>Rxrw@&t>Uv1g
z`$J_!^%}aePDT{d5Y;JqSEz!NIknq;Ul~|@eR+vIy$$p>?hnYahX$3g?o2tRFZGg$
z;*=<=ll5Lu3IR;hRdh1S>ou@(To!6s{PNiMKEUn~&<(1f$;xDu*F9V!8NK_8Vl};c
z^zJJew+Tl@FDh5d$_B}prc2>cM`y}BG)P7dDS9=1i6|NrK!YgO)p@$NrXKDFqF4=k
zMipG5hf?}PNLdrr+dwJ26ljo#MhA2~^sojReM=C)G&!aoYZ{c&8^>o5bFn0%7()*Y
zO6j6F0t)E6ffPNaH;KjdPH~CVFs7HpLWs_ldqxV5g#eNf)t4C4w^J{K02&m));5+!
zXZAOU;u1$iie6Ih57W?K26}6DfVpT;4=Fg-x^gvqjxeUTfk0G7u@D**MK3D@D1ar=
z!(2?md3EwknXDH_3NA5CFOD4`84W6<!8Q;@gG<asgM}~ymG#Nd!%h*!T-4KxqlXj}
zMHG!rMieJnvn!pfd#Hy}dP5jPfi+u00CTZ%T}AhBb8xwFOg%6IRq&3;$}!vn-9s|Q
zX38jL$g+oNx<Oz=8Nggj!zFt6b%`-d!(mWVSH^}AMIJ8E=z3Uu<Nm$iHtBmcQwET{
zW*b-yV<@j1)WZxtSMNY?ZGBl;r(j00e@xFnKu57P<e@-T&Oj8$($i2Go7`BYpob_L
zY!W>jAuCgKbj>zU0MT`29#+FP(3>g8Favq$p+Qua+EC_UOs}T%Fc)iJYd9N{bpS`e
z7><etw^LU^iazR^0w{%KT~r@hzcSb<%3~Wi>fHWQqPJ#DSH_aMK><W{qmPd~oEJSL
z<MO^Xi4;`9p7By34-KMdX3CwS(aD&B`=ZCN4V+T%08z|EDclnD)-<SxWHiWIvm`Ec
zv!=kBOML|F3THzK7Sj7e9>x&W$H$B{JHRvqkcUf@M;<QmGNPU?p!bZxn!ARmuA-As
zZ{wJHU>XAGVR4;@g=CqxCRuNCLm9=oSO`Z&gEeN#sJ_G+n1Kc<SOb@cqCqM2kfIyZ
pL$oL>;}Q)nv5+3qy)pGD_jqjCQIFI1F=igIuX8h9J%8Wz{{X;q$-n>r
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/read.lock" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/FileContentIndex/read.lock"
deleted file mode 100755
index e69de29..0000000
diff --git "a/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/v17/.wsuo" "b/src/azer_robocup_project/.vs/\320\240\320\276\320\261\320\276\320\232\320\260\320\2772023 disk image/v17/.wsuo"
deleted file mode 100755
index 4776324bb6c98704562f635042663ac69c7ded79..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 93184
zcmeHw349yH_5YeA97zZeAV9cG2oP$=)@92IxnxW7DIbz0%SIq{OSUfSw4H<!8b}Lm
z`IWO=B~Z?A6bj`k1PEt2+Hm#HQGT3-hW=@xrNF-}<o|uU+E|h$du2N~COi5(&F<{%
zyt(JidvD%u`@=!+Tzl8TcbOcwl9|DLvtvJI?~L%ha81Cv0~zLUT+9G`vt!2&3Zv^q
z0Njdhn$&=S3F8-MN*OH^f*)lDsVN1!1<b6S?Y++!Z<QW?+ADK@{Cdd=i1(<Di`)*3
zVWyX91a=Ruw9LR5sboK1fF7Ay)!9$;-@S!kbYIrBF$t79!1#fmz{?nMZwI&;Gb3fH
z872M;yCuwCC4~PtL?OR$%<LzBOWz8?UzPo8)*azL86n>T2*p1WasQi6h1%?EdNIcx
zgzzH)hXH7OlOsIG0_Fqe0QLp!2cYMK|1JQfbrk%I0P_I!-u`L75`H=0007M^M+24s
zC=R(3)7Mm99_JCbRss$N91mCk;9~FCF*oCy4<o-C;U)mJac20pb!`UTE`SBl4d?-!
z3a|ox0yrID1DpY{0~`P+zzv{0df<BjK0t3etPg$w5CnwNVG;QKfG8lI4okpK0#bmp
z0ArQ;+Z_L^J>vLBldx`g8UK9$n1x)?*yYDPA3hu5G!|(r)A(Nq$T$8^LO6|odXL6G
z|31w%H2!I<)A(Nspz+T?Cx0>S2{(;@8t?r0r!h~@Y5bE<<DdKr_%y$W<A37ge?98)
zLck9I8vy43sJ%DB{~_Q<>HG8H{}@2?7d^iK{{H}emcFO<pm2h#aeW!!kATYoPXMk2
zTm_)#*TcUieSIzb>j1w@-`@cLM!+qAo6}*N;ol0l4RAZ)4#2MfcLJ!-{TgsL;2yxe
zfQJG10qzGp3V0CkTfjpA!oLOnBkAkM;BN&y4tNUiB!J?)3jZ0v?*T6Wo&!7&pl2_^
z|3muvMffkLuU|>~6z2_GzXo_c9rh;tKc%nfIk|Um|1RLYbT}8v{eOh8zW}!9Jo^Oq
ze*^qIC;Xqd|2!S`1^h1oU#0Ie)1~Wg5WWMzl+YR##JbXsb;!x|;opl@sEvtWJ@I3m
z2{AE>H;P*f`eR=_KL|i+j}mp~qf2nLZg1|Er~ioViy$XqtY&V&Fcr4*wY(R=&*CNA
z<nl(!32eqawFSA+=r0ZGAj(B6_)q1Zho>_ELh;j>Is9NQ$y$^+jG7Ok?xXO9mYaK<
zVU7ZRG$Tt-3B`Xg;x9=jupYiR{>AYxj{lMSzdd36n=#{PFylI~5B4%@#)Wx^W`CN4
z8!$7)aBak0oLv!Uf1JG{RAc@j8j<Kiq4Q54;_d|?H`@Hm@4t!uE97YZI|C0F1BBu~
z8*yn3B_{-b4Hq@<KfnJ!9#4hhKNoR7O{XRVzbY?gE)e)xyo8(FC{cGlIuBRHlwSre
zNFnz<)c+}XxB?&)|I>&|v=liZ<(IJ>04{#+Kk&17SK@(C{J%roi_!@R!(X@uQ27b(
zsv_|JAH*g4m7FmAg?j*Be#!$mF4fWhx8bo6`iCEXQbtlRK`Q?uJUtR16#q{VmuOXT
zM$ny3(4WKwOoHBWu`^Ev!o#2ji8sg5p%qLELhN{62R{MIG|s-iavc3Xh&PF5C07i7
zJ3AZJBCmGfce4DM)#Jb)0^T(F%jg*ccvYn%%3I3lQ1Sq3BYVDOGoA5w;yCb!@qRB&
zns7q;pMr~;_kTR|&j3>TFrAtZ{L)c4u>AO|M0!H|-vfwC`!jOF@XNT!dH%;U{(ggW
zXm3nT7=AeyInO_pzXIV@c5->5j0k!bAvdK%hu41E`9;CEB;Ggm?Xh|umq6(_`d<@X
zJ){7B8eMMWp$}9@7$bnj`Di16)?Z4C=n0|auSeVs1@O~8)QKDj-2r3#vGYG$`2y=-
z2i*MplUx6UpQx`8I}^HCdB|w_A^MAOA6`WHwHf%YD5+zXfirDFEmQl)aBT-@Q2S2I
zvq3g5)CN|j9_<kaRhR&^XvMr0hEID}3bit&Ggr=(0v67c%v?27hBZUR$T73I@H~cC
zrSNH&Oa08s&UZBP-#^9psmxY(ZE+%RBnOJY=T{voau~pU3_lynPy7i7Q-R(w2nooC
z`l~<r`oTAkd$NAZt(Uxh%f0h9KE0^oKNnnfweGjqJ#-XxFuqstK80nw2dB9Fr=o{Z
z3&l~gD03QGsf8^sClTbHP#E=w;d&V^L(MqGe+sW3mQF&*{8xDPBR<AnC1psf96+_4
zPIgppEuI!be;1~HS^Qb8JSyV!!z<uFn7$TL{=%!z`0$?v+(ZMC>q6VpToZ=GNRJi{
z0l1Z*0?%j_2w{ziu)Z7da-D1(?LP;JSkeiNhQAIo4edX}m^bRtJHxn-Fs;ZT%}U&=
z0Q#?l(mN3#wEbznI=4vtoxn~MDa{O#G;T^_S$h6G0oOwD?}xZN{tkvuIhMnP)v)8u
z&NQQ}gM=!tqxF9|o=|#1@zdJFPEYWK^}oVBkf*;0H#sCZruzkAtV_odhQII(K=c>!
z2aW{@#eZ<gu>4yX{=yXSc*=hm@E5oLCW%;nN9A9JM{MN_tbd~Y`1<E%5&ZiT@ee_{
z4h1X#P`Zc1KLW5YeLs=3#dmj24HQHF@Kit`O#%JV@{uxrbxo=Nh+m%Z_sfDO9=z}Q
zUmDJ={qx#Gb?VTDAN`toY;LY8jOObWx`5j~=s!K?_WU$o<`Wy{eWGDG`tQyMo6mZq
zV%-OIk<UZNT=>SB5Bzs&%ggUSaoS&vXMA(Pnk7G{(&Z~Xr!&LP|68Vr{%gd#PE_AR
zQ;dJZ6yc|x2vI&<Ti*M~5udl7yYb;?)URJTr)mBbr{DU+FTdPiTD&Q7-io*Oy5RYI
zZOOgw^2hp0{Xx6eZ8e6SPIuI5_Q(9;kTvQKgzc`12)lIey*!i%1c*mT&Q-U}YKpm|
zF{|C$pkcRPd~?`9T#z7;NF3U78L;@%wq*D!J4OW*gktEtA!gI>x31cpTypuP7yaj!
z#lfC|S;4`JZ>ac7)hU0Utvmmen@Hw6vOAVC>k&-+@E-tZPkavGT);-a4*@>{5Wo0*
z0KZlc|E?6k_ipOybbT>=lI>m!xC~GRxEydDfR9_qr!^_}+06*M1+W=#E8sQ&pNGu2
zG|}7*xCd}3AoH1cJ$elc_exbpuhfK{iJ&_ak6G*e@rH!MY6v^Reg3#r*Jx1NWD=Ro
z8jDBmakqCcZx1&Xl~!rHp^YCs`f!{^k24&MB;szHIuP;MZAnF5ER902Y}>SL%eK1!
zS8Tg;+at)jyfkCX2D69L@Gu*`%8p?JS7Uh4wLhym^ZfYj8y>!Lq5MqaE6Z#jNw1xE
z@2r2l`QZIGENWcBZX9##Uy^o9$zN*shw_%qAh2wgPH_I*$-dD+1=j!pAQg~UH!=+I
zc7(KH?x9T>%}K<V@?tZ>O`7jN{K6$~eYoY;2P)dXe(u6MOiz72pg*DdnC3alekDD0
z;~gYr88H+k>(<I^q~%iiTAe)*bDzaf9k3O#^N%jbty^0yDX*4r5s6$qf#%HiXxQTq
zxKFSa?4n~dI9K6o*KkNiU}rPKKU=p}Ql8EL+xzn*z2O>1i+8{IsaHNZ<m<y`y8mcv
z2|Mk9yiMv9*rcO$*Ypy@;_%1p7%JCIu>FPXe?<8Y(Fr_X?7JcV@ulTIB^X%5JEvP(
zTW7)F8?aCMp2Bz_nw7uK^yjC;i4RZf?*ahv_lPHb1YjY6<b%X>I2v#afcER9fa3s*
z0LKGJhEIGH;^mwKSOPd1@I3(066JuU0Frl*yoaQcQUGx$h(kcTblNG?v`ajJ8h{Ew
z`(u((6Q5)SfatJQfYksspcYUE&;T;a)qwlPjIb75>jAA9VI8<Or0+Z7n*e4&7r+AO
z2J`?<1y}*60Zs?l0A~Q~00+PcAnuwQz~{k-Yd@ejBaDAWegxtDfGB{tVR1kLkOW)`
zpg4o@hX8BR_cW273HW~c{>}-`9~0&ObNkQCT~;>FJ@W~->^%O_@5~jC&-(1F&~cyr
zZm9O7Bi7%(`G>c@^y&5k*8a6c{tk7D5z9{DHDjvz@5PkghL+~G16)m9%B*_#-yJL7
zdiLP6mmGbzJ$CZ{-fU`cY(H*Z?6uR@xT@dGR};DNn_T%5Nu7xwO!D^x@R96kG)MAh
z8viuz(cN~87MFi^FfJT4nZOmsKZq?5$(I6L!~*}k{Eg-dq4;TB@X6)k*Q2~aV5M_V
znY?KlU-?`)%1?NCmoH9k*g*)Oxq@6S{`~UK%$#uV@&5|&e`#JV2LFMGzi&E$PRu|g
zoohk6bGlwWw)7+eChdzt<>v8|{wu8+#o(tF<?(0ozdM1Exa-;4$Q7s~B0ckS!m0c;
z<cq=YrT74HYV>y6U=O0CR0E_B5=I`}SZOp^6|`7c<cKB1N}|T9Qq8U^+`IHo^4I;~
zQ=g_hP}o?`g9tOyk$8D;G@XxOtq)!^+Kn8=S*;h6g_ArcvsFm`Kx-_msY3A^5%<EG
zTn5HFf0J~cB*v$6{0o{cNSvE*I-}vlGuMcPMnY6xpzMxLmiT$({-w~qWzl(|8x`jD
zS3iB*jx|>Y9=PY}bzAT4xoxl4A2{WXCfmz*yiOIvcPHK-A;0GNM<|I6i+%+E26P}V
zL}u}f4dM~5)sf2c{`}I-%gR>GK33JH*!t-&&iwV6*Ix8wWQNMU@9}ueg-WBo=S9m>
z^>K6i0@vLi{JX?EuvNLg@{@%toi7f|?R)>Jd%IfZeSFT<&uMM>GFgoLoo~O~`G=cn
zIH?=0Hfj_t0;-2tOuUqs>p9#%`hQksWj-r(eE=S$yC23It!T8)_*g82nV4SGm8hq8
zU{0V~@#8<XbCw!T8S>|nQrs>92<`uw^MB^dnk)Zo2i6eVDlTYaaZEX0?g(_O8&CP?
z;m!2&m$k;^;y1G;k7KQ{V<qHgXv(n*HRD5l5DmLm3DML$J1U#2(f{_x<3rL>O5pDW
zp!taEh30aR{^#~Q4XBInI{lwJ{zdw452ODQ{z$vs*#`ked8rvP32<LmNR<^5`jwZ~
zB?9q8)V(6)PJrJVC@*bKI0AlWvwN^J+~*Fha7g5yDv#15mAa}VcDa2P%_l4LerGfs
z3wz=f5QZe|0b@Mj@`o!r-LZIQI2?#obowLs8%AAWOoVk{>uKz9JEd^ka*Ato$K(Ex
zH%3oZ_StN;$S3Y~es{oCLX+0)In0`M<JCOlNEOQ;W+NfmJ7=;smt(!-bY52C^y+L-
zUxjq!*q8Cg^Z!O_y3tHcD`^Zq?Pgq9^W#|6xYa&rkHy{5Vg4a$mT@Q7pC1M5ux1|d
zacDiAg;{$ee1S~ToqYU^MiAS7Cu@89v_I)_Po(|VqtBcT_yJ%8;2glY0P0IW1pEka
z9w1l#cO1fannRTT@ij61d`{GnBZA2hf+&^p@~X5Pg!+QOIVO*^Bt!C+7G-3J5rxWR
zs+Hvm87CH6$vjn3cy1n^CHd_p?O-NHf4Pvr-U#YE2u`mNn?eJAZO~j)gMz0JdSb)Q
zhSM{4U{x5FB)(fG|McAEEAOzM>VE1%^U`;&fA$~#eNS%cF+>ht{mvy9KY?oBIo|kQ
zMk3jX>dy$1y$Wrbo#L`9Cb^sNV&Q;27PE0TwwON%Ih)X|$z|2$@+z*|&;&Oroj4MX
zxos|Y%<m1kqeWz9XFbuhI-#79oQ3B9eE>mDws{~JvT4b@vSblM@^o9&%?c%#%uq4e
z3M&)sKjY;e6&gRZ_ZOz{BKzP!UC$m+-nHeP{a?OD{r*7*{ZsXy9~}0Rp2s#jmYj6#
zp%<b;MvNE=%XZUm{>W4i8lFGG8*bTjyXh|dUDtR1^{LOEIP0bQ%2PetUwQg+oBiZ}
z|Lne(snq<u!TS{UZM0wh8Sm)pBOh1pf8FiB3C(YR_2s9ob+s&cviIxFLoc~+DO=@x
zn*>`Uay(Kro-2Q+x3aaZD5S7^cG|5UneCsgjq<Xt=AJJd*S@{-jmtlKW#inpX5N3_
zBmGw`Pu+I6(skLZq>VeWMzghBhP7UXuWZ<}(76B?Rxi#*MJ!|8y0|B1HT`Q)-E->Q
zx4--L-0K%R)<3qQwQ=(kAD(=eMZ4h0d^m|$&dYzeU08kvlV=}XbpBOme|_wSJIdBB
z`10`!A6@=X^n&LOKI5v5@11wdj0=hVHKK<Udwyv#q`%n~IATmGB)Ag%q$nr-lOxK(
z3NB!-k_>G_NuJU@PUVgq<-u`~LvpFCT&Cm}wo8zc;+7L}{hx?7(ez*ch3Wq&(6914
zI{!dA87cs+EBRw(hkl6L=hH#L@K<Fgka0)nANci5DE_zc{%b|yCry(4j>=E9Lpmoy
z@&5~P|B+5kSouktCBGX_`TsAH`XZg2IREe2^Z!om_4MX}x<xkql(FMV?;<kejNH*@
zC?`UXm))?{2f~i=?2ax2rs9^f0&2dEA%EN!u}AH}nAL!-5c`)7sOXJ_Lqd0vJ2!47
zWKFZoI{67-Iay^+Gg$KNoYU<{g}!mp=--jF(Xaw>Vf6r%Hgo!Br_f@~pOdy2oezz$
z+A41FSJa?aBjB~$ah-%tASX|6_ujN)ao>B}w{H5Ucl~}F&-=$?=j^p{$HvBr`{Z+9
zTt1@?`52KARz-P?9s0-}$<7g#OB@i2?4gDFFVJBNH(t&-8k!*cOKkr=yZxt${{Qze
zb|z&dNH23j=X+^&xwMAcTjBIe9*%Y{|6#i6|L5BI$kc~v$IMEaNxbqFyWyr;EW=4a
z|B^ll@garMzoY~GvW829R)$PFPh{PREWwjL6zQMlvpK=N$^AqBvSa}+`9&o2S)uE8
zJh(Caunn3uz0f@J0vCsSSjWzsqwAM1y+y!J`+wrW3N3#d;u7D0Tpi0@A^Rd`l-Ug{
z47s`B;uZQIkNwk3{U_1>-`Mv53-A6|`x~tNo#W9!LffCj51*zRRLK61_SS{mc<gVH
z4DWC01atL&rmOy6G2=h8|7Xs3x%zk0ME`H++$XgEW$OP;g8d&>|9B4ilTiFA#9fr`
ze?t1dJRLWmC))ohp8s_gDso%8GDqwG)5QKy(fyw^zJ%!CX4)U(t@Pxv|5G%6IxoQE
z&piK-eePkjQxECJgGc;FI@d<&2_1iQ_JYogkjtI_$D#jOGR5bAimw06`9HV(d)oOQ
zm)m29j;Adij7X(gZABb<m$9_oCD%hn|EGS(3KmC?R`BUW-?yak?+t(bl1&%vkiA&?
z(jQjUetPCh*WIT7^M#M&`ou%RKN&o4-rcw3dhq#01Brism^kIA)wN%>{g34+)8KQB
zN3O0-RDACI;^Yrsdkk?Xvv<At&09a6xtfiC+2zk|`*g-?Hk?m~;*cXfdMf{L9Yj{+
zR<N6B`2`~?5OFx)*%A2tNoHuiFnXRZd?I1iv^`+jlaV2mOk22C?A83>`H?la7=U-Q
z2fG)*vBS8tQ$pVNV0TOMUOM}g0JZ?`j0j6W-c6Q=tvLC`nM1I$JF_72L%O@PD;~sC
zGeWHyHryzG#&oEZ<vz;O|5C`v=q#BP?~?Uk+V#4y<D*l-Tc#}MLg+903R)pY>kmoD
ziAEE;{(OzN|C>%w2!3jTLT)_ruT7L#y5kGMPc2u-QTeIdB%u^q{+}amG4emz81|24
zNRRfYLh%#s`vK(WEUF$ok+W<_J)d+usQ=SBOd4gqh_M3agP>@PQ}iV3Tmv4v47w0n
zoQG84Uy8Gmwd}PVS7fbOgBDwgSoBPdE7GP=1L{zd61+*jI`o-!;Lf|i^T@`_eSe0-
z3Ut6)*!t`Q+21?apX0M$%W1D<o>3p`^ty1(@?ELDIxLSNJ{+xKi>0a>w@l`$S?W>B
z6-yNog=cAv(kWZ2bj##+k4)*t8IQf0%_W*;68zSvWjd`=p_MMJRVw65rBaP#sah*l
zLvvUysZ&XGI$fQ7-P+}Pf6Ph8H)1Q-j#EJ<L%_)MZr84<sg>8r@LO69gFb{^y0oT7
z$yt)o)hMf!DvfO2;JC9I3kMRUHVGSEu(6VKFDnxBqf=Q+ql=CN*4+i;FTse)o0JUw
zX8akNc*jeZAX7O@U_EXaf3^$N<2Z70_xN)$@yg2;gRT`=b!U%1(<7>Gym@$H{CSvo
z#T6bmyz0Ban!ITIr5I6@la{F`$DgTPTD81xguuGJc>KjErf!SO6^L8*t8hd;`B#gJ
zwh(e*Z$=}|ley<v1df6Ob0^pmq4o=l(4UzaBtJfYw+1-cA-M`J(ptZ{w#|@|G}n8>
zYPeRT$)`1WaaEh5duN@xn{Ea*t}+t^s4cBVgQQU%jVYYf9TZ>)b(o~JYIWT}Zz@^S
z-N8m{a%z1&P9&j;=@Cb%Q9E79{(5@B<T9EJwPvlaC24lVjncTKp`j`v@9%Hwbag6x
z0a?i0-KI!{9L=?{rY=ciQ?$;RYA-8m?(VT$l=@JeuG`X}iuJ0z61}clRUOi`RLLZc
z<{_6aRqgCk1RODAnIrD%k=C}DWX66+r&Qmmh=zhnRhgoyRq5%BYqSwvyF;C->8on0
zbk?XUgDyG1Us+@B?~gQ>#cKMxn&n*{qrG#`9+8IzoP*V7mCNFi>Rmd0Z^W)~DBV>_
zM}4Zs7|^MjG?Ffz&z?#~6>h)Q-fOAsl&Z{fNttCJS{CmZlIglq{ng2+y}l|D?X5O8
z^h#9iW?xU2IT);IYr%~%ka9NEMhCltO}!3DwXvgH)o$0B%z>VuD`}KOnmm?((k+c^
zIwTFDltNqYXpfk*VU;47a+?P|&bkJT6vxiPgYCV8`fiOzsWH|0wUzAyHO8i9?VvNz
z6myq(jSW3IgR{$>a0IK=y;Y7DeR9xLr|$5#g<9;f-f&-inX@klq&?AY$)GpV><T%g
z@t&^EF0DLXZE!VID%$NG7M(ZN5^#i4b>5nKb-&b|R3wIaVr9KCrQTTOEOW&?(Xz@=
zKyUV2RIx^x#9kBcRVykR!pW|V?p~9_ByBZ>TT~XOym2rX@%N~jy!A<`BGK0CtyaYq
zGQB2<qx;fUqhB2`_`3Vt<_=k7e_fBZ+up77EA5`P!M2okAS8D-RF$>+Et)=KZ+&G`
zQc>ejc;uDJvQ}jz+1cN!lr$z=TCq8FTZ~;TvU;tgEMzjP0`*;eT7xOnpfC+-6G2OD
z+^y~&G7iMLL*A%Aka7=I*UASJu4vq#?Cn+!IO~(fn8MZWPbK4xGJV)C>2LOin~ZJE
zYEPT5)7)W_wP_mlef5SOyRX?&AGb)O&XnF!R*#CWY;Cd2;{I+|vOCpgko44>dK-hy
zm1=Z`2IW9sn|{bRSl5%P8nm=(wLK<FO(38%S?U|ZEk<dpwr{{8^QGEq2Mjui)2@{%
zz5bLV);*9MGDiG%g|0g=VDa~uJKKX6r!zL>RashNbv@P9^(JMqcfhQ1#Ht3g9sQQD
zF`#U3t4qaOhUz=aJ%JXJ$}j2b@-%ql4V6RvHAcNrXRmK*G*q?wbef<;(bd+Y2`1Xi
zdW9$IjnzxbJmz{`r?OXP(li@Y=5B}7W%1PaX{yR9b>3)qRdtUR+v<Sd8?@-VBm<ph
zc{py?YOCd4<}!WC=#55ud>u}O-lD6QB~?CeZ`?2t?DY9f&1IFI<bX7y?+E(h&B}VO
zvrgOPDQig?&1D_xa8;~Mp)}RhrTSw79W6}*UUwwbUKMq8_~Nc)yenqv2s<Q0kur_Z
zQPpZMtM6zwS_~eGQeiPjhtv(hR;^!Rud9o;%GA|0U1dSFytB2{=hHSxnj`H2xuVw_
z@w!{|PNT}+W{eu`si;3<Z>kFn>AF0AiMvCWkjf+yvr4IxNiCYTHhZ<Ntkv8h_YKL1
zG`eJMXHU3alQ1>gQ$0=HgTCI5P*ci<aUE)mRrMRXhjePQuhr*NIjc-v{<0xibB#LO
zR$UuzuWR)w`qYxzShcZlKoUsx*Vd|nolQ}L#9XiM?UwoUy0|Xk(%1Vfv9Q+P+1}hA
z4)>;pDm!Z8YLDLIulM(byLz>Ws*v7Z)?F)Uv&+1R21kE&BGO?o%UXlcCV#Ruo>FUF
z?q>9QgHh{@_exCRLG_T%8@ALcOaV)>*%<05Ge-KF5{}-k4ogQ&(HnLfT(W__?s|<k
zm26ct8GtF?;c>P#xAyy+;|^(ys-sb(Z1fs*9X|h{yWRt{xLvJ{PD5*Lw`OQStM7I=
zT-9!QQq>->luK1nhfJaG>63Mb46R*F16_?UnbEJ%D>@CeJ=N0g4&_i+f1}dnZf!U8
z#>=WJqtfbuGEaL*UaeO|T{>AZR-M!|!z72@=jv-!Dl6rhaKB!oi{m(Ljb9Q`B*U(z
zfhN6ItLV{0BK9tKDAirlQ&T1JCHn%Ap-P><EgsiKl%b9xZ%0={Z);RP;7&C(B^%=Q
zhU$j8wy<iT)1ON8E2{ndRhmwf#A1o~q^*+Hh)$vKT54N{>~4?CtPA;_o<6f~(5oEK
zc;YP{c}=C>ZHTIqaz${kHB}Yus%oSaQLPQ=I{S=?j$j?Drb|RE?+#~JLm>QZhOZI#
zZn<2nN!z;}%&GNfox?7fJoCo+RMxq}-p7qugxw??BGo8TjUv@3QjH?jC{m3g)wp|7
zjpb<-03sT7xF!{DjmRq%!Ykb;Ef6i4j8Cv|R%>=)k|HrC5@RAUCK6*JF(wjYA~Cjm
z5@VzX%{$WNoe$86dQ_Q8XoY!SWD`K3r1pSJ)*ap}vF%E23LrRo)ND^NdXX{|DMOJm
z6e&ZIG88F8kuuypDZ`U^vJgI<bS2P;+M~Nrd-RY@8j&|0U6T3L@^OBAw=gNlV_7tI
zl-(vHBJn5^k0S9X5|7iGc;xL`&<V@pPcA!QVUg@&Gt5@NbUb?<#Mx3hiyfWfbJ!DW
zhZy_)Qfi0kat5AG@Keu;!FNupy3ft|08Ym%rgaJE#5aAjhGFP4Gt*%d=e7aafF+v>
zT%Tf?N2h5y{5TU|gZ@Nq@ys+WLx8n!(T5g^V1(1UGL`l%iknyY9>mW|^j(U;v~G<g
zMg@(ID9Yi7txIZ+XQye;qOwF;>nmi1E10&OGF|2@vSb=Su01Hl@Vdv}PoKFre>u3&
z>9mGqmt&+&9t;xJk?j)taaK%nuS(K}WcXOTPp=BGk};D*l3sO9m@mnu2+f+pte}$)
z+W=r{?4YDGpW}_R-!fQGLYbk1H=GIuDuh{NAzPLtFJ{vjN?#Q5z8#i--LOKFLY+t0
zR0JZ<evv*0Kv*02_UHZly=LT7K7{v?7HEM|)(X|gZ^{naZ=rY-vEL)w|KYyuB&@e_
zjZWWk472O1X#YpF|C7h^kf^sZ>3S=o{hwuN(THgOXG~LLBV=Q_<|%|~ONnPlci9<I
zQGz?7myRv_6xwe=ii-AsiZt3M+W&Dc8}B?hHKuvK6(d8UlwXwce}|>~qGgyp+cJ!3
z|4Fp}B-(!x?LUe3pC;8*`6ydC&XSSPo!o9|yH~XT6m~k@QEOpCRnunI@Ex}d_t%W?
z(9FgcEGi)`(}=v}VP3LmHCD75D_V^et;UL0V@0d6dzjT&(f$`ca1^!2-QL3LStGxy
zG$%v;T+%sd#v?LPoj+30{jDLIHgx-B(QV`Vz|}4^*0u{w%I$HDDt^L2{Dgz}2?y~L
z4&o;q#7{Vk@d*cUso1lZirwWC9-{rf-0$TUp&nT9bj`@^n`r+p7PE138`PS(GZ2-#
zzQf90qH>R@+%tx9kNCAe@oRtL*Z#z>{Y|5<{YmJcHR~jg*5LX`T+{Xe>K&{@&>~!9
z28ka4E%X6UvU{b+X=bvhK{j-(IM*D2?SUA6wyY;M*d=h|NdTwiBd|BM9PuLv2{9{~
z{h8TQWPxBjC$ViP8`%%poyrkJ&5{j@-6>BIv@qGDu)zv}o2@f1a!6<1^XF2-EJOck
zL>xL(Z$s}O%TjjKG}*H-;7#g1!&licDwtlxqdvGh_AIglL}iJi6khcH{53%HPcWzC
zqnC^3vSMw17*I<NKp*OvwK}l_j-M3glE&MB-tUZtV_{Fc!t9SF?D*Vk!sQQFbl}n-
zAM6Z=1F?!ue*}LCf27^+?6Z5_<)vnKG=`JgE2PQ_3H{1T>k@%@BI;fdawqT+=|FjD
zd%_X$JDc4|qt6{$;gHBZRUV~BDs@#!>~i}o%EpQU*gM^^cz)Cs#za`>54bh>m@TDn
z+;WO*bjRcV5I)3>CoB7Gwp!#9_d35j;9@`fI(rVYW}TIpi0uZ;A7%p|Ge)}>wywgy
zjK5btIppiZX1f14(pE>D@v$>!41O5>--Z4WXRR2~NDA6xad&inihrMGR?ecy^MYnn
zGn)GVyfK5B$y@^e1XIi%ZI6aMsIwESW83M?9)rVUbNI@}t_LCHo93t}dR+{?pC79i
z_r$EGe+{a8PQCl~ci*0S{bI-Z$5ym9Zhqp!lMl0K7tq?wQ1{Gq2P4dpX83UyH1dzG
zJHK~&>dy6z>RWF3#k%*7Kkxs&@z^Ta(yu=H_n!{_=aSEsoEpiOws6Zi+$dVi)S^{r
zEDe{glrggwf21g<gpm>&c1E)@I?QecMvWGf;fV}C8f|2MrhA;45L>q0wQb9`E4JOa
z?Gc$oCNFjQV|}ImAcmurKfk(0l6FhU&-vq@ty?Rtl9wx_Yjt*fKw75a{O==YqxXno
z5aU5XJ&m*FR?pT%1Ntx763d*4$eLquBe9s3!pW{3WWkJ9AHMf7%;CuY)@hLc&JnXf
z<|twqvcvQ9Nh=3gy`xpPFWpb2Od(4`;>X>^kGt>QA9ok+|EnZAoo)i+A>O#Y&@)G(
z{r||IfDaLf!Xr_5Bnpp2`~TD0{=aDdVb8YzAfEqJXr<q#5zCQ>8-$W&8%i|GB>1gS
z%XC_0QQ0n}OO$cyLTE0;^B)p$Mp?JkZ?0`Kq$JJt-mn_3)oAi*O<r8prs&>Tr|zbk
zL5-`-L;-3`tI;57R7Yb9XLSb!7(yK;X{}maH_)3()^vBU(VCoEUyl<>XkvQAQEJpq
zSF*pJUNE_gCPS@R>zl0eAEN%}o~{2W>VJs(AEN$;sQ>ZZ*8iBejraT#s$5AmnE2C7
z{+q2ok-wA-GY{nwCXf8~q(nwYqUfttM#cm=l!0ku>LHoyWJBmlwq%necA0XcLr|j4
zlO}H|PVP0jM3}TwNGh7l(6r!WjL+<X{EGTNxnsDntltT#Cw+A&1o=J5nI}^gD(e3X
zkIvnrMTE?ZxjCJF^S`M7BkKQ*rKLyuprZZ{>9>gbKa;uUiTXdodQ4NL|09{XY9^=4
zR`_dUh1zLs3fx&hYwIv9lvAa3mc6bE)qT&@7-yLMkpB&nMvdq0x<=|-+n|+Cn$aHY
zdAwQsm_w11Ur#6}q(w|~n;NN;c5@WEV#4*uvTHN9B%GGCLOS@`Q(qr=<=vlsam1o$
zq91R+>xmWmOJDfYL2KvN2ZHa+A*v};Ka{R{y+(;--C9Yxgi~ikV6OUO$m$H(aiq6C
z5O&}|FD142@=ziWSV@;~9YPc{)gCC}mUf1Nu|8;FN;9=EN6Kp`)odK?LU0(u4(K?C
z{INJt$aoZ&mQ2=uV{+T9?}J;{-m9-W_J(V=g)Z5C&6|Jv`2+7?9Ev{w-j;aDy-z>1
znkpf;&8Y7btA9(1u0pDKmslD@86KUq`Y(<LP-srgcS>S(-BDf1>5m?;{gT&Sc=D^q
zf7bPW#ThSbUGl3RZT#%8*FLYi?-eVnJ~yJJ>;bF6Z4cNYIX(GQAw7B9Ca5eIR9lYw
zqVAY49B|p<iI6*LgFbrF9nCv7opsC3p0xVUoqL4OqVrdU|9lHm^62)p^Ikpeo0Xl%
zBs;J8u&z^i_z4$0z4EMes(gK&*Vle%((*q!{bzVh=6GPE_nmt#UHIfLZt1C+=ep;l
z6X*QJw(yHXUzmIPm)Cr3Ic%+JNh+1E<k@zwt#sZXX0;1AYLHjSJw5gdThgX1WY4zE
znSHcjvJM@+f9DgmKmYGhhg|WW&!4$G{J`Rq?ofZ=y7Hoy>o2+d$N|Is^nuqA6CQ@u
z%~RC=#q$s=9>4I5R~NPEzM6g3j_ucee5!NC)(?JrljMT!S62RZo#L(^=PO{ou<Z5^
zI}<^7C?2!c`{NA>hm}^jK7ZUg?zPmG*}8>NAAJTY+^6!+Latqjk=MPYn#}ITDj{ff
zjRv)i=ml$rs>aB)8ik~lx6*{rZzBJHa(TVQkKJ9z{}=av!p=T_clUq%Nz&raZ)MLN
z!gkW!j*!7=MTXfG@~mCkpEFN156deUW)@0w2E&meqt!|fyLkSjJ<0h3>^&z-a^r~&
zvdjqn4eUPOhmcGxbeMAY;F`1`-#l+81|APzrflm0^=$bZC<Aw@bsyya#tHE%CabTS
zQNJ{zIg1Bm*MM5Wjx(-&UWqAj{iNk}*YQ@k)4xsWzE{kyl5fA<6O~6yVJ|Bt|AodP
zogY7U(ps*#U0Jpi7Ze}za@7>&C3_yq-5vA&oX#)bJZU)`DIY0i+F9KU($k<h!^v_O
zN7@{r^QsZZ*NFpZ11~awJ0C`*4SEoYoul$-<o3Er%PoI;S%;Z`Y*tVm@#ne=otMon
zMWI@y!|e}ET0V>GgV{M0wkqe^cHxZaJUT@sC}tKIO*YBEDh<PoXKpMsw=6(<YbUK%
zMjI`Kde%|ML3|hHz{-w-k(N#v*!W?XEt7`1xISG>kH~DhT&*v%Jh6L>OJNk$a4del
z-gh@_h2nUQ3^N~Pm^@nt#r34IY#K4lLgbFY%EMbO%A~-*d*gqq`PUn!h<}<*shxEE
zU)CrVJi6CKzF!+Y>HV$wy5r}Z^k8uPK0){S4oTg8S5S}0m7#F<x-#swq}h%zxC9$Y
z)~GuWw!6k;I8GY>deiT>uG*Yja`~kf{pXg&!JdIx!NH4fsQ63ODSw}>JO7lMzR6bv
zNP34YNs0Ua;dxAmMseo6Fn#y`&rZ?!E3~2)*9O_^`^&nTd%kpB`}WE=F8}P6jdS0c
zdH;Qn^k20+b=%!a*JZCBP8-7!qj+o!S)+-qTEb2kREyc;G~zi$1U4w|u!ppBlB8uB
zXvZ;s<lp!oeSPHP%Kfjq{Wqcc?XSN4)U~dbB~SK#y?N**_bp|sd~f&Xt328_O>_N=
zsn$Qp5ej|l^7l#Vk>i%%7LxFdTYk7g$SB(>Rx52cw2dxW8$%wsX@c`7CoRn@n%ijz
zZg-YvUO&O|b5<-TN_&jO&;9H57S+moet*m*O~<P|Cf5bbS8>fRC0~4c#Wh;nuWtSR
z3Hc_sk-BDSowN*vwDUnf@%nSSikqZ(;@Ugz{aa$`<I>r-ptZgBCnsz&{Pd=i<}RxZ
zT=(9A2fFj+U6=*m7d|?3+sc^>X9|&-WOJ2<_oc)&6Z!vR$%jS$zp!3bh$_sd6_?59
z$)v?a{=XMI^jv;>A%3FB{}1C-2k2Ol|Brc`x9u+S|MwLBU#>l4k^jG|PWX<8f*Pi?
zME?Ke*qj#1|F;U-l4USrct*%H<Nt{Kf6`6bbNT<n2c;%cAK6$u6DEhr5e`NYakot!
zi1_Tbq#`dvb^Kb-J6S8iY%3wdUp*MU%8rrEac+NBb>{i;+c!LX<wE(H##ffvK9XKL
z@7`Jedh@~iZ&=j0q&nZ;oIlydon*87LwU<)5K@Dfqb7QO?8Zss-;AcskiGV0>ZG8C
zas_LkL#@zfBW<Cv?QM|8p%C4ncJEC)7Wcikee0%wde`r_@w|ULcFtZKcWi8|xKBR!
z#pN^V^0gHEDTVX#WRy=RjB*ev5{|iTE_clD#WAS7jf^ilY`DhJ;@xlf`ya#S8~J{+
zZmnEeQ!Y(^*&+AC9z&CLd~mep%fESi|KS%ddF#V1w?0tO{`GSg-eG#`^8x({)yFi?
zS@tXGp&Re`UA`*CX-;{Kv|K9Z4s@Qy6g)<j{`v(al6`JM=*XB*%j7@Ql7}Y9Z9m($
zy#8M|!SO4~{|X#n%G1^(W&P8i{@eK}6;b{->Ith|b^bVKauel$6FomD%KyIA^NgbW
JZ+grB{y!BK0Ve<e
diff --git a/src/azer_robocup_project/Error.txt b/src/azer_robocup_project/Error.txt
deleted file mode 100644
index 0df3d7a..0000000
--- a/src/azer_robocup_project/Error.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-Traceback (most recent call last):
- File "/main.py", line 55, in <module>
-NameError: name 'first_pressed_button' isn't defined
diff --git a/src/azer_robocup_project/Init_params/Real/Real_Thresholds.json b/src/azer_robocup_project/Init_params/Real/Real_Thresholds.json
deleted file mode 100644
index 5df0ce5..0000000
--- a/src/azer_robocup_project/Init_params/Real/Real_Thresholds.json
+++ /dev/null
@@ -1 +0,0 @@
-{"orange ball": {"th": [0, 100, 9, 54, 40, 84], "pixel": 10, "area": 10}, "blue posts": {"th": [4, 80, 10, 39, -44, -8], "pixel": 50, "area": 50}, "yellow posts": {"th": [40, 93, -32, 5, 49, 75], "pixel": 50, "area": 50}, "white posts": {"th": [44, 100, -18, 29, -14, 93], "pixel": 50, "area": 50}, "green field": {"th": [3, 62, -75, -29, -2, 47], "pixel": 5, "area": 5}, "white marking": {"th": [47, 100, -61, 50, -32, 14], "pixel": 10, "area": 10}, "follow line": {"th": [45, 60, -20, 10, -50, -35], "pixel": 5, "area": 5}}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Real/Real_calibr.json b/src/azer_robocup_project/Init_params/Real/Real_calibr.json
deleted file mode 100644
index 7500335..0000000
--- a/src/azer_robocup_project/Init_params/Real/Real_calibr.json
+++ /dev/null
@@ -1 +0,0 @@
-{"head_pitch_with_horizontal_camera": -0.003272492, "neck_calibr": 110, "neck_play_pose": -536}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Real/Real_landmarks.json b/src/azer_robocup_project/Init_params/Real/Real_landmarks.json
deleted file mode 100644
index b43aa70..0000000
--- a/src/azer_robocup_project/Init_params/Real/Real_landmarks.json
+++ /dev/null
@@ -1,43 +0,0 @@
-{
- "post1": [
- [ 1.675, -0.575 ]
- ],
- "post2": [
- [ 1.675, 0.575 ]
- ],
- "post3": [
- [ -1.675, 0.575 ]
- ],
- "post4": [
- [ -1.675, -0.575 ]
- ],
- "unsorted_posts": [
- [ 1.675, 0.575 ],
- [ 1.675, -0.575 ],
- [ -1.675, 0.575 ],
- [ -1.675, -0.575 ]
- ],
- "Line_crosses": [
- [ 0.0, -1.175 ],
- [ 0.0, 1.175 ],
- [ 1.425, 1.175 ],
- [ 1.425, -1.175 ],
- [ -1.425, 1.175 ],
- [ -1.425, -1.175 ],
- [ 1.675, 1.175 ],
- [ 1.675, -1.175 ],
- [ -1.675, 1.175 ],
- [ -1.675, -1.175 ]
- ],
- "lines": {
- "x": [ 1.675, 1.425, -1.425, -1.675, 0 ],
- "y": [ 1.175, -1.175 ]
- },
-
- "penalty": [
- [ 0.805, 0.0 ],
- [ -0.805, 0.0 ]
- ],
- "FIELD_WIDTH": 2.35,
- "FIELD_LENGTH": 3.35
-}
diff --git a/src/azer_robocup_project/Init_params/Real/Real_params.json b/src/azer_robocup_project/Init_params/Real/Real_params.json
deleted file mode 100644
index 2d73e18..0000000
--- a/src/azer_robocup_project/Init_params/Real/Real_params.json
+++ /dev/null
@@ -1,17 +0,0 @@
-{
-"BODY_TILT_AT_WALK": 0.04,
-"SOLE_LANDING_SKEW": 0.01,
-"BODY_TILT_AT_KICK": 0.07,
-"ROTATION_YIELD_RIGHT": 0.261,
-"ROTATION_YIELD_LEFT": 0.255,
-"MOTION_SHIFT_TEST_X": -50,
-"MOTION_SHIFT_TEST_Y": -70,
-"SIDE_STEP_RIGHT_TEST_RESULT": 285,
-"SIDE_STEP_LEFT_TEST_RESULT": 295,
-"RUN_TEST_10_STEPS": 1080,
-"RUN_TEST_20_STEPS": 2220,
-"KICK_ADJUSTMENT_DISTANCE_1": 90,
-"KICK_ADJUSTMENT_DISTANCE_2": 170,
-"KICK_OFFSET_OF_BALL": 52,
-"IMU_DRIFT_IN_DEGREES_DURING_6_MIN_MEASUREMENT": 41.0
-}
diff --git a/src/azer_robocup_project/Init_params/Real/Real_params_2.json b/src/azer_robocup_project/Init_params/Real/Real_params_2.json
deleted file mode 100644
index d730697..0000000
--- a/src/azer_robocup_project/Init_params/Real/Real_params_2.json
+++ /dev/null
@@ -1,19 +0,0 @@
-{
- "DIAMETER_OF_BALL": 80,
- "APERTURE_PER_PIXEL": 0.1703,
- "APERTURE_PER_PIXEL_VERTICAL": 0.1817,
- "HEIGHT_OF_CAMERA": 413,
- "HEIGHT_OF_NECK": 36.76,
- "POST_WIDTH_IN_MM": 50,
- "UART_PORT": 2,
- "UART_SPEED": 1250000,
- "FRAME_DELAY": 25,
- "FRAMES_PER_CYCLE": 2,
- "DRIBBLING": 0,
- "USE_LANDMARKS_FOR_LOCALISATION": false,
- "USE_LINES_FOR_LOCALISATION": true,
- "USE_LINE_CROSSES_FOR_LOCALISATION": true,
- "USE_PENALTY_MARKS_FOR_LOCALISATION": true,
- "DIRECT_COORD_MEASUREMENT_BY_PAIRS_OF_POST": true,
- "USE_SINGLE_POST_MEASUREMENT": true
-}
diff --git a/src/azer_robocup_project/Init_params/Real/wifi_params.json b/src/azer_robocup_project/Init_params/Real/wifi_params.json
deleted file mode 100644
index b57e4bf..0000000
--- a/src/azer_robocup_project/Init_params/Real/wifi_params.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
- "WIFI_IS_ON": false,
- "ROBOT_ID": "v",
- "SSID": "Starkit",
- "PASSWORD": "MIPT2018",
- "HOST": "192.168.66.71",
- "PORT": 37020,
- "MQTT_IS_ON": true,
- "MQTT_PORT": 1901,
- "MQTT_BALL_TOPIC": "starkit1/ball",
- "MQTT_ROBOT_TOPIC": "starkit1/0/robot"
-}
diff --git a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_calibr.json b/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_calibr.json
deleted file mode 100644
index 555eb49..0000000
--- a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_calibr.json
+++ /dev/null
@@ -1 +0,0 @@
-{"neck_calibr": -42, "neck_play_pose": -788, "head_pitch_with_horizontal_camera": -0.009198110623709512}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params.json b/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params.json
deleted file mode 100644
index d3b512f..0000000
--- a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params.json
+++ /dev/null
@@ -1,16 +0,0 @@
-{
- "BODY_TILT_AT_WALK": 0.01,
- "SOLE_LANDING_SKEW": 0.035,
- "BODY_TILT_AT_KICK": 0.01,
- "MOTION_SHIFT_TEST_X": 85,
- "MOTION_SHIFT_TEST_Y": 0,
- "SIDE_STEP_RIGHT_TEST_RESULT": 393,
- "SIDE_STEP_LEFT_TEST_RESULT": 432,
- "RUN_TEST_10_STEPS": 690,
- "RUN_TEST_20_STEPS": 1422,
- "ROTATION_YIELD_RIGHT": 0.299,
- "ROTATION_YIELD_LEFT": 0.301,
- "KICK_ADJUSTMENT_DISTANCE": 70,
- "KICK_OFFSET_OF_BALL": 52,
- "IMU_DRIFT_IN_DEGREES_DURING_6_MIN_MEASUREMENT": 0.0
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params_2.json b/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params_2.json
deleted file mode 100644
index c768015..0000000
--- a/src/azer_robocup_project/Init_params/Sim/SURROGAT/Sim_params_2.json
+++ /dev/null
@@ -1,20 +0,0 @@
-{
- "DIAMETER_OF_BALL": 80,
- "APERTURE_PER_PIXEL": 0.18925,
- "APERTURE_PER_PIXEL_VERTICAL": 0.21,
- "HEIGHT_OF_CAMERA": 420,
- "HEIGHT_OF_NECK": 36.76,
- "POST_WIDTH_IN_MM": 50,
- "UART_PORT": 1,
- "UART_SPEED": 1250000,
- "FRAME_DELAY": 25,
- "FRAMES_PER_CYCLE": 2,
- "DRIBBLING": 0,
- "USE_LANDMARKS_FOR_LOCALISATION": false,
- "USE_LINES_FOR_LOCALISATION": true,
- "USE_LINE_CROSSES_FOR_LOCALISATION": true,
- "USE_PENALTY_MARKS_FOR_LOCALISATION": true,
- "DIRECT_COORD_MEASUREMENT_BY_PAIRS_OF_POST": true,
- "USE_SINGLE_POST_MEASUREMENT": true,
- "Vision_Sensor_Display_On": true
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Sim/SURROGAT/wifi_params.json b/src/azer_robocup_project/Init_params/Sim/SURROGAT/wifi_params.json
deleted file mode 100644
index d53b2f6..0000000
--- a/src/azer_robocup_project/Init_params/Sim/SURROGAT/wifi_params.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
- "WIFI_IS_ON": false,
- "ROBOT_ID": "v",
- "SSID": "FED1",
- "PASSWORD": "7684067a",
- "HOST": "192.168.1.13",
- "PORT": 37020
-}
diff --git a/src/azer_robocup_project/Init_params/Sim/Sim_Thresholds.json b/src/azer_robocup_project/Init_params/Sim/Sim_Thresholds.json
deleted file mode 100644
index 73754c8..0000000
--- a/src/azer_robocup_project/Init_params/Sim/Sim_Thresholds.json
+++ /dev/null
@@ -1,37 +0,0 @@
-{
- "orange ball": {
- "th": [ 13, 58, 27, 98, 8, 95 ],
- "pixel": 10,
- "area": 10
- },
- "blue posts": {
- "th": [ 0, 100, -128, 127, -128, -23 ],
- "pixel": 50,
- "area": 50
- },
- "yellow posts": {
- "th": [ 0, 100, -55, -8, 57, 127 ],
- "pixel": 50,
- "area": 50
- },
- "white posts": {
- "th": [ 94, 100, -25, 40, -5, 127 ],
- "pixel": 50,
- "area": 50
- },
- "green field": {
- "th": [ 0, 100, -128, -28, -128, 127 ],
- "pixel": 200,
- "area": 200
- },
- "white marking": {
- "th": [ 94, 100, -25, 40, -5, 127 ],
- "pixel": 10,
- "area": 10
- },
- "follow line": {
- "th": [ 13, 58, 27, 98, 8, 95 ],
- "pixel": 10,
- "area": 10
- }
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/Sim/Sim_landmarks.json b/src/azer_robocup_project/Init_params/Sim/Sim_landmarks.json
deleted file mode 100644
index 531a1b9..0000000
--- a/src/azer_robocup_project/Init_params/Sim/Sim_landmarks.json
+++ /dev/null
@@ -1,43 +0,0 @@
-{
- "post1": [
- [ 1.8, -0.6 ]
- ],
- "post2": [
- [ 1.8, 0.6 ]
- ],
- "post3": [
- [ -1.8, 0.6 ]
- ],
- "post4": [
- [ -1.8, -0.6 ]
- ],
- "unsorted_posts": [
- [ 1.8, 0.6 ],
- [ 1.8, -0.6 ],
- [ -1.8, 0.6 ],
- [ -1.8, -0.6 ]
- ],
- "Line_crosses": [
- [ 0.0, -1.3 ],
- [ 0.0, 1.3 ],
- [ 1.6, 1.3 ],
- [ 1.6, -1.3 ],
- [ -1.6, 1.3 ],
- [ -1.6, -1.3 ],
- [ 1.8, 1.3 ],
- [ 1.8, -1.3 ],
- [ -1.8, 1.3 ],
- [ -1.8, -1.3 ]
- ],
- "lines": {
- "x": [ 1.8, 1.6, -1.6, -1.8, 0 ],
- "y": [ 1.3, -1.3 ]
- },
-
- "penalty": [
- [ 0.9, 0.0 ],
- [ -0.9, 0.0 ]
- ],
- "FIELD_WIDTH": 2.6,
- "FIELD_LENGTH": 3.6
-}
diff --git a/src/azer_robocup_project/Init_params/Sim/Sim_landmarks_field2.json b/src/azer_robocup_project/Init_params/Sim/Sim_landmarks_field2.json
deleted file mode 100644
index cfbec2e..0000000
--- a/src/azer_robocup_project/Init_params/Sim/Sim_landmarks_field2.json
+++ /dev/null
@@ -1,43 +0,0 @@
-{
- "post1": [
- [ 1.7, -0.5 ]
- ],
- "post2": [
- [ 1.7, 0.5 ]
- ],
- "post3": [
- [ -1.7, 0.5 ]
- ],
- "post4": [
- [ -1.7, -0.5 ]
- ],
- "unsorted_posts": [
- [ 1.7, 0.5 ],
- [ 1.7, -0.5 ],
- [ -1.7, 0.5 ],
- [ -1.7, -0.5 ]
- ],
- "Line_crosses": [
- [ 0.0, -1.2 ],
- [ 0.0, 1.2 ],
- [ 1.5, 1.2 ],
- [ 1.5, -1.2 ],
- [ -1.5, 1.2 ],
- [ -1.5, -1.2 ],
- [ 1.7, 1.2 ],
- [ 1.7, -1.2 ],
- [ -1.7, 1.2 ],
- [ -1.7, -1.2 ]
- ],
- "lines": {
- "x": [ 1.7, 1.5, -1.5, -1.7, 0 ],
- "y": [ 1.2, -1.2 ]
- },
-
- "penalty": [
- [ 0.9, 0.0 ],
- [ -0.9, 0.0 ]
- ],
- "FIELD_WIDTH": 2.4,
- "FIELD_LENGTH": 3.4
-}
diff --git a/src/azer_robocup_project/Init_params/Sprint_params.json b/src/azer_robocup_project/Init_params/Sprint_params.json
deleted file mode 100644
index ed4535f..0000000
--- a/src/azer_robocup_project/Init_params/Sprint_params.json
+++ /dev/null
@@ -1,4 +0,0 @@
-{
- "proportional": 0,
- "differential": 0
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Init_params/strategy_data.json b/src/azer_robocup_project/Init_params/strategy_data.json
deleted file mode 100644
index ef9bcbf..0000000
--- a/src/azer_robocup_project/Init_params/strategy_data.json
+++ /dev/null
@@ -1 +0,0 @@
-{"strategy_data": [[0, 0, 1, -0.185], [0, 1, 1, -0.155], [0, 2, 1, -0.124], [0, 3, 1, -0.0935], [0, 4, 1, -0.0624], [0, 5, 1, -0.0312], [0, 6, 1, -0.0], [0, 7, 1, 0.0312], [0, 8, 1, 0.0624], [0, 9, 1, 0.0935], [0, 10, 1, 0.124], [0, 11, 1, 0.155], [0, 12, 1, 0.185], [1, 0, 1, -0.197], [1, 1, 1, -0.165], [1, 2, 1, -0.132], [1, 3, 1, -0.099], [1, 4, 1, -0.066], [1, 5, 1, -0.0332], [1, 6, 1, -0.0], [1, 7, 1, 0.0332], [1, 8, 1, 0.0663], [1, 9, 1, 0.0993], [1, 10, 1, 0.132], [1, 11, 1, 0.165], [1, 12, 1, 0.197], [2, 0, 1, -0.2096], [2, 1, 1, -0.1755], [2, 2, 1, -0.141], [2, 3, 1, -0.106], [2, 4, 1, -0.0708], [2, 5, 1, -0.0354], [2, 6, 1, -0.0], [2, 7, 1, 0.0354], [2, 8, 1, 0.0708], [2, 9, 1, 0.106], [2, 10, 1, 0.141], [2, 11, 1, 0.175], [2, 12, 1, 0.21], [3, 0, 1, -0.223], [3, 1, 1, -0.187], [3, 2, 1, -0.15], [3, 3, 1, -0.113], [3, 4, 1, -0.0756], [3, 5, 1, -0.03786], [3, 6, 1, -0.0], [3, 7, 1, 0.03786], [3, 8, 1, 0.0756], [3, 9, 1, 0.11315], [3, 10, 1, 0.15], [3, 11, 1, 0.187], [3, 12, 1, 0.223], [4, 0, 1, -0.24], [4, 1, 1, -0.2013], [4, 2, 1, -0.162], [4, 3, 1, -0.122], [4, 4, 1, -0.081], [4, 5, 1, -0.0408], [4, 6, 1, -0.0], [4, 7, 1, 0.04079], [4, 8, 1, 0.08145], [4, 9, 1, 0.1218], [4, 10, 1, 0.16184], [4, 11, 1, 0.20132], [4, 12, 1, 0.2402], [5, 0, 1, -0.2595], [5, 1, 1, -0.2177], [5, 2, 1, -0.1752], [5, 3, 1, -0.132], [5, 4, 1, -0.0883], [5, 5, 1, -0.0442], [5, 6, 1, -0.0], [5, 7, 1, 0.0442], [5, 8, 1, 0.0883], [5, 9, 1, 0.132], [5, 10, 1, 0.1752], [5, 11, 1, 0.2178], [5, 12, 1, 0.2595], [6, 0, 1, -0.282], [6, 1, 1, -0.237], [6, 2, 1, -0.191], [6, 3, 1, -0.144], [6, 4, 1, -0.096], [6, 5, 1, -0.0483], [6, 6, 1, -0.0], [6, 7, 1, 0.0483], [6, 8, 1, 0.0964], [6, 9, 1, 0.144], [6, 10, 1, 0.191], [6, 11, 1, 0.2370061668955651], [6, 12, 1, 0.282], [7, 0, 1, -0.309], [7, 1, 1, -0.26], [7, 2, 1, -0.21], [7, 3, 1, -0.159], [7, 4, 1, -0.106], [7, 5, 1, -0.053], [7, 6, 1, -0.0], [7, 7, 1, 0.053], [7, 8, 1, 0.106], [7, 9, 1, 0.159], [7, 10, 1, 0.21], [7, 11, 1, 0.26], [7, 12, 1, 0.31], [8, 0, 1, -0.341], [8, 1, 1, -0.288], [8, 2, 1, -0.232], [8, 3, 1, -0.1757], [8, 4, 1, -0.1178], [8, 5, 1, -0.059], [8, 6, 1, -0.0], [8, 7, 1, 0.0591], [8, 8, 1, 0.1178], [8, 9, 1, 0.176], [8, 10, 1, 0.232], [8, 11, 1, 0.2877], [8, 12, 1, 0.341], [9, 0, 1, -0.576], [9, 1, 1, -0.4538], [9, 2, 1, -0.25895], [9, 3, 1, -0.196], [9, 4, 1, -0.1317], [9, 5, 1, -0.0661], [9, 6, 1, -0.0], [9, 7, 1, 0.0661], [9, 8, 1, 0.1317], [9, 9, 1, 0.1961], [9, 10, 1, 0.259], [9, 11, 1, 0.5236], [9, 12, 1, 0.576], [10, 0, 1, -0.576], [10, 1, 1, -0.5236], [10, 2, 1, -0.294], [10, 3, 1, -0.223], [10, 4, 1, -0.1504], [10, 5, 1, -0.0756], [10, 6, 1, -0.0], [10, 7, 1, 0.0756], [10, 8, 1, 0.15], [10, 9, 1, 0.2235], [10, 10, 1, 0.294], [10, 11, 1, 0.5236], [10, 12, 1, 0.698], [11, 0, 1, -0.768], [11, 1, 1, -0.524], [11, 2, 1, -0.34], [11, 3, 1, -0.2595], [11, 4, 1, -0.175], [11, 5, 1, -0.088], [11, 6, 1, -0.0], [11, 7, 1, 0.0883], [11, 8, 1, 0.1752], [11, 9, 1, 0.2595], [11, 10, 1, 0.34], [11, 11, 1, 0.6458], [11, 12, 1, 0.698], [12, 0, 1, -0.768], [12, 1, 1, -0.698], [12, 2, 1, -0.402], [12, 3, 1, -0.3089], [12, 4, 1, -0.2096], [12, 5, 1, -0.106], [12, 6, 1, -0.0], [12, 7, 1, 0.10598], [12, 8, 1, 0.2097], [12, 9, 1, 0.3089], [12, 10, 1, 0.402], [12, 11, 1, 0.6459], [12, 12, 1, 0.698], [13, 0, 1, -0.89], [13, 1, 1, -0.698], [13, 2, 1, -0.6457718232379019], [13, 3, 1, -0.3805], [13, 4, 1, -0.2606], [13, 5, 1, -0.1326], [13, 6, 1, 0.279], [13, 7, 1, 0.1326], [13, 8, 1, 0.2606], [13, 9, 1, 0.3805], [13, 10, 1, 0.48996], [13, 11, 1, 0.64577], [13, 12, 1, 0.82], [14, 0, 1, -0.94], [14, 1, 1, -0.82], [14, 2, 1, -0.62], [14, 3, 1, -0.6457718232379019], [14, 4, 1, -0.343], [14, 5, 1, 0.0], [14, 6, 1, -0.279], [14, 7, 1, 0.0], [14, 8, 1, 0.0], [14, 9, 1, 0.4918], [14, 10, 1, 0.62], [14, 11, 1, 0.82], [14, 12, 1, 1.0123], [15, 0, 1, -1.065], [15, 1, 1, -0.9425], [15, 2, 1, -0.9424777960769379], [15, 3, 1, -0.767944870877505], [15, 4, 1, -0.4537856055185257], [15, 5, 1, 0.0], [15, 6, 1, -0.279], [15, 7, 1, 0.0], [15, 8, 1, 0.0], [15, 9, 1, 0.668], [15, 10, 1, 0.811], [15, 11, 1, 1.012], [15, 12, 1, 1.012], [16, 0, 1, -1.309], [16, 1, 1, -1.187], [16, 2, 1, -1.127], [16, 3, 1, -1.0122909661567112], [16, 4, 1, 0.0], [16, 5, 1, 0.0], [16, 6, 1, -0.279], [16, 7, 1, 0.0], [16, 8, 1, 0.0], [16, 9, 1, 1.006], [16, 10, 1, 1.127], [16, 11, 1, 1.2076], [16, 12, 1, 1.26], [17, 0, 2, -2.09], [17, 1, 2, -2.09], [17, 2, 2, -2.09], [17, 3, 2, -2.09], [17, 4, 1, 0], [17, 5, 1, 0], [17, 6, 1, -0.646], [17, 7, 1, 0], [17, 8, 1, 0.0], [17, 9, 2, 2.09], [17, 10, 2, 2.09], [17, 11, 2, 2.09], [17, 12, 2, 2.09]]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Localisation/PF/ParticleFilter.py b/src/azer_robocup_project/Soccer/Localisation/PF/ParticleFilter.py
deleted file mode 100644
index 661d469..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/PF/ParticleFilter.py
+++ /dev/null
@@ -1,455 +0,0 @@
-
-import sys, os, json, array
-
-if sys.version != '3.4.0':
- from random import *
- used_with_OpenMV = False
-else:
- import time
- from urandom import getrandbits
- import starkit
- used_with_OpenMV = True
-
-used_with_OpenMV_firmware = True
-
-import math
-import json
-import time
-
-SDVIG = 32768
-
-
-def weight_calculation( n, weights, observations, landmarks, gauss_noise, line_gauss_noise, p):
-
- def gaussian( x, sigma):
- # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
- return math.exp(-(x ** 2) / 2*(sigma ** 2)) / math.sqrt(2.0 * math.pi * (sigma ** 2))
-
- def new_observation_score( i, observations, landmarks, gauss_noise, p):
- # particle weight calculation
- prob = 1.0
- part0 = (p[i * 4] - SDVIG)/ 1000
- part1 = (p[i * 4 + 1] - SDVIG)/ 1000
- part2 = (p[i * 4 + 2] - SDVIG)/ 1000
- sin_p = math.sin(part2)
- cos_p = math.cos(part2)
- for color_landmarks in observations:
- if (color_landmarks not in landmarks):
- continue
- if (color_landmarks == 'lines'):
- continue
- for landmark in landmarks[color_landmarks]:
- min_dist = 1000
- if observations[color_landmarks]:
- for observation in observations[color_landmarks]:
- # calc posts coords in field for every mesurement
- x_posts = part0 + observation[0] * cos_p - observation[1] * sin_p
- y_posts = part1 + observation[0] * sin_p + observation[1] * cos_p
- dist =(x_posts - landmark[0])**2 + (y_posts - landmark[1])**2
- if min_dist > dist:
- min_dist = dist
- weight = observation[2]
- if min_dist != 1000:
- prob *= gaussian(math.sqrt(min_dist), gauss_noise)*weight
- return prob
- def new_calc_lines_score( i, lines, landmarks, line_gauss_noise, p):
- '''
- line = (ro, theta)
- '''
- part0 = (p[i * 4] - SDVIG)/ 1000
- part1 = (p[i * 4 + 1] - SDVIG)/ 1000
- part2 = (p[i * 4 + 2] - SDVIG)/ 1000
- prob = 1
- if lines != []:
- for line in lines:
- min_dist = 1000
- for landmark_line in landmarks:
- for coord in landmarks[landmark_line]:
- yaw = (part2 + line[1])%(2*math.pi)
- if landmark_line == 'x':
- dist = math.fabs(coord - (part0 + line[0]*math.cos(yaw)))
- else:
- dist = math.fabs(coord - (part1 + line[0]*math.sin(yaw)))
- if min_dist > dist:
- min_dist = dist
- if min_dist != 1000:
- prob *= gaussian(min_dist, line_gauss_noise)*line[2]
- return prob
- # тело функции
- S = 0.0
- for i in range(n):
- weight = int(new_observation_score(i,observations, landmarks, gauss_noise, p)
- *new_calc_lines_score(i,observations['lines'], landmarks['lines'], line_gauss_noise, p)
- * 20000)
- weights[i] = weight
- S += weight
- return weights, S
-
-def randrange( start, stop=None):
-#helper function for working with random bit sequence
- if stop is None:
- stop = start
- start = 0
- upper = stop - start
- bits = 0
- pwr2 = 1
- while upper > pwr2:
- pwr2 <<= 1
- bits += 1
- while True:
- r = getrandbits(bits)
- if r < upper:
- break
- return r + start
-
-def random():
- #getting a random number from 0 to 1
- return randrange(10000) / 10000
-
-def gauss( mu, sigma):
- #getting a random number from Gaussian distribution
- x2pi = random() * math.pi * 2
- g2rad = math.sqrt(-2.0 * math.log(1.0 - random()))
- z = math.cos(x2pi) * g2rad
- return mu + z * sigma
-
-def gaussian( x, sigma):
- # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
- return math.exp(-(x ** 2) / 2*(sigma ** 2)) / math.sqrt(2.0 * math.pi * (sigma ** 2))
-
-
-class ParticleFilter():
- def __init__(self, myrobot, glob):
- self.counter1 = 0
- self.counter2 = 0
- self.n = glob.particles_number
- self.myrobot = myrobot
- self.count = 0
- self.p = glob.pf_alloc1
- self.tmp = glob.pf_alloc2
- self.weights = glob.weights
- self.new_p = glob.new_p
- self.landmarks = glob.landmarks
- self.landmark_lines_x = array.array('f',self.landmarks['lines']['x'])
- self.landmark_lines_y = array.array('f',self.landmarks['lines']['y'])
- self.land_keys =list(self.landmarks.keys())
- self.land_keys.pop(self.land_keys.index('lines'))
- self.land_keys.pop(self.land_keys.index('FIELD_WIDTH'))
- self.land_keys.pop(self.land_keys.index('FIELD_LENGTH'))
- self.array_landmarks = []
- for i in range(len(self.land_keys)):
- marks = array.array('f',[])
- for j in range(len(self.landmarks[self.land_keys[i]])):
- mark1 = array.array('f',self.landmarks[self.land_keys[i]][j])
- marks.extend(mark1)
- self.array_landmarks.append(marks)
- with open(glob.current_work_directory + "Soccer/Localisation/PF/pf_constants.json", 'r') as constants:
- constants = json.load(constants)
- self.forward_noise = constants['noise']['forward_noise']
- self.turn_noise = constants['noise']['turn_noise']
- self.sense_noise = constants['noise']['sense_noise']
- self.gauss_noise = constants['noise']['gauss_noise']
- self.yaw_noise = constants['noise']['yaw_noise']
- self.line_gauss_noise = constants['noise']['line_gauss_noise']
- self.other_coord_noise = constants['noise']['other_coord_noise']
-
- self.number_of_res = constants['consistency']['number_of_res']
- self.consistency = constants['consistency']['consistency']
- self.goodObsGain = constants['consistency']['goodObsGain']
- self.badObsCost = constants['consistency']['badObsCost']
- self.stepCost = constants['consistency']['stepCost']
- self.dist_threshold = constants['consistency']['dist_threshold']
- self.con_threshold = constants['consistency']['con_threshold']
- self.spec_threshold = constants['consistency']['spec_threshold']
- self.gen_particles()
-
- def return_coord(self):
- coord = [self.myrobot.x, self.myrobot.y, self.myrobot.yaw]
- return coord
-
- def gen_particles(self):
- for i in range(self.n):
- x_coord = self.myrobot.x + gauss(0, self.sense_noise)
- y_coord = self.myrobot.y + gauss(0, self.sense_noise)
- yaw = self.myrobot.yaw + gauss(0, self.yaw_noise)*math.pi
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi)
- self.p[i * 4 + 0] = int(x_coord * 1000) + SDVIG
- self.p[i * 4 + 1] = int(y_coord * 1000) + SDVIG
- self.p[i * 4 + 2] = int(yaw * 1000) + SDVIG
- self.limit_paricles_coord(self.p, self.n)
- self.count += 1
-
- def gen_n_particles_robot(self, start_row):
- for i in range(start_row, self.n, 1):
- x_coord = self.myrobot.x + gauss(0, self.sense_noise*3)
- y_coord = self.myrobot.y + gauss(0, self.sense_noise*3)
- yaw = self.myrobot.yaw + gauss(0, self.yaw_noise)*math.pi
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi)
- self.p[i * 4 + 0] = int(x_coord * 1000) + SDVIG
- self.p[i * 4 + 1] = int(y_coord * 1000) + SDVIG
- self.p[i * 4 + 2] = int(yaw * 1000) + SDVIG
-
-
- def gen_n_particles_robot_coord(self, start_row, coord):
- for i in range(start_row, self.n, 1):
- x_coord = coord[0] + gauss(0, self.other_coord_noise)
- y_coord = coord[1] + gauss(0, self.other_coord_noise)
- yaw = coord[2] + gauss(0, self.yaw_noise)*2
- if yaw < 0:
- yaw = 2*math.pi + yaw
- if yaw > 2*math.pi:
- yaw %= (2 * math.pi)
- self.p[i * 4 + 0] = int(x_coord * 1000) + SDVIG
- self.p[i * 4 + 1] = int(y_coord * 1000) + SDVIG
- self.p[i * 4 + 2] = int(yaw * 1000) + SDVIG
-
- def update_consistency(self, observations):
- stepConsistency = 0
- for color_landmarks in observations:
- if (color_landmarks not in self.landmarks):
- continue
- if (color_landmarks == 'lines'):
- continue
- if len(observations[color_landmarks]) != 0:
- for observation in observations[color_landmarks]:
- dists = []
- for landmark in self.landmarks[color_landmarks]:
-
- # calc posts coords in field for every mesurement
- x_posts = (self.myrobot.x + observation[0]*math.cos(self.myrobot.yaw)
- - observation[1]*math.sin(self.myrobot.yaw))
- y_posts = (self.myrobot.y + observation[0]*math.sin(self.myrobot.yaw)
- + observation[1]*math.cos(self.myrobot.yaw))
- #print('x_posts, y_posts', x_posts, y_posts)
- dist = math.sqrt(
- (x_posts - landmark[0])**2 + (y_posts - landmark[1])**2)
- dists.append(dist)
- #print('dist, len =', dist, len(dists))
- if min(dists) < self.dist_threshold:
- stepConsistency += self.goodObsGain
-
- #print('good step', stepConsistency)
- else:
- stepConsistency -= self.badObsCost
- #print('bad step', stepConsistency)
- else:
- stepConsistency -= self.stepCost
- #print('step cons', stepConsistency)
- self.consistency += stepConsistency
- if self.consistency > self.spec_threshold:
- self.consistency = self.spec_threshold
- elif self.consistency < 0.0:
- self.consistency = 0.0
- print('consistency', self.consistency)
-
- def particles_move(self, coord):
- self.myrobot.move(coord['shift_x'],
- coord['shift_y'], coord['shift_yaw'])
- for i in range(self.n):
- orientation = (self.p[i * 4 + 2] - SDVIG) / 1000 + float(coord['shift_yaw'])
- if orientation < 0:
- orientation += (math.pi*2)
- orientation %= (2 * math.pi)
- x = self.p[i * 4]
- y = self.p[i * 4 + 1]
- yaw = (self.p[i * 4 + 2] - SDVIG) / 1000
- x1 = int(x + (coord['shift_x'] * math.cos(yaw) - coord['shift_y'] * math.sin(yaw)) * 1000)
- y1 = int(y + (coord['shift_x'] * math.sin(yaw) + coord['shift_y'] * math.cos(yaw)) * 1000)
- yaw1 = int(orientation * 1000 + SDVIG)
- self.p[i * 4] = x1
- self.p[i * 4 + 1] = y1
- self.p[i * 4 + 2] = yaw1
- self.count += 1
- self.limit_paricles_coord(self.p, self.n)
-
- def observation_to_predict(self, observations):
- predicts = []
- for color_landmarks in observations:
- if (color_landmarks not in self.landmarks):
- continue
- if ((color_landmarks == 'angle') or (color_landmarks == 'lines')):
- continue
- for landmark in self.landmarks[color_landmarks]:
- if len(observations[color_landmarks]) != 0:
- for obs in observations[color_landmarks]:
- y_posts = self.myrobot.x + \
- obs[0]*math.sin(-self.myrobot.yaw) + \
- obs[1]*math.cos(-self.myrobot.yaw)
- x_posts = self.myrobot.y + \
- obs[0]*math.cos(-self.myrobot.yaw) - \
- obs[1]*math.sin(-self.myrobot.yaw)
- predicts.append([x_posts, y_posts])
- return predicts
-
- def limit_paricles_coord(self, p, stop_row):
- half_w = self.landmarks['FIELD_WIDTH'] / 2
- half_l = self.landmarks['FIELD_LENGTH'] / 2
- for i in range(stop_row):
- x = (p[i * 4] - SDVIG) / 1000
- y = (p[i * 4 + 1] - SDVIG) / 1000
- if math.fabs(x) > half_l + 0.3:
- p[i * 4] = int(math.copysign(half_l + 0.3, x) * 1000 + SDVIG)
- if math.fabs(y) > half_w + 0.3:
- p[i * 4 + 1] = int(math.copysign(half_w + 0.3, y) * 1000 + SDVIG)
-
- def resampling_wheel(self):
- row = 0
- for i in range(self.n): self.new_p[i] = 0
- index = int(random() * self.n)
- beta = 0.0
- mw = 0
- for i in range(self.n):
- m = self.weights[i]
- if m > mw : mw = m
- for i in range(self.n):
- beta += random() * 2.0 * mw
- while beta > self.weights[index]:
- beta -= self.weights[index]
- index = (index + 1) % self.n
- if self.new_p[index] == 0: self.new_p[index] = 1
- else:
- self.new_p[index] += 1
- for el in range(self.n):
- if self.new_p[el] == 0: continue
- self.tmp[row*4:row*4+3] = self.p[el*4:el*4+3]
- self.tmp[row *4 + 3] = self.weights[el] * self.new_p[el]
- row += 1
- return row
-
- def resampling(self, observations, other_coord):
- for i in range(self.n):
- self.tmp[i *4] = 0
- self.tmp[i *4 + 1] = 0
- self.tmp[i *4 + 2] = 0
- self.tmp[i *4 + 3] = 0
- self.weights[i] = 0
-
- if used_with_OpenMV:
- clock = time.clock()
- clock.tick()
- S = self.weight_calc_wrap(observations)
- if used_with_OpenMV: print('timestamp 1 =', clock.avg())
- if S == 0: return
- S = S / 20000
- for i in range(self.n):
- w = int(self.weights[i]/S)
- self.weights[i] = w
- row = self.resampling_wheel()
- S = 0.0
- for i in range(row):
- S += (self.tmp[i *4 + 3] / 20000)
- for i in range(row):
- tempo = int(self.tmp[i * 4 + 3]/S)
- self.tmp[i * 4 + 3] = tempo
- if not other_coord:
- self.gen_n_particles_robot(row)
- else:
- self.gen_n_particles_robot_coord(row, other_coord)
- self.p[:row *4] = self.tmp[:row *4]
- self.limit_paricles_coord(self.p, self.n)
- self.update_coord(self.p, row)
- self.count += 1
- self.update_consistency(observations)
-
- def weight_calc_wrap(self, observations):
- if used_with_OpenMV and used_with_OpenMV_firmware:
- array_observation_lines = array.array('f',[])
- for line in observations['lines']:
- array_observation_lines.extend(array.array('f',line))
- array_observations =[]
- for i in range(len(self.land_keys)):
- obs = array.array('f',[])
- for j in range(len(observations[self.land_keys[i]])):
- ob1 = array.array('f',observations[self.land_keys[i]][j])
- obs.extend(ob1)
- array_observations.append(obs)
- S = starkit.weight_calculation(self.n, array_observations, self.array_landmarks, self.gauss_noise,
- self.p, array_observation_lines, self.landmark_lines_x, self.landmark_lines_y,
- self.line_gauss_noise, self.weights)
- else:
- landmarks = self.landmarks
- n = self.n
- p = self.p
- weights = self.weights
- gauss_noise = self.gauss_noise
- line_gauss_noise = self.line_gauss_noise
- weights, S = weight_calculation(n, weights, observations, landmarks, gauss_noise, line_gauss_noise, p)
- self.weights = weights
- return S
-
- def custom_reset(self, x, y, yaw):
- self.myrobot.x = x
- self.myrobot.y = y
- self.myrobot.yaw = yaw
- self.p = gen_n_particles_robot(0)
-
- # ------------------------------
- # need to add to handle the fall
- # ------------------------------
-
- def fall_reset(self, noise = 0.3):
- self.myrobot.x += gauss(0, self.sense_noise)
- self.myrobot.y += gauss(0, self.sense_noise)
- self.myrobot.yaw += gauss(0, self.yaw_noise)
- for i in range(self.n):
- x1 = self.p[i * 4] + int(gauss(0, noise) * 1000)
- y1 = self.p[i * 4 + 1] + int(gauss(0, noise) * 1000)
- yaw1 = self.p[i * 4 + 2] + int(gauss(0, noise / 3) * 1000)
- self.p[i * 4] = x1
- self.p[i * 4 +1] = y1
- self.p[i * 4 + 2] = yaw1
- self.consistency *= 0.5
-
- def update_coord(self, particles, stop_row):
- x = 0.0
- y = 0.0
- orientation = 0.0
- adding = 0.0
- if (self.myrobot.yaw < math.pi/2) or (self.myrobot.yaw > math.pi*3/2):
- adding = math.pi*2
- for i in range(stop_row):
- x_old = (particles[i * 4]- SDVIG) / 1000
- y_old = (particles[i * 4 + 1]- SDVIG) / 1000
- yaw_old = (particles[i * 4 + 2]- SDVIG) / 1000
- w_old = particles[i * 4 + 3] / 20000
- x += x_old * w_old
- y += y_old * w_old
- if (yaw_old < math.pi):
- culc_yaw = yaw_old + adding
- else:
- culc_yaw = yaw_old
- orientation += culc_yaw * w_old
- self.myrobot.x = x
- self.myrobot.y = y
- self.myrobot.yaw = orientation % (2*math.pi)
-
- def return_coord(self):
- return self.myrobot.x, self.myrobot.y, self.myrobot.yaw
-
- def updatePF(self, measurement, other_coord):
- for i in range(self.number_of_res):
- self.resampling(measurement, other_coord)
- return self.return_coord()
-
-class Robot:
-
- def __init__(self, x=0, y=0, yaw=0):
- self.x = x # robot's x coordinate
- self.y = y # robot's y coordinate
- self.yaw = yaw # robot's angle
-
- def move(self, x, y, yaw):
- # turn, and add randomomness to the turning command
- orientation = self.yaw + float(yaw)
- if orientation < 0:
- orientation += (math.pi*2)
- orientation %= (2 * math.pi)
- self.x += x*math.cos(self.yaw) - y*math.sin(self.yaw)
- self.y += x*math.sin(self.yaw) + y*math.cos(self.yaw)
- self.yaw = orientation
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/ParticleFilter.cpython-39.pyc b/src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/ParticleFilter.cpython-39.pyc
deleted file mode 100644
index 1f0606991188ffb324cdff6fa7c4bc6b2aa4d4fa..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 13434
zcmb_jTZ~-ESw7e9(>*;s7mw#+d%d$>@2=a~__ii{b2iS#*-NsSn6<MZ&4#8u)idsy
zp6;GHXV&&`nk+j8LI_C!5y2+F43`jxfC5MecpxY*NIXPVKnNZ25CKx~3*rHukP^P{
zuhUmg0z&lY)T!I4s&neA|Ns8KYO3Soc?-WAe|Sz!ebBQ0nTf%li^N4-;WrQ{OW6&p
zZ0>g1mb6oL<nET;nziOBXWi9LA;)d_S1sk0GtEUQ&z7^e`;A~DSI*g{jeI$;GUW`e
zF{x817v$b;lh0(!<0??O+g7=#W>sE|-L}gUs-VV^<Ex^Yz<p9pswv#3UbNJ-I&j-k
z2X>tDG*UC_AW{dBI-u+;R%z}Y-oskj_MbR){?wV0o!BQ|xML?{tF=g1o9bFU3a{3!
zH!W*9%Zll*fO`%Xzec!?K-tQ<ZT+0}v(}2OT;<&kI$4SPDuZ~SvYlLH#nz5}$@=)E
zPCl~ZY@Cms*p0n7v*X7BQz}@nVpl?+xigl^&ET2NSUeWnasGl8=i;#^txj(9ainuA
z4&ETYwCrF6-5=u_Tvu>~=MhBKt{r252dpuKhppIVe8$?dQQ{~kvew+0f!xD*^p3l9
zj}3hX7k?M;c!{@Bjjktd?Pfdi!c83|PP=}`N$i`6AJ$hls&_0siK_p(hTwhAy#A4}
zro+?K)0bMyTN|}z6rO&u9=)`6?ewdyYpwNqbo%b@-M_s1N4Va;`?b4YKX>Nb6N{=I
zt}oU%s;jlrS6a)<HGTS()^fE`535nV)jWOqh1V}vbyQz&)Ly7JqMAO{zLf;4)vYkB
zSDW+bH<T~oY#+Z-tFK;<DtLH#t07OHy3k--;d7_D4=nq5qWe352xpl0@Vw0#?f94*
zOF1*v0zTqA5PaC`WV(6En;Xo}_VWFq{2;P-ojBODG5_|iE3te`6ns-=)p^6#&CMT=
z$5dwB(Kiuu;;{t_<zMX-l-n7{JR|)%TNg|WCGAcz&csEP1zPem)|`d7hd8h@Z*?Ys
zEK`PbuT1K7CgX{?z~}Hb{sAkVL<&!u)ST6sipS%rbx*$?Pod|c@)xYm^yb^~xN@<I
z-;DiFT6haD9^bQLcgKmRXRN2Kj=%XP#_a=(!BUEte}B(C4`1@$zhZ^l!`HMlF`4*{
zdb1X4W|aKIzjEodmtWLeNQoQPn~A&J3iTKZ4<vRyDYUKuL~qFSg-Nb~^|(>h>mgPr
z=e*Kv)x%n11CMRpx|aBDfa8q0oi(>}Wni;eX_tMpTxlm0%dJLBSNbgl-DnbQpdtVm
zC1V5KCfS>nb}Nj+Bzr53cv2~25O&Ab(-^NHUr+Hx4mN8yDg!N4!sV8(9YIle27%>x
z_KZD`zdZf|{5f(JY!B&zbHqMkFCvGj0P*d!KM2bE_BDj-33SOXD!9(z!wXLB9;O{&
z+Ta{{%I#s7`3CFE?_-_$S>Q5=?Nw{t1uh*y^H(}K<#Zs!fX|-<vb!j5cM8B`PPq%f
zqVghp4eM#v0=mYO55)hmtcpVHqfQ3vEJYWQI}Z`nDK;0=8kt@VpZM$7faEL!v(32J
z$!|6{uf=)MIj_aRC$T;Rr#Yfruo`S4A4tt^AI~qQKZ0`c-HRu5>+!|tdM&D!dPRB$
z-%cN7Fw1}_)<p(_;f$u#5d>@Mgv48^UJLahW-cHoP3Xgn&oOuaK{CnCDg*e{53=kb
z2FDmY%wQ2g;<2iJ6w&f{7p4{FB|vjatHixky`fLC{^JZNeGLX@hzrU-sU8{#K8fP+
zI0DNv05lNu0JDNUi}093;PG$X-ahj`!EUtquBtFEdv9?GZ|@{Nl;~<LaqG>fR7mV*
zk~Qsynkzy+i9X8MX$JBs-pSY#2on2>`FihQj=Y_`r!2GS`^U!#3i;_RC-4e~3-cac
zY|@_np5r**&A0(plztYaL+X+fN~Q5WT;Xd7pdPI)bQfD4D9R4fF{P@5xC?}PK(+_8
zJrjf3e9%G~xzNb&<|6dF3tjC4%g(x`pV?g6d<JnK8F+J-1Th9hqz$DE^kxK!1#KF&
z+KIo_Zr60;k#Us0_6>b5v9)>Y{>0|UN#v9M2*;bgEQAGT(S&$ShjZxJ`n{;LjBDuE
zds*T&f|}dGYf|TzoTd8YV;GouaG>jc{~7k*+Nk5vRMqO|5g+=Z)T-2^@<lHp5?MP~
z*If4stG45CJ%e^&r7M?ZT<4@C9T$1{cGNAX2i*%<CF>l6^Oj`ot)e~t{z_Azl`_(O
zfBlK_NZdwiRn{PMjI*(&DFYMl=DBt~@mJ63s!Htbf!7l}`LZN8P%{h=Sk9z<$S&IY
zCB%nbkKd1=k=Ogb@+-}cq}h=kXCPm=_nAedM9A-cW2%_@CelUbMOOMef|^D44dyL8
zwCpN!P^;`I@=x;OtO{_?sGQ2<o>gP2fP0|ERT1}`noyIt=hc*&#(hj3P&2p})Il|i
z`?#7@^SBq)f;xozggUN{s0YBjC)H8)AabVEV%p<F>KM|~>S4^<BS~?Xu`P4d-5=kd
zmwd|C@L?6!TE&ER?5sIRxm<r0PfCD|wG2{O+ymTmxaV<))>#|F9a?9tfO{bt-yK(Y
zdJPO}0a|Cl+GAVAk$kWl$)AX@+SdxZQ+wdrs5gz&^zH!(XOz2V@7XrE`#@f1pS-Ne
zTSd*v!Du#`;}bhrUeJiwfNdGmDthdAX(`6KHXUPx3+PwsJ>K#AW9*m4GE(Z}-T4@O
z+u(zn9MQquh1g?y^B<x^c>BXDfR@eeIG;V#$wo(ZABZukPWGd)t~~thxVF`tK8vTr
z<{8VDzmFk|vnr481fGepA5BvQNj;dR#wE3wrizk!C{0aB>R6hZl+?p%YD!Yak(x#i
z2PE}KnnDixdo)eWNb0dPbx=|#($uV^N@;3NQYX{YyrkZdrWPdicx>&ya}RH(DA*1e
z*_+raMiEngC-$vm?1h)#|H`GxM_;~l^`)d>60f{}>BXxrZJ*fR{y5!UsVujeklQe5
zPOXKlroPC=d_zUYS6cc;RjZVl=A$j$Oygr=tr^x#LNs2IgZ56-RDQ`+3*1Aad8qd8
zL+lHbwANg{m5evHHm;#UYo(%V<iM?#`tY^z#VUw%{#ups_kpP<0qVCURwPrcMAuRM
zdaI$5aWprOEQamcaz9-<RSuS0TTR$iXM3@8$s}1dO)l_l8GwF@qs?yI0_Sc;<RW+`
z$>GaX8!)lXiWcvu&n0fO(bi9!&jeo5Hbls;YF)hrK9sWhl&ajkFXtAC5Z=pMuiZ+m
zfQm%jHq~amscJWq32CiQ?Df0YpWCQ4lT7U<KA=i6RVY~8y%uiC?DCe@FiR^pT6!J4
zU)PqSmPTKkRP@-YD*a)!p)WJ|VFtoeUS&+k$`!^w!r&?catHlU22{=ZV+_g+eiT76
zzFLDB(*2U907}2sSkWJ6nYY?#T@$8L_KkgGubJ4{B-caRh>l2tN~PYcN0rJp+6Xx;
z%g*30NJGQq$mQlif@b6|V^4z;(Znd?Z`wX7cgE$<>BZ8V?YUv;eX85%GTCqUM~z6a
zE@GUN1cJ$0gCIpnAq@Y*s@t(+xLfYnOLshZYeS;pVY2i&1SLmP;pwvshP7c*&^1{|
zvNFDe3L#BP3!HWP@W}WEYYvT(`j(fB5pn^f1S2y-_gfenM!M!kqGs0o9S5c`M-Fu}
zl;=ubHq8rmoG7<u3~LW`&<5#S0kM(17$y@a3`n9$gEGF=Pd$sW2<l3K=<L{00cN_j
z22)>oBuR7D1q-E6;`@}sy&$C^Py3W&<QJ?>D8=2Pw1{`ybEv#LYi&aLPlD@KCPXM^
zQbI~DV+@=b#%2+eCT01^LLZncQX#Vvdef$5InaO?^GDOhFeVYHd@|FhZvdjey%8WX
zgZjr%RPxPy(>kJ!Z44UN*rp00F#-b}BJV4>4C@6#iRnYYyFu+y`zQ?ONqhUq2quPB
z-Vl}JtM{nfzaaro5DFl~J1(?#l7fK1ARK)8<OGKS0>Lqy56%kE5FpGS1PH7GSf!TM
zuz?LgFp;zsx@iF9GyN0*;tK@e0~`Q|qcQ-9OfpKr?)E9|!pGmY6tDz9iUNV`k}*fP
znD21~8}nAIU4o%h{QtoLXgU{nk`sHUqFXnNUDsikKgnR9`A!aU;s%5!o`b&mb5sur
zQVa5nvTYav+sF5r{regjVxhEEdZOW9kQhP1UCcJzr0`83#H_;aB}*Gzdn6D1lzcO-
z=k3VyM%_%Dk-5JPd&ghH491x>H%(*iZ=+6{!rW&f_!B7%F!x||=rsp%K!32A`z(>N
zPpO&vEZW+y6yBZQ+++FkD;lG8SO%2x1Jf>xUR2!=T<WE)S<i2>O%dYa_xQmxe0bj(
zHX`7!(NJ%mxzgu{R{HkI|IIv`riRu$cQ+`nL=iNAeAxBk!TOy2@p;TNSGEP8?4iwo
zmOVUF$2Z}_g<QbQ(o`0o*StwXM*MxpTje^&Q>=lUFJit$onQuu5Dzx3s1oC|LM}1l
z?PmMA@GqED(0gW}6!YJGW~il3fL3zwu4UK{e7GJwx&d+2P0i7&&c*(2KF*1ia2Zm=
zr;W2#Sf@{!I}lJjPeR>oJ}2?wQxeD9vt&Y2u(hB*CS&+Hpgw3T@q(WiEqs76u=vR)
zt)@x%^w{p}3L~vdM$?MlVsZy6s4H<OYzz$-{0a2VGw&*b(gEWVYA}Q3$j}5)Fk}`+
zP|&lcMF)*B)<lrFaNioz^&0c~M2yWE4(7Kt-B5XhRQjLF6NW4INVcYbOh1Mo@kyRT
z{RlHoFeot)Z`%kzncixvYE%Q*dhPrj>V_8(ST?<E`P9iv_T||{_lQ07JrAy4gsx{F
zMZ5riL%}P$1>s<D1cSwGKQw~z`}?^M+C6jn4SZ*OdhY!Iv@nX`7?2R)*n=4u8Bqq~
z6u>_ziVm(V;U8uL!v~-;hpi}sC-KY<L<^UP%0h`<kKHJ{7EmAoI?VN%p0OahxPa1@
zuz5g**libr3^{~wH)p6P2)!H~KC)=CfGiAMh)NlEl-(UOX^x7nUpU&bsPFgcr{%r+
z>GLp$yY&OqcbSUNFP`Vp>5T<jYk+xgWEiV#w)J!H420*SvJ?s`kfi_ves^4+f?6QY
z5|HPXl5BXrz7kb#b|bf-`y_`R+Zh6>FLGr;a4kUH8hc=)^+s(#1dCUce&)nqfn5>}
zxVwaJ$-<B|L1v|6HkE?BCyBACq3bYrWG`iedneA#vx#%-Y~o>bXGdsPQfzmvmI}x9
zUuZpi6oCbd7J%GI`_Z0co`yC)gTF~|vT4ZdqP_jV2!@AS8^SNK+r#faA_2i_HW$7*
zAeiSxFx%@+*cH$xk<%ByyI|IRam{V3^F3kxmY@$zAL`Qx;Z&fGi3<{f7HV}+ICt|?
zkskD(OT}}yy!YHtdm`2yBcfN~)`Xkp^5%JpRyZa6rLqs-c%x>7VIKwg?=J#nl7%bD
zoQnvg^eY7eGA(H9nyN2Dc8M^f083nKNErbpqHIKf%?@r?MJ-t9(!d196jI<(h|zoQ
zU}7CiDnu2;6aDJ5-}T&nY{uSxWMsPcZ+VD+amu+GL?izVBAiSL0mZ#0(Z`%DNpWf&
z@s#aiDs6st*~xOagNfP@Rwhq@VUdZY`Ps<Dt|bHtG--Cu0^j;n<RcFrHKtMGP?$wv
zSI~}W8!)r@6g+4Yd-Bxr^eIWBWS^(lGX%wP5e31Leg$u2R{Mt-lf~cLPx{Ui^84HO
zfImIJ;&(CNS~rXQ6k}ZE30yPnTi9oBcIR3QEckj_Z4&Bo=6bcUQt93shF}-@xS(Ws
z#zjUP<Md9Wh9wqMK{(d5eH<%y`{9vk+|Q01vag3G@hVydt-|>NOr^20i`+m#%84CP
z&<G~gj+BzbnvE4a;SW4<NOTe?HFO3DbnIi&F2)&bHeVd5)kH0iZP7G~jXr|ajh=kc
zF+B6-ehOrU+chwOI4*2Lx|^|#iSU9%(ce&Fz{M`cxME6tz({#d!ok73p`Jws#Mn%i
zXa8gK+jt@aHzY=3LPFuEsdcTjEc6odB{ccaFB~N|@%deFM|rQNff4GX?~aYOc~}Jo
zo5+dtoLAZ{hQkY{K7k~#ApoL>PG4gn3gUUjo@OAbq>5OIqQvbPV{6F8?h|%Dv1f84
zu{RQ@))-X=lW-+gH`>^QT&>)=UaK|UNf32q#34uqZbB6yD`=EYONfmWhpJ!_is2Ya
zwjUe;*JztV(t*^kXPx~idK;L6R{<OYDlqjKOweG8d)Xs7V*3u&5v49UATgNWeB@~z
z`Ma4tLIoV1K4d6)>~@1m`zZZPgneq1<gr%(g%4>%;JnS%j(8R_Xb{AU@cUzD0tyEZ
ziy@%^c7j=Q(BD_lb^(21cSfGeiflUysV9mAVCAkEMhTFiZ9u`_{3m<?Q}0~bI!+Pw
z>!rN}LT3g_E`aF4G33Dae_3b?<QjGZNMVW~LJsK*fv5;x^qK+}&Z2IbfZZkYpTL#6
zm3G1Xk%lBpolMip6>0=)06QqMbCQxG16{!?*^ZHkMD6x?RNtt<H?YxOJiGXA{X;Az
z#7i>+a3*Sv7kP3ktf|V4dUU<=VYqTX^jhMv0eGU}eqJvlvAmv44(tzCZs=-T-$nyv
zxBAAaSrlJjdq2hCEe5hONS%^Gk7mOWFU~6cGsrLH`Z^YDs_&~eqV_k~$TNK2zM(&l
zd)bd_8|~KU3d;A<{(i2V=TOxM;AyNE7zhq^EwwF;1v<CPoqO3wa2<l%GlN<Mo6at-
zvh9WMyPk&V6;%NDd_~XB%1Y9ABO8~4V_-PVft@MPEt(0iVrPLtXV*1lG03gZmm{EL
z5VsN0nMwDo%0O<v1^C2J%2f%GJ+QNN0(4z!4VF>pp;y?f=42EIQiH3Gni}yJq5$N{
z!X6*RGS?B%3OA{PknT<5P%P2Cd?(j@9;s=ZKtX++KS4d5Jb`VBlO}i$C#F!hGXWD|
z3}>8hmM$*xEYwc+b*D29QvkIpQ*jPL7CZGHz-JO#27C02h$jo9kgKfJ^#-<jYI>P<
zxo>Q+DwgYYCdDkx(&dHIZgTLzI8@hJc$9%)PBB(vAiD}8rbz|hgpHVc1VMSuY;O#*
zDINp~^%<5<-&YzMQ2rttc!O0y0M80M4b2b*A>B)VG8*ZAgeUwap1n?(vL6d@1m_1&
z@C1~H07j-M=7aBMJnlYp|K<_9PtcDp$z_kNra#8HlOXV<fgwP#Dj<BAI;+B?lDvzT
zUbbm{5iE|;iSRM@{RKjKgeS`a=Zj+7K|W`n5=r^+`zD3uEikGLZm`v&xA@{DHhgjZ
z-M8H^SX=)xDh@3;l3FThd7}W0ar|(k*l-YXfxwX{qLOjudn5S-J@c`=q<$u?4_g~0
zfvAe#5|)IGaEfdY-n`Vcjq5SB+37r#^7I(j{dL$=w~i>AZaee<473GK)vpP4F!-D(
zS5aF7EkVV-8uJ~-RQ72)k8rFPOANFapJS8^_cg*Q3i12)drF0YS$zpv`|dyVIgdbE
zIL}p7PW{e$i4TfD@)sEU41>=y7@7V23U)Bf+<%&LPc>zUlX)HxUj#KifjbS``($qa
z$szW_!yN>+Kp>AcL2|)aI`9m3z#PP^6hN#&gCDlmt?(&MIw67_P+H0L^z)L&6vIgZ
z=ZQ@Hufk`DQ^ctSM0X7>ATSk^M*;~zbn|n)eAqYx`8bQ3)(2lQECbRL=Tzgd)VbsF
z%(2A17|>aB;Q;y;<R{wi6q?9`ob}^ETF2k~K)O)n^dJ^2ebo-1<GDY+T?Hpq3<#*?
z`?Ee`8wn2pPnBgYi&fl!RAENcikZ|~&!ur$;N@Jq>#xwi!m7Or`qN03GgYO)UIh+i
z|7HdI;${Dq+_Kz|m$bPBI6Aysp~q$zCwdnR{Tcys0udU==I;cQwy~n-0TD_@c>bWy
zAszP<9)mPv$d=;LaB&G}h(QAh2}KsJxhw7!OW#6~-bR2ETO!*=hWQgH`WlBxv1-{v
zz$7SpwE8#_1GIXQMmG*EL-LZ=$!?;%hbfaDOP=}qAHhTVw;8Y><Z=k*v5i_a+|o6k
z_||(Ym322fvLrC?e))wz;qWLY4F$Syzqwm)39m@`igUr(_ik1<YL!ZoufQXvwi=8V
zDwWNxYQvQ1o9vjwl1;+_mPaC|KgZyg7?AJjFEEf7p|;b%$KcBhb{X7Z&>z0UrkM9K
z0?t5q3;`zi@xTivf?_ZpJQmCZ2ZFI+K6p4d6y)>c!Cc@<{bf`g`ngD5#1&El_s(#E
ze=8Sft6-`sA7`mNmEn3xd>);TM09?C)hU31N~u^>oKT;b%nDe2clz9c6(OtYOXzyw
z4dvzd0so>uH`MCeMd`hN1b`6g{s^U>s^j-1SmCR|c}Pf!?&kP$nCg^kkYli9n48Ll
zXPFWe7(?>Oeu+5bVwA&2gSO}jp2t&AA*Lo|-(Z>4Br8oFc~Vo(<l{UUrA_zzCBX)v
zB5(flhz~eZMN;&gsU#8IZR)-dkq0SBMnBo7koR?dGS;==#y1<&CvnJPJ8wQ6{ijPM
z*XTY}O%D<K`YVk8K7+3^5csmafj;`c`vVmG89U+x1Ww-9%qiJRr5T)}#}OF^hv+x?
zG~t$H;Y$wj-xKDfB-yO~b;;i{XE`;S(Z9>!cNmC}718=Elka69ns=HpqF(yjZ)izy
u!7Rw<C~?Fsu;}m@dj=;`Y0P+zCudUw$8&?{^HW$d*}yM;!uo{u-v0&@GUQbN
diff --git a/src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/call_par_filter.cpython-39.pyc b/src/azer_robocup_project/Soccer/Localisation/PF/__pycache__/call_par_filter.cpython-39.pyc
deleted file mode 100644
index 0c604dd105836973870a4fa4a1e815792fb71e40..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 1413
zcmaJ>!EO^V5VgIVWZR~ppi)|iLy&q2NC^~92tlQylmm(?D3?_wYrA&aZ8p29?SLBP
z)_w)2_Sg%8AAn!T*(W4Ud;oW5yrhZ(!ivXZd&ckic{b7F;yi)&;nRjVnIq%}PNtg&
zlTGM)1B@Vorldzg(xV}zWSa;kymKPFGZr%G3BSu!Q~IKG2D_sCmNY6qA&)eet?fk0
zBylR+Nou6Br8oJ0ZerY>ZVV=y(Df5Al7v){kO?Z-ISD-h2r$C=q6}k6R73z{S<H!f
z7%O4{5UcjaR+Oe3@OXh0<0U2y{WA0|==wewK}UoWLk$}-!PW>^&sl$soOvU9#$ewQ
z1dc9s+KNKTPiK&7!?$y39!F_1ltST(LIBJ92My2qTBZlqKTPv|TZ;2si7TS2u>Y$I
zX6xzRJ1v!NMXi^4Jm|^H=+<^(UJdqJC{8yqt?|Y9>-Y=wkK@ndZyOIc9@j;pyY-|O
z9m>|*JdUMmy><k2WRg5<?X>rRPRjdm{R1a;vwvy>&XX)LoZkUp?Smm+xjbB*QK~r!
z`%5xRagsH`8IfahK}U4NE>Rrl$K==(wCk&Nfh&Gi79NgHHotR-fP{R1vD<ExQ~+)m
z4EiE6a;F{E6jlQ7=Q_a~*o8RHbfS&S;!{<D*oLQ|G>P>gAqp3Ab0igaayl^!vxpJp
z;?Q;QHKGu@M5{Dl!<E^K7J)Io*rcJ4kLRwr1kU7|msrg3{#UfqU<Lac$m^<E^Ira5
z!m?u+<Rb4$qfTdu`55&8&eRT(4wq*MVL*&^HEHN$u%m+?X+h@_h^SjI{X@l8l{5pD
z6@pw*M`Q+G-G+F4xC#2*S@h{M{$g=0%JMY}_u_0KOu@%<(%5PtZNbOVzFL8-SwdKb
zkN!t!`jAc|zyi*qp5)xlbC6sNQuM2wpA4e3*ikhEx`_sfx=SkrngYj<PhqJjENEn=
zQQCk8i$Nvu0?(}@S}#g6&WEeJM~SY(FUw6G*XjlMy$%#IK9Bp}U6xvdsV((Yl9_)&
U+TdA{`yA<M{1aMYwR-{m4L1-zf&c&j
diff --git a/src/azer_robocup_project/Soccer/Localisation/PF/call_par_filter.py b/src/azer_robocup_project/Soccer/Localisation/PF/call_par_filter.py
deleted file mode 100644
index 585d27e..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/PF/call_par_filter.py
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#import sys, os
-
-#current_work_directory = os.getcwd()
-#current_work_directory = current_work_directory.replace('\\', '/')
-#if sys.version != '3.4.0':
- #current_work_directory += '/'
-
-#sys.path.append( current_work_directory + 'Soccer/')
-#sys.path.append( current_work_directory + 'Soccer/Motion/')
-#sys.path.append( current_work_directory + 'Soccer/Vision/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-
-
-#import math
-#import json
-
-from ParticleFilter import ParticleFilter, Robot
-
-class Call_Par_Filter:
- def __init__(self, glob, coord):
- #self.ballPosSelf = None
- #self.ball_position = None
- self.localized = False
- #self.seeBall = False
- #with open(current_work_directory + "Soccer/Localisation/PF/landmarks.json", "r") as f:
- # landmarks = json.loads(f.read())
- #self.pf = ParticleFilter(Robot(coord[0], coord[1], coord[2]), Field(
- #current_work_directory + "Soccer/Localisation/PF/parfield.json"), landmarks, n = 1000)
- self.pf = ParticleFilter(Robot(coord[0], coord[1], coord[2]), glob)
-
- #self.robot_position =(x, y, yaw) #(pf.x, pf.y, pf.yaw)
-
- def update(self, data, other_coord): # data: {'yellow_posts': [[metr, metr, weight],...,[metr,metr, weight]], 'blue_posts': [[metr, metr, weight],...,[metr,metr, weight]]}
- self.robot_position = self.pf.updatePF( data, other_coord) # other_coord: [metr,metr,radians] or []
- if self.pf.consistency > 0.5: #{'Line_crosses': [[metr, metr, weight],...,[metr,metr, weight]]}
- self.localized = True #{'lines': [[rho,theta,weight],..]}
- else:
- self.localized = False #{'penalty': [[metr, metr, weight],...,[metr,metr, weight]]}
-
- def move(self, odometry): # odometry: {'shift_x': Metr, 'shift_y': Metr, 'shift_yaw': Radian}
- self.pf.particles_move(odometry)
-
- def return_coord(self): # (metr,metr)
- return self.pf.return_coord()
-
- def update_coord(self):
- self.pf.update_coord(self.pf.p)
-
-
-
-
-if __name__=="__main__":
- print('This is not main module!')
diff --git a/src/azer_robocup_project/Soccer/Localisation/PF/pf_constants.json b/src/azer_robocup_project/Soccer/Localisation/PF/pf_constants.json
deleted file mode 100644
index 242b4f0..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/PF/pf_constants.json
+++ /dev/null
@@ -1,22 +0,0 @@
-{
- "noise": {
- "forward_noise": 0.025,
- "turn_noise": 0.1,
- "sense_noise": 0.1,
- "gauss_noise": 0.4,
- "yaw_noise": 0.1,
- "line_gauss_noise": 0.4,
- "other_coord_noise": 0.1
- },
-
- "consistency":{
- "number_of_res": 1,
- "consistency":0.5,
- "goodObsGain":0.05,
- "badObsCost":0.01,
- "stepCost":0.01,
- "dist_threshold":0.3,
- "con_threshold": 0.0,
- "spec_threshold":1.0
- }
-}
diff --git a/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Glob.cpython-39.pyc b/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Glob.cpython-39.pyc
deleted file mode 100644
index b59ba437422d13703459fa62ea9f3dbc9196962a..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 3973
zcmcIn&2QVt6`!FfiIVMDb`(3#hogK)*+g=@Z3+}YP&alQVH4Sq<hn~_LC_pZp)HZh
zkg6krIwjYlIrk*@wm=W<KhR$K2LwG7i@ENh)w$@s?~Ohzdy@tB5EAn^^Lroj-prd1
znlm#=0?*6)zh&Q@CFFZ-Og<54{0fSH7YHMa)`>p&Qk}}SqAQz(DJ=4iu*h3Qk1$#$
zS+x)Hq-Y_Xe8%>c0ksL*&=nS!`V@3k7JWx_mBqk@Ce+Qkx7Ys<C|T-+l^LjOP@X{X
zQ@~*~BnCK9!!KnKpbArgMpzW68Y)1e5|4$*jR#s!V=?Ghdq;vq7-5Mtc}v%!9HDG-
z$WMNPKQ-h}jrbF~(?j0$M|d-mH^a^hd1N2F*;mtq&7SE0Oo)-pvJ^|z(4SO|u(>}W
zHdiA+=Rs?h%^xW5slOoW%0&Gv*7-kSP)Yr4S7m9?NQVkLw@A8C_%1Ahjs5f@VT*vo
zP9Wz2i3jISU`rr*;jQwAOjipQdW)gfJAXuu(3_=QNCp+&w-HL*`3D62dA~xae7^<?
z{VcJIOXR)6{}rmev!S|w0m?-=%9kYf($YZc@)D_Hte4+Y7v>J)n}s+)PsXv_O~~(+
zZW4T39!7Fy=znJH_0^x?tKvWUl&?XCU3*I@={yXRy{kv0I~7jtUz7e`ANqR(zEk_x
z+0EZW>_H}+>fJbk{Gnu*xR+EC+<dRJ|2!B8uyzYMUlUhZAz=c>Apzr%`0;UoS%}JU
zUp_hR-RU2Udc==SMgp3#Pt4e@kLdpTlx|$=-s)-)@2%eL$vHog@tvCUi2rWV0=tcR
z?9LK_yw1w{U96uc?2C(-y>~{~NN&s-NZ(r`?7oz|D<yK{_7~wa=-&%T?|!JDFLLGQ
z80Jl0(iKKv>&O?Matgacr9Tg}j~j-d1q-3kqd*0Vj|HVd@=dY}`_XS8ZC@+6i+!s|
zBgJo_CY0QYpbp1<N0R^H-uC2|FL!*K`*}0J?pdvd?FN2+(+Qrn_VOif&pU8}{D<C$
zzkT>Cls|s>%ZI<OuB?8UVUB;0aT;dL&X+yQvU&cwXPI@!Hv`9WUs`q3_YK_pa?Q3#
zJgV8Q{iewuV;KL1LOL1AGz`ae0>j8Xs-wF9I5!sU<9_*5<K=Ns8bp{F*JQxc*u?J7
zU<Ju%us!=3ZKp=I1>O4$uFs9k_c5jZ4MjfL2*=DiCVkAnJ|2R)R^BO<wl>!)oofZ0
z#iq&4hMzAx4J=08bXmjX2Yznf_uNkEv`i-@#RggfnfVFCSRG(<`9r7b42?@jP_8F;
z3r`D1p=@jwb@ka+xuUDvTct|p#$+!g+pOo2oc87F$E7A+x&C7!Bj0{#=d`b#yA|Gc
z+*&5^GM4AMwiV=Zxy+r0+0F#!ft@ikhp(Kvodr}^%jLqlu5GWC%ez~p^{gTy2X<T6
zeA{YqCunyTie3hL$TU3Gs@oaQ-SbS&`f%?d`dn$JXjC@J730B*QLb!km;1N|Ch*k}
zjyER#B6PMP1&vZ+^I2t}TiPh^Jg@ZAC~@If;`zqY69U->dcrarHaBqwlFY8wAv9R}
zdYYMmX}oU1W)QRu$2V%8$3*<e*7Kbg#j;RKTf1ek00D8>YK%jV9~jKxpzQIs9zS$~
zS4OicQZN^RW7Tcna9fQ%n~SK)x!D#`Zn`yFPc*BBS%*yn{1{bNMf}iqYOexckGl4u
z(G)X&z|Fv}wGDJiB+G>tJI~iD1$222)D6q?I1?IJkTpHw?fHR;-HMp+SqFBYqmr?;
zjY>lGnzk!c%x4j;dnWUR%59VBQxo$p;%iThLUE%4{cSz@%2?klt-a8bE!H%KCeH>Y
zuh|ehCbr2Kw|!s4TLZ3$yLN!XBBD+1xPh+j7K%?qe3(6Ye0O`z*e-viYfcrgs#6n*
zjW?Fv#L*YAK@N*ps|g|7daCMhKQR2jZW?XJt}{JlwIRMq<ecxYA;59?hJxv1xNcX6
zs7Q>LwMaXSrpM9v#31YEMnkyvJI`UJYuN>V3(~@Av<$`_Fl2c!_k(0vK1Sihm}D7&
z)3CEMLiO!>RZsdLSeIuDzYM9#@$$`YVv^2}9Wtu0kX%SgeRePk$H3eWSRF%xR{1?N
zavzPThhT+&iS6<6$nnS!+USVF<H&ylgwPn(pd7y$nxU8Ay#rVh-Wa_hB{Vt@?*+O-
zr{#B=rU1t`B{7XEP+wM3^b9?t%+m#s%Rxc;hx8g~rf4dPHnFeH%!wn@!hL%5;DR3i
zPy7QK0SPIiaFbNv*1OzQ0@_nTr5C|lZ#Qz{29<h1{ME_ZPZ;T`EOG=FiGv6)!JkEB
ziFDNf{zLX-_?Pg_UN<URaoH-t{Tq4lGznvPiw)H<4)+}uw6s7QwxWQ<@DHJz2=P{y
z2;&U?8Xm!2^0>v~i-5e|fBjP4fwW?0d2$;*k!bQL;O8J#ehrB{z;N5@-@ztsiP<E-
zh4tG=B$|bqh``$88N@Ckxg;@IY0?hp{2Wo4We=gIPvJthnX&Fz0}&Ckyn3tQiU{`(
z^_b%_`^^Jbt=^%{^+?-1Je5QIXJF<T6n_B-fl0jw6RzQ;!#hpWKs!rvazD1Bido9>
zyv;^288N>NLnIAja0Wnq+K}7rfWv<QqO!*cFEBM^I&(}KH0EPEYc#nok*qoxqbmRk
ZwP6OYB>XW8LVGmm_%U7te6)97`9IiH$R_{*
diff --git a/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Local.cpython-39.pyc b/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Local.cpython-39.pyc
deleted file mode 100644
index 193a9ce234ab53ffcf665a6c3dcf8f2d10acd4cb..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 38344
zcmc(I3v?XUdEU<K`-ugxcn}0h2%;#0qWAzQnUN`pq$rAXXp$wV*H+3J#4NxCfnDJ4
zQY4UBMK$Fnb*+R=-PTdv6m(nHa&;UhaZ@M9O`OE_aa~u{Npli+>f`3sICOkc*KJ(K
zaixCWfA8$hE<jp}+f#yj=RW`YocllTovCa#6~ezOpO|z$dc+F-Ap^mGVFV80SN|X!
zC*)Z3p`w0TMN7hV(Uxbp7?x+G7?EeR7!9P`^Rb0^F>bMJcs{X^EGFTP6jM&Ln8q*T
z#GLqxp<?#2kdttdFNU1tHM^KYDCMLPN+Z-$&O4b)wz^*Kb+Xqil()`>CUQ5?ve2ms
z%j<b$YJR?SW=fTgRpuLIg{YXCsyelW^T(!{$^U1;W8?Q2e)SBTM#v5QWQdJB7kb4a
ztmvYuR2%R_8s)2vS3)X|Y=7>+dE|j-9<P^GefQMvqqXVF3*~B~zWcFC<M`zZyU*4x
z)Gk#TyI)^@{a0WAdHh~}{gbc%+T@<eePd3gerc?-Fg07=eXce=T~@nK)~2WCEA^>H
zrB;1rdVZ>2FG<v{#bqycS?V7{eRZ}p6jn*N9lc;Q(+=TRKMAKC;*?-IEXT%BhMlkz
z!877Sofw``C+;Nhj5$dsg=gGJI~hC^PS(lcnRI%bJf1117sJ};#ok@IFkibc9R+Cr
zAN{QJ5&Y_-a9qm`l|rr!*K)&fZ8riJOW|BdMUdf@h-Y6ddG<1XSMa-7nuvOldU<|E
zapYB&PL56w9S*a067YG6QmIm{G)kpy2-O)CvSRYL(xaQV%g-^*S@r+X&!4n229Fz>
z^IJ4wNzJcVDvyNj8m+545vvE?T1l8nuD%`U(xtxjU@z}p2ln#Yfh{AoF6=9l>%hM9
zc3{f{tPA_%ee1x!xCS=oTSmM958tUdtd2RX$Bm((&^5fQuh^$<a`{Yzy)a%cy#j98
zC42hnfk3@UGZ)iqC*%88tcIjkwE+*k<ZjtndwThu>(#n;nmTIb0`ohJXLu%JUI3M5
zo$C#2M3tu-4+ZK<n;tU$)I^%AzdE&0E|t7gskBgYF3;1SDV3HkPtEHT)l2C8bU5g0
zfX*PD5jq>`@I4JIb9u%1Dk*OLci~s(i;%Dqu|&B2C#h>ToG@}8Vv*P3_#4hFcH)m%
zGuU>V@QW5U9&9<-NFruWkLn#A8;|Ve*myEdRyLlT(<2*C-szQ%r_brfGwtki2An}a
z$~Z&LFk-UK24@7%oU_px#k0rR<ZQ+>?`(0#@a%Q&aK`cMbMADu;@R)q<!r;V;N0zO
z$8*4$aCYE1=-lJ%#B<2m@9c7RqlRH;kFyss8=OgOdiQxz*@9mQe+}J}e`*{LaXL0a
zzT?~sWx40EraLx5EPO8e4OkH+Y>E=HC4|jULSYF-O(-Ivm<dHC6gQ!mgc7ral@vE6
zZW^N={|L9=Ov{&*%{M1*kCQ-JUN+@kDbXixzmr6Kp;cx;Vh5$RAt!aouK%&|Cj$QT
zCHuOi7BQ2d`QavKG&H}V8I$Ko6Nm-jjm?D1I!9f!y)@#Ycco1(dR5xc*u1*s2755p
zxWl#P#;;kZDFldzoD^d3tj3%SpMaB{-@0&D6V!p5yv7pS5SN%Gg1Nip+g^Q_X=6^>
z^(#{UL^CDN9nG{n?`dY_xfAUo^n2Vi+#N1!oNzO%_nH>%A}lv@jdkp9>{;C_uqU0K
z4IwAjk2YY&CY`(v4TWx4PH%xN+vg_c_B(xW?{oTJV(I&3%-=!eH&Hm{ZGQaRqotFF
zPaS>t;j=${u5|45+0x0=j~qUE;@si$Cr+R8MkM{jsYlmIyTeZ_J#zN+xpV7f8E2L=
zkDfYw^8C}KTU0S|^u*amA30xo<n-yYM@#R1^zgaI&p!I@M^BwE9eKKR=J1KL=SruK
zmCl?#ci!7baOX~(dh8^iA|jYu-NCnj81>jqx;MEG+<X)NKq6(IsMN6`8B#_KBgso2
zJMrkrqopTL96f*B%jm$#M^8O={`lAS$-n>c@PvJH6<O5|IHb6o#`0o$;(!-hsF5V{
zBD0`Ayx8KDnp&u<JDGJW9qw3)^nkjX&UQFndS<>>Q>FPzwOsf5_$bY)+U3R4bZudA
zYP#WNrd16D)?y84r|zYuk=v<Mry6B1EB?}Rm3rmEeA&zCU=37bxuKT5zRqx|Ht%?G
z?!T1=$TXHhQ{#)}>eM_Ua>nCVlvbb!8`!&_D?46VgQ{Pw%rv~-nM!%yDP2H2%Bti{
zHKx4OxfAbx{G?tQF;$+gO*vk8`ngFjGF<~r7_DAjz{9RCdbTQqFs&?<z06`+&7hZ;
zs~~v27~|9)FFbXj?)5trA`-N(biP(P+-OWqU-HuRic>DlU`T4J*srP1&gmoh<7LsY
z_LQQQnUYg}t|GK*F*9|c9r9xJa<yJlUQ~!%FTA*Xp%}S5KXsuPTd2%qRVZS7uLs0#
zkT~}u3sa4Ypahpx!%NPCOkbE%m(c2ksjDSDHV!&dmGO<Plreq9C?;?3{a){4t=^C!
z196Cvu9v;xHBlvu)&=kYSxx1UI8yuQJV@sOIw$BnO6M4z<8;`2b(YRKIG(i_K%9az
ziEoeKS9joOVl-#9|AaWTQ&!55shU`gNqC!;u=4n|<A&kSASMMjhiAgZ&!nfqIculY
zXRYKkS8z)@m&2>k5IoqduwAX%Hw-r;H26eVJ&rWbUYseWW;hS3T*vBOk0=?CMJlfb
zmWiKr1;u@uc%yF5y7|_Sge@YRw1#fnoNXv0(=7EPNGjsxz+BfG<?8hEx((YHsIZ|f
zV+EA!SnjeUn}O!tv8H+5xu>~#`Ts~5lC_c%{5q!Cv6|u8P&49&9UCYrp<dI?M#xrM
zU8@<b?m&#a8g{K42*Dp&jS~IOMdo5R>?YB8d^N#TATe-ZDEA|L4uzT|29k3r7sP-A
zq(5sl<8E{=O@!Hs7xdo>0sWJ2dns}hZFzRQ3Ff$&bQ7x?Ag{z~_68_xpwfu?%+kwC
zFC!**1I#kQUtD^*(bJi)Gvs7YGJGuzxZ!6%-%LsFyg=11AO&)KunFYJwZ(C`ZmZ8A
z)Vtc}MpyfRR#P`@j0KP^+wEj^?}Px#(2az`A)@<Qqbmo<pi7}@#Kvkx>UX+QNit;|
z<Y|@vR9BANZOa3JqdcIr%Ky)<@|kw|X4a$;(PM7bwBD{wcI61R-md*pS88|b?V9S!
z5p4b1^4+bsYroJ{KG^!)b*uV0lO7!Zn47{Vq;-Fs-i`iPgll{qgR>RvO}O^qu2lA>
z4GVdlzTq~klU=!kJzEQlJ!``P<PKOL>Vg&Q<$80}hJ=;Y(ZfG5ko*}9_ApX=zAN>1
zJ&e>yByzNTSiv5yg>}0gMrtcvu!23zxmhQUH!M>BpqtY^^&uyYcRnd0OBL|0M3!E?
zZY>pbocfl;d}ireCd_>xus<|=+#bgs4mH6zH1oH8&#h;_*zul6)$6*g{*JZgt%%l+
zcjX9<a<ulPuGH?gB3k>2t{lOUU0c5Ut%%mX&{aM-n!Q!C20Ge*s}&Qg6}n&r`x2{t
zvMcp=D<)QZPgm|>zt+OK-HM6TUhRU_(MPw}tdsA#`PG7(Cf>mc`Cend&4bahu$p4(
z53nW%Z-knC)v?Bqo0oaan;hZc&<5`H_C`o)ntiA%zB=soxji@RXY6Kwv*2=EHe8Ba
zkE?NDnS#?_2sH=XfyT({M*Kzr8`y&7<~BKn4WR;<q#G9U47y}289RV?bFe@x5LYj_
zq1DZ9KQKnW8$pZv0WmxqX1fK40slBT=B$56u&|9XzopBpUmII~!y3jf0MYJ*sN0BM
z4zVBgZviry$AsGX{UMX-3qa1bAcr}+>SZS^nBB*Y3H5xV$FWc&m|l%zpWR9f>kMDB
zj)tBs;2pD8$A~wb4L8D=t;Es={GK4r?kcw{P;N8JwfA!{p(b>Y5MIXbCoz7yO$mfv
zTS{PtBUl4A`kYW;>{st_Bj`)Sg?MUW<dm><!m+6d5J8>|l03Rtxmuq0B2%h7b^evG
zmzQw8)VK($@gn)39Fqd!D434f^0AwL3J^CR0LgcgqLJ^WXH~ge9TQ?+xV)Q`n0+^U
z<zl5#9wWJ5sm^|FM*c+(-F$(i{{*yIvG4GiN6(%IqYCc!Z0XF2ryf07%nM5@{AB6G
zsnWaOZ6q|`dARZ*Ydi$!m9Xv)iB9oNjBA>o#GR1)Ui1V-@Iv)_iRtGWkJRSDyJR0Z
zeG<(1k;l$Xo;-X+ky$7XXx^@LPS~5$(Mo-Berj3R2hRpmo0_Rq!M}k=t!vSnNZlxZ
z^P<o?pc0YF2^*18vlmK@S_xc{mz*s(N_vQ6Q;Xmi!Q{=Ct6E-1`DAembAY8&^>PV(
zQ3<Mp##D8>ObFz2)YI(F6RatW1TU_poXS+SuJ$u3Tdk=Du%|2K((=?5kpOy0Q2VCN
zKFt>sPQ7$tnRzCLghwhyDhsovVoF%4X^u;AkPN_VZEC($sSB@FnQmML@0pZ5;O!ca
z<1Wm4F?0;iVvoeP7E0ZVaD0jxx}_8K(iyg1<dhs6&zkZQ04&R-NS~~XVpg&*mlo$Q
z*Gm_Rn%$EI=|0qpS>z2gGcE<DuNGr^)Vv6X4AMe>;J_v>EO16u16*zCO2uhhEY)W8
zXo@^k95Q%b!6`SuHcMMeC#of3YhOT0{UdNfRs?_K*J5@C+!?vIgxzNsto)l9J7Nvn
zIct|S4sNbs9kLF-8L<z(VTXNREOOF1j=1B9Yx{<gmwX|A5h?pw`wy&P{EeXo@`YQi
zLDbo24_n*ej)}{>tR;gw>5khu)Sa?cMl=JoUN@%6EBgO3@UYv$`TP+)pbqQ?=w8{Q
zeya)ory0R7+OS;2HDi$I$0Y3(1b%ii(F*TuCL1>PqB!<=;zY|$YTq#M3HYCoI^ZUp
z$U8#K#1edN6#U6{Hv!%$-i$15cVpO}!YbnmPvwI4CMG})DL^764w%h!tG4@_an#10
zFz%*^xtQLn>7&?LBRYjpu*0(6)Y3<n03Ul^^pxK2xJP{L<)7idUw&9Nzlo??Mx-J$
zsoqV;g#&>`ZE+%_r3lv;5#`FOQ-R?WdoF+p)k_h<^3`HWBI?sMRW3&HT9+0*t5S?g
zglElp)+H~g$}?pJKDXR0TX-+P{|<h2ip03mGWbO>|9obw9kM=RaB~vQSflV|td(Is
z<7?mSj``-?V|Uf_&D{edig|Uc287kX%n>!_wnFa=H8JZ<LHvPs{DD?{8n|4YZKq(A
zQ39;JQMJej$|hwmRwis2KrhTKsq=+XFQ6&^9Y3Sq8^gLEu~zzZpE@?Pj_we}5TD5M
z-G%7{;-o5)(pp=Al_Z1i*kH(Q*9OvkS2NroQ%@!#T)@IaPBQOgu&sk3N#kPcPKz7_
z%n0&vA=#l)Y-zKB9R+M4(*LWl$r{9+7>Eio!wK>I5$a1CaY6_zh)G?=D<DfeGzaZD
zNEu5FI+VO;P|}*qk|e_mKz|R?AhcM`>k=ZLPH5!+-PDEo%jGdy(>Gto)X2N=-OO@%
ze!g}^$Dcno(W?$(tOc8T@hcT<pNli<{frWo+Fz&d89H~;p>(#jk@_N0k?)BJx0NJ<
zT5Qg;$Ergtl>|P8q;ei|+$h{B<qh~Tho7re93sLK_0v@^Z#FnTwT|~!6uK`0jKN~W
zP<y=yI~RzV>{@}|5Xc!vE8i<CB2X$xW4{AP_3dyd=i^(Lz#BJe<-=`X3Og`&U&`^Y
z_ioaw_7?Cu)-M&1)J)6zb?|axrmzw*^P-Os-zerxM0rTlWF690Bb?U?HfzL2S7SFq
zX$Y1mvcfNlUktxEe$d5WEs^}U)cFQON!h-?j_n#c5Qv?yO=DwEVUxrb+Q_VC;UjAw
z*$BZEiQa<vL3a8%a<|9<F$)AJu;$<!gg$%~#BUEa>5QAz#Ii}F5be7l`qxW~N=t_!
zQd%!nukicb*t)4;nbk!%-kG*^5y-}N5_p3ncqZ|T;+euThG!bjIG&k<Kz(>-%`<17
zJxDimf)OMmshKHDQPh*yCE)8dWBeuf`w)xr@X36ca{Y(}G|Wkk8z%Z`W3$bnwV^q%
zJ{sY*!k7zrul;n4sd>)_oV=9XkGx>!<n0~;t7f~o8@7Zt%#Ao4M_Zk(<^Vl9x6$d9
z@O$K~?>lu~UT{q)|9z}KjSh)%)I6MG?lQ&sjj|(X4R0aep1t}psUQ3FW4{*_g-bDW
zk_s2mu$1eH8tr3VXkxUjv+;&CZj$Vf?t`fHbIVrIZ%~)kqUe`TsBkY+S-1=(i6exn
z=z27g=3e7P8<!X7%StvM!8GCN8W3~6QWey#?q{b&UGxt69;Wj{bnd3}VLBoghqVn9
zFy%I{;AOPl=9JbU^l19Hp!GN83!o_?Nn9;oQBMI(5eg+`lr4Ir*(>@9?Vk+z_XYg>
zwLjY$aB6|nPqA75n2zudq?Wzdm2zeFVxt(97Ibf4NoZ$u5Bh-y>ugYn<Rboj5`ts|
z{2&l>ATY<Rh|P^`v;0xz5d6!4*u-CqWNu#m3MiGcZIoW=(HmT=#twl$sf!j$=Gwvn
z#7m=|1HFb9f*Y5h-B3vo?`qsE;n-^8hRz!$dWNKmrUW)#mGFndvdx8o03l(PCGO`=
z3EEx*xr$A5F19}GgJR(|5ed+MuY`+n(a4pq`kv)tQpb=tO6yheA@s(J5TEEJlTl@G
zLRGM7P~dq9jE9~YVIBKH;gMG^omLf?hY3beuLuNc0!8}-<p;5qm(o?0o}Rh_RT@=;
z_3ruYHH(Y9ns8w=r#R1C86nJEALeNg=qH1@DnRwLGNk8dO{rrYijQHGPT?o`Cj(4m
zCTnD!$@2HTPwPC`)wW|t<6izd9Bl4jc*gm}<Wb21YRSamiMR+vBvCiqNMItlD5As^
z#4m0Hal}S}I4DYDZa5uUn<BZpARsr@C$ZHu7f~tb&s0IH&1KmxQOji6D#@K?{b*4P
z7y#yRSZ2hJ;nkd*;4%|hlDl($wbv}tKJBYsM=aYKj)L{@p|8^~){ENK_F*{m4$$Em
zw*XG01xM%Z>Rq6QHKXLiS?xD%#LI@c`N}#o$Tk-oFlb2s7HEipR*69;Li|wYYZICq
zB=!!3U;}bJs{W(_EF-Vz2F#`3LOtsvm_t1xXg_=iJIUEFY%!qnGAS7AMgo{9x)JmQ
z^9(~oI21xIDwRg(HbET7=N56tXcNNNxjSw|Ab`v?vw}GpH|{`{0fBaC?oPp@U?dw`
z5rUu-qHX&6;OlMN<>uX98l6GCWj%Yc3ELM4@cP_5#EM}x=k~8|gP>jaAn*2(YLU50
zLg<NAImDjmcz+Hho+3tULb|l>Z?JUg7NuKIS-SdBwu7xhTOultn(j7z$#=A?kNJeb
zUE8*H+V^W;JEw*cY-2l3L+NNI+v12&7SzTzifbXLw3oiz*p5~oL8~WLcfcKmJxv<)
zN5l#>?!j6i_Ys=gDL$<%L>xxH(6~$HCp35OH7#Ozzsw56$bil=_xkgoXGZ3ry}HnP
zGy0wRIScZpb>&C=y7_%-s(@Mtq)(BhMd=Oc?oqcOl$36HOeGq-Av{ks_DBo&iqbTD
z&Hky6HU}G%uuviw8J^o$+K>1AKC_~Ph8_+%QP>Vi%Ru`|YZxP<<RO&0U+*B~D$Q<i
zzu7a9LK>X!h&v>`k{HnBECqhF@eT);b2@~ad3VU^bqAfkmtbH0Bv4!gf7c*ZuJ-K>
zU60nkvImqR|DMLLI)-w5&q*bsX9yiw{UNv;VAs^Y5pTD#UOInF(}<J{YZ~$A;qy{Y
zo;Y@*bOL5Wr$navwI3~f;<4XfdGTR&4>Nuc&dLU3;XGfP-Q6-9*)><MRn?CZhV`Q>
z+3_hLLWp9=7H7sESn1h0Ha>kBBviGbBkbl*8eh*;K;JE*%)|q2Jgk0!HIZ^tzewj(
zbi|^AhMOW7RsVv%SLqz1bAZlaI{%W+Av!`63K1xT;inPjrLR<GDkW`l<mFU(5tJ~+
zuI#iTrd02u^TTvb(m74%@6sXWC+!J2Bb=*`l^dro%t4TNrdF?rJyT+Op47mJqs8pe
z%L@z3rM<@?-JLJ%{H59=EIi945E$jD1qjBAc_DktF#1#w1JB}dC+iXMlnAO^28Dhe
z3!0Uco;Y#t@s^R9nW{6$TkM|?n4E*W7h9;&(?`yoKl})czSb-Ek66b)WZ|sOXbXAn
z4W2yxSn15M($PnsI3XrWx*{)91HbPLn#n9(si{j4V3RhksbwuJpuGoZcbY1xCkg+r
z(V;v?orhD*mFK4x>tH~%{m{g)rj!ybo2;nNNyUUAot|p>o>spk0Q`iKbhwx^;iq+2
z{VQhDwo-eGy?Qdx6)-w7Q^i6ns@5jpiYZO<J%U*(_Lv!L6-l*<952R6r$Hv~Syj(k
z5Q4tg-%fAW(^H==gK_c)wqES>y`>pgvq`FF&FJyzl$v=U3VjncSo46^bDkkl$n_k8
z{EQYz36V`iU`4bG#9>aXk|?>Rv;t52xOE(P$S4d#wwMF?*az7(t&||kcq0)e^FXNv
zbC4Mrw+7#g*>n^3h_%fcK}fqPD;JKTwh`nj;I{{Vg8~sGDW1CkVW;&FB-Cx<w5#<U
zj0Bko3gd+~{rl(_k2JLY$QHyQY~(&>k$O>+;<riDfoX^x3{~?D(jJkyD0XOxX~ZNC
z<RREG<FNI#C>LnOQedrPAsuMNV%IjY=~iqa00EK%lNV+=t-M*pLc-EWuco<A^H4=2
zgYzt)MXp&lY+^su-iut2P+qfW#3M|O=?y3}(lNQY9#GO@*q4SuiG=5R@q>NzOTe>;
zEg;m7AD<?6<&5-jV0DnkF)%hmMkb))h5=W#N5vVy{*Rbe8I~AsmwPf)9gTz<8?IZ+
z*ERNNj5vclb@OznYVQG-fiQi@C=kanR}*p1K5?|ov<fQl><d@%gYn{p<pyK}WIBq8
zv!{=oK3_uSe~+MG5nW7B4cQtgt6rn;Kfv*<6CKNgl5GC!{Pn?cL(irteSQK6ZVAVc
zeA9crGZVIF@7kl;>aKSvd2kl*-_=F+&}Xp5{s%ILR_@l!Q|H#d>!55T^@nz~N~qV7
zyJ(-81e&s^X5ibWK1u+&nJFMGXRJ{hgJq;Mv9T>yQ2!bkfFl}8u!Z^rgHhP2RvkfN
zUg`oBZkm&neV&zg(J7^%dE}n&#h^F92BbI2@R>@r7(ER8-804Hna59_EFC@l<f&rf
z@Ui<!=gyp%EXGbhex8YONqqc_m%8jMmg=?XOXY?bZaR&c*AJlsHI9P%rW^;HYaJTC
znG)Jko8IdsAH6zVUL*>WJ$eEpD_T<QSwKD@aH7ZK5D`J?2JuBPya?$_=QyihMgCvp
z?z9u05O##k*lR?{AnrEoGSqT0#s&+U!)`&%M#5SttT!7SYk_D3%&Xoi$QR2JCL46W
z7M^mW(6)MH>%dOfq6i=h2SJ<Yry}*^5dSj<FFazXpE~2jbY6)^P8T{7_@S$UA4&ot
z%%BGoMovn$asWyohbR!pFa}`I--Z#!R-hSUX_H?INXW35pKIubsn)SVMnd@j%a4%y
z9+|3As+xlMpQ2m%Aw3C|z+BM|2pa~NQDdPIDv@vmbAu5A5pS4U5bZjrlaTT1lTy&;
zOe9ZD*t6PX`#(NBtCjVC{IDhs`8I25;u3t&@nHB(#NUSR{6v47X!ABKs@gP`j8?32
ztx%Q$>Zq(f$zq}}66u_faze3DsHaJUgY-Q^hjdyu@kc7f>(N3)ABEP{mk?`|0-izM
ze0T6Z%`aY@!j@EqO|+M6QK((Z)*FzQ-k7zQ9)TtKJXFJ9;zB0XMHw5>X9B2=GPQB*
zek|*htrc_ynDSC}w_?m?*M-%wlE(m&FODF+PXhgMp1>I5c$@>xC0Hov;}|DG{C7cD
z%WXbP%O<X3pdZYAM;y@MnmC|8u-!mB(2{D4o6!cWSHU!kh~{}ff~MC3TBF@alzLSt
zxQ+rrQG^~HqP~@@7a{E%hEN^v%lmJg4lxU?VwaeM$~KmUoR4S1pvg5gNiC;zEhbNV
zIMj&GCGeh^y6~p8YGn#){iYv7Aqq5^h%JN;2t&2>P4o+WXeOSgVk9XwB?5iAh}x(g
zX!i*rzfYnRG4NnJW2p~=i8Ep7Mxip%EkMdI`SH|7BPEHEg@x~o$SYx`_akUWl1HIl
zvYYAWlX$~27~M2lVhQ9o+_Y9oqVD7+?p$v;sg69h8b=<^CTE#>5aYn!HUP&~$B>8V
z?L5bvXdn-j0xuHs8dz1rT2uPPKxwT~cy--;uQDI(Y;~SrF>5u8wTd{ienv6YS&U0s
zMwmUps2hR87oSgKgm9p@nR`A9Q9}g1!aOk*^VWm)n8&Xdzdo76oDm;<po!yV&3^m}
zs4J)61FY3;uuSX6%+lfvV}Vr$`!W+o%?KB+hnMbmd(fVK3RrYmw{JLvy)!-}s}8XR
zx3@FRw^4KZn|Zg;?A7I(eSo(4`NZ=%X<d(L-FEc2$6rrq0adu#;;rA0ht*bi=`$E5
zEo6hR1wsXorSdN9#QP0VjSu0PC80sxr)$_1AjbM0-m5{t_#T*litQrm0nEivm2dJ;
z3(^B<iR}(b`ctqAJhl$pp#a<nR+8NuVoua=YJ|_^!J3VzUvZ))2C8!iOLac@-J#}i
zbyQMW(obbsrVciT-C@VV9BjZdh3AO70c|JD@4%P98??b4k(j7T!N-^p2n2iF4F+BU
z{t@lJZdbpG@x!4th%$Z&R<2z8m}B^4jd8Z&C&@1}@1!8$*ciyS5#tzDKiSUjZoCvV
zF{YG`sei7c6!UzcqZHvVrd>*6FcYI|^1ZPJ7Gv5aqt|2g{T-zc11pSn|JZIF(=H`3
zn19yS8C&frNmz_ABP9Kc)&FTnDdzcDM=3&QOuLlCU`IfGFhv;U`zrebEdDVc3B{2&
z`;jS!5N30elWwON%lw%74vzNIHuOiTU~sAzBoFIC8IvcjMmzF6<ZhC65?4o^>@#@V
zq&BWH-AeeX!%prNDXZ-^04gm)-y4WYqb&&)F_72`o#&&07(dS@llCRFc~h&cUqIW~
zpFfuVj5s~FXveUVzeNgcug--682_ia-)W5RSTm$Md$8MGZEnW<yqSGBY2g`SG0*~H
z6V>adUV~U$dbL$zFt#Rtbe;T2ZRZDctfyejur1w+9~8{18H&7{Tih+675lVa%?J*u
z02J!*JXoZ-Dns5^FN=<O_8Wig)0^M-r@#KN@P4z%hkxJuHQ$4ifm+~5eh~ha1QPz1
zo*MpnBx?woKK;#yaoS60L=lgPlu&fTE;54M5v{hCp9x_pV!IazL}<?sFqamqlyF#W
z8fVzZ^h}Iten;CpwpFx0j_k#>QI!kpljG4fr!r%#yNZR*Fvwk6h{c`Cb2x*iiV0|s
zwFWrdE>y=UX@P*V$}LEB9Jiw|2qzY2iv?f*4A~P*IQ22NB5ZPS(Z!W|vA3pXr>e02
zbimL_lzNFRjZ!GJNa!?JE5@ZUrA5tY|1A!}b!LMND0xb{lf}W=%QSCO)IEzefa$`(
zMI4S?8(om-`SMIh=F~!Yil_7-Y%C_qS8<sGs-}hJ%#2oZ{~;?U;j10Webn#6`9l)g
zd9oCcU1?iK*h#(-5$l@>I7P~u#=t33@{|WpxeMgBj-_gQ1YBfB!dor=(PoYurhd}h
zfmD{DU;pLhNu_tedO-YyU?WU97>Iks(GHLK$g}Q1Eh0~YEhugDs79yS9jaJJ+qQ!t
z$i0b{n_uMnh6z6f`?uq0?+DyOl&IM`oCHijqP7##xe+^MuWZp=@husu4m+a_7{Hb+
z?c-pV!2~DB1UDniXfxJ~HxnR^xzT_L5Yod15i5q}WOcEP*<chU{ERMRjL;>OI^g%T
zQVBJfO4;1=tyI<(Oa;M6lS3M`12S6COfQXr<fFDIORX%>h+RJbX_xPdz<$OS?Vk<%
z8k(Dup}^0LHh2gxN8KGz40US39NZk4Bi72%i}T-cYT;&mvjXN%1R%m%5D<;luLaT1
zL5R!|Yvt&Ih_nQ2lP92nZI4|Fg9Ja&)i$<Xw>AiSP5Z$Nt$`>k)Ovk=Tks0Eg4_#|
zJupU?6|F@T`^4+v`frh~!5q;S!^|XFGecRZKQnQ(d+p5lIXI5Y5uBO6V6A3sutr)C
zlLE1OX8at4$Q;3$>41o|^=2l_BPOzH{I<1cCJ4JT4{H0qGvmW!%XI0t2=6vC(;plo
zC)O-D>1XWb02v531J;sjEuuaFdpFqe@pw#g2wFlCF%gIpvdv)!N9jsQh>XJUgRCee
z-2sr&{;bKdONKx|4^!9$rfSfnvmZJgzE=7m4)NF~FBvj0bgfnlKo4plJ2{Ba+7NX-
z-~{3cDHnhQjlMvNVlRoi$u<4?>Mi@z=hy1?331rnADzzr==9t5r$5jNFql5%K)X3Y
zI{r5OLEre!MAZkJo^v6Lc>zo(dMw)A6m3{$IU5-#4}z-q)I>rH)rbbakFPC_YY|Dh
z#0{4H2%UdI=byrP#mMRic2--#|HZ@VSC~8p=h%5rQbI(D6nY=ypQ0mlQ(L!6!b&|)
z=O7)bK*i2Utn4N>2;rp3kDMkYgg5{tQtSzY<*<f{(6k0~<b%YAWXT1{4uKH}ONMNl
z0Aae$FI6&GQ~+Mg5KKcNWj7~a<cLj4Dl86mBmj<%$DIp{MSC8<tBs<)+~~^33QDZL
zs1MNlfLWf=>gO;vI0tZf0d{8cSjIgHS_Lk(7i0QThHgse>nJw`Wk>yGcqlt3rNh55
z&<a}H770`MnA5H`67I7e0Hx4p9fg~N8Q)vW#Zv{OazMp^+KK`G7{WO@hr;LW(ndP<
zdO`BJgBIyU7bvh75$8c@`5MsI;k}}eGf%Y*_D5Lnld^!&Li_nHQ4gmX7N&A1F)_gP
zuzcn{IIzU0pb~e}fO4P!K7uDPDnw8RLrZ5GFh7L294TN|VkQKb2*phZxe-d35Lh0B
zlDZ6pZq%0TK?r3KqBe~o*3b@-?e;@Bqz{}=3g{b7q?xK7!c9Ebharf9hzWb3q+?%B
zLnwwAh)iIQP3i@48MGnlW>9~sfm@T@)HPW5LfG*qt=6#q&$e2_wlUOh4MXkLd{);c
zmR^=}JK7WDj*9jq+@#da_QXtkAWU7SJ+KBh?RhHD9$TGhwTE;LL%KZ_JTm0BhoQI|
z)1!m5Oh<c?tR3x<95OyhH;ud;pExnmn(@h)_E^;~bhPL9I@-gKKR&i%NVkWr`@N3#
ze7K`M{(QCDlW4Ukwr+c12QTAeRSyK(6H$dgdvwTej}H0c!%%DWfV$crXk8d0KCY%O
zzm3(DHF@94Oy^AAw=xSoU2VAC2<2Ufb}5X*FOSugTQiG2dek7iH2VO~+~9!M0iL;`
zO=n^*xtbCiM5~k*?Q>pRAgNv0Q8a!NEGgmw;=GB-%@mXVTc;y<l5s1&+@0H&+k3|z
z?2ejT8?|q3)c&ytMHtat`o6ITadK=u6iS?l6Sg?LDLPfJKr?qo!TLAQ9&J(-y)2e8
z?_!C~?QFO&GMKl-OgN=O)vRD#&pPQ@PpUs+nAYS_Qt!oY62E;#duo3%*V%cRurDmu
zs<2SntNtqr6l0<v+iRXsCQEpqdG7B*RNesF)*eh_k^XOpx*UKD2@}0tSmZ&mL#Ivo
z0XfuI0NTk}2cY`pQOMi-$0ZN*`9GE*^PO+$+SYAJ$MtO#H?*|IKZyX?P%^qWAVt|4
z5E1lU2yb)k2F)k1aWl4o<2c%A18(>lm^wV6V}c8_4sMm$vZJjWNC62iIdu+hr>#^e
z4~-oT^te$nt}s!Kxsb+z@Cye{W<!P97FT@}ZS|tK?P{ta=4}&knR48*iP>Go?<!_^
z8Rs+_7t2s?hSkMd_e6ofse<qjyEZw1^_&=I*<s8S-xto*Ak2FnuvSL3pke(29hm(-
zlw)DR>|f~d)$k$;j;oK8?^z8SRkXbd(WZzp?YZa;ZQ$P>2L&Z<$f4MVg6^d-RM>tv
zY{E8n2iX;b9)K)h7OE3oZTb?V6tIf%Q+f4CXDZZ!t0@QA6;Pw_`KRa-I1v@)WvGDD
z^HK!2+nIAz;C!(kiY4)Dkd092DHnmj#YGHM42p{j9>iWbjeF<vjVx7sHY6F~j1d-!
zVbbYs@Kg`##{P#K2<1&Ehgy&#CwwgRlrHbXar$N>O`PJZZbv*6U{HzW-Mk2!sW80i
z^oio|x(J&K5XkpBdFdZ~uJ{^Rr{B#2uVNQoQ~uMY{Of`8V(&{xrhE>U{h-~_6I;E&
zcBEn7Dpy9JHn-Qb##S+>UsRMk1=@H63NO%j1qh#QT(&@ny?)ztE8DPO?F5L@!cLe)
z1M`J-V7|}=vyaeQwc6^pp_1*ToCfoqfDbL#QlF*YMlPt^aOFjx^ed*m3?HwBh~j3#
ze6zm+PE=%K$8iZp5-P!d+GB&_9QcSn#q}B>f`?<gz)yCB`kc%GNKL>MX5IlcB&?@j
zgP&)(H2eV^i{^zg_!%G86V}Wixb+0rmPw&crwl_qrY+EP8PW_oWdw??L3bGIEUezZ
zhz_~1)pv*8L2gUfqOwSpIIIB^OY7om)~wwe#r)xCR4o=Dd=aNRsABw%jQ0*nM?Gea
z!|Lmz-tf~??tt`gfY`e^s$JAFB2tJCpzaOsMzm(c9d5N|SjV9?Cf40xTC)kY;n%7y
zqMnl0MAYAGwFXL;K>Af_ji27xnoZK0P3S#a6ICNJHo7&PV}m$pjl{ObW;0rYU!b->
zmexd7q1Bqrf!0K^{;B%$(>q(USz5Ch<7HME*N@qOuy(W$YBa7$uCS<DHS18zX4hYb
zzz}b19Y&X)Fz~4g{41=%1}=8NMFF;~Wx{|O3$mq{>Vr%WW5pkQm?Qd_QZGs#Dh=;9
zdAMupJn&(}n5R{`1zE>%o%drrq`ep?y?b-S#E1732pIg|dz!u2N4K~M_{W-K?iRf7
zW6~Pl**V4*8N-9v(tB|c&KA7=cc3(oF-mNPoHy^@!M3PR0ZR<K*!{3a0smZ&Eqw~V
z=iDUnJ%F9sTHOF;Ao~i`4IlfeeQc}t0VB{i8*v72kb{dYeFacQ3?58CZ5jc=Fpd_-
z+;PA?3%G9?X7f&*h3$3klzoS-xl`BD*ys#l$7Ox-rTtx)ceI6r#dkekg_)e3zX2Ac
zp#bEH9at5yv_{>sI_BDQcR0g1^G+osPwc>asCk!rSF41<w20+-4r0@7xVQIq*z~u4
z0Bo92FT6!;nou9bJ5PffgOzWKO%v+#f(;YutMY!oU2K}f`%i3~RCw2+%GTv?hfR~}
zHMDPiY?@T(1@k4<ia-luQ^a*)K7;9fShv8YN%b)U^R;zgzSae^3!6eMK7K2F3Y9S1
z7sRMJ=sPq<&8VM#3m7#8jnLb{sHvqfgHbj7|2G<<DJ6_^V^oG>jLBQPb*OGUnYF
zHLVT`*6`Ciu|_9GO{=e>H9rVOO{<Ta)_l3unjl6^1OF3;rIk+aY)vOd&8T@9o4-0n
z&8P#?nv9xhwI+yBGr<3Bji27xnof+0Rq%Z<YDRtQEnw6Pj9uQ&I?OD6%wW{4`s@#k
zQ3*q1)U0yaIMHBBjZw4pR}DtZs-G}<x-lyAbYfIO?!>6lULTL*&2D4V5y6So07l)q
zK1L;uGgvhXn!1fuHwuL9Ev%YTJ3u>+;@0x5E-3_@fvCoSr?N|5xSm@|@d?ZkTOD_|
z0*l-Y|DCH_olShs-383E9lkAd+uXa|?YKjJGdMlw*|K`KdpB-`-vVAz!gJeKCnSCc
za<arday=>i#9Ma*Hl)O>J8|w}g1EO)z^*l6wBOsBJCL@)op5(h0t}vkSXM%-xG|hD
zXzQmTFAbG;kt=-m%f#BxM4I<BcQ)^B?rQ9I?{V*?@eO=C-CguSf3_R7^KS8b+?}{q
zFWlVI+}oUV_lRUUSN}RvC&3NwbN4z{f5@Gr%Q3~e?@?z_BXh9)z3whkeqVF{I_39C
z`TZT`e_EFZ-@4bGboaUYeLfX1_oH{br^BSUyJzF3k6#xl+n6~lzX#m?&8u*Xna=M$
zJb|P0S8+;d3TN2j%f<sS(&94D1k7Ae3&^de;NSuoyO>1eg-T;#YEhrbyTnuq>uy3^
zV6FiV<Ivqy^-|z&xtxe7ptQl|-V2q6m%fZU)67lZTD^4-L0q9Dw<gdk2xk;T6o~VX
zA(fy*-$eN4o2*Ha=&bYz@OpWcUvU|Ga12I8?4|<k6uK#fC<?)w6uKxLHbMFv{d6X*
zVhZ=zDKQeN>!|wzXFbj$4<$91>lceTIZ9E&mt@ow2BsK0``D5D?!QmQ!HZqMZOb?t
z_aGs&AL?@~>fb{aY+_wu^_eSoL?$G<w^v!lr6Za-kvEF+juuVocL^*EQzZ2_;KNs5
z6oj4x>Lph$;#}dm%eZ_l$t2vU)7egaLw18u9wii9N?wO$k$RDVgi|gra<b*(>|z9>
zL)_4P8D>5QXk`TBsd6#%*wp2E9e2|mnZFE!r>nRljz%k9qH(cOt-_i~U1r6(_Vs&@
zojv??u}^LY>F7A`UZ|GU5w=CFwWw?K`t?nDJPD_-b3^UwY36y74uw(^_i34^w3#>D
z?Uj4(CdJ(^?!KbFG2W~LSX=QR3+}-KH1SKd#et3++x^VNOt75_hb=;itE?DVEKQ-V
z#nOenyt37NpQE2i1H2fl1DcqX{T;UXr<frhyse%lUXa-CDcTqIsBbgb2FU-VJ(I<3
zAn%^N9l^a_sgquxxo>_MDms{xp;}(%#~b!>vO#$XKiSI$dsQzcBnX4GViH4W?&*V-
z*zBY#rsV!UoC>X9f>{|;OSlp6Qdw2Y^Xi|mPuJ=EC7til`QLQjp!0Kd{t%9r&=>ea
zifMeydy9VDr0-kqzPU+JDVn6<5?4q=aLN1ccpnoixWd$t;_PAu8cm#2)b?Gp;mY9j
zBJ=So6x_ClV;ZB(hZO%wyDrA?R3x6T?LumrM?I(=<@G2mE2kt89+AHk?6VFbJ%*TD
z`!$5MPzWOro39uw#RjRheM5d>=GS&*n@xR#s3noraXTedpX<!oned?8Of`s7KWL9R
zK7FY1|4WZC-n66}H$CPJh?zL8>hh6Mj0TU@<PkR*)pv4H4X~3xyI^nbzWq>^t$)E-
z$7!1pte2L+`h|$xerSmq97NTI6!<h4Vrw=8J-{T`eX_kaMZL2kFkb4D+`iG=qB&It
z7Me^yh#P1Ww2vXN#RUVPJ}xKCbS|U^av>zSBFv@Zb)AGrlnhfHFqBK5bfD~BdWZ^4
zw5-o>8FTq915pzVw(h$9Y!24LQJ4t92#@D|{8mzU9tc5DevLOf#sYEc<w$nGfEqK9
zALdSg0mXG7ZoM37njEqn5W&D^yqRsltcV07F6<bGkinGqM^WdM5X`3vxI_g)lXxIj
zk0#)C<cUMvE4BEsD3|qHFLAB@Nl1@C4re<OAiZog>gL+xaohY!h`DTa68T%BBl%l>
zmyjME!siEYY~><<YpiaYAGi8L)c=0uPvcb4`;Ce?24WEkd72lJU=#ro@BjY_R!bg)
zEXNTdb&M3<yt%pfEAr7FoW62#Kw3`k=rb4>0&{SNw}C?a0s}J4i!JkVKvp@<b75t3
zykTCZeO$RX5!36$aM26Q7|_<Ry3h{4GsIM;Ns8;|;~5*A?a8=WeL#`(2c0ZNCalJV
z8T<`i*=F9TR=p6B_u)*9uAT2%s~t!wGza9;ojw{>!I$PWAJD_m2QdsZaJU_vK3uP&
zb7XW5qrV$;`y?-nxhT|!q6VgOc=t`K+##JR8oOZ$W!ymt;UtR~ipDYO6mOf+#tEHq
z{CFK3ls_VX<EN1rwslwt?VQ&68FQ@vq%S2k*CYSV!|EH@u(Vk`DO_y`PdfA_^_Yex
z|K`K9@B35Q*8k^+XTQDunV<TNKaU)m$OkOBND*mLkwg^8LLAoZA#w5!CUmM919xZ$
zNp(N7d>GEeh>%ppl<cl@>QW!CCZ5Ng9rIJO`s%edF@#USR7+Q(OQuQuRp^o#28x$3
z(E7hc$zrBaYw&hneb*VYAh)5-KrzkY(#bl$yrYaURYp{trTQXoTP)fN%Fo7LJfQ$>
zOp4;$vyK+ynmmHtz4k13sYBI2N7Xb%s*|_jrK?1IePZ-|5@>~l0}%~<OwlJj#|16t
z#IU?z^Sa;muMe7OASsw?vm6PH0!RxQq>l=z@DSk8pbF#%u2CS#fIGW3YfbB|uF31r
zwGN@3E!6dSOg|P9mDC0uG;O){%KtzK?Wj6GP$G9lA{K`wwS_kk2M=k^CGjG)<7q|J
z4$<z3hZy-GlIXK@R94d-1Bcf_^+ot$x!Cg4P9w$x#jy0i3-6~RuKmH1PcJPxX<Q_n
zF}B~(tNOjb%O7jxeE85CNlh(?I1<Y3=-Hou?Nn&42O3mccv#a0ytMwuNV?Km^#g+B
z$%2c|@J1=vr@|c~Qh^;5;_wL##KD!gBo1`ShfENM?MC8KF7A>*{`k_jrLEF`X)Aoe
znxw7ohZeZqRx@(Qje8p*jo_V0ep}b{)#UlV(pDgjkD$+Spn{~h=*31J2<D-1t_Nx?
zxTSMUdgzMA4~VMfcICj>v_9RUKPD0^&rP|9<JT(xnXdBq;K6OmQ#vHa*jnYk*Hs>9
z)i2-7!#3`7Z(+r}h82^5)lzGvpceFz{WLSbHVu718dt#DH1rZ_sW!~uyx$7;I(gRA
zu}&VpF0<-@P`1B?b@v+W^3ih4D`3j|DmRJ+d)%ECY)yBX---^HfUMtjyjNYL*(%?a
zXWcrw^7wW6J&r7myuGy>_!vyIcCc&xpauT=xvoDVvhMl~e3~X&d!Y+Pa1F18(OSbD
zFh17>qhlRI+X(A$wFoU?pBp35zqANq9#$ieP{N!@<f&(%9ggs>G>$qy#bH9&Tg}@N
z!)?#)90fV}7gc{I1Xa%EXPYEd<b2VkKNIM^2Kq3d+xRF)uhf8h0wq^$>10Q)Q^<u|
z#8!t)KHd(fOKd<1$~W=Z5FW&qduDKS6uM6ZJN-eF8bSR9SiTQxVpLLio6Mk*G`7Ts
z>H;vGnuc>;IrNg=Q585hzfK<rmyo)H4#kK{1d2Q86AGzHUyaUYQaHQeJJxX<%L%5n
zq+242QqM82Er}yd5V+E2fWbjJB40a4-wK@{r6VFZO5ePk2-$e)mA<%Zl*wWpKftDt
z_tc+eeF#4D`2~@xsX?Z{53$9ej(f8M@|slp`wHrB5|R*4QA$P67Ul7nejL#)|8eB;
ztPAqp1zgX9OSgW4@UtE8H3280n79$ec*e7=#WZnNC5{M2e+I$lNj9+wArLhBdkC;Q
z$JYq>ErcT=m`MC=1KG0`BoO~n9U)3m4w-Kx#3IbcKYdy+BRPZNLF=e>2(pnw_;vz`
zvAlH<gcMEHC^;c9)%E3rj{bG<{v@8*lSlBYuVBQnC_}3jDRaDRmcBy+{EQ7=U7Hnr
zQm$emfprM{Hi;xy^|kozF<gEfX+%)Ymj*Z>r%WCr3n2M~4Nc1e%)KrGLyXnIhnaK`
zUEt+#?+Z<^AqhAG?iFe6Iz<u?g9Lyk)jWPTwqA=mTJr&LZD<=?j9!6=!|SP!{ozIq
z*;0<6z2v2l!*2;m8*I1jIJC?l$#xv_6FGvc0D1ud8pg%n97{rriLpCf7|)T&knN1=
zVLjHu^_Xku&$mzf&Zd6%pDaF@`rU_*X_<RWU))U2L)+5wrO_`S-?)jC7Vt{!7nvj?
z)a;ch9FE1eH*up;+0i0Mz9`*qbJw!|7_xkx%bNQLZ+PJ%rqHPX5n>)kG7F&BaRof$
z7|%t`k8-SR2-5GJE02;%{~v9S`TEGpdoSwdXc^M|0ln_#u+7W7(sUy%pC}0Pirrz#
zq4smk@C`z{N|MohWf1F>SF)y1E`k~Ob^~{X$gF<Ie8~`LAeYB6#}RP8`gjCxAA*lZ
zDO$?Ar8y{aQ9DTccB{SsXmNpt&-`*$MLK2UqTDbD-~>%$mOj4pOe@#sR<75f>PtY?
zXV3NU%OEapIa+!hxxc;i4B+Q+Z8{IeL6&1eB*fYwn$xsHK-Pf}hzf=Pzvm;?Y)r21
zHKPa#$ycG?<@ZQ9dS)yUOVr&SO-LPJOk~wR7RcAf81-sLJ{fh!I{hF4ZCvjT_5hc!
z>Gre5xMd^|;++=-XXvF^Gj%;8--$#U<k*F+j;;e4vF%ZEhMmy+DC@xA`)u3^m4^Au
zu&@uKO@8j#A5GRf;%sCdm=x03hP8~M{kHn-y0v^3ur~23JFJ7XutLse14|*_7L#x5
znmRY*j+iajoi-0i`%w4Gfx5$rm-u#%F=7F8%)s2W1||$O`OPxt4!|A9^}u)X+hL|P
ze`cU_X2Ll9w;uH81JL($LthVe+Kes;=#w4r$zZfwuV)^2tlW0ww(6MztXBggx2?M!
zX58)?3ZeF`(!21|Z=2e&hvQJ@3+vR5*!HYD+t#cO{azmIt_>eSzngWS*6onC;j-Q~
z6k&JnHm!~<9qDMpH#*v&-;s#=26{GO=5K9X<IWCK*F93#C%Wp|(W>hcrmmgnZxkf%
z+R;XAyY$=W@4e2hK!2lZvJFN08*=WIF^?`ycC_Iu9c|G4jpF+3+x2(1vnSBssQTG8
zb?r8F{jAw9ZogL)oV_na<;#pPD#zQIhgo<AyL8r&-8|mSgSnH=KHMJ#xiRwm5%OgD
z0SLH5B4pW*TWex46NrhyUl@jq*;NqYow<Z8?`h^CiN)AU9+)Don+MWWygM6V42fHj
z#Gs&;(oo;1fH(uUE=dlZ>ZgF@&+|~LzTXQtW&rQ>eexy1-gV<CS;e)>=rPTc3oVvl
zKX4jMud#RXtAh_=EH~kMn4m#cH-qW9-+9MNP|{0T5q$CHn8p?`E@MbN;QSC%<*MU5
zR>z$Oh=XW=iTro0-s!v(X6y$Kf!Y%a<U!4LR4#!mtXz$PL;yt?Qq=rwz9LPUoa+CT
zbWOhOHpXij#jrvBJ?8ocbcFjL>oO5*TNte8^NkbQwXlN9XFw-Z?nCu!(`l1%zRe6x
zM2lfnt9Y@Boa0bsRzE{$7S7H8Lg#flXBc-8PUYWy_sutNKEt>lr}JZUK2GNqI>+dc
zJ*h0?i|#jRHgNNo8RYjQZVDeI77>5Wgx_Pr-}`gq+fHAOS$>!a--Uy=ceX~|gLDNV
z0rnCyVmJRAvd#YcFY(_uVM3xuD9K!Oa8(RGa)EEp$z_@dg>mi4O7^t*DA3pw6?}4o
zAaw4n0fUQ{SpaWVnZATuoU6s8_!^aYd<{-V;v0Uv%A_2yZ4iTrBZMhj9f?XU6#pCa
zy-25vBcz}lUu7>>=DoCh`*7`d0NEko^QiIH<_Nq-S#4v&D1De}eCm*=u6W}n?rY(l
zzq}ddQ6|JH3;5XL6<q8xRcZWidD+~89N-cK@DRZb)7d~rE>FE1zG4I`1Yd38m+|VF
zBF_0YBbvsNaq}|Le8)tQ|MQjYSEqg7^0XQy@J)o<+k%2`4vN))`dcWaCYWOfoi2*{
zXOa3<Qq-S^hm7KOvWif_Q_Y*Q#z0&1@)XG2q4=eoJtneo%Exn7>@OoRs_D1OB`Un0
z7@yb!dzp&xf|MM7Wc}zH7vD~CceP^4O!A5rem+m<^?mEb7H|PFO$8FTaD@tfme>l&
zgQ$~#tgFw)IkQ1c?RS5HN_d!#k8JS3a}qzH_X}asGUI3y_VDdIq_oOwS0enx6I>jR
zf*TPxdLwi#BK#6{u`o^tZ^ipM!*UO#eB`MS*C|4qkAv!tt%AlTr7e)O4{)?$9t{5R
z-4yJp;P)VK#y9vRH-t~DB?t?;;yL-gUy@(o!!K>1vXx;MCD>kY0pIP2XT4}UbcHs4
z?R>b`4O+2Obyqvi_=34vnoz0X+|K+4HpfK6f)83jURb<t9|N^AC*iCDPP$cZ8<LEP
z$!|uho!^CSRejP;(M5TDbs3arpbe%q8pgUUfqs#n*J_P`X)k(59qFx7OnyBk8ZP`S
zaZMk@$u&3+X7t-Ip=r-xR)Z<b6NIDZtu;Tm`Z@!hYqkYXkBk`!z{|DZb&uJ){RO-n
z;Pqf7XQ0*2;^;v{G|xEJ&9#M<hL0fN5E)R&(nrj?Lv6CU5r*Io-(;M?OLFYIAcKjV
zmZSk4ui@53_9pu^w`wmcw^-@TTDJc&Za~CeM2^_x8|FwX4y|${#I5WE`UrC2R_b|m
z6rN)ILaouLEtp+0zNqjuI+)`TmlorCcZc>+dzLjI+~FVqOIOU(kH4rTa$Q^C5ma*w
z+kO2wU{b}HfON~hYibOe3U55t7D~g`E_)X?o;`5$*a~++*f9B~{bnpYg4?6`6Wb+j
zLcMwu3gDj(Ay*nzwL}fCg18XT7D*u7p&%OyUy@Jh@M@SRk6<E#(;W`XOCUaK<>Uqf
zlZ<CzQi89|!5{!)EL{rPLVTqH%qU_&9P+YaKv<KC7@f+khz12~av%ocuJA^@;?ygu
zgShj_hF~fD|Gvf{in(;3P^X*#lMxE`jT;)oSh{cP=RzM_x!6BH7jAZ7S!mcOkif{p
zG(bj8UJVHF(96iuK|LxYc5&&Tg%dS$E7cM%u4Snbgm*w0Ulu`!1uY$CKwOLuML&!~
ztu#VFeWKMx3N@;M6uhK<Ue1x9%a03|2mH0=F;ckG9;ftq39*H5PEusaIpYab2pG)Q
zP?$ooQY(dqDsuTJ^kn9`Y>oqjiyQ-p4nK>tsd0g6^VCvMET{=^O3l>YrgJZyU2s5(
z``?LtU<^Bk><RmrDpVG?08P!2RMX^_OW&vH{0lm-(veN$Df)<|@v*Nw9>rc!!-FDg
z)lqDHmU2%qqm}gf9vKxd+PqwaJ#es)-0(+>*<!+nR7}ibBZ8H~)s}CKMZzDT4GY{Z
zDAL4sfva3d1M&L0qt<4jffB~I4I9ZIuEgUvz%&(=fJpt2wI5pytr%7Yf^tc}5p~?@
zee4(rat<u4IX2;;Hq+TcheV4i(RmM@_tSYVofqh=()lEvPt*AlonNK%H9Egf=j(L-
zf(|A)Bu1KIW}$wDaUY{Ys!(xXRP1elW|Xay`<5Dma~QumITL&x`9LC!zb6tK6LI|B
znaHJ{Ol(LDC7w?>i3bt)W1$}l-Iq9$IGz|zTuj|9<-J4+hN-YMDXk2jzgVe{;Xj1J
zV~iWa_g3(6l{+T-a3%k8-Lq?T^`B6cHiLN=d|rg#tSR=u!hN0}g@MiPJj`SjHd7ca
zpP5pP3O-SKOh<ya3#nhH^BZ)2lMWY^5<E6WpNuof{`LCnB@XHaJw!+%e;)4V6+HZ`
TH2xxqXl^vePImu2ov{87LT2Rv
diff --git a/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Visualisation.cpython-39.pyc b/src/azer_robocup_project/Soccer/Localisation/__pycache__/class_Visualisation.cpython-39.pyc
deleted file mode 100644
index 320eccb8cee6fc65bf3ccf6e5ca4d05396af14ea..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 1774
zcmb7E&2Jk;6rb5IuN^ySn>0!bR6)>#FEt7`RMAL@+9DyhGy<V0Xtds$y6fzEotZU|
zXe|yc{{!TJ5^lL5xb+18g3oh>i^Z91!+W#-tjd9r=C^O=ea+7My$Ncy3W4#>FI)V*
zB|`qh#r&{gu?<t*1mJ|znCz1Wgj?KxLAd?g+NYfEk-9SiJ!yuB^TUG0Hca(CfFNK^
z_AO4i^@8l%+~y8^9qw`uzAi68BwyFQh*Up_BNb#(+6^gqP6o;tatEfm0?;NoVFf~H
zaHq|>rB`}S8Sf{_F?$wtc`Wow+EQ5%#$votr`k~>etJQq2T}fsG3G}HpDQ6%BWUcV
zVLuVwOf?=v*~5OT@g!}fhf&rTo(+E={s!}O`0MbGtq-<7+~kou+>DZ-EgE}i7z)|=
zG!3VxKM3PMDYo!OcY4R#XDsSQ8DkFq2?C)WJy~BgoEU{J8jJ@P%!g_2;4ctz(jmL#
ztD6HiryY7`0ou8dJI=X1@QysRICIYlC7IWTBK}OD(?Q8tBih``&8p*{I|tUl8~Dbf
zocrgL5@S~}QGIUb7@tC9ci<V#(o|#5X&gAw{XyAS6giy#k;C{EIgWlXb#<E;Rf**v
zeFtEq*I=}b3Lx<q(0FFjKK&8L#l6ubjP}c0-~avD%b)Jms}d=$Z51W@3c$BdM3R0j
z9<@3m%(NfI5%A!XUA>ArmiCgA3zl|;dX{F|1x5u@ms`gfd}VN~SI}%niXaoaNI<<d
z@7Idc@G$DOwHNeyqRaJl)b5F_NcT8Z5mH`nENnlLcm~r9^@>C)XfH{P;Z>7_J-*L+
z6W_JzRyy<3mFez7?TwqFB1P7h-5vNNz06I!K;MLEoXX--R%cGr_@-Y9DB(#~;g_at
z2dfCW2HB~$00^zn4VYEBOub9rs)FQD@0DX$Y30(feBeCHS3ax@&B?oq9q@+eAr@>h
zFjX0*X@*k}rp;i;IBtdsG3|l#AI;$A77+H!F{KS*bB|j$$heV;#&Do%u$9Vr>9xiv
zL1TE_0eg?THwbsylbkN(ylW`zH6<Q(gb$dkA{ag+)R!c_0kVd$jIe@$FPyxJa1CJ<
zVGZFr!a6|RH-wNIAnWqvl~eL<G<XN$7Ha&7rbvTt-dqi96D>;ud58`Q{b$v~I#s82
zx(3q8TZ?sCh|sKi_`r38L@=f+j6u`%;q7I*%GeP+?S)2S@#H4LZG_pWjD+-(cvo0&
hg<C%L9e>^I)ve;L%w4|0QTum`q#xt;Dm3%0e*yob!1DkA
diff --git a/src/azer_robocup_project/Soccer/Localisation/class_Glob.py b/src/azer_robocup_project/Soccer/Localisation/class_Glob.py
deleted file mode 100644
index c5ef07f..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/class_Glob.py
+++ /dev/null
@@ -1,143 +0,0 @@
-import json, array, math
-
-class Glob:
- def __init__(self, simulation, current_work_directory, particles_number = 1000):
- self.camera_ON = False
- self.deflection = [0]
- self.data_quality_is_good = True
- self.COLUMNS = 18
- self.ROWS = 13
- self.current_work_directory = current_work_directory
- self.with_pf = True
- self.particles_number = particles_number
- self.pf_alloc1 = array.array('I',(0 for i in range(particles_number*4)))
- self.pf_alloc2 = array.array('I',(0 for i in range(particles_number*4)))
- self.weights = array.array('I',(0 for i in range(particles_number)))
- self.new_p = array.array('I',(0 for i in range(particles_number)))
- self.strategy_data = array.array('b',(0 for i in range(self.COLUMNS * self.ROWS * 2)))
- self.SIMULATION = simulation # 0 - Simulation without physics, 1 - Simulation with physics, 2 - live on openMV
- self.ball_coord =[0.0,0.0]
- self.pf_coord = [0.0,0.0,0.0]
- self.obstacles = []
- self.mqttc = None
- self.mqtt_ball_coord = None
- self.mqtt_pf_coord = None
- import pyb as p
- self.pyb = p
- if self.SIMULATION == 1 or self.SIMULATION == 0 or self.SIMULATION == 3:
- import socket
- self.SIM_OPTION = 'SURROGAT' # other variants: 'KONDOv23', 'ODE'
- landmarks_filename = current_work_directory + "Init_params/Sim/Sim_landmarks.json"
- params_filename = current_work_directory + "Init_params/Sim/" + self.SIM_OPTION + "/Sim_params.json"
- params_2_filename = current_work_directory + "Init_params/Sim/" + self.SIM_OPTION + "/Sim_params_2.json"
- with open(current_work_directory + "Init_params/Sim/" + self.SIM_OPTION + "/wifi_params.json", "r") as f:
- self.wifi_params = json.loads(f.read())
- if self.wifi_params['WIFI_IS_ON']:
- self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
- self.target_wifi_address = (self.wifi_params['HOST'], self.wifi_params['PORT'])
- elif self.SIMULATION == 2 :
- import usocket, network
- from mqtt import MQTTClient
- landmarks_filename = current_work_directory + "Init_params/Real/Real_landmarks.json"
- params_filename = current_work_directory + "Init_params/Real/Real_params.json"
- params_2_filename = current_work_directory + "Init_params/Real/Real_params_2.json"
- with open(current_work_directory + "Init_params/Real/wifi_params.json", "r") as f:
- self.wifi_params = json.loads(f.read())
- if 1:
- print("Trying to connect... (may take a while)...")
- try:
- wlan = network.WINC()
- wlan.connect(self.wifi_params['SSID'], key=self.wifi_params['PASSWORD'], security=wlan.WPA_PSK)
- # We should have a valid IP now via DHCP
- print(wlan.ifconfig())
- except Exception as e:
- print('No wifi module onboard', e)
- # Create server socket
- try:
- # self.udp_socket = usocket.socket(usocket.AF_INET, usocket.SOCK_DGRAM)
- # self.target_wifi_address = (self.wifi_params['HOST'], self.wifi_params['PORT'])
- print("Successful conect to: " + self.wifi_params['HOST'])
- except Exception as e:
- print("Unsuccessful conect to: " + self.wifi_params['HOST'], e)
- if (self.wifi_params['MQTT_IS_ON']):
- self.mqttc = MQTTClient("openmv", self.wifi_params['HOST'] ,port=self.wifi_params['MQTT_PORT'])
- self.ball_topic = self.wifi_params['MQTT_BALL_TOPIC']
- self.robot_topic = self.wifi_params['MQTT_ROBOT_TOPIC']
- self.mqttc.set_callback(self.mqtt_callback)
- self.mqttc.connect()
- self.mqttc.subscribe(self.ball_topic)
- self.mqttc.subscribe(self.robot_topic)
- import time
- time.sleep(2000)
- print ("Successful connection to mqtt: " , self.wifi_params['HOST'] )
- with open(landmarks_filename, "r") as f:
- landmarks = json.loads(f.read())
- with open(params_filename, "r") as f:
- self.params = json.loads(f.read())
- with open(params_2_filename, "r") as f:
- self.params.update(json.loads(f.read()))
- self.first_step_yield = (19 * self.params['RUN_TEST_10_STEPS'] - 9 * self.params['RUN_TEST_20_STEPS']) / 10
- self.cycle_step_yield = ( self.params['RUN_TEST_20_STEPS'] - self.params['RUN_TEST_10_STEPS']) / 10
- self.side_step_right_yield = self.params['SIDE_STEP_RIGHT_TEST_RESULT'] / 20
- self.side_step_left_yield = self.params['SIDE_STEP_LEFT_TEST_RESULT'] / 20
- #print("self.first_step_yield", self.first_step_yield)
- #print("self.cycle_step_yield", self.cycle_step_yield)
- #print("self.side_step_right_yield", self.side_step_right_yield)
- #print(self.params)
- self.landmarks = landmarks
- self.import_strategy_data(current_work_directory)
- self.obstacleAvoidanceIsOn = False
- self.imu_drift_correction = 0
- self.imu_drift_last_correction_time = 0
- def mqtt_callback(self, topic, msg):
- print(topic)
- if (topic == bytes(self.ball_topic,'utf-8')):
- # print(msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
- self.mqtt_ball_coord = [float(i) for i in msg.split(b" ")]
- print("MQTT ball coordinate:", self.mqtt_ball_coord)
- self.ball_coord = self.mqtt_ball_coord
- elif (topic == bytes(self.robot_topic, 'utf-8')):
- self.mqtt_pf_coord = [float(i) for i in msg.split(b" ")]
- print("MQTT robot coordinate", self.mqtt_pf_coord)
- self.pf_coord[0] = self.mqtt_pf_coord[0]
- self.pf_coord[1] = self.mqtt_pf_coord[1]
- def update_coord_by_mqtt(self):
- pin = self.pyb.Pin
- pin('P2', pin.AF_PP, pin.PULL_DOWN, af = pin.AF5_SPI2) # pyb.Pin.AF_PP = 2, pyb.Pin.PULL_DOWN = 2, pyb.Pin.AF5_SPI2 = 5
- pin('P3', pin.OUT_PP, pin.PULL_UP, af = pin.AF5_SPI2) # pyb.Pin.OUT_PP = 1, pyb.Pin.PULL_UP = 1
- import time
- time.sleep(1)
- self.mqttc.disconnect()
- self.mqttc.connect()
- self.mqttc.subscribe(self.ball_topic)
- self.mqttc.subscribe(self.robot_topic)
- tmp_coord = self.pf_coord
- tmp_ball = self.ball_coord
- # while (self.pf_coord == tmp_coord and self.ball_coord == tmp_ball):
- self.mqttc.check_msg()
- # print("Try get mqtt msg")
- # print('coord =', self.pf_coord, 'ball =', self.ball_coord)
- success_Code = True
- napravl = direction_To_Ball = math.atan2((self.ball_coord[1] - self.pf_coord[1]), (self.ball_coord[0] - self.pf_coord[0]))
- dist = math.sqrt((self.pf_coord[0] - self.ball_coord[0]) ** 2 + (self.pf_coord[1] - self.ball_coord[1]) ** 2)
-
- return success_Code, napravl, dist
-
- def import_strategy_data(self, current_work_directory):
- with open(current_work_directory + "Init_params/strategy_data.json", "r") as f:
- loaded_Dict = json.loads(f.read())
- if loaded_Dict.get('strategy_data') != None:
- strategy_data = loaded_Dict['strategy_data']
- for column in range(self.COLUMNS):
- for row in range(self.ROWS):
- index1 = column * self.ROWS + row
- power = strategy_data[index1][2]
- yaw = int(strategy_data[index1][3] * 40) # yaw in radians multiplied by 40
- self.strategy_data[index1*2] = power
- self.strategy_data[index1*2+1] = yaw
-if __name__ == "__main__":
- glob = Glob(2, "../")
- while 1:
- glob.update_coord_by_mqtt()
- import time
- time.sleep(7000)
diff --git a/src/azer_robocup_project/Soccer/Localisation/class_Local.py b/src/azer_robocup_project/Soccer/Localisation/class_Local.py
deleted file mode 100644
index 1bfddae..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/class_Local.py
+++ /dev/null
@@ -1,1434 +0,0 @@
-
-
-import sys, os, array
-
-#current_work_directory = os.getcwd()
-#current_work_directory = current_work_directory.replace('\\', '/')
-#if sys.version != '3.4.0':
- #current_work_directory += '/'
- #import numpy as np
-#else:
- #import ulab as np
- #import sensor, time
-
-
-#sys.path.append( current_work_directory + 'Soccer/')
-#sys.path.append( current_work_directory + 'Soccer/Motion/')
-#sys.path.append( current_work_directory + 'Soccer/Vision/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-#import class_Motion
-import math, time, json, array
-from call_par_filter import Call_Par_Filter
-from ParticleFilter import random
-
-LOCALISATION_VISUALISATION_IS_ON = True
-OBSTACLE_VISUALISATION_IS_ON = False
-LOG_PF_DEVIATION_IS_ON = False
-
-
-
-def uprint(*text):
- #with open(current_work_directory + "Soccer/log/output.txt",'a') as f:
- # print(*text, file = f)
- print(*text )
-
-class M_blob:
- def __init__ (self, x_, y_, w_, h_):
- self.x_ = x_
- self.y_ = y_
- self.w_ = w_
- self.h_ = h_
-
- def x (self):
- return self.x_
-
- def y (self):
- return self.y_
-
- def w (self):
- return self.w_
-
- def h (self):
- return self.h_
-
- def cx (self):
- return int (self.x_ + self.w_ / 2)
-
- def cy (self):
- return int (self.y_ + self.h_ / 2)
-
- def rect (self):
- return (self.x_, self.y_, self.w_, self.h_)
-
-class Local():
- def __init__ (self, motion,glob, vision, coord_odometry = [0.0,0.0,0.0]):
- self.motion = motion
- self.glob = glob
- self.USE_LANDMARKS_FOR_LOCALISATION = self.glob.params['USE_LANDMARKS_FOR_LOCALISATION']
- self.USE_LINES_FOR_LOCALISATION = self.glob.params["USE_LINES_FOR_LOCALISATION"]
- self.USE_LINE_CROSSES_FOR_LOCALISATION = self.glob.params["USE_LINE_CROSSES_FOR_LOCALISATION"]
- self.USE_PENALTY_MARKS_FOR_LOCALISATION = self.glob.params["USE_PENALTY_MARKS_FOR_LOCALISATION"]
- self.DIRECT_COORD_MEASUREMENT_BY_PAIRS_OF_POST = self.glob.params["DIRECT_COORD_MEASUREMENT_BY_PAIRS_OF_POST"]
- self.USE_SINGLE_POST_MEASUREMENT = self.glob.params["USE_SINGLE_POST_MEASUREMENT"]
- self.floor_lines = [] # detected raw lines on floor [rho:meter, theta: radians] in local robot coordinates
- self.line_group_compact = [] # grouped lines [rho:meter, theta: radians] in local robot coordinates
- self.cross_points = [] # detected cross points of lines on floor [meter,meter] in local robot coordinates
- #self.post_data_in_pose = []
- self.coordinate = [0.0,0.0,0.0]
- self.coord_visible = [0.0,0.0,0.0]
- self.coord_odometry = coord_odometry
- self.coord_odometry_old = coord_odometry
- self.quality = 0
- self.vision = vision
- self.penalty = []
- self.penalty_points =[]
- self.robot_moved = False
- self.coord_shift = [0.0, 0.0, 0.0]
- self.field_border_data = []
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- import reload as re
- import cv2, copy
- import numpy as np
- self.np =np
- self.copy = copy
- self.re = re
- self.cv2 = cv2
- self.timer0 = time.perf_counter()
- if abs(motion.direction_To_Attack) < 1: self.side_factor = 1
- else: self.side_factor = -1
- from class_Visualisation import Visualisation
- self.visualisation = Visualisation()
- self.pf_deviation = [0,0]
- self.ab_deviation = [0,0]
- elif self.glob.SIMULATION == 2 :
- import sensor
- import image
- #import micropython, gc
- import pyb as p
- import ulab as np
- self.pyb = p
- self.image = image
- self.sensor = sensor
- self.timer0 = p.millis()
- #gc.collect()
- #micropython.mem_info()
- self.call_Par_Filter = Call_Par_Filter(self.glob, coord_odometry)
- self.max_field_dimension = math.sqrt(self.glob.landmarks['FIELD_WIDTH']**2 + self.glob.landmarks['FIELD_LENGTH']**2)* 1.2
- self.post_data_in_pose = np.zeros((160,3), dtype = np.int16) # (course (rad) * 2000, distance (m) * 2000, post Number)
- self.post_data_in_pose_number = 0
-
- def coordinate_fall_reset(self):
- self.call_Par_Filter.pf.fall_reset()
-
- def coordinate_trust_estimation(self):
- return self.call_Par_Filter.pf.consistency
-
- def detect_Post_In_image(self,img_, post_color): # color: 1 - blue, 2- yellow
- all_goal_is_in_picture = 0
- post_list = []
- while (all_goal_is_in_picture < 2):
- if self.glob.SIMULATION == 2 :
- img = img_
- else:
- img = self.re.Image(img_)
- labimg = self.cv2.cvtColor (img_, self.cv2.COLOR_BGR2LAB)
- self.motion.vision_Sensor_Display( img_)
- if all_goal_is_in_picture == 1 :
- all_goal_is_in_picture = all_goal_is_in_picture + 1
- #if self.glob.SIMULATION == 2 :
- post_thresholds = [self.vision.TH[post_color]['th']]
- #else: post_thresholds = self.vision.TH[post_color]['th']
- for blob in img.find_blobs(post_thresholds, pixels_threshold = self.vision.TH[post_color]['pixel'],
- area_threshold = self.vision.TH[post_color]['area'], merge=True):
- blob_Is_Post = False
- if blob.y() + blob.h() > 235 : continue # blob connected to bottom of picture. No opportunity to recognize data
- else:
- if blob.w() > 314: continue # blob connected to both sides of picture. No opportunity to recognize data
- for y in range (blob.y() + blob.h(), blob.y() + blob.h() + 5, 1 ):
- for x in range (blob.x(), blob.x() + blob.w(), 1 ):
- if self.glob.SIMULATION == 2 :
- a=self.image.rgb_to_lab(img.get_pixel(x , y))
- is_green = (self.vision.TH['green field']['th'][0] < a[0] < self.vision.TH['green field']['th'][1]) and \
- (self.vision.TH['green field']['th'][2] < a[1] < self.vision.TH['green field']['th'][3]) and \
- (self.vision.TH['green field']['th'][4] < a[2] < self.vision.TH['green field']['th'][5])
- is_white = (self.vision.TH['white marking']['th'][0] < a[0] < self.vision.TH['white marking']['th'][1]) and \
- (self.vision.TH['white marking']['th'][2] < a[1] < self.vision.TH['white marking']['th'][3]) and \
- (self.vision.TH['white marking']['th'][4] < a[2] < self.vision.TH['white marking']['th'][5])
- else:
- is_green = (self.vision.TH['green field']['th'][0] < labimg[y][x][0]/ 2.55 < self.vision.TH['green field']['th'][1]) and \
- (self.vision.TH['green field']['th'][2] < labimg[y][x][1]- 128 < self.vision.TH['green field']['th'][3]) and \
- (self.vision.TH['green field']['th'][4] < labimg[y][x][2]- 128 < self.vision.TH['green field']['th'][5])
- is_white = (self.vision.TH['white marking']['th'][0] < labimg[y][x][0]/ 2.55 < self.vision.TH['white marking']['th'][1]) and \
- (self.vision.TH['white marking']['th'][2] < labimg[y][x][1]- 128 < self.vision.TH['white marking']['th'][3]) and \
- (self.vision.TH['white marking']['th'][4] < labimg[y][x][2]- 128 < self.vision.TH['white marking']['th'][5])
- if is_green == True or is_white == True : blob_Is_Post = True
- if blob_Is_Post == True: break
- if blob_Is_Post == True: break
- if blob_Is_Post == True:
- blob_y_plus_h = blob.y() + blob.h()
- y = blob_y_plus_h - 2
- post_color_pixels = []
- for x in range (blob.x(), blob.x() + blob.w(), 1 ):
- if self.glob.SIMULATION == 2 :
- is_post_color = self.vision.TH[post_color]['th'][0] < a[0] < self.vision.TH[post_color]['th'][1] and self.vision.TH[post_color]['th'][2] < a[1] < self.vision.TH[post_color]['th'][3] and self.vision.TH[post_color]['th'][4] < a[2] < self.vision.TH[post_color]['th'][5]
- else:
- is_post_color = self.vision.TH[post_color]['th'][0] < labimg[y][x][0]/ 2.55 < self.vision.TH[post_color]['th'][1] and self.vision.TH[post_color]['th'][2] < labimg[y][x][1]- 128 < self.vision.TH[post_color]['th'][3] and self.vision.TH[post_color]['th'][4] < labimg[y][x][2]- 128 < self.vision.TH[post_color]['th'][5]
- if is_post_color == True: post_color_pixels.append(x)
- if len(post_color_pixels) == 0: blob_cx = blob.cx()
- else: blob_cx = int((post_color_pixels[0] + post_color_pixels[len(post_color_pixels)-1])/2)
- course, dist = self.motion.get_course_and_distance_to_post( blob_cx, blob_y_plus_h )
- if dist > self.max_field_dimension : continue # filter off too far measurements
- distance_in_mm = dist *1000
- virtual_width_of_post = distance_in_mm* math.tan(math.radians(blob.w() * self.motion.params['APERTURE_PER_PIXEL']))
- if virtual_width_of_post < self.motion.params['POST_WIDTH_IN_MM'] * 1.5 :
- course = self.normalize_yaw( course )
- post_list.append([course, dist, 1]) # recognized post
- else:
- if blob.x() < 3 or all_goal_is_in_picture == 2:
- course = self.normalize_yaw( course - math.radians(blob.w() * self.motion.params['APERTURE_PER_PIXEL']/2))
- post_list.append([course , dist, 2]) # blob connected to left side of picture
- else:
- if blob.x() + blob.w() > 317 or all_goal_is_in_picture == 3:
- course = self.normalize_yaw( course + math.radians(blob.w() * self.motion.params['APERTURE_PER_PIXEL']/2))
- post_list.append([course , dist, 3]) # blob connected to right side of picture
- else:
- if all_goal_is_in_picture == 0:
- all_goal_is_in_picture =1
- if self.glob.SIMULATION == 2 :
- for y in range(0,blob.cy(),1):
- for x in range(320):
- img_.set_pixel(x,y,(0,0,0))
- else:
- for y in range(0,blob.cy(),1):
- for x in range(320):
- img_[y][x] = img_[y][x]*0
-
- if all_goal_is_in_picture == 0: break
- #uprint('posts number in frame = ', len(post_list))
- if len(post_list) > 2:
- post_list = self.group_posts_by_yaw(post_list)
- return post_list
-
- def group_posts_by_yaw(self, post_list):
- posts = []
- for ind in range(2):
- best_post_index = 0
- best_score = 0
- len_p = len(post_list)
- for i in range(len_p):
- score = 0
- for j in range(len_p):
- if abs(post_list[i][0] - post_list[j][0]) < 0.1: score += 1
- if score > best_score:
- best_score = score
- best_post_index = i
- if len_p != 0:
- posts.append(post_list[best_post_index])
- k = 0
- reference = post_list[best_post_index][0]
- for i in range(len_p):
- if abs( reference - post_list[i - k][0] ) < 0.1:
- post_list.pop(i - k)
- k += 1
- return posts
-
-
-
-
-
- def normalize_yaw(self, yaw):
- if abs(yaw) > 2 * math.pi: yaw %= (2 * math.pi)
- if yaw > math.pi : yaw -= (2 * math.pi)
- if yaw < -math.pi : yaw += (2 * math.pi)
- return yaw
-
-
- def read_Localization_marks(self, img):
- self.quality = 0
- if self.glob.with_pf:
- post_list1 = self.detect_Post_In_image(img, "blue posts")
- for post in post_list1:
- self.post_data_in_pose[self.post_data_in_pose_number][0] = int(post[0] * 2000)
- self.post_data_in_pose[self.post_data_in_pose_number][1] = int(post[1] * 2000)
- self.post_data_in_pose_number += 1
- #if len(post_list1) != 0:
- # self.post_data_in_pose.extend(post_list1)
- post_list2 = self.detect_Post_In_image(img, "yellow posts")
- for post in post_list2:
- self.post_data_in_pose[self.post_data_in_pose_number][0] = int(post[0] * 2000)
- self.post_data_in_pose[self.post_data_in_pose_number][1] = int(post[1] * 2000)
- self.post_data_in_pose_number += 1
- #if len(post_list2) != 0:
- # self.post_data_in_pose.extend(post_list2)
- if self.USE_LANDMARKS_FOR_LOCALISATION == True:
- if self.USE_PENALTY_MARKS_FOR_LOCALISATION ==True: self.detect_penalty_marks(img)
- if self.USE_LINES_FOR_LOCALISATION == True : self.detect_line_in_image(img)
- if self.robot_moved == True:
- self.robot_moved = False
- self.glob.obstacles.clear()
- if self.glob.obstacleAvoidanceIsOn:
- self.detect_obstacles(img)
-
-
- def pf_update(self, coord_visible_is_new):
- if self.USE_LANDMARKS_FOR_LOCALISATION == True:
- if self.USE_LINES_FOR_LOCALISATION == True : self.group_lines_find_crosses()
- if self.USE_PENALTY_MARKS_FOR_LOCALISATION ==True: self.group_penalty_marks()
- landmarks ={}
- correction = -self.motion.imu_body_yaw()
- post1_data = []
- post2_data = []
- post3_data = []
- post4_data = []
- unsorted_data = []
- if self.USE_SINGLE_POST_MEASUREMENT == True:
- #for post in self.post_data_in_pose :
- for i in range(self.post_data_in_pose_number):
- post = tuple(self.post_data_in_pose[i])
- x = post[1]*math.cos(post[0] / 2000 + correction) / 2000
- y = post[1]*math.sin(post[0] / 2000 + correction) / 2000
- weight = 1
- #if x*x+y*y > 6.25: weight = 0.5
- if post[2] == 0 : unsorted_data.append([x,y,weight])
- if post[2] == 1 : post1_data.append([x,y,weight])
- if post[2] == 2: post2_data.append([x,y,weight])
- if post[2] == 3: post3_data.append([x,y,weight])
- if post[2] == 4: post4_data.append([x,y,weight])
- landmarks['unsorted_posts']= unsorted_data
- landmarks['post1']= post1_data
- landmarks['post2']= post2_data
- landmarks['post3']= post3_data
- landmarks['post4']= post4_data
- landmarks['lines'] = []
- landmarks['Line_crosses'] = []
- landmarks['penalty'] = []
- if self.USE_LANDMARKS_FOR_LOCALISATION == True:
- if self.USE_LINES_FOR_LOCALISATION ==True:landmarks['lines'] = self.line_group_compact
- if self.USE_LINE_CROSSES_FOR_LOCALISATION ==True:landmarks['Line_crosses'] = self.cross_points
- if self.USE_PENALTY_MARKS_FOR_LOCALISATION ==True: landmarks['penalty'] = self.penalty_points
- #uprint('self.cross_points = ', self.cross_points)
- #for i in range(len(self.line_group_compact)): print('rho = ', self.line_group_compact[i][0], '\t', 'theta =', math.degrees(self.line_group_compact[i][1]))
- #uprint('Number of cross_points = ', len(self.cross_points), 'lines =', len(self.line_group_compact),
- #'penalty =', len(self.penalty_points), 'posts =',
- #len(unsorted_data)+len(post1_data)+len(post2_data)+len(post3_data)+len(post4_data))
- coord = []
- if coord_visible_is_new == True:
- coord.append(self.coord_visible[0])
- coord.append(self.coord_visible[1])
- coord.append(self.coord_visible[2])
- if self.DIRECT_COORD_MEASUREMENT_BY_PAIRS_OF_POST == False: coord.clear() #!!!!!!!!
- self.call_Par_Filter.update(landmarks, coord)
- if self.glob.SIMULATION == 2:
- #uprint( 'coord =', coord)
- pass
- else:
- if self.motion.robot_Number == '':
- pass
- #uprint( 'coord =', coord)
- #with open(current_work_directory + "Soccer/log/pf_data.json",'a') as f:
- # print(landmarks, ',{"coord":', coord, '},', file = f)
-
-
- def correct_yaw_in_pf(self):
- if self.glob.with_pf:
- self.glob.pf_coord = list(self.call_Par_Filter.return_coord())
- self.motion.refresh_Orientation()
- shift__Yaw = self.motion.imu_body_yaw() - self.glob.pf_coord[2]
- shifts = {'shift_x': 0, 'shift_y': 0,'shift_yaw': shift__Yaw}
- self.call_Par_Filter.move(shifts)
- self.coord_odometry[2] = self.motion.imu_body_yaw()
- self.glob.pf_coord = list(self.call_Par_Filter.return_coord())
- else:
- self.motion.refresh_Orientation()
- self.glob.pf_coord[2] = self.motion.imu_body_yaw()
-
-
- def coordinate_record(self, odometry = False, shift = False):
- if self.glob.with_pf: self.glob.pf_coord = list(self.call_Par_Filter.return_coord())
- if odometry == True:
- if self.glob.with_pf:
- if shift == False:
- shift__X = (self.coord_odometry[0] -self.coord_odometry_old[0])
- shift__Y = (self.coord_odometry[1] -self.coord_odometry_old[1])
- shift__Yaw = 0
- shift__X_local = shift__X * math.cos(self.glob.pf_coord[2]) + shift__Y * math.sin(self.glob.pf_coord[2])
- shift__Y_local = - shift__X * math.sin(self.glob.pf_coord[2]) + shift__Y * math.cos(self.glob.pf_coord[2])
- self.coord_odometry_old = self.coord_odometry.copy()
- self.coordinate = self.coord_odometry.copy()
- else:
- shift__X_local = self.coord_shift[0]
- shift__Y_local = self.coord_shift[1]
- shift__Yaw = self.coord_shift[2]
- shifts = {'shift_x': shift__X_local, 'shift_y': shift__Y_local, 'shift_yaw': shift__Yaw}
- self.call_Par_Filter.move(shifts)
- self.robot_moved = True
- self.cross_points.clear()
- self.penalty_points.clear()
- else:
- self.glob.pf_coord[0] += self.coord_shift[0] * math.cos(self.glob.pf_coord[2]) - self.coord_shift[1] * math.sin(self.glob.pf_coord[2])
- self.glob.pf_coord[1] += self.coord_shift[1] * math.cos(self.glob.pf_coord[2]) + self.coord_shift[0] * math.sin(self.glob.pf_coord[2])
- self.correct_yaw_in_pf()
- if self.glob.with_pf:
- self.glob.pf_coord = list(self.call_Par_Filter.return_coord())
- if self.glob.wifi_params['WIFI_IS_ON']: self.report_to_WIFI()
- if (self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3):
- timer1 = time.perf_counter() - self.timer0
- Dummy_PF_position = [self.glob.pf_coord[0] * self.side_factor,
- self.glob.pf_coord[1] * self.side_factor, 0.01]
- Dummy_PF_orientation = [0, 0, self.glob.pf_coord[2] + (math.pi/2 *(1 - self.side_factor))]
- returnCode, Dummy_1position= self.motion.sim.simxGetObjectPosition(
- self.motion.clientID, self.motion.Dummy_1Handle ,
- -1, self.motion.sim.simx_opmode_streaming)
- Dummy_PF_H = []
- yaw_rad = self.motion.body_euler_angle['yaw']
- if self.USE_LANDMARKS_FOR_LOCALISATION == True:
- n = len(self.cross_points)
- m = len(self.penalty_points)
- for i in range(n+m):
- Dummy_PF_H_position = []
- if i < n:
- Dummy_PF_H_position.append((Dummy_1position[0] + self.cross_points[i][0]*math.cos(yaw_rad) -
- self.cross_points[i][1]*math.sin(yaw_rad)))
- Dummy_PF_H_position.append((Dummy_1position[1] + self.cross_points[i][0]*math.sin(yaw_rad) +
- self.cross_points[i][1]*math.cos(yaw_rad)))
- elif i < n+m and self.USE_PENALTY_MARKS_FOR_LOCALISATION ==True:
- Dummy_PF_H_position.append((Dummy_1position[0] + self.penalty_points[i-n][0]*math.cos(yaw_rad) -
- self.penalty_points[i-n][1]*math.sin(yaw_rad)))
- Dummy_PF_H_position.append((Dummy_1position[1] + self.penalty_points[i-n][0]*math.sin(yaw_rad) +
- self.penalty_points[i-n][1]*math.cos(yaw_rad)))
- Dummy_PF_H_position.append(0)
- Dummy_PF_H.append(Dummy_PF_H_position)
-
-
- if LOCALISATION_VISUALISATION_IS_ON :
- self.visualisation.localisation_points(self.motion, Dummy_PF_position, Dummy_PF_orientation, Dummy_PF_H)
- if OBSTACLE_VISUALISATION_IS_ON and self.glob.obstacleAvoidanceIsOn and odometry == False:
- scene_obstacles = []
- for i in range(len(self.glob.obstacles)):
- obstacle_for_scene = []
- obstacle_for_scene.append((self.glob.obstacles[i][0] + Dummy_1position[0]) * self.side_factor - self.glob.pf_coord[0])
- obstacle_for_scene.append((self.glob.obstacles[i][1] + Dummy_1position[1]) * self.side_factor - self.glob.pf_coord[1])
- obstacle_for_scene.append(self.glob.obstacles[i][2])
- scene_obstacles.append(obstacle_for_scene)
- self.visualisation.obstacle_mark(self.motion, scene_obstacles)
- if odometry == False and LOG_PF_DEVIATION_IS_ON:
- with open(self.glob.current_work_directory + "Soccer/log/deviation.json",'a') as f:
- self.call_Par_Filter.pf.timer = int(timer1)
- pf_deviation = math.sqrt((Dummy_1position[0] - Dummy_PF_position[0])**2 + (Dummy_1position[1] - Dummy_PF_position[1])**2)
- self.pf_deviation[0] += 1
- self.pf_deviation[1] = (self.pf_deviation[1] *(self.pf_deviation[0]-1) + pf_deviation)/self.pf_deviation[0]
- print('{','"average pf":', self.pf_deviation[1], ', "current pf":', pf_deviation,'},', file = f)
-
- elif self.glob.SIMULATION == 2 :
- #if self.glob.wifi_params['WIFI_IS_ON']: self.report_to_WIFI()
- timer1 = self.pyb.elapsed_millis(self.timer0)/1000
- #with open(current_work_directory + "Soccer/log/pf_data.json",'a') as f:
- # print('{"time":', round(timer1,0),
- # ',"glob.pf_coord":',
- # [round(self.glob.pf_coord[0], 2), round(self.glob.pf_coord[1], 2), round(self.glob.pf_coord[2], 2)],
- # ',"glob.ball_coord":',
- # [round(self.glob.ball_coord[0], 2), round(self.glob.ball_coord[1], 2)],
- # '},', file = f)
-
- def report_to_WIFI(self):
- message_to_Host = {"ID": 0, "x": 0.0, "y": 0.0, "yaw": 0.0, "bx": 0.0, "by": 0.0, "bytes": 0}
- if self.glob.SIMULATION == 2 : message_to_Host["ID"] = self.glob.wifi_params['ROBOT_ID']
- else: message_to_Host["ID"] = str(self.motion.robot_Number)
- message_to_Host["x"] = round(self.glob.pf_coord[0], 3)
- message_to_Host["y"] = round(self.glob.pf_coord[1], 3)
- message_to_Host["yaw"] = round(self.glob.pf_coord[2], 3)
- message_to_Host["bx"] = round(self.glob.ball_coord[0], 3)
- message_to_Host["by"] = round(self.glob.ball_coord[1], 3)
- message_to_Host["bytes"] = len(bytes(array.array('I', (i for i in range(1)))))
- data = str(message_to_Host).encode()
- if self.glob.SIMULATION == 2 :
- pin = self.pyb.Pin
- pin('P2', pin.AF_PP, pin.PULL_DOWN, af = pin.AF5_SPI2) # pyb.Pin.AF_PP = 2, pyb.Pin.PULL_DOWN = 2, pyb.Pin.AF5_SPI2 = 5
- pin('P3', pin.OUT_PP, pin.PULL_UP, af = pin.AF5_SPI2) # pyb.Pin.OUT_PP = 1, pyb.Pin.PULL_UP = 1
- try:
- self.glob.udp_socket.sendto(data, self.glob.target_wifi_address)
- #self.glob.udp_socket.sendto('particles'.encode(), self.glob.target_wifi_address)
- if self.glob.SIMULATION != 2:
- if str(self.motion.robot_Number) == '':
- self.glob.udp_socket.sendto(self.glob.pf_alloc1, self.glob.target_wifi_address)
- except Exception:
- print('failed to send')
- pass
-
- def localisation_Complete(self):
- if self.glob.with_pf:
- returncode = self.process_Post_data_in_Pose()
- self.pf_update(returncode)
- alpha, betta = 0.3, 0.7
- if abs(self.coord_visible[0]) > 2 or abs(self.coord_visible[1]) > 1.5 : alpha, betta = 0,1
- self.coordinate[0] = (self.coord_visible[0] * alpha +self.coord_odometry[0] * betta)
- self.coordinate[1] = (self.coord_visible[1] * alpha +self.coord_odometry[1] * betta)
- self.coordinate[2] = (self.coord_visible[2] * alpha +self.coord_odometry[2] * betta)
- deviation = math.sqrt((self.coord_visible[0] -self.coord_odometry[0])**2 + (self.coord_visible[1] -self.coord_odometry[1])**2)
- if deviation < 0.1: self.quality = 1
- else: self.quality = 0.1/deviation
- #self.coord_odometry = self.coordinate.copy()
- self.post_data_in_pose_number = 0
- self.correct_yaw_in_pf()
- self.glob.pf_coord = self.call_Par_Filter.return_coord()
- if self.glob.obstacleAvoidanceIsOn: self.group_obstacles()
- else:
- self.correct_yaw_in_pf()
- returncode = True
- self.coordinate_record()
- return returncode
-
- def process_Post_data_in_Pose(self):
- if self.post_data_in_pose_number < 2: return False
- #uprint('len(self.post_data_in_pose) = ', len(self.post_data_in_pose))
- post_data = []
- for i in range (self.post_data_in_pose_number): #|
- a = [list(self.post_data_in_pose[i])[0] / 2000,
- list(self.post_data_in_pose[i])[1] / 2000,
- list(self.post_data_in_pose[i])[2]] #|Copy class data into method
- post_data.append(a) #|
- sorted_data = sorted(post_data)
- max_difference = 0
- del post_data
- max_difference_index = 0
- for i in range(len(sorted_data)-1): #|
- if sorted_data[i+1][0]- sorted_data[i][0] > max_difference: #|find index of post in sorted list with biggest
- max_difference = sorted_data[i+1][0]- sorted_data[i][0] #| difference in yaw
- max_difference_index = i #|
- divider = sorted_data[max_difference_index][0] + max_difference/2 # define yaw andgle of divider
- for i in range(len(sorted_data)): #| rotate all data in order to put biggest difference into gap -180/+180
- sorted_data[i][0] = sorted_data[i][0] - divider + math.pi #| !!!
- if sorted_data[i][0] > math.pi : sorted_data[i][0] -= ( 2 * math.pi ) #|
- new_data = sorted(sorted_data) # sort all data again
- del sorted_data
- differences = [] #|
- for i in range(len(new_data)-1): #| create list of differences in yaw with index
- differ = new_data[i+1][0] - new_data[i][0] #|
- if differ > 2 *math.pi: differ %= (2 * math.pi) #|
- if differ > math.pi: differ = 2 * math.pi - differ #|
- differences.append((differ, i)) #|
- differences_descending = sorted(differences, reverse = True) #|
- del differences
- visible_posts_number = 4 #|
- if len(differences_descending) > 2 and differences_descending[2][0] <= 0.14: visible_posts_number = 3 #|
- if len(differences_descending) > 2 and differences_descending[1][0] <= 0.14: visible_posts_number = 2 #|
- if len(differences_descending) > 2 and differences_descending[0][0] <= 0.14: visible_posts_number = 1 #|
- if len(differences_descending) == 2 : #|detect visible posts number
- visible_posts_number = 3 #|
- if differences_descending[1][0] <= 0.14: visible_posts_number = 2 #|
- if differences_descending[0][0] <= 0.14: visible_posts_number = 1 #|
- if len(differences_descending) == 1: #|
- visible_posts_number = 2 #|
- if differences_descending[0][0] <= 0.14: visible_posts_number = 1 #|
- if visible_posts_number == 1: return False #|
- windows = []
- for i in range(visible_posts_number - 1): windows.append(differences_descending[i][1]+1)
- del differences_descending
- organized_windows = sorted(windows)
- del windows
- organized_windows.append(len(new_data))
- posts = []
- yaw_p = 0
- dist_p = 0
- index_p = 0
- for i in range( len(organized_windows)):
- yaw = 0
- dist = 0
- for j in range (organized_windows[i]):
- yaw = yaw + new_data[j][0]
- dist = dist + new_data[j][1]
- posts.append([(yaw-yaw_p)/(organized_windows[i] -index_p) + divider - math.pi, (dist-dist_p)/(organized_windows[i]-index_p),0])
- yaw_p = yaw
- dist_p = dist
- index_p = organized_windows[i]
- del new_data
- del organized_windows
- for i in range(len(posts)):
- yaw = posts[i][0]
- yaw = self.normalize_yaw( yaw )
- posts[i][0] = yaw
- if visible_posts_number == 4:
- imin = 0
- for i in range(4):
- if abs(posts[i][0]) < abs(posts[imin][0]): imin = i
- if imin == 0: imin2 = 1
- else: imin2 = 0
- for i in range(4):
- if i == imin : continue
- if abs(posts[i][0]) < abs(posts[imin2][0]): imin2 = i
- if posts[imin][0] > posts[imin2][0]:
- posts[imin2][2] = 1
- posts[imin][2] = 2
- guest_right_post_index = imin2
- else:
- posts[imin][2] = 1
- posts[imin2][2] = 2
- guest_right_post_index = imin
- home_right_post_index = guest_right_post_index + 2
- home_left_post_index = guest_right_post_index + 3
- if home_right_post_index > 0: home_right_post_index = home_right_post_index - 4
- if home_left_post_index > 0: home_left_post_index = home_left_post_index - 4
- posts[home_right_post_index][2] = 3
- posts[home_left_post_index][2] = 4
- else:
- if visible_posts_number == 3:
- mean_point = (posts[0][1] + posts[1][1] + posts[2][1] ) / 3 # recognition of useless post
- if posts[0][1] < mean_point and posts[1][1] >= mean_point and posts[2][1] >= mean_point \
- or posts[0][1] >= mean_point and posts[1][1] < mean_point and posts[2][1] < mean_point : exit_ind = 0
- if posts[1][1] < mean_point and posts[0][1] >= mean_point and posts[2][1] >= mean_point \
- or posts[1][1] >= mean_point and posts[0][1] < mean_point and posts[2][1] < mean_point : exit_ind = 1
- if posts[2][1] < mean_point and posts[0][1] >= mean_point and posts[1][1] >= mean_point \
- or posts[2][1] >= mean_point and posts[0][1] < mean_point and posts[1][1] < mean_point : exit_ind = 2
- posts.pop(exit_ind) # deleting of useless post
- visible_posts_number = 2
- if visible_posts_number == 2:
- diff = posts[0][0] - posts[1][0]
- if -1.75 < posts[0][0] < 1.75 and -1.75 < posts[1][0] < 1.75 :
- if diff > 0: posts[0][2], posts[1][2] = 2 , 1
- else: posts[0][2], posts[1][2] = 1 , 2
- elif (posts[0][0] > 1.75 or posts[0][0] < -1.75) and (posts[1][0] > 1.75 or posts[1][0] < -1.75):
- if (diff > 1.75) or ( -1.75 < diff <0 ): posts[0][2], posts[1][2] = 3 , 4
- else: posts[0][2], posts[1][2] = 4 , 3
- else: return False
- #uprint(posts)
- for i in range(self.post_data_in_pose_number):
- #for raw_post in self.post_data_in_pose:
- #raw_post[2] = 0
- self.post_data_in_pose[i,2] = 0
- for post in posts:
- if abs(post[0] - self.post_data_in_pose[i,0] / 2000)< 0.15 *abs(post[0])\
- and abs(post[1] - self.post_data_in_pose[i,1] / 2000)< 0.15 *abs(post[1]):
- self.post_data_in_pose[i,2] = post[2]
-
- returncode = self.coord_calculation(posts)
- #self.imu_correction_update(posts)
- return returncode
-
- def coord_calculation(self,posts):
- guest_left_yaw, guest_left_dist, guest_right_yaw, guest_right_dist, home_right_yaw, home_right_dist, home_left_yaw, home_left_dist = 0,0,0,0,0,0,0,0
- for i in range(len(posts)):
- if posts[i][2] == 1: guest_right_yaw, guest_right_dist, number = posts[i]
- if posts[i][2] == 2: guest_left_yaw, guest_left_dist, number = posts[i]
- if posts[i][2] == 3: home_right_yaw, home_right_dist, number = posts[i]
- if posts[i][2] == 4: home_left_yaw, home_left_dist, number = posts[i]
- coord_by_yaw_dist_guest =[]
- coord_by_yaw_yaw_guest =[]
- if not (guest_right_dist == 0 or guest_left_dist == 0) :
- rp = guest_right_yaw
- lp = guest_left_yaw
- #xt = GUEST_GOAL_POST_RIGHT_X - guest_right_dist * math.cos(rp)
- #yt = GUEST_GOAL_POST_RIGHT_Y - guest_right_dist * math.sin(rp)
- xt = self.glob.landmarks['post1'][0][0] - guest_right_dist * math.cos(rp)
- yt = self.glob.landmarks['post1'][0][1] - guest_right_dist * math.sin(rp)
- coord_by_yaw_dist_guest.append([xt,yt])
- #xt = GUEST_GOAL_POST_LEFT_X - guest_left_dist * math.cos(lp)
- #yt = GUEST_GOAL_POST_LEFT_Y - guest_left_dist * math.sin(lp)
- xt = self.glob.landmarks['post2'][0][0] - guest_left_dist * math.cos(lp)
- yt = self.glob.landmarks['post2'][0][1] - guest_left_dist * math.sin(lp)
- coord_by_yaw_dist_guest.append([xt,yt])
- if (math.tan(rp)- math.tan(lp)) != 0:
- #xt = GUEST_GOAL_POST_RIGHT_X + ( GUEST_GOAL_POST_LEFT_Y - GUEST_GOAL_POST_RIGHT_Y) /(math.tan(rp)- math.tan(lp))
- #yt = GUEST_GOAL_POST_RIGHT_Y + ( GUEST_GOAL_POST_LEFT_Y - GUEST_GOAL_POST_RIGHT_Y) * math.tan(rp)/ (math.tan(rp) - math.tan(lp))
- xt = self.glob.landmarks['post1'][0][0]\
- + ( self.glob.landmarks['post2'][0][1] - self.glob.landmarks['post1'][0][1]) /(math.tan(rp)- math.tan(lp))
- yt = self.glob.landmarks['post1'][0][1]\
- + ( self.glob.landmarks['post2'][0][1] - self.glob.landmarks['post1'][0][1]) * math.tan(rp)/ (math.tan(rp) - math.tan(lp))
- coord_by_yaw_yaw_guest.append([xt,yt])
- #uprint(' From GUEST GOALS:')
- #uprint( 'МЕТОД 1:')
- #uprint( coord_by_yaw_dist_guest )
- #uprint( 'МЕТОД 2:')
- #uprint(coord_by_yaw_yaw_guest)
- coord_by_yaw_dist_home =[]
- coord_by_yaw_yaw_home =[]
- if not (home_right_dist == 0 or home_left_dist == 0) :
- rp = home_right_yaw
- lp = home_left_yaw
- #xt = HOME_GOAL_POST_RIGHT_X - home_right_dist * math.cos(rp)
- #yt = HOME_GOAL_POST_RIGHT_Y - home_right_dist * math.sin(rp)
- xt = self.glob.landmarks['post3'][0][0] - home_right_dist * math.cos(rp)
- yt = self.glob.landmarks['post3'][0][1] - home_right_dist * math.sin(rp)
- coord_by_yaw_dist_home.append([xt,yt])
- #xt = HOME_GOAL_POST_LEFT_X - home_left_dist * math.cos(lp)
- #yt = HOME_GOAL_POST_LEFT_Y - home_left_dist * math.sin(lp)
- xt = self.glob.landmarks['post4'][0][0] - home_left_dist * math.cos(lp)
- yt = self.glob.landmarks['post4'][0][1] - home_left_dist * math.sin(lp)
- coord_by_yaw_dist_home.append([xt,yt])
- if (math.tan(rp)- math.tan(lp)) != 0:
- #xt = HOME_GOAL_POST_RIGHT_X + ( HOME_GOAL_POST_LEFT_Y - HOME_GOAL_POST_RIGHT_Y) /(math.tan(rp)- math.tan(lp))
- #yt = HOME_GOAL_POST_RIGHT_Y + ( HOME_GOAL_POST_LEFT_Y - HOME_GOAL_POST_RIGHT_Y) * math.tan(rp)/ (math.tan(rp) - math.tan(lp))
- xt = self.glob.landmarks['post3'][0][0]\
- + ( self.glob.landmarks['post4'][0][1] - self.glob.landmarks['post3'][0][1]) /(math.tan(rp)- math.tan(lp))
- yt = self.glob.landmarks['post3'][0][1]\
- + ( self.glob.landmarks['post4'][0][1] - self.glob.landmarks['post3'][0][1]) * math.tan(rp)/ (math.tan(rp) - math.tan(lp))
- coord_by_yaw_yaw_home.append( [xt,yt])
- #uprint(' From HOME GOALS:')
- #uprint( 'МЕТОД 1:')
- #uprint( coord_by_yaw_dist_home )
- #uprint( 'МЕТОД 2:')
- #uprint(coord_by_yaw_yaw_home)
- weight = 0
- sum_x, sum_y = 0, 0
- if len(coord_by_yaw_dist_guest) != 0:
- average_dist = (guest_right_dist + guest_left_dist)/2
- if average_dist > 1.6:
- factor = 0.01
- self.quality = 0.01
- else:
- factor = 1
- self.quality = 1
- sum_x = sum_x + (coord_by_yaw_dist_guest[0][0] + coord_by_yaw_dist_guest[1][0]) * factor
- sum_y = sum_y + (coord_by_yaw_dist_guest[0][1] + coord_by_yaw_dist_guest[1][1]) * factor
- weight = weight + 2 * factor
- if len(coord_by_yaw_yaw_guest) != 0:
- sum_x = sum_x + coord_by_yaw_yaw_guest[0][0] * 5 * factor
- sum_y = sum_y + coord_by_yaw_yaw_guest[0][1] * 5 * factor
- weight = weight + 5 * factor
- if len(coord_by_yaw_dist_home) != 0:
- average_dist = (home_right_dist+ home_left_dist)/2
- if average_dist > 1.6:
- factor = 0.01
- self.quality = 0.01
- else:
- factor = 1
- self.quality = 1
- sum_x = sum_x + (coord_by_yaw_dist_home[0][0] + coord_by_yaw_dist_home[1][0]) * factor
- sum_y = sum_y + (coord_by_yaw_dist_home[0][1] + coord_by_yaw_dist_home[1][1]) * factor
- weight = weight + 2 * factor
- if len(coord_by_yaw_yaw_home) != 0:
- sum_x = sum_x + coord_by_yaw_yaw_home[0][0] * 5 * factor
- sum_y = sum_y + coord_by_yaw_yaw_home[0][1] * 5 * factor
- weight = weight + 5 * factor
- if weight == 0: return False
- self.coord_visible = [sum_x/weight, sum_y/weight, self.motion.imu_body_yaw()]
- #uprint('self.coord_visible = ', self.coord_visible)
- return True
-
- def imu_correction_update(self, posts):
- if len(posts) < 3: return
- L = self.glob.landmarks["FIELD_LENGTH"] # 3.6
- W = self.glob.landmarks["post2"][0][1] - self.glob.landmarks["post1"][0][1] # 1.2
- alpha = [10,10,10,10]
- for post in posts:
- alpha[post[2]-1] = post[0]
-
- a1 = math.tan(alpha[0])
- a2 = math.tan(alpha[1])
- a3 = math.tan(alpha[2])
- a4 = math.tan(alpha[3])
-
- if alpha[0] == 10 and alpha[1] != 10 and alpha[2] != 10 and alpha[3] != 10:
- if a4 - a3 == 0: yaw_correction = 0
- else:
- component1 = W/L*(a3+a2)/(a4-a3)
- betta = (component1 + a2)/(1 - component1 * a4)
- yaw_correction = math.atan(betta)
- print('yaw_correction1 =', yaw_correction)
- elif alpha[0] != 10 and alpha[1] == 10 and alpha[2] != 10 and alpha[3] != 10:
- if a4 - a3 == 0: yaw_correction = 0
- else:
- component1 = W/L*(a1+a4)/(a3-a4)
- betta = (component1 + a1)/(1 - component1 * a3)
- yaw_correction = math.atan(betta)
- print('yaw_correction2 =', yaw_correction)
- elif alpha[0] != 10 and alpha[1] != 10 and alpha[2] == 10 and alpha[3] != 10:
- if a2 - a1 == 0: yaw_correction = 0
- else:
- component1 = W/L*(a4+a1)/(a2-a1)
- betta = (a4 - component1)/(1 + component1 * a2)
- yaw_correction = math.atan(betta)
- print('yaw_correction3 =', yaw_correction)
- elif alpha[0]!= 10 and alpha[1] != 10 and alpha[2] != 10 and alpha[3] == 10:
- if a2 - a1 == 0: yaw_correction = 0
- else:
- component1 = W/L*(a3+a2)/(a2-a1)
- betta = (a3 - component1)/(1 + component1 * a1)
- yaw_correction = math.atan(betta)
- print('yaw_correction4 =', yaw_correction)
- elif alpha[0] != 10 and alpha[1] != 10 and alpha[2] != 10 and alpha[3] != 10:
- if a4 - a3 == 0 or a2 - a1 == 0 : yaw_correction = 0
- else:
- component1 = W/L*(a3+a2)/(a4-a3)
- betta1 = (component1 + a2)/(1 - component1 * a4)
- component1 = W/L*(a1+a4)/(a3-a4)
- betta2 = (component1 + a1)/(1 - component1 * a3)
- component1 = W/L*(a4+a1)/(a2-a1)
- betta3 = (a4 - component1)/(1 + component1 * a2)
- component1 = W/L*(a3+a2)/(a2-a1)
- betta4 = (a3 - component1)/(1 + component1 * a1)
- betta = (betta1 + betta2 + betta3 + betta4) / 4
- yaw_correction = math.atan(betta)
- print('yaw_correction5 =', yaw_correction)
- else: return
- self.glob.imu_drift_correction -= yaw_correction
- self.glob.imu_drift_last_correction_time = self.motion.utime.time()
-
-
-
- def convert_to_line_vector(self, x1 , y1 , x2 , y2): # converts lines from (x1,y1),(x2,y2) into [rho,theta]
- if x1 == x2: return x1, 0
- if y1 == y2: return y1, math.pi/2
- theta = math.atan((float(x1) - float(x2))/(float(y2) - float(y1)))
- rho = (float(x1) * float(y2) - float(x2) * float(y1))/(float(x1) - float(x2)) * math.sin(theta)
- return rho, theta
-
- def detect_line_in_image(self, img):
- if self.glob.SIMULATION == 2 :
- self.sensor.flush()
- img1 = self.sensor.alloc_extra_fb(160, 120, self.sensor.RGB565)
- img2 = self.sensor.alloc_extra_fb(160, 120, self.sensor.RGB565)
- img.copy(x_scale = 0.5, y_scale = 0.5, copy_to_fb = img1)
- img.copy(x_scale = 0.5, y_scale = 0.5, copy_to_fb = img2)
- #img.copy(x_scale = 0.5, y_scale = 0.5, copy_to_fb = img)
- #pyb.delay(1000)
- img4 = img1.binary([self.vision.TH["white marking"]['th']],to_bitmap=True, copy=False)
- img3 = img2.binary([self.vision.TH["green field"]['th']],to_bitmap=True, copy=False)
- img4.open(1)
- img3.open(2)
- #print(img3.compressed_for_ide(), end="")
- #pyb.delay(1000)
- #point_data = np.zeros((160,2), dtype = np.uint8)
- #for x in range(160):
- # for y in range(120):
- # #if img3.get_pixel(x,y)[0] > 200: break
- # if img3.get_pixel(x,y) > 0: break
- # point_data[x,0] = x
- # point_data[x,1] = y
- #line_segments_data = self.vision.detect_line_segments( point_data, 160,
- # rank_threshold = 15, line_num_limit = 3 )
- line_segments_data = self.vision.detect_line_segments( img3, rank_threshold = 15,
- line_num_limit = 3, upper_lines = True )
- point_data = None
- field_border_data = []
- field_border_segments = []
- for line_segment in line_segments_data:
- rho, theta = self.convert_to_line_vector(line_segment[0], line_segment[1],
- line_segment[2], line_segment[3])
- p_a1, p_b1 = 0, int(rho)
- if theta != 0:
- p_a1 = -1 / math.tan(theta)
- p_b1 = rho/math.sin(theta)
- line = []
- if theta == 0: line = [int(rho),0,int(rho),119]
- else:
- y = int(p_b1)
- if 0 <= y <= 119 :
- line.append(0)
- line.append(y)
- y = int(159 * p_a1 + p_b1)
- if 0 <= y <= 119 :
- line.append(159)
- line.append(y)
- if math.cos(theta) != 0:
- x = int(- p_b1 / p_a1)
- if 0 <= x <= 159 and len(line) < 4:
- line.append(x)
- line.append(0)
- x = int(- p_b1 / p_a1 + 119 / p_a1)
- if 0 <= x <= 159 and len(line) < 4:
- line.append(x)
- line.append(119)
- #print('line = ', line)
- if len(line) == 4:
- field_border_segments.append(line)
- field_border_data.append([p_a1, p_b1])
- #img.draw_line(line, (255,0,0))
- #print( 'line = ', line)
- #print(img3.compressed_for_ide(), end="")
- #pyb.delay(1000)
- #print(img4.compressed_for_ide(), end="")
- #pyb.delay(1000)
- #print(img.compressed_for_ide(), end="")
- #pyb.delay(1000)
- #print( 'field_border_data = ', field_border_data)
- #print( 'field_border_segments =', field_border_segments)
- len_field_border_data = len(field_border_data)
- if len_field_border_data != 0:
- if len_field_border_data == 3:
- k = 0
- for i in range(3):
- if field_border_segments[i - k] == [0, 119, 159, 119]:
- field_border_data.pop(i - k)
- field_border_segments.pop(i - k)
- len_field_border_data -= 1
- k += 1
- if k == 0:
- cross_points_num = 0
- a0, b0 = field_border_data[0][0], field_border_data[0][1]
- a1, b1 = field_border_data[1][0], field_border_data[1][1]
- a2, b2 = field_border_data[2][0], field_border_data[2][1]
- if a0 != a1:
- cross_point01 = [int((b0 - b1)/(a1 - a0)), int(a0 * (b0 - b1)/(a1 - a0) + b0)]
- if 0 <= cross_point01[0] < 160 and 0 <= cross_point01[1] < 120:
- cross_points_num += 1
- else: cross_point01 = None
- else: cross_point01 = None
- if a1 != a2:
- cross_point12 = [int((b1 - b2)/(a2 - a1)), int(a1 * (b1 - b2)/(a2 - a1) + b1)]
- if 0 <= cross_point12[0] < 160 and 0 <= cross_point12[1] < 120:
- cross_points_num += 1
- else: cross_point12 = None
- else: cross_point12 = None
- if a0 != a2:
- cross_point02 = [int((b0 - b2)/(a2 - a0)), int(a0 * (b0 - b2)/(a2 - a0) + b0)]
- if 0 <= cross_point02[0] < 160 and 0 <= cross_point02[1] < 120:
- cross_points_num += 1
- else: cross_point02 = None
- else: cross_point02 = None
- if cross_points_num == 3:
- field_border_data.pop(2)
- field_border_segments.pop(2)
- len_field_border_data = 2
- if len_field_border_data == 2:
- k = 0
- for i in range(2):
- if field_border_segments[i - k] == [0, 119, 159, 119]:
- field_border_data.pop(i - k)
- field_border_segments.pop(i - k)
- len_field_border_data -= 1
- k += 1
- if len_field_border_data == 2:
- a0, b0 = field_border_data[0][0], field_border_data[0][1]
- a1, b1 = field_border_data[1][0], field_border_data[1][1]
- if round(a0, 2) == 0:
- if (a1 > 0 and field_border_segments[0][0] > field_border_segments[1][0]) or\
- (a1 < 0 and field_border_segments[0][2] < field_border_segments[1][2]) or\
- (field_border_segments[0][0] == 0 and field_border_segments[0][2] == 159) :
- field_border_data.pop(1)
- field_border_segments.pop(1)
- for x in range(160):
- border_y_candidate = []
- for parameter in field_border_data:
- border_y_candidate.append(parameter[0] * x + parameter[1])
- border_y = max(border_y_candidate)
- for y in range(120):
- if y < border_y:
- img4.set_pixel(x, y, 0)
- for line in field_border_segments: img.draw_line([line[0] * 2, line[1] * 2, line[2] * 2, line[3] * 2], (255,0,0))
- img4.zhangSuen()
- #self.vision.thinning(img4, 120, 160, 100)
- #print(img4.compressed_for_ide(), end="")
- #pyb.delay(1000)
- #point_data = self.glob.line_alloc
- #data_size = 0
- #for x in range(160):
- # for y in range(120):
- # if img4.get_pixel(x,y) > 0:
- # if data_size >= 2000: break
- # point_data[data_size,0] = x
- # point_data[data_size,1] = y
- # data_size += 1
- line_segments = self.vision.detect_line_segments( img4 )
- line_segments_data = []
- for line in line_segments:
- line_new = [line[0]*2, line[1]*2, line[2]*2, line[3]*2]
- line_segments_data.append(line_new)
- img.draw_line(line_new, (255,0,0))
- uprint( 'line_segment = ', line)
- #print(img.compressed_for_ide(), end="")
- self.sensor.flush()
- self.sensor.dealloc_extra_fb()
- self.sensor.dealloc_extra_fb()
- else: # simulation
- img1 = self.re.Image(img)
- #self.cv2.imshow('Vision Binary', img1.img)
- #self.cv2.waitKey(0) & 0xFF
- img_white = img1.binary(self.vision.TH["white marking"]['th']) # detect white color areas
- img2 = self.cv2.resize(img1.img, (160, 120))
- #self.cv2.imshow('Vision Binary', img2)
- #self.cv2.waitKey(0) & 0xFF
- img2r = self.re.Image(img2)
- green_mask = img2r.binary(self.vision.TH["green field"]['th']) # detect green color areas
- #self.cv2.imshow('Vision Binary', green_mask)
- #self.cv2.waitKey(0) & 0xFF
- line_segments_data = self.vision.detect_line_segments( green_mask, rank_threshold = 8,
- line_num_limit = 3, upper_lines = True )
- point_data = None
- field_border_data = []
- field_border_segments = []
- for line_segment in line_segments_data:
- rho, theta = self.convert_to_line_vector(line_segment[0], line_segment[1],
- line_segment[2], line_segment[3])
- p_a1, p_b1 = 0, int(rho)
- if theta != 0:
- p_a1 = -1 / math.tan(theta)
- p_b1 = rho/math.sin(theta)
- line = []
- if theta == 0: line = [int(rho),0,int(rho),119]
- else:
- y = int(p_b1)
- if 0 <= y <= 119 :
- line.append(0)
- line.append(y)
- y = int(159 * p_a1 + p_b1)
- if 0 <= y <= 119 :
- line.append(159)
- line.append(y)
- if math.cos(theta) != 0:
- x = int(- p_b1 / p_a1)
- if 0 <= x <= 159 and len(line) < 4:
- line.append(x)
- line.append(0)
- x = int(- p_b1 / p_a1 + 119 / p_a1)
- if 0 <= x <= 159 and len(line) < 4:
- line.append(x)
- line.append(119)
- #print('line = ', line)
- if len(line) == 4:
- field_border_segments.append(line)
- field_border_data.append([p_a1, p_b1])
- len_field_border_data = len(field_border_data)
- if len_field_border_data != 0:
- if len_field_border_data == 3:
- k = 0
- for i in range(3):
- if field_border_segments[i - k] == [0, 119, 159, 119]:
- field_border_data.pop(i - k)
- field_border_segments.pop(i - k)
- len_field_border_data -= 1
- k += 1
- if k == 0:
- cross_points_num = 0
- a0, b0 = field_border_data[0][0], field_border_data[0][1]
- a1, b1 = field_border_data[1][0], field_border_data[1][1]
- a2, b2 = field_border_data[2][0], field_border_data[2][1]
- if a0 != a1:
- cross_point01 = [int((b0 - b1)/(a1 - a0)), int(a0 * (b0 - b1)/(a1 - a0) + b0)]
- if 0 <= cross_point01[0] < 160 and 0 <= cross_point01[1] < 120:
- cross_points_num += 1
- else: cross_point01 = None
- else: cross_point01 = None
- if a1 != a2:
- cross_point12 = [int((b1 - b2)/(a2 - a1)), int(a1 * (b1 - b2)/(a2 - a1) + b1)]
- if 0 <= cross_point12[0] < 160 and 0 <= cross_point12[1] < 120:
- cross_points_num += 1
- else: cross_point12 = None
- else: cross_point12 = None
- if a0 != a2:
- cross_point02 = [int((b0 - b2)/(a2 - a0)), int(a0 * (b0 - b2)/(a2 - a0) + b0)]
- if 0 <= cross_point02[0] < 160 and 0 <= cross_point02[1] < 120:
- cross_points_num += 1
- else: cross_point02 = None
- else: cross_point02 = None
- if cross_points_num == 3:
- field_border_data.pop(2)
- field_border_segments.pop(2)
- len_field_border_data = 2
- if len_field_border_data == 2:
- k = 0
- for i in range(2):
- if field_border_segments[i - k] == [0, 119, 159, 119]:
- field_border_data.pop(i - k)
- field_border_segments.pop(i - k)
- len_field_border_data -= 1
- k += 1
- if len_field_border_data == 2:
- a0, b0 = field_border_data[0][0], field_border_data[0][1]
- a1, b1 = field_border_data[1][0], field_border_data[1][1]
- if round(a0, 2) == 0:
- if (a1 > 0 and field_border_segments[0][0] > field_border_segments[1][0]) or\
- (a1 < 0 and field_border_segments[0][2] < field_border_segments[1][2]) or\
- (field_border_segments[0][0] == 0 and field_border_segments[0][2] == 159) :
- field_border_data.pop(1)
- field_border_segments.pop(1)
-
- for x in range(320):
- border_y_candidate = []
- for parameter in field_border_data:
- border_y_candidate.append(parameter[0] * x + parameter[1] * 2)
- border_y = max(border_y_candidate)
- for y in range(240):
- if y < border_y:
- img_white[y][x] = [0,0,0]
- else: break
- img_w_g = self.copy.deepcopy(img_white)
- #for x in range(320): #| set to black area over green field
- # for y in range (240): #|
- # if green_mask[y][x].all() == 0: img_w_g[y][x] = [0,0,0] #|
- # else: break #|
- #self.cv2.imshow('Vision Binary', img_w_g)
- #self.cv2.waitKey(0) & 0xFF
- kernel = self.np.ones((5,5), self.np.uint8)
- img_w_g = self.cv2.dilate(img_w_g,kernel,iterations = 1)
- img_w_g = self.cv2.GaussianBlur(img_w_g,(5,5),5)
- #img_w_g = self.cv2.dilate(img_w_g,kernel,iterations = 1)
- #img_w_g = self.cv2.erode(img_w_g,kernel,iterations = 2)
- #img_w_g = self.cv2.dilate(img_w_g,kernel,iterations = 1)
- img_w_g= self.cv2.ximgproc.thinning(self.cv2.cvtColor(img_w_g, self.cv2.COLOR_BGR2GRAY))
- img3 = self.re.Image(img_w_g)
- lines = img3.find_line_segments() # raw line segments are detected in image
- line_segments_data =[]
- for j in range(len(lines)):
- img3.draw_line(lines[j])
- line_segments_data.append(lines[j].line())
- #self.cv2.imshow('Vision Binary', img3.img)
- #self.cv2.waitKey(0) & 0xFF
-
- #self.floor_lines = []
- self.field_border_data = field_border_data
- for line in line_segments_data:
- x1,y1,x2,y2 = line
- weight = abs(x1 - x2) + abs(y1 - y2)
- returncode, floor_x1, floor_y1 = self.motion.get_cooord_of_point(x1, y1)
- if returncode == False or self.max_field_dimension < math.sqrt(floor_x1**2 + floor_y1**2): continue
- returncode, floor_x2, floor_y2 = self.motion.get_cooord_of_point(x2, y2)
- if returncode == False or self.max_field_dimension < math.sqrt(floor_x2**2 + floor_y2**2): continue
- #line_length = math.sqrt((floor_x1 - floor_x2)**2+ (floor_y1 - floor_y2)**2) # check length of segment on floor
- rho, theta = self.convert_to_line_vector(floor_x1, floor_y1, floor_x2, floor_y2)
- #print( 'floor_x1 = ', int(floor_x1*1000),'\t', 'floor_y1 =', int(floor_y1*1000),'\t', 'floor_x2 =', int(floor_x2*1000),'\t', 'floor_y2 =', int(floor_y2*1000) )
- self.floor_lines.append([rho,theta, floor_x1, floor_y1, floor_x2, floor_y2, weight])
- return
-
- def group_lines_find_crosses(self):
- line_group =[]
- for i in range(len(self.floor_lines)):
- if line_group == []:
- line_group.append([self.floor_lines[0]])
- continue
- for j in range(len(line_group)):
- if abs(self.floor_lines[i][0] - line_group[j][0][0]) < 0.4 and abs(self.floor_lines[i][1] - line_group[j][0][1]) < 0.35 :
- line_group[j].append(self.floor_lines[i])
- flag = 1
- break
- flag = 0
- if flag == 0:
- line_group.append([self.floor_lines[i]])
- line_group_compact = []
- for i in range(len(line_group)):
- rho, theta = 0, 0
- min_x1 = min(line_group[i][0][2], line_group[i][0][4])
- max_x2 = max(line_group[i][0][2], line_group[i][0][4])
- min_y1 = min(line_group[i][0][3], line_group[i][0][5])
- max_y2 = max(line_group[i][0][3], line_group[i][0][5])
- n = len(line_group[i])
- total_weight = 0
- for j in range(n):
- rho += line_group[i][j][0] * line_group[i][j][6]
- theta += line_group[i][j][1] * line_group[i][j][6]
- total_weight += line_group[i][j][6]
- #line_length += line_group[i][j][2]
- minmax = min(line_group[i][j][2], line_group[i][j][4])
- if minmax < min_x1 : min_x1 = minmax
- minmax = max(line_group[i][j][2], line_group[i][j][4])
- if minmax > max_x2 : max_x2 = minmax
- minmax = min(line_group[i][j][3], line_group[i][j][5])
- if minmax < min_y1 : min_y1 = minmax
- minmax = max(line_group[i][j][3], line_group[i][j][5])
- if minmax > max_y2 : max_y2 = minmax
- line_length_square = (max_x2-min_x1)**2+(max_y2-min_y1)**2
- if line_length_square > 1.0 : # add only if visible length of line is big enougth
- weight = 1
- #if rho/total_weight > 2.5: weight = 0.5
- line_group_compact.append([rho/total_weight,theta/total_weight, weight])
- #print('min_x1 = ', min_x1, '\t', 'max_x2 = ', max_x2, '\t', 'min_y1 = ', min_y1, '\t', 'max_y2 = ', max_y2 )
- self.cross_points =[]
- n = len(line_group_compact)
- if n > 1:
- for i in range(n):
- for j in range(i+1,n):
- theta_spread = abs(line_group_compact[i][1] - line_group_compact[j][1])
- if theta_spread > 2*math.pi: theta_spread %= (2 * math.pi)
- if theta_spread > math.pi: theta_spread = (2 * math.pi) - theta_spread
- if 0.43 < theta_spread < 2.62 :
- r1, t1 = line_group_compact[i][0], line_group_compact[i][1]
- r2, t2 = line_group_compact[j][0], line_group_compact[j][1]
- alpha = math.atan((r1/r2*math.cos(t2) -math.cos(t1))/(math.sin(t1) -r1/r2*math.sin(t2)))
- D = r1/math.cos(alpha -t1)
- if D < self.max_field_dimension:
- cross_x = D * math.cos(alpha)
- cross_y = D * math.sin(alpha)
- weight = 1
- #if D*D > 6.25:weight = 0.5
- self.cross_points.append([cross_x, cross_y, weight])
- self.line_group_compact.clear()
- self.line_group_compact = line_group_compact
- self.floor_lines.clear()
- return
-
- def detect_penalty_marks(self, img):
- if self.glob.SIMULATION == 2 :
- img1 = img
- else:
- img1 = self.re.Image(img)
- labimg = self.cv2.cvtColor (img, self.cv2.COLOR_BGR2LAB)
- penalty_marks_candidate = []
- #if self.glob.SIMULATION == 2 :
- thresholds = [self.vision.TH['white marking']['th']]
- #else: thresholds = self.vision.TH['white marking']['th']
- for blob in img1.find_blobs(thresholds, pixels_threshold=self.vision.TH['white marking']['pixel'],
- area_threshold=self.vision.TH['white marking']['area'], merge=True):
- if blob.x() < 5 or blob.y() < 5 or blob.x() + blob.w() > 314 or blob.y() + blob.h() > 234 : continue
- n = 0
- per_R, per_G, per_B = 0.0,0.0,0.0
- if self.glob.SIMULATION == 2 :
- for x in range((blob.x()-5), (blob.x()+blob.w()+5)):
- a=self.image.rgb_to_lab(img1.get_pixel(x , blob.y()-5))
- b=self.image.rgb_to_lab(img1.get_pixel(x , blob.y()+ blob.h()+ 5))
- per_R += a[0] + b[0]
- per_G += a[1] + b[1]
- per_B += a[2] + b[2]
- n += 2
- for y in range((blob.y() - 5), (blob.y()+ blob.h()+ 5)):
- a=self.image.rgb_to_lab(img1.get_pixel(blob.x()-5 , y))
- b=self.image.rgb_to_lab(img1.get_pixel(blob.x()+ blob.w()+ 5, y))
- per_R += a[0] + b[0]
- per_G += a[1] + b[1]
- per_B += a[2] + b[2]
- n += 2
- per = [per_R/n, per_G/n, per_B/n]
- is_green = (self.vision.TH['green field']['th'][0] < per[0] < self.vision.TH['green field']['th'][1]) and \
- (self.vision.TH['green field']['th'][2] < per[1] < self.vision.TH['green field']['th'][3]) and \
- (self.vision.TH['green field']['th'][4] < per[2] < self.vision.TH['green field']['th'][5])
- else:
- for x in range((blob.x()-5), (blob.x()+blob.w()+5)):
- per_R += labimg[blob.y()-5][x][0]
- per_G += labimg[blob.y()-5][x][1]
- per_B += labimg[blob.y()-5][x][2]
- per_R += labimg[blob.y()+ blob.h()+ 5][x][0]
- per_G += labimg[blob.y()+ blob.h()+ 5][x][1]
- per_B += labimg[blob.y()+ blob.h()+ 5][x][2]
- n += 2
- for y in range((blob.y() - 5), (blob.y()+ blob.h()+ 5)):
- per_R += labimg[y][blob.x()-5][0]
- per_G += labimg[y][blob.x()-5][1]
- per_B += labimg[y][blob.x()-5][2]
- per_R += labimg[y][blob.x()+ blob.w()+ 5][0]
- per_G += labimg[y][blob.x()+ blob.w()+ 5][1]
- per_B += labimg[y][blob.x()+ blob.w()+ 5][2]
- n += 2
- per = [per_R/n, per_G/n, per_B/n]
- is_green = (self.vision.TH['green field']['th'][0] < per[0]/ 2.55 < self.vision.TH['green field']['th'][1])\
- and (self.vision.TH['green field']['th'][2] < per[1]- 128 < self.vision.TH['green field']['th'][3])\
- and (self.vision.TH['green field']['th'][4] < per[2]- 128 < self.vision.TH['green field']['th'][5])
- if is_green == True: penalty_marks_candidate.append(blob)
- penalty_marks = []
- for i in range(len(penalty_marks_candidate)):
- if penalty_marks_candidate[i].w() * 0.35 < penalty_marks_candidate[i].h() < penalty_marks_candidate[i].w() * 1.2 :
- penalty_marks.append(penalty_marks_candidate[i])
- for i in range(len(penalty_marks)):
- img1.draw_rectangle(penalty_marks[i].rect())
- for i in range (len(penalty_marks)):
- returncode, floor_x, floor_y = self.motion.get_cooord_of_point(penalty_marks[i].cx(), penalty_marks[i].cy())
- if returncode == False: continue
- weight = 1
- #if floor_x**2 + floor_y**2 > 6.25: weight = 0.5
- self.penalty.append([floor_x, floor_y, weight])
- #uprint(penalty_marks)
- #self.cv2.imshow('Vision Binary', img1.img)
- #self.cv2.waitKey(0) & 0xFF
-
- def group_penalty_marks(self):
- self.penalty_points.clear()
- penalty = []
- m = len(self.penalty)
- for i in range(m):
- if i == 0:
- penalty.append([self.penalty[i]])
- else:
- was_not_added = True
- for j in range(len(penalty)):
- if (self.penalty[i][0] - penalty[j][0][0])**2 + (self.penalty[i][1] - penalty[j][0][1])**2 < 0.36 :
- penalty[j].append(self.penalty[i])
- was_not_added = False
- if was_not_added == True: penalty.append([self.penalty[i]])
- for i in range(len(penalty)):
- n = len(penalty[i])
- x, y, w = 0, 0, 0
- for j in range(n):
- x += penalty[i][j][0]
- y += penalty[i][j][1]
- w += penalty[i][j][2]
- if n != 0 : self.penalty_points.append([x/n,y/n,w/n])
- self.penalty.clear()
-
- def detect_obstacles(self, img):
- if self.glob.SIMULATION == 2 :
- if abs(self.motion.neck_pan) == 2667 and self.motion.neck_tilt == self.motion.neck_play_pose -1400: return
- img1 = img
- else:
- img1 = self.re.Image(img)
- #green_mask = img1.binary(self.vision.TH["green field"]['th']) # detect green color areas
- if len(self.field_border_data) != 0:
- for x in range(320):
- border_y_candidate = []
- for parameter in self.field_border_data:
- border_y_candidate.append(parameter[0] * x + parameter[1] * 2)
- border_y = max(border_y_candidate)
- if self.glob.SIMULATION == 2 :
- for y in range(240):
- if y < border_y:
- img1.set_pixel(x,y,(0,0,0))
- else:
- for y in range(240):
- if y < border_y:
- img1.img[y][x] = [0,0,0]
- else: break
- thresholds = [self.vision.TH['orange ball']['th'],
- self.vision.TH['blue posts']['th'],
- self.vision.TH['yellow posts']['th'],
- self.vision.TH['green field']['th'],
- self.vision.TH['white marking']['th'],
- [0,0,0,0,0,0]]
- if self.glob.SIMULATION == 2 :
- roi1 = (0,0,320,240)
- if (abs(self.motion.neck_pan) == 2667 and self.motion.neck_tilt == self.motion.neck_play_pose -700) or\
- (abs(self.motion.neck_pan) == 1333 and self.motion.neck_tilt == self.motion.neck_play_pose -1400):
- roi1 = (0,0,320,160)
- blobs = img1.find_blobs(thresholds, roi = roi1, invert = True, pixels_threshold=self.vision.TH['blue posts']['pixel'],
- area_threshold=self.vision.TH['blue posts']['area'], merge=True)
-
- else:
- if self.motion.neck_pan == 0 and self.motion.neck_tilt == self.motion.neck_play_pose -1400:
- img1.img[223:][:] = [0,0,0]
- for i in range(208,223):
- img1.img[i][80:235] = [0,0,0]
- if self.motion.neck_pan == -1333 and self.motion.neck_tilt == self.motion.neck_play_pose -1400:
- for i in range(92,240):
- img1.img[i][:135] = [0,0,0]
- for i in range(127,240):
- img1.img[i][135:i-127+135] = [0,0,0]
- if self.motion.neck_pan == -1333 and self.motion.neck_tilt == self.motion.neck_play_pose -700:
- for i in range(170,240):
- img1.img[i][:math.ceil((i-169)/70*50)] = [0,0,0]
- if self.motion.neck_pan == 1333 and self.motion.neck_tilt == self.motion.neck_play_pose -700:
- for i in range(170,240):
- img1.img[i][-math.ceil((i-169)/70*50):] = [0,0,0]
- if self.motion.neck_pan == 1333 and self.motion.neck_tilt == self.motion.neck_play_pose -1400:
- for i in range(92,240):
- img1.img[i][-135:] = [0,0,0]
- for i in range(127,240):
- img1.img[i][-(i-127+140):-135] = [0,0,0]
- if self.motion.neck_pan == 2667 and self.motion.neck_tilt == self.motion.neck_play_pose -1400:
- img1.img[185:][:] = [0,0,0]
- for i in range(240):
- img1.img[i][50:275] = [0,0,0]
- if i > 110:
- img1.img[i][275:] = [0,0,0]
- if self.motion.neck_pan == 2667 and self.motion.neck_tilt == self.motion.neck_play_pose -700:
- for i in range(120,240):
- img1.img[i][70:245] = [0,0,0]
- if i > 224:
- img1.img[i][245:] = [0,0,0]
- if self.motion.neck_pan == -2667 and self.motion.neck_tilt == self.motion.neck_play_pose -1400:
- img1.img[185:][:] = [0,0,0]
- for i in range(240):
- img1.img[i][40:270] = [0,0,0]
- if i > 110:
- img1.img[i][:40] = [0,0,0]
- if self.motion.neck_pan == -2667 and self.motion.neck_tilt == self.motion.neck_play_pose -700:
- for i in range(120,240):
- img1.img[i][75:250] = [0,0,0]
- if i > 224:
- img1.img[i][:75] = [0,0,0]
- blobs = img1.find_blobs(thresholds, pixels_threshold=self.vision.TH['blue posts']['pixel'],
- area_threshold=self.vision.TH['blue posts']['area'], merge=True, invert = True)
- obstacle_blobs = []
- for blob in blobs:
- #img1.draw_rectangle(blob.rect())
- #uprint('neck_pan = ', self.motion.neck_pan, 'neck_tilt = ', self.motion.neck_tilt, 'blob_rect =', blob.rect())
- obstacle_blobs.append(blob)
- merged_blobs = self.merge_blobs(obstacle_blobs, th=15 )
- for blob in merged_blobs:
- if blob.y() + blob.h() >= 239: continue
- returncode, floor_xc, floor_yc = self.motion.get_cooord_of_point(blob.cx(), blob.y()+blob.h())
- returncode, floor_x1, floor_y1 = self.motion.get_cooord_of_point(blob.x(), blob.y()+blob.h())
- obstacle_diameter = math.sqrt((floor_xc-floor_x1)**2 + (floor_yc-floor_y1)**2) * 2
- dist = math.sqrt((floor_xc)**2 + (floor_yc)**2)
- floor_x = floor_xc * (dist + obstacle_diameter/2)/dist * 1.05
- floor_y = floor_yc * (dist + obstacle_diameter/2)/dist * 1.05
- self.glob.obstacles.append([floor_x, floor_y, obstacle_diameter])
- if len(merged_blobs) >0 : img1.draw_rectangle(merged_blobs[0].rect(), color = (0,255,255))
- if self.glob.SIMULATION == 2 :
- print(img1.compressed_for_ide(), end="")
- else:
- self.cv2.imshow('Obstacle View', img1.img)
- self.cv2.waitKey(10) & 0xFF
- #self.cv2.waitKey(0) & 0xFF
-
- def merge_blobs(self, blobs_, th=2):
- blobs = []
- for bl in blobs_:
- blobs.append(M_blob(bl.x(), bl.y(), bl.w(), bl.h()))
- if len(blobs) <= 1: return blobs
- merged_blobs = []
- bottoms = []
- for i in range(len(blobs)):
- bottoms.append(blobs[i].y()+blobs[i].h())
- primary = bottoms.index(max(bottoms))
- merged_blobs.append(blobs[primary])
- for i in range(len(blobs)):
- merge = False
- merge_x = False
- merge_y = False
- if blobs[i].x() < blobs[primary].x():
- if blobs[i].x()+blobs[i].w() + th > blobs[primary].x() : merge_x = True
- else:
- if blobs[primary].x()+blobs[primary].w() + th > blobs[i].x() : merge_x = True
- if blobs[i].y() < blobs[primary].y():
- if blobs[i].y()+blobs[i].h() + th > blobs[primary].y() : merge_y = True
- else:
- if blobs[primary].y()+blobs[primary].h() + th > blobs[i].y() : merge_y = True
- if merge_y and merge_x: merge = True
- if merge:
- if (blobs[primary].y()+blobs[primary].h()) - (blobs[i].y()+blobs[i].h()) <\
- max(abs(blobs[primary].x()-blobs[i].x()),
- abs(blobs[primary].x()-blobs[i].x()+ blobs[primary].w()-blobs[i].w())):
- blob_x = min(blobs[primary].x(), blobs[i].x())
- blob_w = max((blobs[primary].x()+blobs[primary].w()), (blobs[i].x() + blobs[i].w())) - blob_x
- blob_y = min(blobs[primary].y(), blobs[i].y())
- blob_h = max((blobs[primary].y()+blobs[primary].h()), (blobs[i].y() + blobs[i].h())) - blob_y
- blob = M_blob(blob_x, blob_y, blob_w, blob_h)
- merged_blobs.pop(0)
- merged_blobs.insert(0, blob)
- else:
- merged_blobs.append(blobs[i])
- return merged_blobs
-
- def group_obstacles(self):
- grouped_obstacles = []
- #uprint('obstacles(raw): ', self.glob.obstacles)
- while(len(self.glob.obstacles) > 0):
- obstacle0 = self.glob.obstacles.pop(0)
- group_number = 1
- k = 0
- for i in range(len(self.glob.obstacles)):
- united_obstacles = math.sqrt((obstacle0[0]-self.glob.obstacles[i-k][0])**2 + (obstacle0[1]-self.glob.obstacles[i-k][1])**2)\
- < (obstacle0[2] + self.glob.obstacles[i-k][2])/2
- if united_obstacles:
- group_number += 1
- new_size = math.sqrt((obstacle0[0]-self.glob.obstacles[i-k][0])**2 + (obstacle0[1]-self.glob.obstacles[i-k][1])**2)\
- + (obstacle0[2] + self.glob.obstacles[i-k][2])/2
- obstacle0 = [(obstacle0[0]*(group_number-1) + self.glob.obstacles[i-k][0])/group_number,
- (obstacle0[1]*(group_number-1) + self.glob.obstacles[i-k][1])/group_number,
- (obstacle0[2]*(group_number-1) + new_size)/group_number,]
- self.glob.obstacles.pop(i-k)
- k += 1
- if obstacle0[2] > 0.1:
- grouped_obstacles.append(obstacle0)
- self.glob.obstacles = []
- for obstacle in grouped_obstacles:
- global_x = self.glob.pf_coord[0] + obstacle[0] * math.cos(self.glob.pf_coord[2]) - obstacle[1] * math.sin(self.glob.pf_coord[2])
- global_y = self.glob.pf_coord[1] + obstacle[1] * math.cos(self.glob.pf_coord[2]) + obstacle[0] * math.sin(self.glob.pf_coord[2])
- if abs(global_y) <= self.glob.landmarks['FIELD_WIDTH']/2 and abs(global_x) <= self.glob.landmarks['FIELD_LENGTH']/2:
- obstacle[0] = global_x
- obstacle[1] = global_y
- self.glob.obstacles.append(obstacle)
- uprint('obstacles: ', self.glob.obstacles)
-
-
-
-
-
-if __name__=="__main__":
- print('This is not main module!')
-
-
-
-
-
-
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Localisation/class_Visualisation.py b/src/azer_robocup_project/Soccer/Localisation/class_Visualisation.py
deleted file mode 100644
index d14bcb4..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/class_Visualisation.py
+++ /dev/null
@@ -1,46 +0,0 @@
-import sim
-
-class Visualisation:
- def __init__(self):
- self.pf_dummy_handle = []
- self.obstacle_handle = []
-
- def localisation_points(self, motion, Dummy_PF_position, Dummy_PF_orientation, Dummy_PF_H):
- for handle in self.pf_dummy_handle:
- returnCode = motion.sim.simxRemoveObject(motion.clientID, handle, motion.sim.simx_opmode_oneshot )
- self.pf_dummy_handle.clear()
- dummy_pf_color = bytearray([0,127,0,0,0,0,64,64,64,0,0,0])
- returnCode, pf_dummy_handle = motion.sim.simxCreateDummy(motion.clientID, 0.05, dummy_pf_color,
- motion.sim.simx_opmode_blocking )
- self.pf_dummy_handle.append(pf_dummy_handle)
- returnCode = motion.sim.simxSetObjectPosition(motion.clientID, pf_dummy_handle ,
- -1,Dummy_PF_position,
- motion.sim.simx_opmode_oneshot)
- returnCode = motion.sim.simxSetObjectOrientation(motion.clientID, pf_dummy_handle ,
- -1,Dummy_PF_orientation, motion.sim.simx_opmode_oneshot)
- for Dummy_PF_H_position in Dummy_PF_H:
- returnCode, pf_dummy_handle = motion.sim.simxCreateDummy(
- motion.clientID, 0.03, dummy_pf_color,
- motion.sim.simx_opmode_blocking )
- self.pf_dummy_handle.append(pf_dummy_handle)
- returnCode = motion.sim.simxSetObjectPosition(motion.clientID, pf_dummy_handle ,
- -1,Dummy_PF_H_position,
- motion.sim.simx_opmode_oneshot)
-
- def obstacle_mark(self, motion, obstacles):
- for handle in self.obstacle_handle:
- returnCode = motion.sim.simxRemoveObject(motion.clientID, handle, motion.sim.simx_opmode_oneshot )
- self.obstacle_handle.clear()
- dummy_pf_color = bytearray([0,127,0,0,0,0,64,64,64,0,0,0])
- for obstacle in obstacles:
- returnCode, obstacle_handle = motion.sim.simxCreateDummy(
- motion.clientID, obstacle[2], dummy_pf_color,
- motion.sim.simx_opmode_blocking )
- self.obstacle_handle.append(obstacle_handle)
- returnCode = motion.sim.simxSetObjectPosition(motion.clientID, obstacle_handle ,
- -1,[obstacle[0],obstacle[1], 0],
- motion.sim.simx_opmode_oneshot)
-
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Localisation/deviation_report.py b/src/azer_robocup_project/Soccer/Localisation/deviation_report.py
deleted file mode 100644
index 69f4cb2..0000000
--- a/src/azer_robocup_project/Soccer/Localisation/deviation_report.py
+++ /dev/null
@@ -1,45 +0,0 @@
-import sys, os
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if sys.version != '3.4.0':
- current_work_directory += '/'
-
-sys.path.append( current_work_directory + 'Soccer/')
-sys.path.append( current_work_directory + 'Soccer/Motion/')
-sys.path.append( current_work_directory + 'Soccer/Vision/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-import cv2
-import numpy as np
-import random, json
-import matplotlib.pyplot as plt
-
-with open(current_work_directory + "Soccer/log/deviation.json", "r") as f:
- data1 = json.loads(f.read())
-#pf_avg, pf_cur, ab_avg, ab_cur = [],[],[],[]
-#for i in range(len(data1)):
-# pf_avg.append(data1[i]['average pf'])
-# pf_cur.append(data1[i]['current pf'])
-# ab_avg.append(data1[i]['average ab'])
-# ab_cur.append(data1[i]['current ab'])
-
-pf_avg, pf_cur = [],[]
-for i in range(len(data1)):
- pf_avg.append(data1[i]['average pf'])
- pf_cur.append(data1[i]['current pf'])
-
-
-rng = np.arange(len(data1))
-fig,ax = plt.subplots(figsize=(10,6))
-plt.plot(rng, pf_avg, label = 'PF average')
-plt.plot(rng, pf_cur, label = 'PF current')
-#plt.plot(rng, ab_avg, label = 'AB average')
-#plt.plot(rng, ab_cur, label = 'AB current')
-
-ax.legend(loc='upper left')
-ax.grid(True)
-plt.show()
-
-#print(a)
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/Head_Tilt_Calibration.py b/src/azer_robocup_project/Soccer/Motion/Head_Tilt_Calibration.py
deleted file mode 100644
index 71ae16b..0000000
--- a/src/azer_robocup_project/Soccer/Motion/Head_Tilt_Calibration.py
+++ /dev/null
@@ -1,74 +0,0 @@
-import sys, os
-import math, time, json
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if current_work_directory.find('Soccer') >= 0:
- current_work_directory = current_work_directory[:-14]
-if sys.version != '3.4.0':
- current_work_directory += '/'
- import random
- SIMULATION = 1
-else:
- SIMULATION = 2
- import pyb
-
-
-sys.path.append( current_work_directory + '/')
-sys.path.append( current_work_directory + 'Soccer/')
-sys.path.append( current_work_directory + 'Soccer/Motion/')
-sys.path.append( current_work_directory + 'Soccer/Vision/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-
-from class_Glob import Glob
-from class_Vision import Vision
-from class_Local import *
-if SIMULATION == 2:
- from class_Motion_real import Motion_real as Motion
-else:
- from class_Motion_sim import *
- from class_Motion_sim import Motion_sim as Motion
-
-
- # 0 - Simulation without physics, 1 - Simulation with physics, 2 - live on openMV
-
-if SIMULATION == 2:
- from button_test import Button_Test
- button = Button_Test()
- glob = Glob(SIMULATION, current_work_directory, particles_number = 100)
- vision = Vision(glob)
- motion = Motion(glob, vision)
- motion.push_Button()
- pyb.delay(2000)
- motion.activation()
- motion.head_Tilt_Calibration()
-else:
- def head_tilt_calibration_in_sim(clientID, event, lock, transfer_Data):
- glob = Glob(SIMULATION, current_work_directory)
- vision = Vision(glob)
- motion = Motion(glob, vision, clientID , event, lock, transfer_Data, 1, robot_Number = '')
- motion.slowTime = 0.0
- motion.sim_Start()
- motion.activation()
- motion.Vision_Sensor_Display_On = True
- motion.sim_Start()
- motion.head_Tilt_Calibration()
- motion.sim_Progress(1)
- motion.sim_Stop()
- motion.sim_Disable()
- import threading
- event = threading.Event()
- events = [event]
- transfer_Data = Transfer_Data()
- transfer_Datas = [transfer_Data]
- lock = threading.Lock()
- clientID = sim_Enable('127.0.0.1', -20000)
- clientIDs = [clientID]
- t0 = threading.Thread( target = simulation_Trigger_Accumulator, args=(clientIDs, events, transfer_Datas, lock))
- t = threading.Thread( target = head_tilt_calibration_in_sim, args=(clientID, event, lock, transfer_Data))
- t0.setDaemon(True)
- t0.start()
- t.start()
-
diff --git a/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion.cpython-39.pyc b/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion.cpython-39.pyc
deleted file mode 100644
index e66d9d728f1869fc1edc01df52302db8e3b4d8c7..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 28248
zcmdsg32<ELUFYl9r@C8*C0Vy+$=6sOk1fgP92wt-W@K4oX|B%9ylLrc$=#N^<*%Q{
zmUvCrcnBf86I@x?;>c#(5V8!*vK%QYq*#(=Q(IIBvjfX6@QR{RkOdU35CVj7WPiW^
zdwt3FWQJjDt5)szf8Tw7|MUBv)6LCs1Akw#->ZK4e;UT`(^LN!gy$rl;!6O^P=O^Q
z<-G%`fcVXnDeqt^DDO}zgtxgAUXG+90Ye2<=v5;Xy<n)Yio9y5$SpG!%QUIzHB+ZD
zaTU82K&rruG19bwI*h52fEzxwv~o2)PjBTf0Eda^0-j<Lz&0G?!v?Y%Gsf!y$+e=h
zc^lb7cIKx2x}hUT_E#3bnMYoDx|q?$gXx233-hbXnY>**cp+<FT%9|3r7%~xmbDMQ
zS$^{u-~4GjKltWPz4@8((eXn=DqFlZlwD3QW)992=I1l|;H83{E#zOAUrHB?mghOJ
zvgSrsrRoIR#zq=J9RqCWfCK}a#8dnNK*rz*V)O#a#0UlbkqUX^lnSe;iXl9rnpB)4
zs1hoPcT6>_7QCBOt7^kLuC}Olyc4QJb>f{=TU8g{&FYZqQN6DQQZ4GR>Qnu2wW<NN
z4RLMih}y1(5VA!Ls~rewS3A`%ygSrxwFmD`^?=%o_f|Ec_Tk;79#s4B?p8<D0d){L
zd(@~JLrAY0$J`xrquy|g&2!0A{&vFg0G=~=ieC&Fh7)iM3p1B9a&!e2G4(%8mkF0C
zE(}Boi>8EuC}GKzA}U(KP?Tb-sf3{@#Z{t&p(rJtpc8V!PQ-~iF{jCiI|(Pb7)Mzl
z)eIO`Evj|VEH$e(wFU1M)vh}5ZdILXE8cCYOLgPDg~Nn*yVLBnIIT{bv&Cs&jFdVs
zXyHZkzOY{nAa<J?1l%t0t<%4C?2wc&TvyHxwX<6LF0~u(J?a6#z0#_Yw`|*6*R@YQ
zh;sG|eCVTlFjf(D05GZ!0>;!RV3RjTYOEKtWnok~V`^M{3Ey|9*GL%Za6eNd{jei-
zVVDKvYC$X>xLV=D16Lbdc;MOs*A~0I+#$YzI@)X838-UIW~a1gtH3T5M#^q=9I!_{
z4A`q40qj$#+3Hu10uHFh0Jo|4psc1owrbEpxz={(&23kY_ZsTNV}?C+i*dtJ;tqj3
z1@02KJImNTitqsm+3T2aW5faX34CyovHQjS5XufG=*Jl52E=txVnzjysgsBs7xy87
zhnWLmN2=+MO87B>$0hy4;(kQh@~Aq6)Q@3@oR%HJ{u#N))tPJN?U4Q~5QedIqSPtx
zlclYApIth&e7e*H>=;((mQF68DRnQOE%hv)EA>j4v2?zKHKNWhU0A+Y>X)z!OB2ga
zlm?ccEN%1COqK?nLDW3ybON4m`qi0Rfzozoz}c=IQ5Rq1s9Zu@Cd*TI*#FB;FH^uZ
z?R2XNr#tr)o-4P42$^wuq}2iS1lJJSA5c$XJ}~lX^0iQD$mx<<nMK@e`Dth94#<K%
zRenZFe#YrT8Rs3N{Oldzd`a`1y2LcjR>YjNpO!M8S5w`Fx_rz0p25<vohrZA87{wY
z2P>+y<5^IO5Pr9U0VCf&X50=HZ;zrZ{+?%<JJhtS?ib;DN?b3&bwykjN`Ddcyo`6X
zw`e)=147d>S97F9BQsMYTSv?dQq>KT0=S#-(xeS=lPpx+(K<ItRV6%J=LX8Gh7+;+
zY2;N&PtsTkuXnQzx&vUu)TEddcXOS)rOw@2=WeTWZ>e*)*SXoHN|~(2b9c+D1~$Hl
zznb@LyE-c+zKtA<NNX71apa`$7(b~w5F7gdyr7q-PcAO-?+-5fi9UPE4_SD);ut--
z!EstV#J`_?;$-&w{_30G%wEGdW-opXt}kIiv)jH3*DoMl_Va%Z*I!`fvM+xHuG<)o
zY=AkQLpm3<^Fqe9o?fvg^58eJ3)%TJnTPCi{fO=M7z#_0Oj|Q{VL3g2)tbtF5F?q|
z6nj2BpJ72$scn9?C$sa{tn&qZBdwKnCA)alwk9)+seyY%Pi7V**6r}yb6yvgt+Sbh
zOnyGI@kdxyZqIZ&pI)-p{J<-jv|6*yr;GN+*D#K5>vX0*`h0djLQ|2{>hf|%r&|3=
zXI3&9Wj#H;Ir){1H?h(;hfa>{kvZ~aSC*2jEm>@``eZrE`jiDLD_FLbB<|w;GGc>C
zHkf#W*)~|f28-I@n%v+r+Sp1UIlAfUA?PLOBfxwZ8v_K}2nGqZ6ATdy6YL-$GqbUa
zU^l>`^Pa_BKYHoRi9du#{`3f5sl@p!r!Jkd&Yqh*^?a&LJTunxxhvM0=g&-@b2~0w
zo}IWnWzAfiI6rI6o|~Dqp3_{X_lkMmeQ4s+)7IH5Ogk}!LN8o7cW%Zy`}CEGsSDOo
z>k<MlojW!2^p$g$&P~m3T*bu9>LVyl%h-GpE^h-ECr@tNMy#&xV9lX9Khm8_V(C~~
z7SY&fDpB)2T<<wv?>ScQIa=>Il1g4p=apaH;RfHK2H$vtZ!8s_E$HHySJo2OEE*GM
zW5n}to%?v5`&ga(Xr22=Dk1HV%!lhehw44!^&Yh0Dwh6eD#F*;xQA2&ZsN-P+~L#d
zVrDWsHxhJ%)7iWmdivCrSvNR&?koh3;KcZu5z~#Fp1M4G<cJ$R_1pyJ{1`=%<$}t%
z=Ja@~i5aZv%U5PoadFN}pF?KTjoR7eOkvedMOM-}Yp>}8n3wh8iG0?!5RzUl9>j)R
z5@5}zm$GyEK(1KGy8-Pc@);lkFVJlf&&pDI%~~lGGwv{ZVXb6sAeI|h`>J)dptI|R
zyq#V`7Hr-0$h;d`Tq?}DtQr-$$x~-$C!RSsbMDGBmuK9_d3Y~hal<ncsdF=KP>qec
zZI$&qd3n}4J8@>#jm}OyIez8TSvOW(D%_YwXWaH;c6s)yM)hab=9e-P`AbDNv49#f
zmdc=gZku?D)=EZO^GxFQE{kYo6|ZJ7_45U-GxJ23*3J7zv7D?f+E&rdtXON=%#w23
zq`3N!t;MX$cmdub_Y!v2f@O_(p`qopebtTXw92OQMYlD(ylN@UCKZX^+&HV6%;Xo5
zD$WvocT5*-LGNyt*Ca07iDC^myG`lkm8Gn`iscYo&|?JSZhSEfDKx{q=N0X{o3fbw
zX-s0#ZCc7Mi#R-{$xpauddZDtZf5QCOX)>7xsYC3%H|g>@x{Cqkj}fw!jiJ8zP7mn
z5PD{H3B#SvFJib`Wo=wvP1_lr$1uC$r2>|b8+j#Lq_?@6-_9=CZimWN=3};CowDup
z{53b8#tglZVKd#}O<TLcHG*{j^Cq4(JnMLFAn>NW#AtwZ0P`lEH9YHhZY;TphNfvq
z<Tj|LHEK!O{-S*1^2F5aj2oW;N}N1JB<e<sc3MMtcY`ZybE&p#g}f?Y?c{A;SVCX*
zX$+XA?5WQZoa6XKikW<|pxr15ZZ@CM=MkEU%;gK%y7~gsU1YlOD)*%u#0r2yfzDpm
zhCNw;mcd#mXsfz7Qca6GlgR^9Dc77pyQZI>oCGOMg;uipaW^atb3=ud3<i20BYn*s
z)R`jK18*N%<JMF`FJoV3Zer`M>Wmxr_QN!HOV|7=wgW1?QP9^cZz>9U%?$|}z|5sp
z(G79C=}T<?6v1V7qDhv3OktPC&f|Ppb67IO5Lxaox5aP5d|@?@#W1qd4HYv>3woM`
z+yqFO=_4s~4$nNE^ie#=@EphUa4MJvmJZGl%mY*x>yeuCXw7-7<~&|=0<~Ay_-HCN
zIdSRK<n+Z;`onC&_YmA`9s@<0x!R2BkFc@9?D%{tw2Fyx0}D{1r0w*WD1tlyPQ-9y
zmc{*LS)_%<KLIcTkw82UZ;G3tz}>$-#K!7=ZGl~Z!RoIk(EhivSOjSzffoEa@k3ei
z>yc2zm|i?#lIhG?C8qMOreMxm>R5v>V#b5a8$&6b0^0ytdJHuuk+wZBB;hQBsUk>s
zK-v=b!`It<P0fMIFwBE5sQ%GSse{|5r~xQgl>@%mWt1WCKzIv^=*5_j_`?-{E<%0Q
zYZS~F8&$QGv6{OvRSq!<>Ymq32%Z$8B2$|D*y{oPy?C8}J?Qf=J)iX*v8v<b>w#24
zFV0!Oq<KrtjWp@o$m51(f4R-vWGmU5*sj3rZsf{^(?^aT@q#@XlwHrHLQg$&;gp*w
zUdygnUZ}f;8PltFp|&M!K{u{u?F_aN8u)QkwrPFoA3_+PA{kNxgWrZ9rzZ)xzNa$$
zZ;b)&KPjI@q3>o=a+s9LaLeRHRK%MyJEo#ia?>5-7A8a7aiosj@qL)MT3XQ&l@oPo
zlMPesr<6JnqoBE~PH}QdlgHLS25?@Js~!oYqW&bx?5{`Ydt@m4ISA0}@d{>qWJtfx
zj6cpS%`(~gKQQ1Y2>v6%PXeUEPjOQ9f1+DfflMZg)Lh=F#3ClRQr}N8q?I9+YOK|w
z{%M5jpCJ(OT!0{uZJVZU2Aowb>yIKwk&;Di>N)*<2xSrMzRx0PEU94)P<6oCWn<>C
zO=VmIw*xvSE+~?rQeS{OSPtERhDn*6=hw_qzzO6c2#;c;g`sna^csuM(p3FX(a2O{
zV@Lr#E9NR(GV{+s=YrKxf)1w~cPg7XQHJ&vT9toF$w_E?Ovfy~0Y!GR=ueut>Cv7R
z$E3~{Ee4V)x5C#<ZD1+ngj7qfVYijHz$IawhHC3HO5tiu`=%K7w?j2ULl)4ZPNWpg
zLz9(5jndbSPQ!_Gv(66H(rxq`r6{z>o;C+s%aDX0gTcY-VA}*KZJ82zz|#&K`kRW}
z_mvYPFlZEQ5NI%ksnB|%u)LlqXz*E?A(C3x1g{td4$3UKp){!Utfm+{5^~!luxQy}
zm5W!wak(vwuk(k&r7vdO$Z}dQX7eLK{VC+sKTq&!0wQaXps{&c^Q+5qVDJhH)*N{H
zqUI7D=@dflHu+Tk@|1p!c?3tsiuv>kDz-t3!4wJC?Zy|f5Z}BKNd8@O_F}5rV+*Yr
z;hjWlF+sbCyR|XmY_>@G!!^Odge8UkVU&?dg5$EG9gt#E+qu!eZ|GtM?5F}q1I`M{
z2n*PjBc|tAZ?Fh4D>z>$>K}o3Bqpeq{Me!!mU7$>+*YdpElP4D!tB68Bk!B3R_|>s
zfUC{ujM}`jlk-<SiHhP8rRU;t0B&Rw+)mlTp?`>lNtPMHEp7foBou-_B)+~tU!dI_
zgde!Rv$D<Wi=8)Fr2j{EmE7)!2WyhLQSN2vJac9_=mbby^U$G#*n+tBV^8>mGU%;I
zZ>qx{&nELrw#f~Pu!X_TE?d*O03Nkigf5UID-|b83pU7FE^Z!!#7cS<Gb3^T18IwF
zpMh?zcT~Dlm!bhju{S|Hf}?uiU~g2Q*8rvf6TzMkXMn_;Q;-MJH_)69A(<P?gFssd
zc-?EC{#Ar-Zdj5Gw}>)G_?t*jBueJw#OsE-KG9u6U2WW1WW-<wYz%g%@hBT8?rR~v
zrxt#^8lEt&8Tw=`=9t7tM=H8kS+(X$R{wXDy{YZ`4TSv;o+8n>fwj@Y0j;uPmF_n*
zn7ym64H?bA3&5SFU?~L57lu^=7O=(8jG@^rsc|IYZIgJNXcxHY2G-obx*NE$sa5XI
z1?mD{(GZ`Rfg2Fw(<{_h7NGKzz~4jJ{t`$A)*q}afwz&fp)=%#*dbXO{|HCc0Gelq
zq&uYuIur&<kE#H>1q2P#UZ>G*AS?z_7J#vjp;3nP7^Nl~3kr`(<KG<<3o9-;Q3gzc
zNZG@j2`8aU%&Asp-gIC%gg=Oxe7_2Me!rBUleojhnUpe6FHMVn?afkq6s7v@LG3No
z+F>-5+FKD4a$4^MK(<0KGSQb5U)cA>#TR+Za7?V5=#<aP8Ygv~n(ix~k)=<90lf{R
zI!Ll3VGkolRfckA(G5eYfm|6%14Zh2jO9q1$B6zq+_Itc|Dub#SN{e9Z2Lwkv{t+h
zdE6ewGv)@K!)x3PJdf9*lxZJ!1F6mHqqnw<xHjs<^glp>#T@_!@Lm$41+im09`d?v
zzK_@uSa7(qX6{>B0}`n)I$YGnUk30j<lk+4m$48aOY>@s=p+Z@BOrD!*@c{0^?_B%
z1#^%mfI_b*G6FCogX{-;pr41iCX&|(i<UxmtlWgz3KOBKD17mnZxLHe;)9iVKZWl{
zd<4d9tdd(Mn1=w^2b^qx%P1$DC}tY<*hv*bO~OhG3(+*yuq9g1H}3BlcJ$)_j#0P2
zxi!GIlwu_`l1mTUBFbfrN@Ma4J590`z?Vt9$#`%zVLv{^_-<~3ki>`mcx)L-7xeQd
zy{(2+Uim>k-t_Y~d$x6_c@wT_al@R^G4C~=GH;AQwS#AEEM>0aePbMcJZpH?$M3Z!
zH!nEBTh{XGlAZkqM}<RVV0|T<h63JieXO#YHY>HL2c19gm?*M}j~@_(CM7itdNT^7
zg^kvM;tGJ^pbAn2O2s+~;ekn)bNoR}ia)baOb8~U6vu#qft3jf%0LQcl7SbTfujjA
zCkYdFlfbw@98Ty*iP-sp^@rj^b$?G)wQx9b;_*`r@g8Dt*8dxg)!cV_gW$^qe@s9P
zMk+dAV2{UBA-?W4)~RUmx+H<v`lrlCbgV_7X=XV1Fu!)Nm2$?$2qm$n$>|IQ)(^jv
zajz65v{5jc@T%)=)GZyTYNdwW0}<{HoAE3`kzkrYm>6;fdX?Z60@7FgIfDO6@FfCn
zAD=bUe-1bKnk5MCRC1=8R?>N2-?`;1j8$vaMfTIvc&3^pB?D41CKT6JSyW0A*V1|;
z^=GV5hRaJ;hYe-v{Vn2sxi8k1j1%=G16|O~4B<x}1>|$vTMl9a8f4c@JVG${z(F*)
zWGsV&f@B`JMYLy@P0D6A&dl%_l@rAB3QfsS!}yRmy=vP~q0eTD_G%I(y!oGTw&~~I
z{Hb4i|C_%E`6Dss#oLJ2Uq!sza|8Nl(QW!sE94oWUzYPsBO#B<NYaRx!flZx<n5)f
z%^NzgvRb@qotC&ATvwERxT8IlS{+MSOQ&h2YuL0T1>CeU7CeeZwCZ!oQ*fJSjTeu5
zWWX^Yzv6MoLAN6g(y#;eK(ra*GRXnP4QYauEd$2&_B_w70EI(VhF=hM*ypRO^404w
z<>X<%z*?)V&u7pO{rv<VB_PkOe+9s80mr=xZEMDxZOCmOXB-!Z8@!fT+dN2ZVzluR
zTQf@q9Ac~NK#UZXddWbax!zM*Je!j@j1|qJ(3p5*1+`86Sbf#@)pyDSx(eW|L=aC%
z&P$kfs0^O69Fg-9!G`k^ZDU69iH6e>kV?SS1My+xKU@kg8l{L05{i**x)rz;K<MdG
z)Q*F)L{+%k=rwKy6eR9a%!z<N8o6Ydr-Pat=pK}(fAHMUPdKqVqG!1N3W#Zo69HQm
zVM$Df^C#3gC7~UNknJn+WD@iOAhe)$GM_KYe70jgo2v5}_vUlG-?LTw=lBjT`!*+x
zc=6zRSj=jAl2b+MurUdHuqR?4V%XOKMmp-|{|6ZQZwWp|@b3u908l<s`genns`V8H
z1Yt68Bk2_w0F<m4w`nG$Un!ggBj>j1%w?@Ix)zdv@=<)Sf7F(hCPf@c)Yg*zgj5Ez
zn_WyL{LU;vn1;eYdIp<PrIw-_$ihrgD5jcy+nc8@a0BVht8uF=ubBdjUfzO}B{|L|
zNv$k5??7NbSiTroKeBxL1-C)-ZPxJHJ<GTYzY)jDp8gGBI?SZMr;B@hw|UL1x`WW=
z;2?{l1SxXy9Ya4Zt`Kk_#5Wbfj{hZuV3$|a)xZ(Rr&kafEz9bpD;K9ev!SF9-c^z<
zB|*FC=|TOHJ}o5?V{jeTm2~}=uG?q>n9kyFAf`Fj0?pG=yA=msJ>1g_tx}sZMXQ9f
zZ{;n1$;~_smunYIRTsiL7;bl#w<?kQ?XGgS0z(N*^)<zlR6W8}Hp`j19_r1?y<SP>
zKFQgy*s=j%e><S!*kOrlA+*L(*!>pHVU@S}bq}hfYW9`&Udonh22y@P@vL5B%C@?c
zTdHNgseofQrG(y1b{<ZR<pvbehM*LXa{o-Re9U4#1dT?Y;@PdnTDq#Wa4l?=Ie!D?
zY%dQ{w=6iG?ryk)VAp%3yt%u2HeA=U9p#;>S115Zb7CY+{eipsH;hxF-be&hzohx`
zyLvW^Bes<^15)oBclBTxr%Ed$irE?2=22tsm`gjB`(Ki#tOzUuA9;iV()Q5I?B(g9
zxwRqChq(gI_59%v{s8}e{>1G1ZsD3X^SeV7!H1rzay0t4Q1asT@BHJZJ~g;;LQ`QN
zII9x$89DKh+UlRzk<&oaIOZdVi}Y_J`NluutX3*1F)N<!I=jZ>TcjzegdaYM!x>tR
z*-$tW-Bb3L&?|k4MU(+XcGa+?YvNo%_}UyyCT<vtKMJ*C0ga~Tudl+aOhw;vssrHW
zG!;*^A#r2#OIhe5CeFGNZ538{oWd&PsV=o)rq3*8a4f@%ouPHME_1UDEL>cGMwD1K
z)l{?#D`F-WUItc9_;F?dx-p!-T1mBd`0(-q4}{w|ufQ1SGt+Z0mj{b$u&~nVE*2>_
zTwKazR<uT6-42~up#Ijntl3yO1)%=|F~Xb*|LR7?-2U{6pl~4oFd1j;Q;JQ~2@YPB
zd#G?(e}^JqUsZ<)4sS-%Rl`4-yE7=D_(7yM0(*dy$!k^=lp(XhCm1^h0X=5Ya!8((
zjF`uaCBtpvN+RB~9#-Bve7|SJs@N9Ent16pz0I29vvNLx&77SlzOVd|N8!2>{Qr4y
zc;F#Ic!Zb0j<4`{6xOK)HF7jH@+2>LINGTy_&g5r+q}~RomeHMa2TQxAeRBnX|7d#
z#I#%d)RFvi@OdYX-%iP1%C}Xr-3he-xOKt02kfn$U$~<4;EF1-rv63%*!KEK@HXH^
zsV9YtyeV9QFNd6Pxl0N6di_h{_Bf+2<=g#INEHM6<RX*^2m!6}YyB~)6@IDp38~eK
zWqNQmsQ1%Oq*^a*b#?WAKe#l%-h5o#QZMqPq-K`n)%<DJjB)HH^#x}AoQ?CxQfCi%
zy->Nw>zf0s5-~7Jc*6;z1>azt0&fR85Er-5xO=*SDEF|$aK;m=sUKR9`{%5-pwDlC
z^pY(|cx^DRPooW{{&p!7YPq-BhEGX)u7D8R(0x}M@_%2gyVCn=-O_vW`WK|;uS-d9
zxA#FkUhTcq{Em7rYpbsJ!Ru3KLr^>KsP{qrbJgC<4BXv&=#OwJ-=T^F)?IFGkLm$A
z;ZR~L;NqWvG=1kE|J@1wD1Bd_`TZaLq0fEsgm*Ic6O$(^@t>(geP*PuMoHvANNP)(
zcY{RWricZ~2?T+vN(A3d@IHd?Aov#q?<crL@G8M~5`2K*y9kIi^>-6c7lh+uh2@o1
zJ7dX_E9;d*`kRcSzM9<gioQxO7L1|44KAEEpk$W{7jcZBn2J@-vA8iFJ}iP#f$OI(
zqL_+$?<*C@q>iB!H;~(m**p9uNDFYr_u2H)HJ;oo=1~*1t^6ee`vRlpJ{;|!Nx2<p
zpXvr$ps0J@14v&outW4WBmo0>;yAcMeU9f(nr{_Ll)m2Y2>@dU?$M?OW<G|dMeqnR
z|0G(AO;SPNABP(V{EqQLumlDUmwTZ17nTsFS<%KnBJ3dasR=OhU~((5Q$Q*Kos!s?
zg7FKQ_kf70V2x2~L{Ya6u@zcgXm|avs24_|U(&>&aR&=3Bm+5m4Pn8f`L9sFtD3km
z{nnb-f||@`+!Imh$~F5(AL0;2A$a0xRTMhoxwbo@7fdn5fSGr20)_;r+)fuZ0xz`G
z@^o&>0}_TjBHBtGxR3{-cehi@0i!H+Z!U*q5CQ@<&eDr`xstM{u}0aNMwuo>Pdm+=
z{pHvl={v5@0F93%>eU`6F`fDZ0c~*`DZ0pE3a6-BxCJ`l9obg5L;p1b+^{IdJayeX
zGl(8<ov!aCpc+Vb6X4>Bp?e5$Ed__-!E)UMlX-bAhr^#W#@222w0C0{v8lj~@p$RY
z3Ntvjx&R(e(;Voj1gS8r<hbbPz!}iGk|9-lXWtUOR#3PwZfZAv8un*DQ<Cn=IXN-l
z$q_SmkQPSoP$=#JHXK%^QMICu<fwUL#ZhzPI%BWu{287H_O+2?bsEO6Fior9o@%As
zQXYN~y`y*HQjp%SptZ#Vh^9E+hQnw>VCZOl=>o$?%QP*}NgvWj9WHD!?UfxZhc~6D
z2JI2GY*kI$UU<A4dL`V?qr3&ZPCK+d!7{W*PRQ4wwTa%*#LX8^TQZDn9_sI+ODMq(
zx+wqXodgsg<kXLDqiZ(-DTLlbK&`)ifS{Rx&G2rpjL=05;z&S{Ff3!lzdFjf3m2$G
zN#U55uURRIS|y`WIF5msDLEgp3Ek=-vi<=F(^E`Rr$PR>rfl1+ooT>()T&mMZKFXd
zX|dC=r+8uxte9DRoT6#Yw1j~bWyAqH43!&}TRsUFnN}#^0vKo<G%-WSOFUo+gR3Ha
z#CR0w<7BFCn=gJ|MBqd&DK}`>5z}05p%NHs0aHZ(R$4xpgDeV4YP)X?F5l(a!5FiJ
zI1;0xymrTYFfthy#rO96!~x5cp{&St<LQAKE@Xr73-d|#k~#ZSTx##T{t|p_Z=9@Z
zkt!SdFgA5QPq}m3kMmf|S=7<*D=G%c+YmRX62!)KlUJLtnI99jzgB)@8NZBN1Lf_^
ziwEp9SZvtr0FB%*-!f7==wfSg1G$}dVD-tLg}d5HQ6CMIcOmQ@l>%mWg=P9}5BQX%
z(pIzhL|rX4pEnk^y}P^z@elZzeW1M8uYZJB9?{Z$LVsW)^h@FR2K7FS+Jj8VeATh%
zZR?k?g<}n4cNR9z{oZ(}X3Rg1LWzPkR(=R294O;DXl|5aBMSA38ww?;UOj=py#5oY
zZwxBi0GMs&8Her5$Q|-kv(4qha!`y+b94C!;87>xv9H%(5}MOGb>71oL=N8wQwuWu
zjrjRflasjOGCVvi+g)(TA`%$J7q5Nx4}Hf;K{B&KhBkf}xKjTEVn&YXe~FjgjZLj@
zz~x4PXL0m?P2_cv{ymoQ7Lz?sK>kjXzpA5U6v&}S&EkB0Q7<u0xJ%(5pQblUAZ({F
zmJhPNS%wt|gm1hKm)k5x6{-_>c|a(i+eLyT>W(vo<>l49e>qHuq1(sEnT-7ePnyr-
z=vRh!0eq5`W1vT&^bCs++d7LwJVHTnsiBs$lBbU9KF5|(ldH!FMggQYOY*Jd=dbEQ
zzOY)H#Z8GtP*rFmeQx*rnG<`@5QVm2O${BMq2J3kwBT&{N)bjNf4F{ysmU>nBn3gc
zO`g)3{olt*BXX90Ow-UIjIcgN*TV!v=DLX>4uJlFsNQvkd&>>5#keGZQNcMCbT#v*
zNL+jwb@DK`cOoGvJTTAScHy*5AE@8}+~AbKId{R;CR{O;8~+#ywt!yJ6$`e3x29?%
z`OTQ=8Sz3$=^dly@y+$#3Jtxh4f7<}l|PcQDs5;36?}KJ0R$Huui(_EVC9%#=_Z@p
zDt03<Ht_;6Ye0XHE?mxz^aHC4j%!vBfN{_<m?saoIj+fzQzlL|Kw(1ctUqPPbFDZy
z*~T|RQC3_|=#c+y0lwfH$1ugL1I3C;fLe)KTrd>*<u;g?;@&X=NsU~Mp`<k^3D`$W
zIZ?%2?;s~^@!)9hS2k;gH6F_PN?U1_M;O&YP+aH8rIT81T?oOt-%@HVwUtN}n^mjO
z2zi^NhDa?uJw7KuMS`S(OX5(1wbh2gglCa%1wp4{FW$xn1wbEj;KM~RLaf+J6+J#I
zU^?xd`~xM#4upG3@&6`rP8laj+BvqEE6znfU6|tmN(*RtCof~?wsG#+E1$*>dZ9wY
zYN4ciLu{)xTy^chL0QsXQ~w*#r!VJmN~yFJ2Ryc-#I3B^5%mi4?8FBZ?hYjt^yjCp
z7hi3h0VuDaV37WNfc-(9Hks$Q-78t!@1$4k1*NB*&er2RxVuw)yL_M5>dL4G^~Y+x
zKwHDQFOP4P+z#Yna`Z+lw_A2vWwe6&YltDGZ9`A7f_ku*_?CI<k}>p5LT&yE>voz;
zJ^9gl=PKPdhm2JV<|m}5H}#-0?>*A8!Te8pY1t-(H1!yg3;Yj8q1JCd7h2sTXI0Qj
zZ*22<KV_H5!@HbLwFNOOom?IE^`_o(7F&9x$8!9LpH~QaZ9qPpglSAqT+xHJU?qW%
zu^y1x8fRy_(<60nZ(Kc2`?bLBi2iGoC#%<=i(zsqDk=R8>n}J-#np<IVy95*<?Q$?
z6Z0ai?Zhmh?5%#!H_u%Ma&|fGmD=Sk_{<;8hz?cSfE^?wDPzAqf84KcQ;Ci9it_4K
zN=a5ktu~Z^dBdDVXvS<<xtIyoQ@6jj=RaD@d5?C!e{%`Qz3r|tpzxM@ycyrg8Q+|y
zR*ToZ`^_x%Et}@Hw&qyNpg*_Zn1yp4L3#VY751VA;-9D{zBDZGs^$GYm+xVwXc2q~
zc|-dlr`MM$qIwGX52&P!{{dny_=#PUEUS3a_esfBA35nJecq3wFRDL``VRW_je>JE
za^tE~q>3)DK3}RJm8_Nj`MNS<n)1?U`4IEsIqdX-R`d!qa!2@<b$OI78Q;;|F-R5t
zB2}<Vum3+TrEn#^qf%~1uX{PYwpXNzN?T*aS5ac6mYR>fkLz{&;qoJhf7F*MM$7nQ
ziq!uenlf{vxyMDS=r0YxccOgK8MrIusm7F;!_!E625F7l>1yh;@UdU#8dH}b$Cb}J
z+aNE{b%8FFa1kjd{Mns=yV4u97^$9MSuDA3-bI?!UxrLG2+G@~61Pk!C&o*C&Y-m9
z$?~LwkHf%FERsq?XuJ}7yXpEI%6_tZ$q{)8rESMEWY@0+yVPe-dGo7)LshRxNyE;N
z)N|RN_i4U;ImvS;u_}gDZ;!DMl2O@SZI_X|PikMPYP$Y&==D?1c8n5fxW_l_kd|I?
z7&3#M1PX_kS*HW3NZ;t*0rzfkKMnYdv&)mUWZe9Z8<Eq&hyy>8pP`g*aps?Oz+I><
zz?XfZ1u%2ZDU+Jh-1AsBzl(P&Vc73=%v(&?-;gf(m?76|Ubufe@|(FA@#&>bHBh|-
zwyj?F(~ptme-FUM?}Bq)y#Ai}Z~n;2ubyx_uUwv$`@Yum6Xzz+T3046T%2_~Hib@}
zJ3s5GnyAPT$<PxRjjva(>U{BpyXExdv(H<z6O*&nsafmUQ<G1+TV^g#p0g%TO`YX8
z6=t41_w2?=mOvHN;<hjT>X}D>`Y%s-LZ&FDHpISgf{mrPDfVyq^uz0>8!M>7a>mBp
z+^}3^A9-n$EUHg3AMM7T9Qp!X8G;uH76|4DTmp7LKShuu_%(uS1R}IbJ3c_~cMyCR
zfe5nGbjb-y%CPzrs}y1NbM%VrS`%AEvK66L#9NVx#h|jx#9}=8Zn`#!w?D$L&l6My
zU45DriDH)qX-~0xmacOI6jt?lx+tpZ3j{TN@I53`eTYf0>kWOF;0S;lqf%D%wFek>
zkYJPUH$j2%oF|a#i%d`zUgajBoUD?sm3Z>)zhc9#5KtFblWO$@y;V_GTS($2<_l0J
zdY>uqj6p9mIGDzTt2A9(BtbTh(|;E3%p)<rbRtShnP!<`&MPj0%ZCrzymJTE<poPV
zmcY~fv~ammd5w;_9U?n>+RN#TmQ?x)v@I3Fwfa#)J?bXZ%~8u<TY)BF2_J)7t9snX
zO<W4Y<+8}lCGWlF``0{wFq_TlCsCdo;71;A;(CC(iSN>_;hS`8qxTfikF)kxQMAfH
zazcup7B+<2Tq`2J`$GP>i1B)o6^JZ9zDWh^#zl^ner+=$#Pjnqn}v8@o`n(1?1M;;
z(+#5Rjt3qI9EH%{$#8Q3rmY@Pbaz6y??NgH{vo=}QN-0k<)W6k7qK*pQMiwoJ@Tf>
ztOaSi00$uaAI8}>stc$rpeo=IQK+{_YUW_NE`;Ga4^nrcHB8keXDiyp^{o{R71g|_
zuxI_A@s1_;p*=MBQT;IL-{YnVg6E0{5F1192kVsjwSjEVQd6H^)kM7duC>&_<+uhc
znrZUf_<|!O-r$S-L^Iu#i;HGDj~HANrDhtgW@?#L4BCJu`g{#_%-1xxe9IKnKR|^7
zNG&(Yp$wDT0!rlRtWlD1hM;P4obIhtb$I#{1q}+gq5y7u`C0(HyKu}p{u=Hi<=a4m
z$@7ysc8QL=yH3Y_1)(^KC$;umr;k04vAm+=`^e|je4bwS4L^>)fc^;T==HhLzH&d}
z27DcN)Rx-nblkP_>&q}TwFG^lV<yiwDBA6cj(dAm$1S>V;YRy%!&MzO%am5?pOjL_
zW4xnMz*%@Xy|!1l(Mns*A~jK!T53LTEPlq`DO&a2KG)bM8gJJBfM}5Va(nA^+?6te
zIJjYyN7z#4tByT&+*d@$T|aizaqqjQj+<KY{hp?~PRD&f+|ZTP>A1g)`VLlg+}^5=
zOYTw8c8_6oP!E-W_8Z)$Xq@QA(Gt-#B}7&@>_B$)T70+WyT<+06yBZ<?Qvj3+}PNQ
zm(PV%d5@7J--5k(OXar57`zM3_CGtp7h?T3b<Ufp^GJ(FWJwoA#Vi!<p2VxBUtVTn
zDw%~kX<n+|Oq*H-rJe^p_q5Fag#?O3ypce8I_4cf$9v`Ug-<a4$$Dm#7bKcr?{3gF
z*V@s5-#hB@JCD-6OHK2pBX*)2m6IDV{lY#dx9R-12|Qf2S0+>xR3VIywc_p@52FcP
z7!V5;4nz|>>QA7A)^Z!@0yUyrfYYO@Nwmc930;8Bn%JEZ%3H>Z?iTs+0a%tG6aktZ
z1zO-#%IHEFI2QL7DR6cI$KX8eS%5mXTo0aJJbj=92~dJ2p#;@X?DZzQA9`LMwIdEE
zwjW>%(B^^bAA*m4Z4%ti-qPn$f{M?>eqZ<FJWB8jz%kWh*<!IAQc2NEB)!^vN<h3*
zE5E)B6CXMN{x6HJ7p@(SSn32)FAAUNC3evzlwdHo8<Zd+dWmW)^>0ZjdjN;tNhzQN
zUQW?E<M2AHMaW-it6BUR-^a12`Mk0C9s2=KOXO36!SX)8{s;Ln-`rqszfgjN;MPi+
zLH%PGHQFkN1g}=cp6l#Gpaji!{n(u?#q9(9P%z|lS}$5fj&E_AJv*nT3zAq~AENex
zDPbAG4PVhA+zws`KLA|b?BjKB9g{a1g`Jb_Jc88$z5y%zsL%`A66nUgV6g|ZmX9Ii
zxPz7Mw-`rPp+yb(Qh5Yx2t1XAy<s&8m1w)SjJ!#beRAT=llLMVdIdP`F0%3Pw;&t$
zk}O3Gt<jTv$(aAYkdh9Mlqiye48a1yB7ulq?Kq!<k5#0v@xyB(#TEZKNlxD;l4G4(
zz@0^l-~9Y*QXkr>8>tV)BJcYXU4bF|c7YC2d}yQn^4-uN5jAkFXUBc0P+il`&xhD9
z|8E%Ys>EU9bl|(ANcaqR2%iBL%7V{;!~UtC!Kc6}eMCS<s6oWe!JY0VaC+Q9LD3uH
zbQ?6uxFlwScSL*wTB`(XXbC0U8gKyLNwHwkO4zKDpkLg_EtM3E2X#s~uu$?tF?^R=
z5GoQ0-rV8sLLtR=D7zfjL<ED#TPP$_%LlLVxf$PjC@zUZ2^NlXLmQrSn#Fn;d2yjc
z>b4cqNJ41L6YzDHdq6druEVz;2DCzWLXL|6f>_&TVYBN{>}RXjVzib$?`4?Xg!6yk
zNfEY{pH0s73!NSC+(N&$$=zpKc6bLGpUwtW-_JVmQ1@P|`Hvb$2yHO+oV4K;wt?wk
zEBa?_iPHk>3M~uxRJ2(5K%L+l_I;#!sJCiYeYMtVu||H>xA5UZLFgH-B(is){AeND
z>aUD$hA^j4O3plNH2$o2Nv}8`rvCD#y0=o6M2Q3W7ray~lj}w#%pgDI($LmgdW^2b
z(B_7fNa(FLS{TJTHtbYWH<ayJ!h-z#lavtkQ*-S7`BJdDqP;;{J-jqBau_T4s^i$$
zW2Xbww~+p9-8c<8E&SXP+a?w>?lr##>><)Sj2B1H>GCaYb$yT>)6G@srIWYpIR|<G
zv@XQs?Xubi8&+a92kR9}Sx7@$U*B?$jJIy{({EYB58gLlj~~-5qc>D5QNrt1SDCUg
zb?&=B<B<!O0LaONiofSut9Cm*K2I3d9IHK^b<Nu2?;-ByEyxir?n5kn%wO}76N<<M
zM#j+>#usOd@&mqpV6Vu$`&7Gdg`Kw4=W~VRS8L^yBdwM3N%;24nK0J7-{}Qz>Jez<
zU}5vQ!UJ@1EOUEv2f-Eg30GKcrG7z5A&2>nO1T}r?&YMszOTv^R@xdV9z}SimYR>f
zkLr)uW00bGnhbJ2-h07idG#MrT;+RnM};fwD^dG!tbE+*zboa#jVZB5#r}n~M(z<m
z^<(}_J%+GK%TPPU{yhlU$5i<@{WuRF?+14|fM=UsKM!`P*M8g^Th)o3u~j(CL1&xP
zcETU?lYIM@BF~L0lFp#QSqtA%G$83ul~1G2`=s_c(g>~v8Ravy(uu|HRQW7E&kw5`
z)+qV#ZrIv75jQ063xF4$p{h-2f91cs@VQT)N?AgYQ0|1c^2$%BE`Ow+RNaCv&D<od
zL$u(+t~05&h^5HPUBZ8|(XM)E;Q^;jN=!~RBAjgRy*b&9Uk9(SaSTAug2mc64_EfD
zE4L0krj{oJ^$fvN1T@Xmo&D2Qg~*0t%yxojkZ@$`TQIs;nNrx+ODuPeUa_)>y`^Sq
z5!L;Dx#D+jbh($kWs`yBn{3N<g41l;|6Q|+ex7xt2x>Z*nq8!7Klv6Gk|GQE0^9r&
z!F%a_5nyE4<ES~@xGRGX-NHnHTbzZ31?w8vT5k%(;KY$mH8Bup68ULPdwB&PO9s?0
zv$!<D9D!(hs0PvZV$`oPf<_IUCCCw6BN!ow5o|hfxWup~f|^Ds!fptN!sUk8E}rr(
zehuD69z0~?2UUIT!>&}_Q*qC8h4lssekqB%9q$7DDAf4%ei{(=BDNi-2^s^S>L<iq
z9p2vAC44=@4@2>f54^%O!89~MB*E?Tt8;D0OC!YFFg=(_NDIu*T6trr-qb)t1I-M)
zCdw~R@G|InvLXp^C>oSd940<z@bCl&5w_ufqvPX56aTNT0P(s|7+<15TcXEtBOONT
zDY*F&C@8b=;9eX3xug&keq+tRS1KwOds_U9J!F3FDhc1Y@cwU`S`68uE#-E(VADax
z)-kEGAvcs$UZ@~Q|0@;Vap;p6#@h88O`BNV^_xz_;<`mm3g^um{SXrg5}T*@Lj-c~
zMzDt%Oy#ys_3&?<>GTyQ7WDZMy2LX3eQ>#*wd*dm&$9d9ne)Ev^V`huTLiMDzCu^y
z&e1#3vQKj33?j06Wkt3JudIme%k0Dg+f{ez>>d}Z@&A0u*I}x!*%9;n>_{KK>CEr5
zSe6^NEZh>ux6a{CTKuffk{6@@Jsb8z1V2k4)@#95Z_@h)LCuu>7xexO!QT@kIJ9!4
zT=4WG^acrpa1`jeP9P|7jV^Y%ZWyXqL{lzq!&5vA5Hn-%Gvcv$tTnbh7Q%-mBY+*T
zkysR8quvs`6n`|<AB)CDk!pABNbHgL`;4%Zz#cF2o5$9A&+OG~aR~qN1$&5LL*4)k
z<6|L1XoYLyGa|AqWL7`M{_J7vpiS`4A+SyQ+ZaYE{$9;?V~=^IKF$>KoujruPcRvb
Og<IZd)c+oj1^y4!(z0d%
diff --git a/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_real.cpython-39.pyc b/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_real.cpython-39.pyc
deleted file mode 100644
index 522292b7ab34f6c542d593c9c4c6ce0c4e4a528d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 29327
zcmd6Q3v?XUdEU<K?E8TQuy_yzL6D*z(1S0@wk*jaDUy(AQKUjbl7q+|FL4*(g1|0t
zcPNs`Y->x3?YMTTG)`hGuDXz&wsaiVaosk@^>K1ipT<ocS4q<}ow}~=^h8FY<2b3~
zrfOpK`~Exo0wKjpnsZ8U|GjfxbD#hF-Yw;F=?MP)f%hZo(|;0){5~JL|2cR#j8FY-
z04k!arO2%Lwq`APwr6emj?PB$Z7(^?vDugvQBmbQ9hr^aA5k$Ce>$S#<=9iv*+e<1
z5*KVeQ%<SmQx@`BXCqUo4b&bvHDv{fhiYD>Ry{Dsh}J&~0UMwD@u_D4yoew9i3kcu
z&PJ|UEEikRm8yq;<CQOaS0g%(Y=5Qzj^6e7BlWVb?<?&)R-3!HT&{ZcefL+q2QHr5
zccyl(cA?_!d!hNl7hd=!e4c&b(=YtW!TkqspH!9lg~`ftX`#IDY;A6?toNCgJwCTo
zs@Glf;OcT|Y46IFAbwHa&#|ntr4dUf0egDEXbva;)xQT&j&KNP?e|9JEoCYD=_H2G
z44Y#HZ8oOvR%w-a+M124dsJ5C5K5@Gs6JIdD5?6@00udw?p1@Th?uk*Qp1SJs1Y@a
z@2uLU#_*j}+toO}^Xib=p(c>CPwiBbh$*ODYB#?7)pcqQz6aD{b%VMQIR@1cb(5Mx
zsHpB!H>+C^8dA5a+mLHm?N$5mJ)-uj1Na_Q2i5KP-lmSKH>fuv=a_nvx)U+m)tl8_
z_#S^RIzPJulYK16bdBCzR0hNVhB-w1t`7isk*6X}%eQ=c-8R#$nKoqyvHHc8vJR52
zTd7t&*PV(6PQAP|A2<t3wR1uIiAo*o=&BW@T-QvQD{De0P+n&M`tS)SYpvMTqP?vu
z=eX`#|8fX$N{`}Ge*le8R>Sfmi;+dl92WF<JzGU(K|2jw#TrqhMVpR_D~nC{qxw6(
zjin#a_o^gPZIv30G;EYg`}R}JVe)6#)rRB6RQ4(BSmf>RX~d8cZzk3wi%FGJdDZtp
zyAk)}iz&pX7c;7`Eu#9jMHVcT!Cy=be9%F;lZ}KQZ)W{OGq)aD$40|b9#4Ji5x@fC
z@J_sa&^$>CBwiISSo)jNMtN>ntIw^@ukL-usvgEFjV$#yl4_{k$ARV`+EK&@D-SK<
zm<%&iWRC_GN7kJNTiTs(q@53;o_x_~Y<-6rr)MmEzI&vut;4qwH!9_a)#jQa&2306
zO3x$BvGqtJ?I)Yt{q(xE_2?mYYcxl<vF)v=ZD?(4RQFyps;x1z(elymQG3Q({jMKr
zj$@{FuoZZoQDeU4V0B-JAood*^fOU?xawGuDprRdaVNZ;c=LqfcuuPA933^jJu(`3
z3ghbTLVdf}f%ljPA3QH!r*@!y6CaH>GI$DCBWnMqw@03@S35@{YVs-j?~gaK-VMzg
z{fwXG+&>2F>EQ3FDBeDB04=Kj?tawGzaPe@da_-^t`WX5vUn3}+WiC87On5;uFXU5
znp1va-I6^LI~!3+e1N~Ft~;f<(Kd)T*ZL>VmrKg^DodU_S6Zr^(;Lqqb!KA_sg*r1
zzw*lT26L}PE0b^8D6(7(;KkeJ-~T%FA|CMXJBO~?L9*`Ya&^HwA2@5Z+Vabpnl4or
z%9H0xOG|<6oxd6l3M-Y%<)ymoo!901`Pz~S@+IK(uIJctSud2=GUtF*_Rd|oX$r5s
zj5ln2iC~Ce7ytx_H3bffZ#==6?MEIyerD#8Gsj(gyAPju+wqg`qlh_i^vKB<ANa;v
z@l*G&A7VLg?mVzprdpo65cWC9%frf2>598jtCxdaGAfeQy;SkeyXR}VvR12lr6m+W
z6H9BmP7n{SlyqsizV8g^l7O(ny^Hl)HLxzN6}oestt{_bE9}EpcebgUHIZGAK6~P!
zM@}A@IdS@w5VIh5p;lG3pinP+q^J+`Rb>xII40v7#Ovj1y#}0Iua;IYnqH7vLX10C
z(>h4cSE|b8DAskJ)!Q=<1bL-Pmq0}4JWR||IdB*R5o7F}uGQ;-J$D%&5LjJas+Cj_
ztyh+VF@Vcw%iddSp!qW;%#ZhQtzIGH5G3c8Dj@MEKxG!0{6MLyP;-3x$kCb8XM#Z%
zb!#ijHC1+NRZhAWBxX)Lc<{`TV^Xmzm0zjOo!7N$?P7gKR~8n~$q_?Q+_Tb2_x`fy
zo*;V=#LU-FZ~C!HozoXYE6WS|R(3dw)&y}gLHahv4C?Yc*PVM>v&|*xK&E_gsjS^_
zKx!-HYA`%^QDY^0?xmW(;HnBosiv<4j?Ai~E-tUgY`Lc&=4=H!F(4TAi@IJ$Jqji2
z7~r|G>(yK?{;8ZM7Zqf6*}JH#?h~b@i{-$o1Xd}q&IQ(@KEY&%vlZC79HdRgqZqWn
zL5B_mjw*S`Ja1?!nJpo#!H_J}8E)UBvVEmfB;|E(iiqP_Su1Hd_{-yywUbsFp{!N3
z_SkvAyj`#cq9#Y$8hgdD4U8j&c_$IW8q&;*Kl?A!PTm?o4Fzl5idz#%8%BK5+G|f*
zw<6?NYrDG1`<5-76RwRd#6PkKWHW@;bF%|?MSN@BdOX^&8_|XX3T1)j?*`6}`p$gx
zEb$}wfNh|ti_wt?X?D}`V<Gj~s_Z+u%EnX_yg*#a#92l;A#RMFnhBy;N|!c1fmU3N
zP79_KFck~>>*ewV_dZOhd!p)M2bA?g$QC%uCEz@Jr4r<2dS`0xQCSRA2{Z47`BL3;
zfg|R);@s0!{V2=D&5{X{t@VQPvbn;wbYM2u^SpWPQ|<OVg6f2Si(pm;Fr#Vt=B%v^
zb<ah+#PrOmIj*ACzX=5T@sTOmjR_$EYFZ)jGsAxvanVIb*sr|^V;Gh5K@49ti>G+&
zNntWvdw~NSQxQztyo!U2M-~$q)|wn8Ocf)IWcA~ig0z<b+Z0m?VFIFyIfjr2QcODr
zp;0hUD93b6JD#jY0wwd;mQ?oO7Nr?G!cr+Ov)HHX?GcnN_>hW(q3KsCd0W3qgS;bk
zG_oDJu}qR|c>_X$pPsi>R$yFUPGDXjQdQq*#N-G=$Q6c=GYlbj7-ES+yMzg`gbA^P
z39*EP7Wx{Q1+bpaTaB!5gI)V1o(A~j4S0h-*jJSp0poHEtk2v3M<eIwRQv+jq;aq4
zbNo}=k;*x6hrD4t51~Z<f`c5kSHx(Hs61oWZDD?fP%0@ihH{8cvTl?et?nU9gW40L
zk!|>E<o&#vD|ZMXnMZdNCBn6W`rn7AZOt(-zf!+2RNI^5Dr6TWZ3t<j!n#KmnKsOg
zS@jXSqe<qAjM^mGPJA;jRuJH>HY2)YaW`1sKE!qBH>@RF)5?z+DH-PX1{B5t4E#W&
zfYo>%a_$uN(O&%yp7*RnlEx@T>klg+jKzq1J>U&$8{_nT??yF72CJGyXggEL*n@44
zo4*Bn-m$^l5n(lNGP?<ESrn+vY4kS+8iS3Z->+g)+j}4~-0TndgItsIHe2Cal4j}8
zhq({}Ya3ntIN00IV_yy6GlbcXV=m*E%LL|<s{zl_s~fNm2K>06!>YzE0;(MFhr(yf
z6rP7r+rRb0GTS4KVLz-7;St|?#?kNcM^JZELc|KUNIynY5;+EauB32RqP9_g5G(0c
ze{|iFQD%y~SK@E$iI?{;$gXzx`u(VBkfX*iI~&338*Z&WvmWuD&MqAn9Ko3Oer;@1
zwm*WmZYy$c=hY6h8@rw{=DQUmHK7K^j<(Y`#}dbkB9AG(ITd{$6AUs2oU*-zQ~d3~
z7~F5;t+u0Fd*>kB+7mJTQ9S+syC){L+!OItr)wSWTaUC?@ecH+wKB|lj_VI&<^8`}
z8`u^84z7!EEn=M`^tZYK{U~-5dl2@kcUMD@+IebPlV%24GWivV2IL8#0&%Kf)MpIu
zAR^PZz}*Ce2acb(|A86z^t5~Q$V11^90>}?P9QJ?X$5ij9XWY&<EQ`Zl~*?YJ*2LU
zH$cqVxBvy$#^=E*ZCu9V)q=ckgY41<8OjZ+RW?Y*H>fJvpct`1W`83^K+Vnu`Kk@-
z$uz~LPMf5vs|oqiSD3U$@S_AiL4)A$5Kt7<*AY<s(qu069)jx$o&<O~c6R3U!@FKi
zPoonjPTjw2*RCMn>F%lHM<3MhM!t;#z|{UAwp4?#8T3<90dMVs<APerU6~L1G*pD(
z^sDZE!(p*cdOtugB;wiG+Pv4&2A*AlFp-D+aB->Rl?|UGoW2ME`WQ<kt2MpM7ih)D
z7})6Q!63OZ4<@Uo6_i-38e%Kc9H!|95!AQyISOe?zm=h+5f4k%AX++Chs;x3xl*qz
zRD+ZxP<qw}Sni;dTe)&Bh^aEf5q&RX4-p(DI6`nA0U0fQ93aTfgQKie7u=b09kS8t
zMey<7l}_~ZQfWa1LVc3i2iiOwM`@ZeVGu8^KyFdNkc?}kE<ym9(B+jS2#i5=u2!cc
zR}In>N5Y=>E2LrYg`qvWQZB2h3E@}ug1iScmX^FL?)|mW(uH!F+P{90g->$wDr-i3
zN<C55SKMhZxItmQqQU1b)oK?YMN|4pK2g$@5){C*`A|FxC8F7^XdGHkg%$_gl9&Q(
zE=U;}oZ@({9dzsERZVs`urBK#!MCQOT=;2yp3x4g(m%||3ZE0^#Rs6Y4W+UJTz4X?
zQFDo1t0CuF#{%m(R>#uH`BIROFIPe7$zj>Vck4OSdk&xaZv#ZYW2Yev<(({~Ax9*s
zVf$X|jn;mVjt*J}zwbCn$VJ1*H(>3t#)Lno6jl%^Di4Wkx5!<Dl&kV0Q&C1sq6AA#
zK&qkySHNEprT8wsH;Ly_YxI>Qr8sN1wSu_3H44e@)#^qGyf2OXrd-xeL(XeGvHrCk
zTS^QqnWsl`BWh@i4RIhGL6?66u>s{1&~?Ur8?1CfU{YX8Aau8gN2<yQ>x>*>2)V)#
za)u$~4nr)FZI>`1mM|ffFd>$Z5SZ;K*y|kFYsa_r*AdP$>?OUF?-=F^Z1xk7JYy;e
z=E&0HluiAZu*A?8t0Y-fH;quAgrF~MX2DJ-!R%Re=vp~eZVoW73XIbaczvorWGDL+
zNl!jxCzDG4y4lG#D`DwRBWI!6FYI+XWTBz|ZL`-*Bg>rWO6#!K#U|6p#*KvRbsAx#
z=iSz3uZxRgV6Q1gbmuqhH8r8F{4Mr6%<n;W9kLSWFZ989Q^;O}DQ}L4Eb$IuB~p!a
zBLm?kB}}hfe+NR8`(N@i9Ao_wSYI2u@t4{;o;Eqaihs}Kkr3M-g;p1WB3RO#pYe7!
zC;eQTC7s~hK*!4XT|M!bsgX#?lJ3S#v!3g~T650O4y=N0&sz%+gBGJMMTO|%ZZppk
zLTNR&AeIQ|iIex>4Qe|C-8?kAX<-OQDT2wWh%8>Oz=EQV>#0!|q4;_=(Gorz5K;X+
zSl2!<vpbtNsGSh43^R)qGPSLkRt;@tR?;`ewwc*2Q^Cv@@Rl3>0+`u;Z|CAo6oLJI
zFtY<_`#`JhsC6n712cZgPg4*iQwG+kYlVU}nhaSZy#xfImjPz9X0T2`g|hYtE)q~t
zr7sas_oJUCAloFYh5j*y-b3)3e8|<PsaEPHSTHK;e{)FJSa^lty#%WS?*@S4^~V{0
zKf&K4Xfqr71AP1-!EFTO2lP)7e2Cy@37#eRIf9Q8kOS*50s6yy{0PBTEC5-PO?-e*
zcl|LYJVQWsMK=l737!WCQqbmlrMabYUHB5itNbLB{{aEH7-4;;1`Q$qI3unSe1hN?
z2uM8jCkeJ7pZXV>@L7U?MnDp(KTYsU1fL=J=LDjG6hLk?7`Rk|R=r0z_B)8LR{`1-
z60|KIrN&TbA}D26s9RPjS%>DmChY=^n`l$3Nj#B4w&@;fT$|{oUe`8D*x()`C_BKO
zkRz+l0zh>Y^&+qmQZ4X+;QuTHaSN)Bn9sOTTC}Jf^J9b;96jmB8?n_%-)h?UM5&1s
z@f{XPqm^9E`p{ZV*~a!V6*DX99=t7xuGCh77_{}}%lbJ)w6umXQQilA)r-sLprEeJ
z8*Tn<O5)vW`I5}#Y;mb}37n&(l8c@vOK3QtO|z7RwZJQfR)HKgNuAp)0?s0fPg1yv
zxNr_@qury}U94y5C{l3vWavJC3eow9pmHc}8!-$Mh5{%!;XBZyB^U-mxAa~=)<{*8
zFkV831aoCV(EPNr8z~h<?xb@3coC==guv48G*68*O2mBV4{2KT<C1FY%NY7J>d)|9
zkk_FF(LZCtASJOzu9}681cq53gdM~nL4A(?4CC;=oS#I=G;~-|=#KDK)M_JN0VVr^
z;NsK(pa%n36odE_@fq^%MUnuho~i930RTxrtBDd99F@W!^o1XcK|8Wl&Mb05zt@wq
zxMj{hzW}|+Ku^vg&+!LCZI89`emoU>o~YHs7+|qwPgM?MP6U@$4lzVlZR0xtf-BcD
z3%XpcPR>`#OG@uxGzGz#DJO{PT1D?<%p}1s0Ib=&7=9B0g-m@X0g>X=ut7J%b)R4?
z@k?M`(fi~Z%JItw0{aR+mk!L@=NUeTID9T0#4|pZZ=bcV;ByI|^S95&xyId<@Ovrz
zu5Ly<JB)=<j4W=Y4g-@bU2>&M+tB6u184!Y;<Ru8|A5yuwhrjX5$3u#7LvGab^xEv
zAJa^_W8Ex&Yg663;x*sh!zEKpCwY)<@-kYbEO;VdVG+ZCi~NFkFIbDVsMf!Qr3JhJ
z{{vC5s~G$b9N$@_`Y{2Y1gO)v?1T$xJqxedPK7F)3s7aT1Zf#b5eJb|1@1b7O=+tf
zmNYHISm3_1ZtE|h{tW#Wz@Li815}TZ#auH_{><xZ78E#xFs`3*R%G=nt847X>Kgdt
zKk%cnh5rDh=y~8U_i9%B7mC6)vUmN$CxbgP9H*(VIRak+Acy9tpF}IEVA_V(K&^)N
zS>TF~nYtxpYDYe#z5Wq_r^Yqoad@loz`MTaJI!!J!gjV>#hz6i_8m&t_XD%r>^sVQ
zRSk-wce@X((&Ip8FdL+WPl^kd6v3t}8VX?aE+beJdX!)r!8n2J!y6d75nx6?1$foj
zv>Ai=GAu=vs_gBn(T!J_9}CL>sGwqVwWXTgILIh!X!HcYl+$I<lI&CCh7ffapF5cO
z4Fqo_*tFX}&$wSFcryVJLXa|BZmFyS8wO#=t3V?Wt1m%i5JYS9^TK2WBe2sew_A1K
zq6-)5`Yo)8iV%I6V4C250`6QfjOw!pO=S#@`FVtcsHaOo+z>{S<#Ws|_=gl+KTdF-
zKscCQI;j645{?o%Jc<CxpD~CU1E=$safgC+i#?&39mn4o&;|3+sFsE4vt<r#<)(4e
zh~;^R-@R<ewJMt)UnhD-weiDc+xR+x&~)R7&o$ty#@(^l!V_OdxP>Rge+_s7d%TS&
zI;6?q4L%zq7qJGPs5%FhP&wcUT0>J@w+O|7CuXs0VIBgW0I36>0PzN%_#M=r!Cobv
zfKS{9VeHt1C;H890qzhyp)w5z`{iq^Ph!7V?7`}j)h~y=7=TRo=ct{M?qImfih>Pb
z;%N>GJ|O-Y!CPTaz`I_5?*_5wVVV@Y(%ruYw(1}D@Hsz*eJ?!>Thw_wF%vv$!kvwt
zVgH}=9ZkbgSBh^p$AlFS6V7VE+fH7xkpVWe7HqI0nb7QZCv4IP(~PF_kOhH9ObF6o
z8uC{La#^k`4^qQC2r&=jL*{`2&d*8BH0aUS=?sjq^l_9oirSoBb4)p}qJoi(+-9UW
z6W)xSru7esp7fWnfQ`uZ8w`D!;41_j)blq-H2OcV#uo^Fi{Q5j{v*LA>>(In3lt*U
zwP9HL3^MsoC=Fe3=*yyi7mu*HpD*h&xe4+ho3TqTVhOCO2urh>c{s0luBZ!V3-qpm
z`v@%pTPhGghWD%x|12Ru{1ZB7SVjBQagYP%1uP1LDB>W(4hC|7lZdel4x%V!l!c5-
zTYK=Aw8o%->{abuqyOz;TGD7*8!2I*W22L|mkp0)C8`T@%C>KNbiAbC6I(Zm3~V@B
zS;c-%bx2$}l0UAX9urlEgisC&JgQ~H;SIJwEdK;;?__w8nxv&vtk13dP}Q0IKy}QK
zGC7E}m>(YpiZ!d5Znp#|k1b&-cnn0bx#4Rt2&YU!@d%(IE-NaI9F#3lx^4O~TK|Fk
zQrIj>h0Qo`gLr37>02-?dN;uo!3x29304V&!EPcH3|#AtCW?ruJ|pNq#O!+sDCh?X
zD50S#uj_jl^&ml$@ll#2*7<lh!A}x=h~O9jrT(dO7xVpVjQtgWAQ!qvoiA4w&U-;$
zOZ!a;ae4VbkO|WnA7sOa`uWQIHF*!}PqEfbG?X;?6I{b0LiSOGJPD=uZmhu@u+qk@
zwOhKG<-fv1sz<6EMAhw;$9G_UAd&-;^id&r7-3m+f{cZ!lV{r1!7P)QEeh+q7;nU@
z3Qq~FCHMf~lMdHn0_xBxawoui$_ijS)M_!T6A+!A5?GgvX_w%tz?$KD4NGAC`EV-A
zVErNns%2T_ITG9=*K^v-H~YHtfY4?njt;W)pUIIN%XcQ+BRPc-@)Z0`xaM<yR%M0N
zoKQIjh`-TS-S6j+ryowHW~a1D4YW&vI3UM>$pI<1kNNTGgTDtX-11JSF5A1LfV4rV
z8!haT0-@}Ze&j3C`5CE0kW2I74jF)zw_o*#J7nOL;cPjF#$31up^fLUdW^a7W+rU4
ze=yIHN)xP*{x8fD5WJJ11^@~3R)#x#>H$2?7O<>(TqC>Gb5y$pMLHM4bx=GPTRw}1
z(LB;lZ@DJ|-_KouA<ih~WDjz0>i<g6B={=9I)Q9N*~792jlL~Nh}a4j%A5q?OuBTj
z?w%JFTwtvkGu2}(Pu<&Wx<z>o9_R`mG}^iLR@VQG1-?e`-wD1>@C|}3_T_8(BI0%4
z7ctItj%5TqEsaeDd?%ersL{-)2#RqNK53hKc{ld*X!Hl(%X~x6Ugm_g_wru6`x$GE
z>Upj@dRGuDM?D)VGrH)TUE>()A*+rIuM_TAFuCE*kPu46Oi6>?fvv&*B8DQZ*$b_F
ziB0+7nT8l>rI-(lAk*W_1Jjb(5e^yT_Krv*4H_qv#EyWXyX{G4gZ?0%I|1!vzcOaG
z!z=->DU^!Q0Umt>qZP&^!kFx1k?OD$@v_fYS9VjNrW?3QpN;GXQ!a{iDh#uy^sDZe
z&OYhqYT!Q6=KyBlZz{XxXBpF3!fmRuc^z{Jcc@B`I(~Vsyh2yS&~bG(A?957aw~9U
z*7%*mej7SQ8~@qa{*aKso=sO+_!z(X>OIf5ev*I$YZHA51`Wlajh?DcX}~wRcE^yJ
zJX66QxAtPCnl(Cx^*c-sXXmw4r>BlNhvmb6d3>kv5yi}BQ9Fbb3idI4;?NF4FA3@Z
zpFtcaxykT9pcpVxAq>SYcp=gO97N%|1QySF@b7{C@loJua3_mSF>+@+K7(TjdZm-{
zCZu>V@!N}CaOzKrPv3cud`u#Ymz4#59yO2&wBb{DZ<6mtyK+!LKz|7%RYDZi&!MHS
z-Kv*5@9u3C#GYg)t*2esuXMYG!4hi};?8W?G90Wy${S4_yaA#8q{pO)k<iBaG0-|{
ze4%Js$LX}y&%v7p2TahyjEq~WrQzsd6tkd!&OK1RtK{?CO<9-Th*|h*iau3DP1XTp
zCnzFJPc&vK3N7HuFm<5-@=k`h9~NHvp<kZ(u|HZrw6OY?^nd=?%5NTe@ken4;2UdC
zAJYE<=`%Rgl&q;5T<YQ49TV1Psy|4GtHC2HU0j;%pfc<IX@a)`1hILzInW^jz7epD
zciexB>quJSk6wYPCg_v!bOqiN#--)QSpgg(WNEh=kTK7O1A~JC5>dh{(_5c@Dj8|l
z8F3(=${UuCJ19t&=HRZ5oIxJ`_3r8U`OrNkNY}mc%1Lt~BVDhkau`nPn(=*~&CIcd
zr^LVjm60?zZhVe|EKX;@$bd5vEAV^CwV&v?F`H^t0Y<Vkk249b6yCIn^*kE*r`)uM
z5io*IOS?*qEIbt;mJHxS-7CB&M6_Y-IO5>vV2>gXEj%PtYh&G8yVujYN1YPGyAt@w
zuKpbau#QBHu?UqN4@|j?f%?<sItxAW^g9raHR;7{4v+~4s6Zh^;$aH8LOB<QZ}5`}
z$`W{f&f_Ts1qpCJK3P$$u-?>S4hC~_t=*|cQH#T95Z!6`$FqdO+;9L$%Wf`+!UBo~
z@SJ$d>SwU(MWMp8c`aE)R=wbUVYjYBSGRJ}dhyp!l)m=+CyyS|??aBMfi5QLHWuk+
zs>FxsuP|S7WDS>eJ3}IWpG4?t%q-0BGgfqxVq<)SkDKI&ZzFEV#?rU}0rDdJqg+cZ
z8eCxDq*4&47;^xD%V2G=djW*|w1?a1nfWiHUW^|P3Q%Ua4`EVx$f=<lgwuqAJ)B>r
z;Fv*IiX0}W!WWK*0Rbr^q{#6@Ee0MRPie#dGY4kL%yoBO&NZA^!o01S4)c0hmFk*l
zx`BvVd-ZwDa0IxI>l5D0JUSTSxpX&gz13u6p@dwvOmzQWV`lHepc&t^&diFuDZB*d
zSOe`|Gn?N-!W3t75`hSG6r4q9BQT2-GK++a;ViF>Tr-abul4NWEGpvKmR&rCx-k~e
zKX5MVcgnFx%f%SM2FK%|H+kS1L)RJbi;So1%mU~G!cZI1r9?lDaC_$>jGd{V(32gW
zfqD|O9_~dcfj9+=a9UtSAX3{7L#+^UwOxl=A><C@SRyBGN2oIlv4n|3-C>9&Bo1yz
zI5{D%Mp3K~cyWt?2-v{OdO06Glr{5xaJoDS({VNueGFEO=uphcY4sX3^9kv9LD4%t
zAFgrsy=BzFp-M&hu5WW%^)h1nje-`Tq8bnxHLnI`RS)r5Lj7>-G44U#xr>s!ubmsu
z-MP`jmUEC%^M-0*@EHMSdo<(}wuwbKNpngggS7N2l*Wse95RLM4h~>A<N%(;dg*c)
z5!6WiMCH3&vzn=O0*~M#!4m|R0AOE`>{l4!6Eq0kMeuF{se70q)D<!tTjAb4l7;WP
zo*~(Z+l{jb3IAq~kzV8*q?hG9{=@Vg?YNK#nwAauQ>>!1ArJDAlJ0B!m>On=W)J<3
z0H(4AO<!fiCkTE4Ah0fMb`E)zY2-4D8Aa?Xy?8tr>~T3Y8~bizf(KA60xXaeheeWh
z{Pnnh7)KA<SGrw3i0{+ku`@CWbertu>bH2~i+oum^7TVK<RFSDeguVZ_B|kPu+x?Y
zMN-lDZM2{6CW{~`VBqh>h75CoRSx5Yfj`qp0i%o*-$s6%E;C`tQ}ykPq+w~o<AbYY
zDr%U=HZO??VmSRy0Qf^!mMh*S5*ZB8b(x;FU;;yqt;CUKxrj9>%#$!iJuWZT7U~gs
zxPsdv&l0MGSV8**-8QH?gNKWvP)vJ}5pg0-8#WoJPC+^oM5-Q6%R|Vv07LaHwqtm#
zi=wc?i9C!lAcZI7f>)&o!4#G<aMnmE3Q6yigJQxoH8NGI>~a*W7Bl);*lTEEg^PxH
zmzVcJENSa#)lSC^KL>&8i+(PAOIKS$qD$2+lyaeZS&n**Ku96;1)B-fx@aMkVN!P=
z;vu|?Jq5}wsM%oM(QowoRL+D{9u6UWDvDX}L)+*c0)%PEvj{hD!-yH1m(v(PjwpQR
z2K+uj*iE>Cw8j(61V>awMOA>4=TNrcAQTP)<>6W~h_Rt9xrsL9Fb|B0O3Y^v14T4q
z`jP%E9Ed<`MsdInPhUYAdcfR>O^31HVhN}tO-iI$#L=Gugta32iG2Qm5zc^86?$N6
zwq;0=F4$R@bJ2&p1)Lb{Q&6wBXZuaqRPb#ysnK?7ILl!w>!Y}bBSf?uhAd3sl&C3!
z{z@n^nZvbEH5l^|QS0=^EBLp75dMAlP#E~up^d|MHb{C1p$(`C4Qied+&nd^$x{ld
z3$i#9;GV0gE8_W2k>5DX$@cqWJ~jbb9_y5o+$;2g2?`>|xSbeL_XWeowf$sy!97uT
z&rqfA;(_`_oNJccOJy8PFqI5-VsV%mStZ&a0HFczXA8bVAV@t)YVjw0{8NJeN$@>@
z(61&)P)QH1eUQLGZ(Vv~N&h*sbhzwa<YS7p{2oF<7Wr{lVy?`~S7yhyZo7LL-R<kN
z)HFR_sluV(3#?`Rzgblw^gcU1?;fRiSl2I`H<ahfQ0~YX01XB;qW^+z_&-d{bmYzh
zn{nE<YPqC42c&Du)rxCKUWKUbZ76TNb&ToiR$&<_8zyb4D;>y%S?dlVDn&e{<wL=t
zB|Y-&s{@H~05QYJJs~wrSQGZz_1$detF~i~LPYD|ZUow*<R?H&u<F5riYFii3mdB&
zVg;B`9))#q912HW8(>v{U1&(4_cwNLox%t38SLHD3Ti#1CZO_p)THWJ#;GJ!D@l(6
z1X$7{FsQk?Li;KOJoja}wc@1YfMSI?RGRVFV=2Mi^qi)1o1;IAx24gdO#5(dTEP>Z
zPslCGMeazv52_<#I^3oL-ya3%INXH-QKX4`Pv94gJkQwG&-m$1tKiF=$5S6H&KwDt
z4a9(e1FCq25dio(OY9Fb151T1W$jQ2p?0Xr3NR}0s3INv3XOi)qa&_(003*IRaWq7
z5x7&{gEkg``3G1tY9-bh6xGvD_=7ZMsD9$(?iTj8i>;y$wcz~$%t#Ss&_ZBJ87s6R
z0Hz)mW!iA~wjt00GPrmvjzD&gulOZ|Z0X24Zu4l2LTrfR=wZAuh7bJP4cm(FxIgX>
ziOu~x6zS;a1ci4NZNOnU_zU2c5`>D434f%q(;vr}(8qkyFRCKmJAoLQdTboOEUJFY
zwuznax1q;45DOoJ34bTt19zaMcoWrZh}|BxZrr@lAC;rhlm3Lt$}CROD2oz1Wp?m{
zFlxuVZ1cBE-P`$Qyki``_P3%I_~v7+OyC5;b~%ke!4&h=+=&C55T(I2?82c@86~tp
zeAwWH|FAzy!3-g;{YixzW7?~6QjLaBTt#Wr6II(nUy>ml>5U6b>Dmizio?`wJ2hNg
zt<=uSRy5X*oeBmYJaP0v9Il%_dtA=l@r>Qf^pqnfob*4&=GOm&;O7ZMe@+6l@S9)e
zzn2az7=_`74lPiV_(Q+^saD{*LsOP65bG70e_sAMhc`Zpr(jfGdgRz!A2~bo(D73<
z?y(bRXO5gYdR(VjLS%*vL!@Fty4H4fFy@&uQ1V46&!N>b=i1cdTY??tPAvDq3iNT)
zHGQcBwRoEhiGzJGzEz%+q)MPvQ?|4hdQNbZOH;dziFuAK5DDaYhCWL0(*!~%n+&ZJ
zkgx{M1-xA7lKvS+j<XMMm6W-1Wl87x+(#fI)6dWVL5Jkg=h)D_4#^Xp!cVY5A%3LQ
z!4POrW&VmQr`1|UvIDq1h8MJiSe%FdgjAw4<cNbj+ZW!}!JA1yhTxR3;4&JzQ1)BG
zGVil_c&6cE6>+OZiZRU<**+nroJl>%$2OG>aDt4d6;bzWdLEwC=Y@_5$<;&5Izli^
zFiJoIs_BFp<fLimu9ztk5;~hC3q<y0w*Q*hM^sx~8!m|19oOXQULzgytGDBgvzbl>
z2hi~Hl2_upc&|(SB2FuAlH&ICl0zAHoGH|`61@6(r3pLm5mIPw8hZ4DKEJ{nLWUvU
z-3gjJOz}?s3UGfLu$)(Hd>qSuC2o%iO(qS86AN-unFqm;IS5~|ZTtH?IR{5O<ly->
zxY!|tKfHld-cWVY^<mV44?=Kii5hpjxLrU#5hQIo-+|hrj<x--PVcwwV2|*~HI>AN
z0%ZL?7}L(RagFHvATW{S3u7C5A6Nm`MqE^oZ<InNSOM{Xhgj&;PpY`d19kygB`^Sv
z_UMNK0`9?so2U1a3~$bdQZbbx!c*xxh3&>$;(QAn>k=MV1raZPwUI=;ScdQoc7TW&
zWkF6!pcVXD8Y!f}3JIbEcVN8D(Z7f5v(h4{q@js{?uh6eEJ+_)@?E55szp2@Cy1n@
zaQlSh&7dB5ufsw~+h;<^&NV$oiHA-980z$pPhvBAeDzt3%d@R8>J(Sw?*1CIzIjBZ
z3a5J1gyhzUGX^<Ll<`poI5c|NLE$$Bz6VAkKSCckrhr<(2mmS-#Q^Mw0gV*PL5J^w
zM2CE6HU|zgUCxe0t5C$Yo<SqgQC@oiBO%<6pn#Klr;<kfG%Xl?<0VKhGkOPB%tjUM
zfthI-mnnf$exlrq&;$p=t^O9Gh`21t%i;GGU@uA*hBd}nU~w-qPggCYIgl+(<}@fQ
z*Ph@3;x>NzWA4-fk|N-M>8@&=#h^mJyL(S;iDmcRBl?y4y?Zy;DvGa-{eO<`O>az~
za_Mi-f8Xh2KkUw&I631UnQ<RGa`Hj_M@;+*!8-{^Bwx;Tb!hS}dLKGH)fc4jlCn!(
zzR;l{Wp3AUajQ~x?!397i`Q)#vLqJlj@(4j5e$i#*L8`Li_^(-L1A;W7LvoI(h;><
ziZITAzLDTf1h)~qf#41TqVO*KDrlWjkU^rbfttIR^z{gRn<IY=iBNr|iJj;eMe9})
z>KeiUe2kroVXD4}->3j1b}ImL32u~pz4s_>Ia@t0dqh`lX$AL$kSFI_rul=aZx}gn
zssLtVkydQ(gGCy(;)@BoR&dSBse;xsTp+qqVZdk~Y>8GM1;+=36Z+LUp%Og}6oc?z
ztL7{`b>Rk5|25pY$USR`-zrRM;110#j7X&e5^q=xLqHl~sBaxSK73$#9v69V)_?};
zxXJ+mo0tJXWTPrYO&!t>hVe$8IH-zn6@zvUDJR2}At`ZE4I>2_K^Vd3!;}$8nU|Ca
zNqJY8GAi$Vm(=Dc>d|8K?NtL3|6#SQk*JP_abr@;C)IYuF>eB<2iziQdOkHCntMjL
z)J6%m5sVY;Ah?0xMgVh2z<A+uUrx)81B?2vnFH$}vhe|iCJ@~C0z!d9ZN<iKGZ+&U
z#m1A&upb$M1b7?rmm6PYa`Ieh?gAgD3SH);JDB?o1aBmG6G5KfPJ(Fy>VNet!TkiE
zC%BtHyghyup{b-nWU~8CFzIsua^aafo9p5I%tn+gDQK^Q`-aQBO^t@~o&q~Sw(EDH
zI;x<H_@isA^OwmO_yicDI<B)kZNCS*jaF#$M-GI#g|*$?xN_^)^l%Iryp3D%C_Z&1
z+Yej<ZNX3!Knx>wh3Zc(Hp5aE{=kCz^{|i0#}++|Wg*`Xi4B(!1?SM>7-;E=2`3OY
zoB--_gcC?KpqwKQaV+xo-+-S;$RXf6VK@P_0A5SN2~hP(KM~MwY||v(oW`3!4Q?V;
z9l#SrPW=UJniLkK;S}<Sc>(EZq{qM;*!pWoC*NSfs}VLNKPG&Ga2MbUz!TsYfe}Wp
z>8orz>20_Hjr#JS@B#O(YHWY3S12SImgDjqvrf-j%0)9i;{DMd+J0!nnVHsiVcfT(
zKl*+qh{$z<A<`dxoZu+I-2m`*x`$!0wmitt7W78TLAt|C-HM=ur&Kxw<#o}L&ay6U
zh3)U*qUMH+$buaQlWeZ;Gla&F6A~0z>>w*<Sqnk22duTp?ghQo3wk&!3aV{OK@qRU
zgkvGYg(8ZxC}3~#iFpwZXI`nzehRnnLQ8=IlVUqCtZ>|ek(~=3%PxU%oGdF{yq`iG
zmLJ@PpzGkF&$I_qo-E-9<Ul4|VhtlJ=M^#v2Q!Tx-?(;*mMX)u`x1id6}T~+^oM2*
zImP8@9vZbD=<VPD;qHyRxmJF03&g1kv=(*;^a|UCnsh8hxV<Xa32?G2ixJ3T1X}I>
zQrK>?%QHCFVYK%5;`%n~@p<Zl8Xl&lWu#1O3^C7|c86MfADbwx=p4!ML@tadN>ig4
z=6JT9qm>$tRjd7A&Cy0U)#DH?#!;N%Uv0!VUaY%q3z)!h*uK{M!s<~#t(><)%W0)G
zdqe<e39h{ZTHrNudpRbY0ceks9Gh0SJw~5tj}Z^<nlZ9*=B9UynAST+HeBCiZe;{)
z{h>~Y_P8)lcd1Qd0tf%sKN|Mxechvhu>_w1;R~zNEeQI7MdQst1`E%Ag#W&ANXV_Z
z7fPt`izeaU9oqOl<N~8)%5)0+$suFC2yZL+{X=3kh96o?*Dm5lpTtG@Tk$%eIPQFf
zub9!EC~QsRmb{0ENK`grNgzk8b8X}#HidP@iC#t&EL6@+^&6E*%(!S)n1dW`kcHZ*
zqt_o}MWWQWl_61O<lDLgF?$##$6v4l>K%*$z&bOV?`SJrUh`|6+^ljiIuU4LTn$O*
z9Yk&KLN(o9VwCf#OE4TWx#&?KHTr}xZWO$-W7DmKS_Ntb;&4|c|2%<Q=EfZA5JsU%
zAW~#4M1`H)un$0Qz&cyGAt7z+Mv|hpX|+*5gdX;o2PoLIQKXG706_@DZ43lNj1V}@
z?U!;?4@<|!)`%LEhvgJA6q<%lm{f5#Mbx!f4_+$p?1UF-xY>A{BL&)dh^aUQ;1hY^
zLJ*iQ(|%6S<}tay(0EtIpq?*qONS^E$Mt|jV6cSbjstoU%0rc&g!;IEn#0YH1HEC(
z@curCy0}0k?2+8LDJKGOBVjvM{IOdn1bF@dYJ1ai+)F6vve0h7i9x$V<1A7{fp7E+
z(_qU}Yza_D4z~{4^>G?9g+V@tH1yJ>wfp-i^tTTNffNvE6TPLf+0ySvDISR%Wd9|m
zi5Q}A6Pre!ubDoF$n=XChjtG1044D*pkFEdHAzKTNojena<;Y}(?K()9MR6Wf}dHe
z1{@jIOj8E>XnF^~wC>)ChJ{>J%#>F64Tocqw?EhJtF1o}_SJ@0Pj6qD*6s=V@!G9T
z17UxaeMLWQ{cD}N_~sDJesxoQd6<nitr-{zC<^uqT1-Lh4v%$FabvGQ5aE^)JlMf}
zg5hR#mMN4v;E*UP=w}c%CfpxqsAD4)<*=Z<9{}6^B+It3-Bi(t7|$biHXYukOFd|G
zp|ldD>RxTdJq=E17QvDhb!<oBD=1N)Wjl!p@$(P7W`kGBwb7xTVX9~Z1P9XosUKyW
zV8cza%EWxP?Gx(hl30gZiq?-42d<$8$}H4@5_<t>3I3uFY)f-suprXxyMVpuAVbW?
z8^JbXxCzKK#CdS2fnJp9{7||v7}KONOxY)Cuie~@{a(AdJ+cqgaBbBC1RZ)wT2Qg!
z3IK}R*qxAJC2Ul0dw^-EhNJ3jKbQ?j3LvRa#sN}dqDhq=dru-vKTW!agc20}G232y
zOrvLJ%cqlR&~b)0$0i}?(7*(zlJImD@fl2TfO6hSLP_|<8Sl;R{5X4sTx?4#KVrJ`
z!&lcZtOHbnEGFTVj(73htA8l00q)=UBB(><p6`M1V@Mz!b7jcxpt|x^?^FbN{t(lD
zmO%E#eufSZ93=QK!AA)0B{)QInBWKj)lXfQ3z0DhhPS?D9ok3gxo4yB@t(!WeOLy5
zgr#YR-$bxQHhPgubT1MkvKS3X<)U+7o;T--THd?eSK9SnLfuyR^@tl(AGB#{9y_Py
zsZBQ|V)y#JFt=l6xb&%pdkt2{Ui@qaE@PAXcVpt8?NAn_*Qdu70~0Ul4^|-NhbKH;
zt_Kssz0=gw!@tsg`@h8c!#rb6@NSr=5yt5_D7W~g0?NV3i+3v<BR6OXYW0D~f#Z$o
zy6~Dgh>77^Bexb0%a4t~DFzyU((~^kew1~irqNY6%t$Nni#<>_@z!IuNhn+UiDHF~
zb0j9s2pM<a4e*i_S$Rw@SsPbLG4W?Sd6#eq&w(NDuD`dAe~PrRChu{D=@>tEK`JlT
z+2Ls?pJeS%GQ`#{jxFxOu8CKVA>3{y)aR^aH{k241r*Yh<OOl#Z4Z3b%dxQQH+Q|I
z^JK<i#=EY$2kF;`O31P14dMH5ghGuiy=mQP^I@$zqk0mfHpQH>XIo>>6>uJxFC@M0
zu{#Dq59i9*dvU4Ch?4`fD7djXvUm$_JdP~hCaH#JGGQuEQ|sbm4m$Y(yl-!)cWJFK
z;-*`g`#@DAi~F%Q=ya8D9^k2F2z@5Z6QXiWVDlhiZs$Elrp23g2yUrXoA(F|@H4Nm
zOFDbt+3r1nQty|yckcr35jrA-tDn0;95)KY+SjDK#;Ql{-$se{y2lOFOL%V0j8h*C
zpSbcnPn_)-93(m`-@#2%uYct`_1-$3M~#2emG9`cN-eK{<vXjU<kl-cR_DrY?XS+0
z8H-bYt(70wz>S@iU;SwK2C65bdg*Ur?Z;N%*R%Ftd&T#xa;}Y#nueFxh^G3-NTA+B
zAmo&S*o^V7_$GptlTeUP-C-{EdzQI`rTjUDK1x6}t?^xm&g%m~dZC0LC^IMJqidcH
z0G2l2j6<S}dv_|htQWWGsssCjR0$XCR=kV&m5%5Ymb&6C1%q?AcK4!JKEkVH;i~Z%
zPKX7uvnOVcpT+O9RF)<FfLucnB+Hj89`E-Ja+lBo{uGLls!7%j&3?Fav0lbazRMS@
zl{w>XcbJVAnlI%3KIV;iy0ToClN;vHbL;0?9&TN3@mnk<8vkS+H^$BImL51N%JJya
z=N94E(8?O*j$K?{zT!R*{+7!CbF~jiU!0#WYjfo1IaW!oBuJet>nCc*_!}iz{P4?(
zM~|O9e&*5BXM=(+pVq3ZJLwsCUC0w&2M0`ER#$g%xv)7w7UW?nU8&=eIy2D1<pj81
z1KC`;QaL>Q7NqDufjZ!lNNxqchC>ei0PaHOvD3P7WhXNe^o8A7Y5itRy54FnCK9n>
z*@Nb1IbyuP=m16(Uk5iaK|xjy^T8*BFQehP{yDtDoTYAgx|7kSNpBd2h21!(igPda
z&3NKj>v5#)w&LH9N12PEWVC?G#l+<S*Z7(nJaD=dO!{7_hZp(MV1XZG0iQ)T3#v2B
z5jh$!3a_hfDneRyv*zB5{<=Gd%c5qc@l$`(up<qli&GX(Ois_>DGn}{Ptob=Y4{aU
z#G9U);7`Nx-gDOt(k=|w3O}oZaMpEsqqRwq-+|LEYSR}8<_P{3ftZJWogq1;@G*wY
z5`2r`4+-W8zD@8w0$3PA6}&hi@oc$RV6|xOPxA3Gf>{F5>%SYJAQ%2T9n@S)Zuk>*
zBBqHuF+~_nE|gq%omMVfxDrhsP3F=%{pNHwd2{-<<Xy?J<V3QNzAjlz4kT|#C)4}W
zk0kdccP4iyr_)E1+oT4L$8rg`ZMbWrGv_PyN&K(Yyh+AQn&H`nyE{a#mA&~e-wp2w
zXyMEc=owK=+l&{4#fIr8*&i&tZvI?4z_tg;drZ~$GDm$J0A^=)AzJvmk>0<@lGgtP
Dr!|U6
diff --git a/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_sim.cpython-39.pyc b/src/azer_robocup_project/Soccer/Motion/__pycache__/class_Motion_sim.cpython-39.pyc
deleted file mode 100644
index fa37dcf4363279fc8201a0c781ba304d46ecc2f7..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 10409
zcmZ`<U5p#ob)Fdxhr{3HQmg%wEYr5^l#Z>nY&Sn{U|CveB_-0XqFp=oRK}FcJG)D2
z__KG0vc!<dgVZK1;?ynNG&S0Q`_KXgiXiP%3lwcp<fTAS7=39m=u1$bsa}dUXn`UK
zpnm7v;Vwzr%bhd#-Z}TqoqNyu&d*FWpU-Ic`^G=t6#sca)BcSr<9{|Pui*&3uWOpn
zgzjsOI_r+kb;B|EoNy9+HXYN`hsguYu{>*-5{5_|7+ki6d4N8*lkyGE61qtKy0JuD
zS|cs2+X?v*S`(kAmJCX%1x?uZjo)5$vR-zW!+80x;r^y(YWEYm)+(OWg7;t0bpE{s
z>g5mbmdBWd;f$9*&~-K2qEqDA%{sGuE;*&v9G8wc#}qf_3GTS3CDIGpL_ITA&ra2I
zQ}w*6;~oWnZi{9y;~#I%JM%oFqQB5wbQX2(vL<Fl>47Fn_YG%B920X7G-p{H7xOru
zz|{q@h^vd<{QZP;(tAuS-8SCS<@??#v3#GNPT$ljCmzzCt^ZuV^>cm2u{PVankVZ&
zd-Uj0FZt4$m(RS=I}y~IUB4Q(rR&$XT~U`_4V8Om!n<Ll%U(+LS?Lwx!u57oZ?}%>
zZ`1>>&&AiTwQE(s9#j?A!Y8%fSX-&+k^U@5Rt@J$uj-?kJnwh*sx_MQ_@9BwYdC^A
z5KkikgK+7>z)~bc0uO3N`7K#(1v{Q}FIK~94N`gdM`apf6-V$Sh|v3*tEkRpLl|gD
zFg1mS0Tb(MlKNGWQ920Q9e2&I?nc?2daEAnsY<d_?FL?C(&flfS3cBH_xzm?HCaUO
z|D{J+edX5cfhU8N>dM7-t=sfkVX$(!9$x8guiR{Jw{O?Om5+uW{q;wGhU0^ee)Q4b
zy!gV4FO@|-xLvL{tGnLH#M0lY`PCqB<AuZao$1_*Y}duiLf3r?Hw;*Hbu0e$<|mfs
z%!qXztA>89YSVMwDC4@#w&?m)&bjV>x9Y1avV<Y|X~xJbwGjr;at5@xw$Ak$l}CRj
z3dF<<zm9^?2S%s~g9J3xhgkH!F0g~b>>CHxAkj~VBozKd@L0stT0|+u+@$o>D<RT!
z?$fSk)*sUCdZkSz#1O=C5LRXAiLLs#k7?RNNcyARsnvF?1+n%-Ic%4u*KCJg`TUKm
zW$elwPxhW!tJZy7tSPp7Yxk4+@;79AS5}*4uO&R8#KVlNZb#NzVU)mDIBAf(n>2;U
z^zPQfis@wP9k(ik^nxHVJ8fJkfoW?GA}?0&)qL-2>v|B`HNOtQxO#DFk)sTl-Ak?N
zw(orw&l=EfXu7G}#++{J8N&v}+Mhyejn;egmqmf<y!QWt0@fa@d{KM*)j_hKY+x5|
z8`2g=+*;yRr~_J05%2yf=FSW(sD<Tf`=8Ub{VV&_BNZB9qOae_592*Mu={%7Uebo<
zffo4ao9Y`>Z$Lva+MCn^eIM+<*$3>S<-7YIgvlX5oq%-T>?ek)0{}J7HlH<&S{S6m
z^f1#;GtUqEbf?@P)6WdE#Iyf*eUr!1`bXTknEPM%|7#z>O1QATw)N9reE8r0bbb5A
zxk_H9u@I8fBT51;cD*R!d#%W-2X5eD%ffooi;{uwd7UU#t?hRKhfzv;zE_2;EK$FC
z-r;+#+MaB;+TCDF)^~S386~lkRq4=n-CbjpsAQe=Xjg;C^6rr0M}=@4ryvr3yLMaV
zY3TgKCC46BoecMNcL;*?x_pf1&u!DvatgO6IEN;<@yy)wHGq!7_%EPm@Hr68Fd;b^
zk|Vvy5@exV#IJy}g)2F3wQ(+SDTj8sf}R9v6MrMA3+|%%5S>qaOWVQzi^PL8FxnJJ
zV6`b=zHv?pTcmNeL`GzBPKlhz<7|tQVn!4n=uTQ0KPM9#zHDsxa?0#Ed9fsx(WCG!
z2)Q$ZoqjCJ96@5F)o7<je?S7pp#;uBha47V0N|kb`XJp;1N>>Ons1PVp$3D1T4Ipx
zXHkQsp_Xh~gB+wZhZb^QG~XWN`+3x$6R4#Jg?<6G4A-)Qnf?rFIj-dg#eNaB!Y<_1
zoncyJb2H2364PVcGRO2d(|M)~e08yJq3=>Z3A)@jK~MA(peOq_=wr-zs-Hsn@vwYf
z8$JQ8G!6`4=9A%5BC&uwH%>R6hWljVNWz6WHJA;b8GfceJN%^sI5mS394V}4>`0Z)
zYVYa6nQ>RDUlP_5JwXMf6qlYw3A&E9&tm)v&d>3Eo)>9uJ;QpKS>OHo+z&7Rr1$<g
z#?4;wDw(TJ6>u7?Y}T7Ay&|1lavrwjnMTlVNfN9m3*^2XH%A4obo}Z)x6_90ePRzF
z>UQd3ZO?tD9`3n&ZCUTNTVd5lm!>DH(V}ut+)b|)z{$B-54ihAtFjVh*KVG_e(Bn!
z^~+mVA`5yc+D)b9$yT=svud<D&TO+9cKmjTdBM_BDYD{oqT>ViYj<8mXU}g}MPvXk
ztY&bh2ADURfqaS{A=zbI$zJR>oA=x+<ex<Lg{tp!DN7eG`PEL~i6|M$dNYW!jW$fm
z6->zYqI}$MvU)bIW@w<eoKw#_s?ilJQ=QO2K8Kk|vLI0o_K)!45&@C|lbnPf61wZ%
z<~E?L2oQ0D(CfIhcDEIxrGVwYm5m+pR{&zkpxf~zdj^r&?f^pPYh5WZp8HN)-ac|8
zBXiepZ%3KUtJmF)8(UX5)+3YlHcHZR0sY7d$`@&FFA;f}CYa`34(nK4OG$|QWwck0
z$r<9=Bp&My`<*;}r!f9RFroCA>VY??be+iOK^)_blwYLUuMqhXkynYlMuhiD--(g}
z8*~<)T-tS&ujMM3Wv7Si?^`&6Ef5V*X^$PbvGS;W<c59f*(m5I^)rB6`q_H#*b$B$
z`f3yYEcrZ19IH5j4?v)B+7OV|fJ4>L8UO`Q^8nEhUjbk=45+sbK+qc2ZBxENCD+>j
zUYHu%LMIc8E9nNH3s<>pYN#RVG5~0OO&MLq1Db;o|2A6lWQs#XR089pM5CE(u=ipE
zoLt{k=C`91{~<ZmKmFhj=<hGjRf<a879eqVFMHutNE?jl4T1#^f~TbOB*`Q6fn~w%
zC@0K?=SGnSdsBDab_d?8=Wcg*5CS;lVbOTkszJEggfj>zApev1Se~X5>q8Eu$fn^p
z;lxXlmMBYAzyx_*aO&;_av9APgN4sYKwGDcZvLGru#CEq()<Rv1L~p~%X-1cKqL&d
zr?9iwYO=b$#j!1(=sdB{<igRx`%J^`Y6BgIfg=%W_qBmZhOD74XmH-J$3xnA7>0(?
zNHXvnLzoP-BtxSClSKvrCc{L{K1kn64y-|HU=PwTdej?caH4mn50_gcu-D(9AggbS
z#FBEVoctE>&fSF0w|A>yl-c#dO*pl*&KxymqxAN@5Dt;7-c$O=&eJh0(%l;rHi0|#
zQ`8>wmC-J?TVAl&4juNjR)J!hHCgY3Yu#3jkjq)-wdS_)UdMB5dv#v~T#x4H7Ndc-
z$+?0*AMx<)myg}}2`p(PtM)ZrZ*{sPe}R+b;u-)7l^j<#0n4r3(WMJ4WRXenQIvpV
z@4!V=ebXq!Ju@hd`cbp$H6=T84|DiFj$jdl{CfJ4|1}3stnjD+FTbewj*rR9(Na&u
zS_BFS94st9LIIme9ys~)w`hAC2yda>T!wNdgwDzhks-gG0RTMUOrpmPv|6m(q|!>c
z;f=>0`wQ{dCOE?pIFOx3JNzBAeg~!eu+V2{X_cvcI1}F`H!L1#@=wXT7fA+-@3D_y
ztxFg4W&x7FhSEcN9{D&(Wmcg+*^$?;ohLhTq`*=T8za3(r5z%qfMeReOVw*cNDk#^
zK%m=RD4#?r)@>HVsr`wRs6nJQKj!lPF}em6cWH2vbL2+H>P<Bp+n8bX?>#-Xc`Ue}
zINF5fr!c66gE!MhffoD_;jwj82f~}T5(9+P2%NEP4U61CoT2IOa#%<D+Bby01eb3I
zdhfzfY*0`|7?Eyd4-CeQ<S^GyQZ{a2^)0y4$#tc~$j1oMFSRWlOal=V>+vEf8Sqv4
z8_Ko1N~sF9hF=D&tP%MlNR(t4fExr1{{no6y(-2e<BO>GPPp3k+tSIeZd}{A>E672
z;l&G=Z#w3VU++{>teX*OQdhQC0q|4~+CCgAAb*ClBb@JX;IP%3yN<=Dvrz(->8)H)
zF2l$Z#PnUB^Iu>l0a*e~FGKC-5bzZA-jidy_9-1drhnwH@|K-J0ZXjOXF-wY;0+@W
zTqjuOHpCQ2dL~|j=Ead}FRzxftk&}(g{co8fmD+0|EMLi;2-X_-(gUVQe-^7;@ulb
z@BrM?k9~Y{_OS!}x8Mv&#K?u2(|hGlecZSMi$skya`aa~f#%3h{O^E$<Z^T%%rZod
zFkqQhpbz0g&PNXs3F7&UiRdE{CWaLKCpr2sC!!BZ<8DpF9>`u`tR(UaR%nH(ep2Nh
z-qYXSgWzfn8_8lIk6KTd$P{xy(BnMC9P$(?2tQgVJHg2F1CFfHF=rO$F14s}R^v3{
zLRJ=GBjTc{Crzo}C2dX<nK{+_A3`V&T_Y6(cOKWWb5tsvz$k-EX!SEZ+K&-QYK@sj
zk>-dr?aM&T*%%K?!(+HMCvt4E>B+Rl_)HwkB-Q6=3NxoO*A}%t-79lKgZW2H+9(Z=
z(>OThnTy&MsKhf`q!Pz~rN+{MImo}K@4vyl)ZO#3WYQedJV)%`koItnRtnt9`*&DU
z3kn|OS?qo9#6)g#rKv)T08vt~(n}u2agyLe&V%g!;g8Edl6${%j$vi@%1?R-3=hsd
z{13?1)>`F6RJ^`H0Pk*I+t_k1UR~XiU!}e`iM&qa4I*D7QYJ!XJj(6BO@NJ7so6{w
zj=~-92E~Y0wSxdfM7i^;TUXz>^yQ7K>sy-u2*f^=36Up>P)s5BiLj+$kYn$Iot4#g
zv)OIcYn)e6{tDTJyPID4WeU``suETicT=I(8pesNHQaCGro2Rz<HS93N`eqUPZM*M
z0j^aF3gm!X6k^F1P2yLn6Y@}4MaRaPA~%m-&!g4JA3fTJFX%Z$q&FP9)Aa)n0bHct
zb_$`_?3_Pb0@<T6i-%;EMlWDW<biog@&6HR0mWfjW?5g<pJM}7f>AT|(?E!M{Yijh
z4(B45OZv%2wn5pVlHOYy!;Z;OCyW|F?4eP+0DlFt6?-VbGjSQXaae}q`JN$P#g*8x
zMV1)mjb+$qSO<w)20|L-V#YYg4gNR>nZ3~%gcRewNy|sqXrzz#R?ac$FPlx^0gaG@
z@m2Rn1_&dJuBRri4=5xv;0dYT(^tu0AN94T`m!s3*mwG>a`};C;&~x-qp^5i<P^ZQ
z=dnk3&P<JMhsAIf9Hkf?8;o~HAw-EW<YVi>p~A7@Tt7poz;__jF)3?yw62VH^We(H
zTnnQzaD;rrc;B(V^E}2PjUm!82GP~=-Cv@s#MdA^qGwFRRx>m1GZMr&sW@BWSX?;k
zn3M)_V%v%|FQtLx0$8^yC&u)iRNmn*0gl1Ob_4GVR31gSG@L^qPfT7Wa)roMBIN5R
z{XXn>2n63HhOZFAD8N+%f3*v}lP&l#vP&#wA_QadH;E8_$Zvx<9L!)`Mw!E52~y;@
z==uz!(2-{<t8{4_q>^J;Vbzaj;x*~S2o=r74S35FPh`Y%cADqx5s&;jjozml&#rQI
zjb0Oto`ldTvi4NY${r0f4LkIL1i?05ko*k%!E+#*@l57P;LAzIm{Yp?mGx6bNq+{I
zv&_zQ0muWlY;F>3)KC+S7(opA&+L}`0tJZ72xd;lWrCSend003Q!g}x$)?g=hVf>z
z{a38McvXRZq)xVAfN7*z`4f0`f-DJC-;Bllh%-0I378_7C@@U$+h^30LsH&P8kg=;
z6o~85uZVIyfB7%uHc+4VezBs+S3)=u!cwinIdUxaUtwdNnpP0`087I1A7^C4bCu&`
zdQhgzzeR)%BdLY_JtFkhK?#42N*9QbkSaAGDVMb5@_R%s5+U<86@yhQ6>g^>2AnM*
zsZsXn_o+mo@U+O}NM!ic8(yIuho2e8%BLY2ryv_;$i*omXDp(O+{(g~gp7Jl*dq8E
zM?&)Fs2nZwp<ql4$q&JEgpZ>Xi}KfyC}DxwA&WHHu3$SZlZYW*hTfwTONRVnv}EHm
zjb;vv{Wazy&y7Xv)41Y@VJu#bqq8GFhVhK^V3gY0oJ_aK+qBMQB1f?E9jY?+u+PHS
zGxBgo*i%U;SovMz{RFpyWAUeHkLJLk!FQO07B6C7<{w$eh0Q;jNN*}IXm=*|hEiTh
z9IU|aW30)v7uH6t^=nW+eus>#g{gdSRse(EE5~h!7`QDl)`p;%+V<x$ZW4#Zu>$SJ
z(YQ(GHz&rm#@a^XrpDSv<F>*idn(p_73(2`LSrKVOJ_>5CWZB`4(~xF-Vli64Gtm?
zVZRHz#yr$wi1buT0`JQvd6FVK)negY+9Xd(<l~kMvNHuS^DbHL%rGyC3NRXl1`@dB
z^9McLoocfj0X(3&P__oC-LYq4=_|!i0H<uoua9hc;Z2`OAR7H98Xgi7S9B+}Q{N5h
zJ+Eh7Xp4L0H|aYMzq;-Dal>2j*})m6y8LE*rgo}3y-XJ_ye#|PPKeG4B=xp9o<Lft
zjC&}wVJu=)Vi=-)nj8}OB8X!lrHuEzocMR_pu0^kastPslVkbFI=6&l?#jA2!a{t?
zfk=ClA<V@#lU~}q>5SLjny5L-Pfyd_74Kp3E$6W4AaU5*brM(`$Ee<&#=KenrT}T^
z>TatYg!NjmLQ3@oJjy6ZSm0d6JREfTjcPe|$xCp{M?V6^llV}ecXA9EKQ{b?2I5vn
zxkTFA5XmR?nm~D(_*6!HB(tkB`G0?It748pJ(+cW%`PcPF!gZKL)$t%4&_TUn%ex>
z)Cd#e^m|3;AQSRLGg9TEJVs@L7kQjWi3mq+dsGUDe1ph+BJUIVBO*T_B8dD65t>Al
zkKgdrJl}WYw>%sty-h3>C&?E;-o(L1$JPta+a)`1AG2+{kU4EXWf$#?{Wx>c$284q
zz2&;S#jU-1P{x0&9hRxBtiG*zqVhPWCnKYccVBq1xThA4!rI6}3|V_eaG*;c9pF7>
z4WAd`!y*x-;<p_t6sGWy17L^zO7&`g^7AcxX+nduPJEuY$s9y!G~s<4CB#$ooeaJ_
zigM$h6GgM*HU26~k`_nBTDyt-xaaZ*TJD{fB)M4p#Seda!e0jPD)9D1NfxlNjptS6
zz|;PcT8md{_k%Bi6!k@Xj6jl0vPvH$klRN&)DpEL%#42t;>sC?G<Kj|{H8X@=>HG(
CbsXsc
diff --git a/src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v3.cpython-39.pyc b/src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v3.cpython-39.pyc
deleted file mode 100644
index 5673cec0b590a2af8e93f631b968f03177e08a4b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 4616
zcmdT{-ES1v6`wmZJ6^BX#x`qvjlmc&#_NDT7Ju_)VuDS;222QK1aj8C!)EIJ^2|cO
zx}!!Vf556z^H5b0uGFe|iRfD&sy2^RA0iaxY1D_Rt&sfGq-v@@L~?)U&e)5Mh<NRe
z?z!ij`#WdOJ?Gr(ai+dLrr`JC=V|`yE=Bn}2`~O=bqd4%D+pIO%PD3sGn36IT;<xw
z3fCU1rfO+iUs0XE@QA5%!;EmnG<c0!!=q-D*P69FX2y7(S;ymMoY$N6mTpBX!>X~O
zR;?AY>a4g`pN*Lfyn*k@s^%Wv$eS=X@@Bpla}!VS7R=4OmA7Hu%agnvbHdtVHCjzp
zv$fYsWDT>$VozGv6|>E1dy-sNm}0df?tQ!iRN?!%Vl`MTk87dd#}5G0v93H;8FqF<
zNp)^um&*Jy(HDOzgeeR+0b(g8!`@WRxcaeTYFvY*?nln&N*`pFWjcYP-+LH!7}FT;
ze`}FInB<f^^J00`tIKO%Jg<B8`H0t$H@rRh8n2NlB6=t0HF?e6UN7Obc&%QWm-O1b
zeWLb`;dOZXy#rpSchKt+HQu4J@<bJf)|q!0oF=&=;F!>bo=`+YL=sAyA`D^B3|u!>
zx;qq4#~RHMtkG-;CJw_w6-~e&A+&cilt|A=msGqSp?SyPgLIJ)*9(qhJ;bDh;q`Ge
zLss?P$SU4(v7fVcg(DX41b98<`?>B7h<<S*sc-`^3<5h;j`Etv?1FOlb1yAUh_o1#
zv7D4UJ&7IE$(<4x7eh~KMUQAk#C3T8F%g%$-Ir8EN^1H74Ug_sM4!~8NQ3u515~f*
zB?-1kNbNhV5{k^^OLl=3WwQge>x?NCw%Ui-hY`CWW50&jheR6tWGi|_bz&rOy-krC
zrJ}lh+v~7)p6^)KrDhm=ONZW{bLNedV`BI*gGUt}zuL}Iua`!gk-*<N3;B_DM7DxA
zISV4~ja1at1u?=6Y5P+F-@KN57pAuGTu&mps(lb`=;Kdh0efJi6(S*-c<g($91UWl
zy;s=X|9E4l+^`sHSHkLzi*YfCj+zh?Vmz$&<V)2aN3|#a52~%LCS^UgI`xMupgCX5
z3TUfeBZE{u-DyDQpDN6T715neUJH5rHEN+d^FbYej}nKho<ep<wlW9q6nai~egf^O
zP)qrrr2H2WFJQN|VO3)27v1?yMPL0=3`oxdVjwuL!3j<(@T@sNt$tb2RuwUdF2G5|
z2~b!6hI*bxUe3sF7=6s-N%)2APD6~Mwr7G4^Uh+QH6m3<XYwqaKgbMxDv;!9l_!(y
z;w<Ufa6*IghEsn|bkS`h&T*W1L>zI-0S`THmQZsVk)4yh+KMRag13__&d8me!5+TZ
z9c@LwyWr7@P!aFccD(1I(ROIwlso8^J2;QqBPHIXeRSY-c+(O+4eo;UMBG~<Aua%S
zRJ<i`sAGbxf}2ZP(kkk6QM3X1Domh1N?5wBr1#U+I6L4W^hw^2QB2U_HU!cTFCgW+
zX2eBt5x5yKBXO;wRn9G<h5Yj<$+(qgL)<K|G<I6eN}cpU_bC2CToRYi<&jE_^NF~K
z48aEPXX4U2Lo_?A;r;F180>5}<}&Of;_Y>Y&kF{>IVoS>A?vj6UBQYju;R@XdS%^Z
z6=mH|qYJ9iRu@E8KYe{9yw@PQR?M2Xvd(0_W~8ic$<zbl3i2?&13kA39m*0t2lQ2O
zMa&~V)8Ma(IdL`QuZwHc=i&hMJbF7?TG-Z5x9i@5xV{Tf-qr+&8)9A}I=6j%vaJaa
z?})3r5Wm~j1c;mBibNdT_Tg`90>rywZWrSFZB2l<B^D~R_*;0RQtMt6?|^R=Hzofr
z_`~9s<biRAX%&?8r4Sz=fq{ljbhVbRuIxwjM)#@o_LRK2PkG$CT|Enwb#bkFw#A`)
z2G)<`_P8y(=0+vreu^lG-Xglo9+eW;Uu|DOe~IJxJglF07(N11HNNhyI%Qv5F1o(v
z+67<FNO~``1VuSR3eb&B{5HrWE6!xQe>FFKZiBuoH|TS6gFZ($@HweuzyIvpujZBp
z&)L5;6y-A|75DXgru>1UVbRfvM2KKfacYP}iIAxuaWaLh<r|q&$trMP%UK0qD-}zr
z_Yk9%yXUJ9%f9-kY^rNzpFQ;1BcHAL>_b1|+H02UM{{;we)7J~%hb$>uP$LMV`N4#
z#xTY)CVVY3H0WzfM3zBnxBQM&+0iOHR%OSl?1UfNG8-B+jSPK?kC-~L42lGgm|CIe
znA*c~$<!VZSp!iYVm!iFEBV@kjDutpvN_8&BXBm9kyAS3Yj%N~5w~oWhRj&ma?8^F
zsL$;1O{1n!D%ypz>$4TJCRok;>_?_fek4sJsc>MbD`_(-5krG{UtL)-b(DKFv_kBv
zZOgbvW!$47cw&6aG_VgjV^=h*mxnOYKFgbtvXw`0a!Lo&$<XD*&=!*pnei>n_?BjD
zOEZ=>4RQrVFo!Zsqznr=!)Y+5QN2ReX{4?q@*Vx_G(CCerfWIwKxW`Varu7ULS_bL
z?DFjWrGbUwQgOvD4?KJF><`a=i}CYkPoMoUJ(xb(#cg+`%g$%A*1(P8^0MU&%oWRa
zv2bU(m@nNgTZ`wZs*4X!_Lm;{jk}f9;%+OQ7}l*OStHX~3{NME$|u1R&@@VY6kHq+
z(SRf5an`)4GGkL`D(2zMm_}>!Kr~?PVXf*YP
kEr^CmMU5H(A(5Oygd#(4A?|r
z9YstH@S|&ukj}C}75-ISYlhU!_5&MZy)4IiwLx&*c)G|Qs0P>?;jFWMJlKVHm{p%y
zU;<P#-avN2+@P(s%Ix*;PTM^GbB!)87BYEjanX-0F6N8;evbI~;^ON4OfFb)CXv``
znFDTubaWTdMdK0^Ri#WG^BKFaxR`}fQ9c@(3YqlOMjsS`^!~M}>^=HDzZ$N8aX!@C
zo7iFor#9lSuz&X5=BB-ZJJ6o}H@JV`b@qXOf%`p{?LYq$+_&ff`~7dgRd7w(j4W;g
z7rdnB@)!0vZdHfw(Y4O9owvHIT&Cn&yvr_h?MTdIYQNJ2n6DE|?#Vd?**Qq0i^w4&
zhlv~^(oN(jkscz)i1ZRUKqN(kHtxJZ<T#NNMEZ$zg7}f;Tyc5DG~_fyFJ4}eS@$C)
u2X_k&+JhGkTD1ax^mI_Ov()@<8l+Kc__5MPZT|9;RD%N%&_*dG+5Z5Jw0yb%
diff --git a/src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v4.cpython-39.pyc b/src/azer_robocup_project/Soccer/Motion/__pycache__/compute_Alpha_v4.cpython-39.pyc
deleted file mode 100755
index d4f467dff298fbb9d088218a6821ea76e0a52ae8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 4816
zcmdT|U2Igx6~1%-_Qz{ujIr1J8ZclxfOi-C$FB(lJAj>(p8yw-vv@9>t-b5@y-Nz#
zjGESYu2z-$QdJSIRH>1wDt(XAJXU>(tO)htKDHIAU#d_weW)tY@65g4#im5Oc2{%e
zoHO4!bAD!qHTmY|q=wJ7?q@SU+OKJUA>qYO6NE7g{|N}!IGfX)aApo0*SOA&k2P*A
z>5lFi+?>_D6>d2uw;hXXj?H6EjK`fgPdEvlbdtQuY2qm-#haaG*K{q{c4KbbO}I(7
z$xXS<g{0HMTll7e?rh?%d^6@&-p03J-pt$iR?Ka@gLh)y!qdD9bGy6AZFM)hZSEGg
zy<j_AU42P&I^2%OoqQYL&UYYcjql`|+v09rPB>||i|=yNOFBa~SGB&~tH?~7nj!k)
zM~5(m;kSdhn!}Kf&KcJ~)*OQyurvef>|Eu8{ER9gQ1rQrK}9=<;r|9JWHP6fm`s**
z*;F!Qs$|OMk|kS8w%k;T$yTO`_?@KOEZgK3*)F%r4%sQwvP*6gi95F3E_cYCa+lmK
zyG2ayscMgPv1ggdz2FSR?E}YzDa>|FSi)-8IyGSnn`Yp8u+p<#lP1<^wy;LCEtuE~
z3temmejlOb{zxJ{BVAgP2ZSLH!UyTB2-gdaWHZ9_30wAa9Dsr@-zaGEkl4Xlm&Or`
zJPh89_#@nu1LBA{oYuIF7&5?St8pG%V&}EHzm++0SmZ=T#d1_-dK4KnsZ5DWiR|Nq
zI3U^(aTE4GC{ilhZD~#PDNTQ<;qfh+=vSIP(qKPmfa(>!B*8WfscXGeyQV7nl3i#;
z)!Yu-WyZ7`Tj)pZ#}K=%Vt*I0XGIQqvRSjHdr6YG*{LawR@43d`Z}z=r|Z^Dr8$P&
za*_9^oXNpzQXE@i@TkM%*Y!H}${gafLVwFF;s?7B*(`SQu85o*tmz9^#2~ko?N0@K
z^91=WkFDXknMQOC`!L$b#~;W7_P{7BL_#w0$a|<74`ZX;YwYg7<S;sSObmBvQTIl~
zh{)hXjfzn*5_S9dOWhtpw~zlHblX@+tA4DV)bH+q;XP3uFczMmg48|JYr)AsQJ#o8
zVtTuH0`kaf^uqL}!ae{WCk|CTf$9#fRSw(<oH^6`F|;QlE!F=x)xX^S0=BM=x{}0s
zF}+{c%!RMSfbu*b2EzLq-r%$b&xQxo!dEq8K@&qb1-Oa00s6w9(a)2p%PDmlhL)JR
z3BORM(-uSM?WypD$<xR)Ci<G_PM)Uw2bqEI3ng`1)y?FlI8C}v+|cm8;ntrK-SnD>
zGaPpw5nEn0<e|sQ5@}8%vNP(eb|A{8aCef$DV5nN<nYbLXlvG8Thfit5${+%-m}mc
z>ojkw40=@tXYqRUi8m>a?YJHCoI;-icV2lS?k&+S&I7k!yro{KgMzHWmrGgFD*AIl
zbOQM*OsL;aSbD9L_mhn{x5Go^le{0Kn4rOH2&64uK&rjQ#RYKzxN$MAa2=vU&0ED*
z^3UTW<5ixBa1+4NSZ_6<bjk<4qxcu%ZSgivxmAmMK%zph!TwCVz045JI%{~pxDkWQ
zHexQp-Vzs=8GceQ_}!u8OY3Bl*5zfam;x(u5?(H1zX^=-Cdy+9noH|6?<hZ0k)N3N
z1aeA1mhXzo>gA4kp9}Cf^)Y`%>9iVfN*8fzPfNU0kA}LQB3XV<Oe!yNPZ!|fCGK}9
zorWyVD_z9N$#Sncr|Q(Hb9xe|wjr&ZT5I8x*GD3fW@O!g*$|hP8TA((8CM#;p;-ox
z&RtoDpL_{l*8o2W{8e#TTtP+8fxjju#np(vF0Rqt6g%mj;yxxSc3nd^)Rgau>l+Zg
zbxnx4A+9LIuDXwlbxnx4DXwln%+)m^;ubnaaqO=9_(@$8A{=yMJ>s*vCPds8@6{6C
z7ve@O;s0CQ1ph~IOYsi)KZ)Cl2gd&!t%5qWq}LPSL*#9xp;xHU&sTT%BkY*;IlK{t
zH=pyAywkY9RCRH!@g9ji_id~n!i#rDotzsr%l`yX61`1ygFQMWuD{y8hBGD(;U{hL
zl*jPHwJ#Q!{(@HxjG1{qF#KXUF!PGu&)<WhnkNP5>T`Tn$)q4o6?%R(dG5?A{Tg1S
zpTMj16M8j4i~srb+1HczGG~gvv^DKht*<38OZn;to`FTrBw`W4qUObj#EFn;V0rm+
z!42$urQ()(V9dGYz^Keu5C8>dQ5d<ty9n5wyYJ`^tB(E%W3d{rhXH#Ou*HD=Ah7)6
zqU#6oxnfEEW`CWX_55I<-@}-}$PZx*V~k*o21Y)c35<J0W<U~aeg_-uP=g(Aup<q2
zG)S(QWiyVQr@tiz9TQoBqS}LwQJ(i4<6*Vp7>|f7g6I!19$_q20^`HHhsu--bFS}L
zaF)%hDVGn7VwpRZUv(>4Cs}p<s&YRRuwt~+P+(W)i{+{xuvsS-u9gD!ea9p}isn#M
zI0*FFoD)}wY^DTh)-h4^p~wp9>&2pqdq~AS6oE%ah8-Jus2N$&tY6Av<N{W5tg2f=
zaB9kh)6vM~Xm*XsIX1y^*>FCZUGtr-b65+*AE`6BNHe^q8IJrwq5Q4MFj4YIw2qCH
zJeos?%LT8Mjv<k6Imo%=cdq-c=MUru&d<+0D7mQKz<9Ab@!;OTmHB(~v&HJb%Hx$U
zR(^-^^Oet5exJ+aj&}2+Kigd_<qPh>z}5Mg8P^+_oUaz=%XenxOO*#zcls=~e)_{>
zM=Fnk){RQK6n;P5G1jA}St~PH65m}cuD<Q89hz39kAq9$OElyNd5E<=*O~p?WIE<!
z&yxnNO##t@`2g$Chk!C!8+^p|@3hp(@&N6oXRvz;b{MdU!#a+bTHwbtS|Ocg86Ez0
z(`bX##&!UkWW8*T^%@y)J@|H$PN&sYwMKam#x`2uDb8$opxHDt@P^Ecy{z$_z*>lE
z!>;5joLh{=4pq@38%sHbzuDu{)8%~0ot_So)6=DS{$P&y)b#YigZx~$;+;aV8@&&?
zlcb{;jh-u?n7FQF@>t3j%hS^ZC^hY)!Lf+Rjji@W5lZjBJ65<)pQrCc>tCLYH1|i>
znB!xsDOePL_U-fM#aX<h#fg7{`zy99?)(S1&#_#b`#ZR2I2XnDzX6wO2FJ+aCgHxq
zlN|1J;L?}HTaY|@k{4s)J8`^ko3|NqU{;GIS7qRxhV1Po(oJL!k-bFr5$Pd<{~Xf1
z14IrI=_Rt0NFNbO(|d!+AtHx~93ip`Bw+clq8SpbiicO{e^ob#pA7qPnhv{9=vJfU
WhuT3p)-Qk4dUyds+G^TtoBm&EhPT`R
diff --git a/src/azer_robocup_project/Soccer/Motion/ball_Approach_Steps_Seq.py b/src/azer_robocup_project/Soccer/Motion/ball_Approach_Steps_Seq.py
deleted file mode 100644
index a7cbc14..0000000
--- a/src/azer_robocup_project/Soccer/Motion/ball_Approach_Steps_Seq.py
+++ /dev/null
@@ -1,131 +0,0 @@
-
-# Module is designed by Vladimir Tkach
-
-
-#import sys, os
-
-#current_work_directory = os.getcwd()
-#current_work_directory = current_work_directory.replace('\\', '/')
-#if sys.version != '3.4.0':
- #current_work_directory += '/'
-
-#sys.path.append( current_work_directory + 'Soccer/')
-#sys.path.append( current_work_directory + 'Soccer/Motion/')
-#sys.path.append( current_work_directory + 'Soccer/Vision/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/')
-#sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-
-import math
-#from class_Motion import *
-#from class_Local import *
-from ball_Approach_calc import ball_Approach_Calc
-
-
-def uprint(*text):
- #with open(current_work_directory + "Soccer/log/output.txt",'a') as f:
- # print(*text, file = f)
- print(*text )
-
-def normalize_rotation(yaw):
- if abs(yaw) > 2 * math.pi: yaw %= (2 * math.pi)
- if yaw > math.pi : yaw -= (2 * math.pi)
- if yaw < -math.pi : yaw += (2 * math.pi)
- if yaw > 0.5 : yaw = 0.5
- if yaw < -0.5 : yaw = -0.5
- return yaw
-
-def steps(motion, x1,y1,u1,x2,y2,u2): #returns list of steps [[step forward/back,step left/right,degrees of rotation,the number of steps]]
- x1 *= 1000
- y1 *= 1000
- x2 *= 1000
- y2 *= 1000
- stepLength = 64
- first_step_yield = motion.first_step_yield #46
- cycle_step_yield = motion.cycle_step_yield #96 #78.2
- rotation_angle = 12
- rotation_angle_yield = 13.2
- a=[]
- b=[]
- Dx=x2-x1 #calculating the x length
- Dy=y2-y1 #calculating the y length
- S=(Dx**2+Dy**2)**0.5 #calculating the distance to destination
- if S > (first_step_yield/4+cycle_step_yield*3/4):
- b=[stepLength/4,0,0,1]
- a.append(b)
- b=[stepLength/2,0,0,1]
- a.append(b)
- n=(S-first_step_yield)/cycle_step_yield+1 #calculating the number of steps
- n1=math.floor(n)+1 #calculating the number of full steps
- #Ds=S-(first_step_yield+cycle_step_yield*(n1-1)) #calculating the remains of the distance
- #steplenght_T=Ds/(first_step_yield+cycle_step_yield) * stepLength #calculating the length of the little steps
- if Dx == 0:
- if Dy > 0: uB = math.pi/2
- if Dy < 0: uB = -math.pi/2
- if Dy == 0: uB = u1
- else:
- uB=math.atan(Dy/Dx) #calculating B angle in radians
- #uB=uB*180/math.pi #makeing degrees from radians
- if Dx < 0: uB = uB + math.pi
- U=uB-u1 #calculating the rotation angle before starting to go
- if U > math.pi : U = U - math.pi *2
- if U < -math.pi : U = U + math.pi *2
- #uprint ('U =', U)
- U2=u2-uB #calculating the rotation angle when the robot is on the destinatiob point
- if U2 > math.pi : U2 = U2 - math.pi *2
- if U2 < -math.pi : U2 = U2 + math.pi *2
- #print(U2)
- Rotates=int(abs(U)/rotation_angle_yield) #calculating the full number of rotates
- if Rotates == 0: Rotation_Angle = 0
- else: Rotation_Angle = U/(rotation_angle_yield*Rotates)* rotation_angle
- if Rotates>0 : a.append([0,0,Rotation_Angle,Rotates])
-
- if S!=0:
- stepLength = stepLength * (S/(first_step_yield*1.25+cycle_step_yield*(n1-1)+ cycle_step_yield*0.75 ))
- #print('stepLength =', stepLength)
- b=[stepLength,0,0,n1+2]
- a.append(b)
- #b=[steplenght_T,0,0,2]
- #a.append(b)
- Rotates=int(math.ceil(abs(U2)/rotation_angle_yield))
- #Rotates=int(abs(U2//rotation_angle_yield))+1 #calculating the angle at which the robot must look when he is near the ball
- Rotation_Angle = U2/(rotation_angle_yield*Rotates)* rotation_angle
- if Rotates>0 : a.append([0,0,Rotation_Angle,Rotates])
- return b, uB #returns a-list value and walk direction
-
-def ball_Approach( motion, local , glob, ball_coord):
- x1 = glob.pf_coord[0]
- y1 = glob.pf_coord[1]
- destination = ball_Approach_Calc(glob, ball_coord) # computation of destination points
- stop_points = len(destination)
- for stop_point in range (stop_points):
- if stop_point > 0: # in case of 2 or more stop points x1 and y1 are re-appointed
- x1,y1,u1 = destination[stop_point-1]
- u1 = glob.pf_coord[2] #motion.imu_body_yaw()
- x2,y2,u2 = destination[stop_point]
- step_Seq, walk_Direction = steps(motion, x1 , y1, u1, x2, y2, u2)
- if stop_point == 0: motion.turn_To_Course(walk_Direction)
- stepLength, sideLength, rotation, cycleNumber = step_Seq
- if stepLength == 0 and sideLength == 0: continue
- motion.walk_Initial_Pose()
- for cycle in range(cycleNumber):
- motion.refresh_Orientation()
- rotation1 = rotation
- if rotation == 0:
- rotation1 = (walk_Direction - motion.imu_body_yaw())*1
- rotation1 = normalize_rotation(rotation1)
- stepLength1 = stepLength
- if cycle ==0 : stepLength1 = stepLength/4
- if cycle ==1 : stepLength1 = stepLength/2
- motion.walk_Cycle(stepLength1, sideLength,rotation1,cycle,cycleNumber)
- motion.walk_Final_Pose()
- #if motion.falling_Flag == 0:
- # local.coord_odometry[0] += (x2 - x1 )
- # local.coord_odometry[1] += (y2 - y1 )
- #local.coordinate_record(odometry = True)
- motion.turn_To_Course(math.radians(u2))
-
-
-if __name__=="__main__":
- print('This is not main module!')
-
diff --git a/src/azer_robocup_project/Soccer/Motion/ball_Approach_calc.py b/src/azer_robocup_project/Soccer/Motion/ball_Approach_calc.py
deleted file mode 100644
index 183b7e0..0000000
--- a/src/azer_robocup_project/Soccer/Motion/ball_Approach_calc.py
+++ /dev/null
@@ -1,169 +0,0 @@
-# Module is designed by Gleb Korostinskij from Phystech Lyceum
-
-import math
-
-
-def uprint(*text):
- #with open("output.txt",'a') as f:
- # print(*text, file = f)
- print(*text )
-
-
-def ball_Approach_Calc(glob, ball_coord):
-
- def rad(a):
- return math.radians(a)
- def gradus(a):
- return math.degrees(a)
-
- def ugolx(x,y):
- try:
- tan=y/x
- a=math.atan(tan)
- a=round(gradus(a),2)
- return a
- except:
- if y>=0:
- return 90
- else:
- return -90
-
- def ProTochkaEnd(xm,ym,L):
- try:
- a = math.atan(ym/(Lx-xm))
- xp, yp = round(xm - L*math.cos(a),4),round(ym + L*math.sin(a),4)
-
- return round(xp,4),round(yp,4)
- except:
- return 0,0
-
- def ProTochkaPodhod(xm,ym,xu,yu,L,Lp):
- if xu>=xm:
- xp,yp = ProTochkaEnd(xm,ym,L)
- xp1=xp
- if yu > ym:
- yp1 = yp + Lp
- else:
- yp1 = yp - Lp
- return round(xp1,4),round(yp1,4)
- else:
- return None
-
- def Napravlenie(x1,y1,x2,y2):
- nx=x2-x1
- ny=y2-y1
- r=math.sqrt(nx**2+ny**2)
- try:
- nx/=r
- ny/=r
- except:
- nx=nx
- return round(nx,4),round(ny,4)
-
-
- def ugol(xn0,yn0,xn1,yn1):
- try:
- xn1 /= math.sqrt((xn1**2 + yn1**2))
- yn1 /= math.sqrt((xn1**2 + yn1**2))
- xn0 /= math.sqrt((xn0**2 + yn0**2))
- yn0 /= math.sqrt((xn0**2 + yn0**2))
- a1 = math.acos((xn0*xn1 + yn0*yn1) )
- a1 = round(gradus(a1),2)
-
- if yn1>=yn0:
- return a1
- else:
- return -a1
- except:
- return 0
-
- #uprint(ugol(1,0,-1,0))
-
- #Lx = 1.8 #поле по x 3 метра
- #Ly = 1.3 #поле по y 4 метра
- Lx = glob.landmarks['FIELD_LENGTH'] / 2
- Ly = glob.landmarks['FIELD_WIDTH'] / 2
- xm, ym = ball_coord[0], ball_coord[1] #координаты мÑча
- xu, yu = glob.pf_coord[0], glob.pf_coord[1] #координаты игрока
- L = 0.2 #раÑÑтоÑние от точки прихода до мÑча
- Lp = 0.3 #раÑÑтоÑние от точки 2 дло точки 1 по оÑи y
- a0 = 0 #начальный угол
- #uprint('ÐÐ°Ñ‡Ð°Ð»ÑŒÐ½Ð°Ñ Ñ‚Ð¾Ñ‡ÐºÐ°: ',xu,yu)
- #uprint('Ðачальное направление: ',a0)
-
- a0 = rad(a0)
-
- xn0, yn0 = Napravlenie(0,0, 1, math.sin(a0)) #начальный вектор направлениÑ
-
- xpk, ypk = ProTochkaEnd(xm,ym,L) #ВычиÑление Конечной Промежуточной точки
-
- destination = []
- stop_point = []
-
- if ProTochkaPodhod(xm,ym,xu,yu,L,Lp)!=None:
-
- xpb,ypb=ProTochkaPodhod(xm,ym,xu,yu,L,Lp)
- #uprint('ÐŸÑ€Ð¾Ð¼ÐµÐ¶ÑƒÑ‚Ð¾Ñ‡Ð½Ð°Ñ Ñ‚Ð¾Ñ‡ÐºÐ°: ', xpb,ypb)
- xn1,yn1=Napravlenie(xu,yu,xpb,ypb)
- a1=ugol(xn0,yn0,xn1,yn1)
- xn2,yn2=Napravlenie(xpb,ypb,xpk,ypk)
- a2=ugol(xn1,yn1,xn2,yn2)
-
- xn3,yn3=Napravlenie(Lx,0,xm,ym)
-
-
- ax1=ugolx(xn1,yn1)
- ax2=ugolx(xn2,yn2)
- ax3=ugolx(xn3,yn3)
- #uprint("ÐšÐ¾Ð½ÐµÑ‡Ð½Ð°Ñ Ñ‚Ð¾Ñ‡ÐºÐ°: ",xpk,ypk)
-
- #uprint('a1 = ',a1)
- #uprint('ax1 = ',ax1)
- # uprint('a2 = ',a2)
- #uprint('ax2 = ',ax2)
- #uprint('ax3 = ',ax3)
-
- propodhod=True
- stop_point = xpb, ypb, ax2
- destination.append(stop_point)
- stop_point = xpk,ypk, ax3
- destination.append(stop_point)
- else:
- xn1,yn1=xn0,yn0
- xn2,yn2=Napravlenie(xu,yu,xpk,ypk)
- a1=ugol(xn1,yn1,xn2,yn2)
-
- xn3,yn3=Napravlenie(Lx,0,xm,ym)
- a2=ugol(xn2,yn2,xn3,yn3)
-
-
- ax1=ugolx(xn2,yn2)
- ax2=ugolx(xn3,yn3)
-
- #uprint("ÐšÐ¾Ð½ÐµÑ‡Ð½Ð°Ñ Ñ‚Ð¾Ñ‡ÐºÐ°: ",xpk,ypk)
- # uprint('a1 = ',a1)
- #uprint('ax1 = ',ax1)
- # uprint('a2 = ',a2)
- #uprint('ax2 = ',ax2)
- propodhod=False
-
- stop_point = xpk,ypk, ax2
- destination.append(stop_point)
- return destination
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/bno055.py b/src/azer_robocup_project/Soccer/Motion/bno055.py
deleted file mode 100644
index d4aa62c..0000000
--- a/src/azer_robocup_project/Soccer/Motion/bno055.py
+++ /dev/null
@@ -1,93 +0,0 @@
-import struct
-import pyb
-CONFIG_MODE = 0x00
-ACCONLY_MODE = 0x01
-MAGONLY_MODE = 0x02
-GYRONLY_MODE = 0x03
-ACCMAG_MODE = 0x04
-ACCGYRO_MODE = 0x05
-MAGGYRO_MODE = 0x06
-AMG_MODE = 0x07
-IMUPLUS_MODE = 0x08
-COMPASS_MODE = 0x09
-M4G_MODE = 0x0a
-NDOF_FMC_OFF_MODE = 0x0b
-NDOF_MODE = 0x0c
-AXIS_P0 = bytes([0x21, 0x04])
-AXIS_P1 = bytes([0x24, 0x00])
-AXIS_P2 = bytes([0x24, 0x06])
-AXIS_P3 = bytes([0x21, 0x02])
-AXIS_P4 = bytes([0x24, 0x03])
-AXIS_P5 = bytes([0x21, 0x01])
-AXIS_P6 = bytes([0x21, 0x07])
-AXIS_P7 = bytes([0x24, 0x05])
-_MODE_REGISTER = 0x3d
-_POWER_REGISTER = 0x3e
-_AXIS_MAP_CONFIG = 0x41
-class BNO055:
- def __init__(self, i2c, address=0x28, mode = NDOF_MODE, axis = AXIS_P4):
- self.i2c = i2c
- self.address = address
- if self.read_id() != bytes([0xA0, 0xFB, 0x32, 0x0F]):
- raise RuntimeError('Failed to find expected ID register values. Check wiring!')
- self.operation_mode(CONFIG_MODE)
- self.system_trigger(0x20)
- pyb.delay(700)
- self.power_mode(0x00)
- self.axis(axis)
- self.page(0)
- pyb.delay(10)
- self.operation_mode(mode)
- self.system_trigger(0x80)
- pyb.delay(200)
- def read_registers(self, register, size=1):
- return(self.i2c.readfrom_mem(self.address, register, size))
- def write_registers(self, register, data):
- self.i2c.writeto_mem(self.address, register, data)
- def operation_mode(self, mode=None):
- if mode:
- self.write_registers(_MODE_REGISTER, bytes([mode]))
- else:
- return(self.read_registers(_MODE_REGISTER, 1)[0])
- def system_trigger(self, data):
- self.write_registers(0x3f, bytes([data]))
- def power_mode(self, mode=None):
- if mode:
- self.write_registers(_POWER_REGISTER, bytes([mode]))
- else:
- return(self.read_registers(_POWER_REGISTER, 1))
- def page(self, num=None):
- if num:
- self.write_registers(0x3f, bytes([num]))
- else:
- self.read_registers(0x3f)
- def temperature(self):
- return(self.read_registers(0x34, 1)[0])
- def read_id(self):
- return(self.read_registers(0x00, 4))
- def axis(self, placement=None):
- if placement:
- self.write_registers(_AXIS_MAP_CONFIG, placement)
- else:
- return(self.read_registers(_AXIS_MAP_CONFIG, 2))
- def quaternion(self):
- data = struct.unpack("<hhhh", self.read_registers(0x20, 8))
- return [d/(1<<14) for d in data]
- def euler(self):
- data = struct.unpack("<hhh", self.read_registers(0x1A, 6))
- return [d/16 for d in data]
- def accelerometer(self):
- data = struct.unpack("<hhh", self.read_registers(0x08, 6))
- return [d/100 for d in data]
- def magnetometer(self):
- data = struct.unpack("<hhh", self.read_registers(0x0E, 6))
- return [d/16 for d in data]
- def gyroscope(self):
- data = struct.unpack("<hhh", self.read_registers(0x14, 6))
- return [d/900 for d in data]
- def linear_acceleration(self):
- data = struct.unpack("<hhh", self.read_registers(0x28, 6))
- return [d/100 for d in data]
- def gravity(self):
- data = struct.unpack("<hhh", self.read_registers(0x2e, 6))
- return [d/100 for d in data]
diff --git a/src/azer_robocup_project/Soccer/Motion/class_Motion.py b/src/azer_robocup_project/Soccer/Motion/class_Motion.py
deleted file mode 100644
index aefe971..0000000
--- a/src/azer_robocup_project/Soccer/Motion/class_Motion.py
+++ /dev/null
@@ -1,974 +0,0 @@
-# Walking engine for Starkit Kondo OpenMV
-# Copyright STARKIT Soccer team of MIPT
-
-import sys, os
-import math, time, json
-
-#from ball_Approach_Steps_Seq import *
-from compute_Alpha_v3 import Alpha
-#from path_planning import PathPlan
-
-def uprint(*text):
- #with open(current_work_directory + "Soccer/log/output.txt",'a') as f:
- # print(*text, file = f)
- print(*text )
-
-class Motion1:
-
- def __init__(self, glob, vision):
- self.glob = glob
- self.params = self.glob.params
- self.ACTIVESERVOS = [(10,2),(9,2),(8,2),(7,2),(6,2),(5,2),(4,2),
- (3,2),(2,2),(1,2),(0,2),(10,1),(9,1),(8,1),
- (7,1),(6,1),(5,1),(4,1),(3,1),(2,1),(1,1)]
- # (10,2) Прав Стопа Вбок, (9,2) Прав Стопа Вперед,(8,2) Прав Голень, (7,2) Прав Колено,
- # (6,2) Прав Бедро, (5,2) Прав Таз, (1,2) Прав Ключица,(4,2) Прав Локоть, (0,2) ТорÑ
- # (10,1) Лев Стопа Вбок, (9,1) Лев Стопа Вперед, (8,1) Лев Голень, (7,1) Лев Колено
- # (6,1) Лев Бедро, (5,1) Лев Таз, (1,1) Лев Ключица, (4,1) Лев Локоть
-
- #FACTOR = [ 1,-1,-1,1,-1,-1, 1,1,1,-1,1,-1,-1, 1,1,-1,-1, 1,1,1,-1,-1, 1] # v2.3
- self.FACTOR = [ 1,1,1,-1,1,1, 1,1,1,1,1,1,1, 1,-1,1,1, 1,1,1,1, 1, 1] # Surrogat 1
- #self.FACTOR = [ 1,1,1,1,-1,1, 1,1,1,1,1,-1,1, 1,-1,1,1,-1,1,1,1, -1, -1] # trunk 1
- a5 = 21.5 # мм раÑÑтоÑние от оÑи Ñимметрии до оÑи Ñервы 5
- b5 = 18.5 # мм раÑÑтоÑние от оÑи Ñервы 5 до оÑи Ñервы 6 по горизонтали
- c5 = 0 # мм раÑÑтоÑние от оÑи Ñервы 6 до Ð½ÑƒÐ»Ñ Z по вертикали
- a6 = 42 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 7
- a7 = 65.5 # мм раÑÑтоÑние от оÑи Ñервы 7 до оÑи Ñервы 8
- a8 = 63.8 # мм раÑÑтоÑние от оÑи Ñервы 8 до оÑи Ñервы 9
- a9 = 35.5 # мм раÑÑтоÑние от оÑи Ñервы 9 до оÑи Ñервы 10
- a10= 25.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до центра Ñтопы по горизонтали
- b10= 16.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до низа Ñтопы
- c10 = 12 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 10 по горизонтали
- self.SIZES = [ a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 ]
- self.d10 = 53.4 #53.4 # раÑÑтоÑние по Y от центра Ñтопы до оÑи робота
- limAlpha5 = [-2667, 2667]
- limAlpha6 = [-3000, 740]
- limAlpha7 = [-3555, 3260]
- limAlpha8 = [-4150, 1777]
- limAlpha9 = [-4000, 2960]
- limAlpha10 =[-2815, 600]
- LIMALPHA = [limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10]
- self.MOTION_SLOT_DICT = {0:['',0], 1:['',0], 2:['',0], 3:['',0], 4:['',0], 5:['Get_Up_Inentification',7000],
- 6:['Soccer_Get_UP_Stomach_N', 5000], 7:['Soccer_Get_UP_Face_Up_N', 5000], 8:['',0], 9:['',0], 10:['',0],
- 11:['',0], 12:['',0], 13:['',0], 14:['',0], 15:['',0],
- 16:['',0], 17:['',0], 18:['Soccer_Kick_Forward_Right_Leg',5000], 19: ['Soccer_Kick_Forward_Left_Leg',5000], 20:['',0],
- 21:['Get_Up_From_Defence',1000], 22:['',0], 23:['PanaltyDefenceReady_Fast',500], 24:['PenaltyDefenceF',300], 25:['Zummer',0],
- 26:['Soccer_Speed_UP',0], 27:['',0], 28:['',0], 29:['',0], 30:['',0],
- 31:['',0], 32:['',0], 33: ['',0], 34:['',0],
- 35: ['',0], 36: ['PenaltyDefenceR',2000], 37: ['PenaltyDefenceL',2000]}
- self.TIK2RAD = 0.00058909
- self.slowTime = 0.0 # seconds
- self.simThreadCycleInMs = 20
- self.frame_delay = self.glob.params['FRAME_DELAY']
- self.frames_per_cycle = self.glob.params['FRAMES_PER_CYCLE']
- self.motion_shift_correction_x = -self.glob.params['MOTION_SHIFT_TEST_X'] / 21
- self.motion_shift_correction_y = -self.glob.params['MOTION_SHIFT_TEST_Y'] / 21
- self.first_step_yield = self.glob.first_step_yield
- self.cycle_step_yield = self.glob.cycle_step_yield
- self.side_step_right_yield = self.glob.side_step_right_yield
- self.side_step_left_yield = self.glob.side_step_left_yield
- self.imu_drift_speed = math.radians(self.glob.params['IMU_DRIFT_IN_DEGREES_DURING_6_MIN_MEASUREMENT'])/360
- self.stepLength = 0.0 # -50 - +70. Best choise 64 for forward. Maximum safe value for backward step -50.
- self.sideLength = 0.0 # -20 - +20. Side step length to right (+) and to left (-)
- self.rotation = 0 # -45 - +45 degrees Centigrade per step + CW, - CCW.
- self.first_Leg_Is_Right_Leg = True
- # Following paramenetrs Not recommended for change
- self.amplitude = 32 # mm side amplitude (maximum distance between most right and most left position of Center of Mass) 53.4*2
- self.fr1 =8 # frame number for 1-st phase of gait ( two legs on floor)
- self.fr2 = 12 # frame number for 2-nd phase of gait ( one leg in air)
- self.gaitHeight= 180 # Distance between Center of mass and floor in walk pose
- self.stepHeight = 32.0 # elevation of sole over floor
- self.initPoses = 400//self.simThreadCycleInMs
- self.limAlpha1 =LIMALPHA
- self.limAlpha1[3][1]=0
- # end of paramenetrs Not recommended for change
- self.al = Alpha()
- self.exitFlag = 0
- self.falling_Flag = 0
- self.neck_pan = 0
- self.old_neck_pan = 0
- self.body_euler_angle ={}
- self.head_quaternion = None
- self.local = 0 # Local
- self.vision = vision
- #self.p = PathPlan(self.glob)
- self.old_neck_tilt = 0
- self.direction_To_Attack = 0
- self.activePose = []
- self.xtr = 0
- self.ytr = -self.d10 #-53.4
- self.ztr = -self.gaitHeight
- self.xr = 0
- self.yr = 0
- self.zr = -1
- self.wr = 0
- self.xtl = 0
- self.ytl = self.d10 # 53.4
- self.ztl = -self.gaitHeight
- self.xl = 0
- self.yl = 0
- self.zl = -1
- self.wl = 0
- self.euler_angle = {}
- self.robot_In_0_Pose = False
- #self.start_point_for_imu_drift = 0
- self.ACTIVEJOINTS = ['Leg_right_10','Leg_right_9','Leg_right_8','Leg_right_7','Leg_right_6','Leg_right_5','hand_right_4',
- 'hand_right_3','hand_right_2','hand_right_1','Tors1','Leg_left_10','Leg_left_9','Leg_left_8',
- 'Leg_left_7','Leg_left_6','Leg_left_5','hand_left_4','hand_left_3','hand_left_2','hand_left_1','head0','head12']
- if self.glob.SIMULATION == 2 :
- #from button_test import wait_for_button_pressing
- import starkit
- import pyb
- from kondo_controller import Rcb4BaseLib
- from pyb import Pin, UART, LED
- import sensor as s
- from machine import I2C
- from bno055 import BNO055, AXIS_P7
- import utime
- self.utime = utime
- #self.wait_for_button_pressing = wait_for_button_pressing
- self.starkit = starkit
- i2c = I2C(2)
- self.imu = BNO055(i2c, mode = 0x08)
- self.start_point_for_imu_drift = self.utime.time()
- self.green_led = LED(2)
- self.pin2 = Pin('P2', Pin.IN, Pin.PULL_UP)
- uart = UART(self.glob.params['UART_PORT'], self.glob.params['UART_SPEED'], timeout=1000, parity=0)
- self.kondo = Rcb4BaseLib()
- self.kondo.open(uart)
- self.clock = time.clock()
- self.sensor = s
- self.reset_camera_2_Normal_exposure()
- self.kondo.motionPlay(25) # zummer
- self.pyb = pyb
- with open(self.glob.current_work_directory + "Init_params/Real/Real_calibr.json", "r") as f:
- data1 = json.loads(f.read())
- self.neck_calibr = data1['neck_calibr']
- self.neck_play_pose = data1['neck_play_pose']
- self.head_pitch_with_horizontal_camera = data1['head_pitch_with_horizontal_camera']
- self.neck_tilt = self.neck_calibr
- self.start_point_for_imu_drift = self.utime.time()
- self.relative_ball_position = [0,0]
- self.camera_counter = 0
-
- #-------------------------------------------------------------------------------------------------------------------------------
- def reset_camera_2_Normal_exposure(self):
- self.sensor.reset()
- self.sensor.set_pixformat(self.sensor.RGB565)
- self.sensor.set_framesize(self.sensor.QVGA)
- self.sensor.skip_frames(time = 500)
- self.sensor.set_auto_exposure(False)
- self.sensor.set_auto_whitebal(False)
- self.sensor.skip_frames(time = 500)
- self.sensor.set_auto_whitebal(False, rgb_gain_db = (-6.0, -3.0, 3.0))
-
- def reset_camera_2_Short_exposure(self):
- self.sensor.reset() # Initialize the camera sensor.
- self.sensor.set_pixformat(self.sensor.RGB565)
- self.sensor.set_framesize(self.sensor.QQVGA)
- self.sensor.skip_frames(time = 2000) # Let new settings take affect.
- self.sensor.set_auto_gain(False, gain_db= 15) # must be turned off for color tracking
- self.sensor.skip_frames(time = 500)
- print('gain: ', self.sensor.get_gain_db())
- self.sensor.set_auto_exposure(False, 2500)
- self.sensor.skip_frames(time = 500)
- print('exposure: ', self.sensor.get_exposure_us())
- self.sensor.set_auto_whitebal(False) # must be turned off for color tracking
- self.sensor.skip_frames(time = 500)
- self.sensor.set_auto_whitebal(False, rgb_gain_db = (-6.0, -3.0, 3.0))
-
- def check_camera(self):
- if self.glob.camera_ON:
- if self.glob.SIMULATION == 2 :
- img = self.sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold=self.vision.TH['orange ball']['pixel'],
- area_threshold=self.vision.TH['orange ball']['area'],
- merge=True, margin=10)
- else:
- img_ = self.vision_Sensor_Get_Image()
- self.vision_Sensor_Display(img_)
- img = self.re.Image(img_)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold = self.vision.TH['orange ball']['pixel'],
- area_threshold = self.vision.TH['orange ball']['area'],
- merge=True)
- if not blobs:
- self.relative_ball_position = [0,0]
- return
- blobs = self.sorted_blobs(blobs, number_of_blobs=1)
- course_and_distance_to_ball = self.get_course_and_distance_to_ball(blobs[0])
- ball_y = course_and_distance_to_ball[1] * math.sin(course_and_distance_to_ball[0]) * 1000
- ball_x = course_and_distance_to_ball[1] * math.cos(course_and_distance_to_ball[0]) * 1000
- if self.camera_counter == 0:
- ball_x_filtered = ball_x
- ball_y_filtered = ball_y
- else:
- ball_x_filtered = (self.relative_ball_position[0] * self.camera_counter + ball_x) / (self.camera_counter +1)
- ball_y_filtered = (self.relative_ball_position[1] * self.camera_counter + ball_y) / (self.camera_counter +1)
- self.relative_ball_position = [ball_x_filtered, ball_y_filtered]
- self.camera_counter += 1
- return
-
-
- def pause_in_ms(self, time_in_ms):
- if self.glob.SIMULATION == 2: self.pyb.delay(time_in_ms)
- else: self.sim_Progress(time_in_ms/1000)
-
- def imu_body_yaw(self):
- yaw = self.euler_angle['yaw'] + self.neck_pan*self.TIK2RAD
- yaw = self.norm_yaw(yaw)
- return yaw
-
- def norm_yaw(self, yaw):
- yaw %= 2 * math.pi
- if yaw > math.pi: yaw -= 2* math.pi
- if yaw < -math.pi: yaw += 2* math.pi
- return yaw
-
- def from_vrep_quat_to_conventional_quat(self, quaternion):
- x,y,z,w = quaternion
- return [w,x,y,z]
-
- def quaternion_to_euler_angle(self, quaternion):
- euler_angle = {}
- w,x,y,z = quaternion
- ysqr = y*y
- t0 = +2.0 * (w * x + y * z)
- t1 = +1.0 - 2.0 * (x * x + ysqr)
- X = math.degrees(math.atan2(t0,t1))
- t2 = +2.0 * (w * y - z * x)
- t2 = +1.0 if t2 > +1.0 else t2
- t2 = -1.0 if t2 < -1.0 else t2
- Y = math.degrees(math.asin(t2))
- t3 = +2.0 * (w * z + x * y)
- t4 = +1.0 - 2.0 * (ysqr + z * z)
- Z = math.degrees(math.atan2(t3,t4))
- euler_angle['yaw'] = math.radians(Z)
- euler_angle['pitch'] = math.radians(Y)
- euler_angle['roll'] = math.radians(X)
- return euler_angle
-
- def body_euler_angle_calc(self):
- def quat_multi(quat1, quat2):
- w1,x1,y1,z1 = quat1
- w2,x2,y2,z2 = quat2
- """
- Quaternion multiplication
- multi = (w1 + x1*i + y1*j + z1*k) * ( w2 + x2*i + y2*j + z2*k) =
- w1*w2 +x1*w2*i + y1*w2*j + z1*w2*k + w1*x2*i - x1*x2 - y1*x2*k + z1*x2*j +
- w1*y2*j + x1*y2*k - y1*y2 - z1*y2*i + w1*z2*k - x1*z2*j + y1*z2*i - z1*z2 =
- (w1*w2 - x1*x2 - y1*y2 - z1*z2) + (x1*w2 + w1*x2 - z1*y2 + y1*z2)*i +
- (y1*w2 + z1*x2 + w1*y2 - x1*z2)*j + (z1*w2 - y1*x2 + x1*y2 + w1*z2)*k
- """
- w = w1*w2 - x1*x2 - y1*y2 - z1*z2
- x = x1*w2 + w1*x2 - z1*y2 + y1*z2
- y = y1*w2 + z1*x2 + w1*y2 - x1*z2
- z = z1*w2 - y1*x2 + x1*y2 + w1*z2
- return [w,x,y,z]
- def quat_division(quat1, quat2):
- """
- Quaternion division
- """
- w2,x2,y2,z2 = quat2
- coquat2 = [w2, -x2, -y2, -z2]
- quat = quat_multi(quat1, coquat2)
- w,x,y,z = quat
- sqquat2 = w2*w2 + x2*x2 + y2*y2 + z2*z2
- return [w/sqquat2, x/sqquat2, y/sqquat2, z/sqquat2]
- tilt_angle = - self.neck_tilt * self.TIK2RAD
- pan_angle = - self.neck_pan * self.TIK2RAD
- if self.glob.SIMULATION == 2:
- self.body_euler_angle = {'yaw': (self.euler_angle['yaw'] - pan_angle)}
- else:
- Dummy_Hquaternion = self.head_quaternion
- tilt_quat = [math.cos(tilt_angle/2), 0, math.sin(tilt_angle/2), 0]
- pan_quat = [math.cos(pan_angle/2), 0, 0, math.sin(pan_angle/2)]
- body_dummy_quaternion = quat_division(Dummy_Hquaternion, tilt_quat)
- body_dummy_quaternion = quat_division(body_dummy_quaternion, pan_quat)
- self.body_euler_angle = self.quaternion_to_euler_angle(body_dummy_quaternion)
- print('self.neck_tilt =', self.neck_tilt)
-
-
- def push_Button(self):
- from button_test import Button_Test
- button = Button_Test()
- pressed_button = button.wait_for_button_pressing()
- uprint("нажато")
- return pressed_button
-
- def play_Motion_Slot(self, name = ''):
- if self.glob.SIMULATION == 2:
- for key in self.MOTION_SLOT_DICT:
- if self.MOTION_SLOT_DICT[key][0] == name:
- self.kondo.motionPlay(key)
- self.pyb.delay(self.MOTION_SLOT_DICT[key][1])
- else:
- self.simulateMotion(name = name)
-
- def play_Soft_Motion_Slot(self, name = ''): # the slot from file will be played in robot
- if self.glob.SIMULATION == 2:
- with open(self.glob.current_work_directory + "Soccer/Motion/motion_slots/" + name + ".json", "r") as f:
- slots = json.loads(f.read())
- motion_list = slots[name]
- for motion in motion_list:
- servoDatas = []
- for i in range(len(self.ACTIVESERVOS)):
- pos = int(motion[i+1]) + 7500 #- self.servo_Trims[i]
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- frames_number = int(motion[0])
- a=self.kondo.setServoPos (servoDatas, frames_number)
- self.pyb.delay(30 * frames_number)
- else:
- self.simulateMotion(name = name)
-
- def falling_Test(self):
- if self.glob.SIMULATION == 0 or self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- key = 0
- if self.ms.kbhit():
- key = self.ms.getch()
- if key == b'p' :
- self.lock.acquire()
- if self.glob.SIMULATION == 3:
- self.sim.simxPauseSimulation(self.clientID, self.sim.simx_opmode_oneshot)
- key = 0
- while (True):
- if self.ms.kbhit():
- key = self.ms.getch()
- if key == b'p':
- self.lock.release()
- if self.glob.SIMULATION == 3:
- self.sim.simxStartSimulation(self.clientID, self.sim.simx_opmode_oneshot)
- key = 0
- break
- if key == b's' or self.transfer_Data.stop_Flag:
- uprint('Simulation STOP by keyboard')
- self.transfer_Data.stop += 1
- self.sim_Stop()
- while True:
- if self.transfer_Data.stop == self.numberOfRobots:
- self.sim_Disable()
- sys.exit(0)
- time.sleep(0.1)
- self.falling_Flag = 3
- self.transfer_Data.stop_Flag = True
- return self.falling_Flag
- self.refresh_Orientation()
- self.body_euler_angle_calc()
- print('self.body_euler_angle from Quaternion', self.body_euler_angle)
- #returnCode, Dummy_1quaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_1Handle , -1, self.sim.simx_opmode_buffer)
- #Dummy_1quaternion = self.from_vrep_quat_to_conventional_quat(Dummy_1quaternion)
- #self.body_euler_angle = self.quaternion_to_euler_angle(Dummy_1quaternion)
- #print('self.body_euler_angle from Dummy', self.body_euler_angle)
- if (self.body_euler_angle['pitch']) > 0.785:
- self.falling_Flag = 1 # on stomach
- self.head_Up()
- self.simulateMotion(name = 'Soccer_Get_UP_Stomach_N')
- if (self.body_euler_angle['pitch']) < -0.785:
- self.falling_Flag = -1 # face up
- self.head_Up()
- self.simulateMotion(name = 'Soccer_Get_UP_Face_Up')
- if (self.body_euler_angle['roll']) > 0.785:
- self.falling_Flag = -2 # on right side
- self.head_Up()
- self.simulateMotion(name = 'Get_Up_Right')
- if -135< (self.body_euler_angle['roll']) < -0.785:
- self.falling_Flag = 2 # on left side
- self.head_Up()
- self.simulateMotion(name = 'Get_Up_Left')
- if self.glob.SIMULATION == 2:
- ad3 = self.kondo.getAdData(3)
- ad4 = self.kondo.getAdData(4)
- if ad3 < 200:
- self.falling_Flag = 1 # on stomach
- self.head_Up()
- self.play_Soft_Motion_Slot(name = 'Soccer_Get_UP_Stomach_N')
- self.pyb.delay(1000)
- if ad3 > 450:
- self.falling_Flag = -1 # face up
- self.head_Up()
- self.play_Soft_Motion_Slot(name = 'Soccer_Get_UP_Face_Up')
- if ad4 > 400:
- self.falling_Flag = -2 # on right side
- self.head_Up()
- self.play_Soft_Motion_Slot(name = 'Get_Up_Right')
- if ad4 < 160:
- self.falling_Flag = 2 # on left side
- self.head_Up()
- self.play_Soft_Motion_Slot(name = 'Get_Up_Left')
-
- return self.falling_Flag
-
- def computeAlphaForWalk(self,sizes, limAlpha, hands_on = True ):
- angles =[]
- anglesR=[]
- anglesL=[]
- if self.glob.SIMULATION == 2:
- anglesR = self.starkit.alpha_calculation(self.xtr,self.ytr,self.ztr,self.xr,self.yr,self.zr,self.wr, sizes, limAlpha)
- anglesL = self.starkit.alpha_calculation(self.xtl,-self.ytl,self.ztl,self.xl,-self.yl,self.zl,self.wl, sizes, limAlpha)
- else:
- anglesR = self.al.compute_Alpha_v3(self.xtr,self.ytr,self.ztr,self.xr,self.yr,self.zr,self.wr, sizes, limAlpha)
- anglesL = self.al.compute_Alpha_v3(self.xtl,-self.ytl,self.ztl,self.xl,-self.yl,self.zl,self.wl, sizes, limAlpha)
- if len(anglesR)>1:
- for i in range(len(anglesR)):
- if len(anglesR)==1: break
- if anglesR[0][2]<anglesR[1][2]: anglesR.pop(1)
- else: anglesR.pop(0)
- elif len(anglesR)==0:
- return[]
- if len(anglesL)>1:
- for i in range(len(anglesL)):
- if len(anglesL)==1: break
- if anglesL[0][2]<anglesL[1][2]: anglesL.pop(1)
- else: anglesL.pop(0)
- elif len(anglesL)==0:
- return[]
- if self.first_Leg_Is_Right_Leg == True:
- for j in range(6): angles.append(anglesR[0][j])
- if hands_on: angles.append(1.745)
- else: angles.append(0.0)
- angles.append(0.0)
- angles.append(0.0)
- if hands_on: angles.append(0.524 - self.xtl/57.3)
- else: angles.append(0.0)
- angles.append(0.0)
- for j in range(6): angles.append(-anglesL[0][j])
- if hands_on: angles.append(-1.745)
- else: angles.append(0.0)
- angles.append(0.0)
- angles.append(0.0)
- if hands_on: angles.append(-0.524 + self.xtr/57.3)
- else: angles.append(0.0)
- else:
- for j in range(6): angles.append(anglesL[0][j])
- if hands_on: angles.append(1.745)
- else: angles.append(0.0)
- angles.append(0.0)
- angles.append(0.0)
- if hands_on: angles.append(0.524 - self.xtr/57.3)
- else: angles.append(0.0)
- angles.append(0.0) # Tors
- for j in range(6): angles.append(-anglesR[0][j])
- if hands_on: angles.append(-1.745)
- else: angles.append(0.0)
- angles.append(0.0)
- angles.append(0.0)
- if hands_on: angles.append(-0.524 + self.xtl/57.3)
- else: angles.append(0.0)
- self.activePose = angles
- return angles
-
- def activation(self):
- if self.glob.SIMULATION == 2:
- yaw, roll, pitch = self.imu.euler()
- if 0<= yaw < 180: yaw = -yaw
- if 180<= yaw <=360: yaw = 360 -yaw
- #self.euler_angle = [yaw, roll, pitch]
- self.euler_angle['yaw'] = math.radians(yaw)
- self.euler_angle['pitch'] = math.radians(90 - pitch)
- self.euler_angle['roll'] = math.radians(roll)
- self.direction_To_Attack += self.euler_angle['yaw']
- else:
- time.sleep(0.1)
- if self.glob.SIMULATION != 0:
- self.sim.simxStartSimulation(self.clientID,self.sim.simx_opmode_oneshot)
- returnCode, Dummy_1quaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_1Handle , -1, self.sim.simx_opmode_buffer)
- Dummy_1quaternion = self.from_vrep_quat_to_conventional_quat(Dummy_1quaternion)
- self.body_euler_angle = self.quaternion_to_euler_angle(Dummy_1quaternion)
- returnCode, Dummy_Hquaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- Dummy_Hquaternion = self.from_vrep_quat_to_conventional_quat(Dummy_Hquaternion)
- self.euler_angle = self.quaternion_to_euler_angle(Dummy_Hquaternion)
- self.direction_To_Attack += self.euler_angle['yaw']
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- self.Dummy_HData.append(Dummy_Hposition)
- returnCode, self.Ballposition= self.sim.simxGetObjectPosition(self.clientID, self.BallHandle , -1, self.sim.simx_opmode_buffer)
- self.direction_To_Attack = self.norm_yaw(self.direction_To_Attack)
- self.glob.imu_drift_last_correction_time = self.utime.time()
-
- def reOrderServoData(self, servoDatas):
- order = [0, 11, 1, 12, 2, 13, 3, 14, 4, 15, 5, 16, 6, 17, 7, 18, 8, 19, 9, 20, 10]
- servoDatasOrdered = []
- for orderNumber in order:
- servoDatasOrdered.append(servoDatas[orderNumber])
- return servoDatasOrdered
-
- def walk_Initial_Pose(self):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- self.xtr = self.xtl = 0
- framestep = self.simThreadCycleInMs//10
- for j in range (self.initPoses):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- self.ztr = -223.0 + j*(223.0-self.gaitHeight)/self.initPoses
- self.ztl = -223.0 + j*(223.0-self.gaitHeight)/self.initPoses
- self.ytr = -self.d10 - j*self.amplitude/2 /self.initPoses
- self.ytl = self.d10 - j*self.amplitude/2 /self.initPoses
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID,
- self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- self.pyb.delay(self.frame_delay - time1)
-
- def walk_Cycle(self, stepLength,sideLength, rotation,cycle, number_Of_Cycles):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- self.stepLength = stepLength + self.motion_shift_correction_x
- self.sideLength = sideLength - self.motion_shift_correction_y
- self.rotation = math.degrees(rotation)
- if self.rotation <= 0:
- rotation = -self.rotation/222 * 0.23 / self.params['ROTATION_YIELD_RIGHT']
- else:
- rotation = -self.rotation/222 * 0.23 / self.params['ROTATION_YIELD_LEFT']
- alpha = 0
- if self.fr1 == 0:
- alpha01 = math.pi
- else:
- alpha01 = math.pi/self.fr1*2
- frameNumberPerCycle = 2*self.fr1+2*self.fr2
- framestep = self.simThreadCycleInMs//10
- xtl0 = self.stepLength * (1 - (self.fr1 + self.fr2 + 2 * framestep) / (2*self.fr1+self.fr2+ 2 * framestep)) * 1.5 # 1.5 - podgon
- xtr0 = self.stepLength * (1/2 - (self.fr1 + self.fr2 + 2 * framestep ) / (2*self.fr1+self.fr2+ 2 * framestep))
- dx0_typical = self.stepLength/(2*self.fr1+self.fr2+ 2 * framestep)*framestep # CoM propulsion forward per framestep
- dy0_typical = (self.sideLength)/(2 * self.fr2)*framestep # CoM propulsion sideways per framestep
- xr_old, xl_old, yr_old, yl_old = self.xr, self.xl, self.yr, self.yl
- # correction of body tilt forward
- self.xr, self.xl = self.params['BODY_TILT_AT_WALK'], self.params['BODY_TILT_AT_WALK'] #
- # correction of sole skew depending on side angle of body when step pushes land
- self.yr, self.yl = - self.params['SOLE_LANDING_SKEW'], self.params['SOLE_LANDING_SKEW']
- for iii in range(0,frameNumberPerCycle,framestep):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- if 0<= iii <self.fr1 : # FASA 1
- alpha = alpha01 * iii/2
- S = (self.amplitude/2 )*math.cos(alpha)
- self.ytr = S - self.d10 + self.sideLength/2
- self.ytl = S + self.d10 + self.sideLength/2
- self.ztl = -self.gaitHeight
- self.ztr = -self.gaitHeight
- if cycle ==0: continue
- else: dx0 = dx0_typical
- self.xtl = xtl0 - dx0 - dx0 * iii/framestep
- self.xtr = xtr0 - dx0 - dx0 * iii/framestep
-
- if self.fr1+self.fr2<=iii<2*self.fr1+self.fr2 : # FASA 3
- alpha = alpha01 * (iii-self.fr2)/2
- S = (self.amplitude/2 )*math.cos(alpha)
- self.ytr = S - self.d10 - self.sideLength/2
- self.ytl = S + self.d10 + self.sideLength/2
- self.ztl = -self.gaitHeight
- self.ztr = -self.gaitHeight
- dx0 = dx0_typical
- self.xtl -= dx0
- self.xtr -= dx0
-
- if self.fr1<= iii <self.fr1+self.fr2: # FASA 2
- self.ztr = -self.gaitHeight + self.stepHeight
- if cycle ==0:
- dx = self.stepLength/(self.fr2- 2 * framestep)*framestep/2
- dx0 = dx0_typical
- dy = self.sideLength/self.fr2*framestep
- dy0 = dy0_typical
- else:
- dx = self.stepLength/(self.fr2- 2 * framestep)*framestep #* 0.75
- dx0 = dx0_typical
- dy = self.sideLength/self.fr2*framestep
- dy0 = dy0_typical
- if iii==self.fr1:
- self.xtr -= dx0
- self.ytr = S - self.d10 + dy0
- elif iii == (self.fr1 +self.fr2 - framestep):
- self.xtr -= dx0
- self.ytr = S - self.d10 + 2*dy0 - self.sideLength
- else:
- self.xtr += dx
- self.ytr = S - 64 + dy0 - dy*self.fr2/(self.fr2- 2 * framestep)*((iii - self.fr1)/2)
- self.wr = self.wl = rotation -(iii-self.fr1)* rotation/(self.fr2- 2 * framestep)*2
- self.xtl -= dx0
- self.ytl += dy0
-
- if 2*self.fr1+self.fr2<= iii : # FASA 4
- self.ztl = -self.gaitHeight + self.stepHeight
- if cycle == number_Of_Cycles - 1:
- dx0 = dx0_typical * 4 / self.fr2 # 8.75/6
- dx = (self.stepLength*(self.fr1+self.fr2)/(4*self.fr1)+2*dx0)/(self.fr2- 2 * framestep) *framestep / 1.23076941 # 1.23076941 = podgon
- if iii== (2*self.fr1 + 2*self.fr2 - framestep):
- self.ztl = -self.gaitHeight
- self.ytl = S + self.d10
- else:
- dx = self.stepLength/(self.fr2- 2 * framestep) *framestep # * 0.75
- dx0 = dx0_typical
- dy = self.sideLength/(self.fr2- 2 * framestep) *framestep
- dy0 = dy0_typical
- if iii== (2*self.fr1 + self.fr2 ):
- self.xtl -= dx0
- self.ytl = S + 64 + self.sideLength/2
- elif iii== (2*self.fr1 + 2*self.fr2 - framestep):
- self.xtl -= dx0
- self.ytl = S + self.d10 + self.sideLength/2
- else:
- self.xtl += dx
- self.ytl = S + 64 + self.sideLength/2
- self.wr = self.wl = (iii-(2*self.fr1+self.fr2))* rotation/(self.fr2- 2 * framestep) *2 - rotation
- self.xtr -= dx0
- self.ytr += dy0
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- #if iii == self.fr1+self.fr2:
- if iii == 0: self.camera_counter = 0
- self.check_camera()
- if not self.falling_Flag ==0: return
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- self.Dummy_HData.append(Dummy_Hposition)
- returnCode, self.Ballposition= self.sim.simxGetObjectPosition(self.clientID, self.BallHandle , -1, self.sim.simx_opmode_buffer)
- self.BallData.append(self.Ballposition)
- returnCode, Dummy_1quaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_1Handle , -1, self.sim.simx_opmode_buffer)
- Dummy_1quaternion = self.from_vrep_quat_to_conventional_quat(Dummy_1quaternion)
- self.body_euler_angle = self.quaternion_to_euler_angle(Dummy_1quaternion)
- self.timeElapsed = self.timeElapsed +1
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoSpeedDatas =[]
- for i in range(7,10):
- servoSpeedDatas.append(self.kondo.ServoData(i, 1, 127))
- servoSpeedDatas.append(self.kondo.ServoData(i, 2, 127))
- self.kondo.setServoSpeed(servoSpeedDatas)
- servoDatas = []
- disp = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- disp.append((self.ACTIVEJOINTS[i],self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos -7500))
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- start2 = self.pyb.millis()
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- time2 = self.pyb.elapsed_millis(start2)
- self.pyb.delay(self.frame_delay - time1)
- # returning xr, xl, yr, yl to initial value
- self.xr, self.xl, self.yr, self.yl = xr_old, xl_old, yr_old, yl_old
- self.local.coord_shift[0] = self.cycle_step_yield*stepLength/64/1000
- if self.first_Leg_Is_Right_Leg:
- self.local.coord_shift[1] = -self.side_step_right_yield * abs(sideLength)/20/1000
- else: self.local.coord_shift[1] = self.side_step_left_yield * abs(sideLength)/20/1000
- self.local.coordinate_record(odometry = True, shift = True)
-
- def walk_Final_Pose(self):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- framestep = self.simThreadCycleInMs//10
- for j in range (self.initPoses):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- self.ztr = -self.gaitHeight - (j+1)*(223.0-self.gaitHeight)/self.initPoses
- self.ztl = -self.gaitHeight - (j+1)*(223.0-self.gaitHeight)/self.initPoses
- self.ytr = -self.d10 + (self.initPoses-(j+1))*self.amplitude/2 /self.initPoses
- self.ytl = self.d10 + (self.initPoses-(j+1))*self.amplitude/2 /self.initPoses
- if j == self.initPoses - 1:
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1, hands_on = False)
- else: angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID,
- self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- start2 = self.pyb.millis()
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- #uprint(servoDatas)
- #uprint(clock.avg())
- time1 = self.pyb.elapsed_millis(start1)
- time2 = self.pyb.elapsed_millis(start2)
- self.pyb.delay(self.frame_delay - time1)
-
- def walk_Final_Pose_After_Kick(self):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- xr_old, xl_old, yr_old, yl_old = self.xr, self.xl, self.yr, self.yl
- # correction of body tilt forward
- self.xr, self.xl = self.params['BODY_TILT_AT_KICK'], self.params['BODY_TILT_AT_KICK']
- framestep = self.simThreadCycleInMs//10
- pose_taking_cycles = 2
- for j in range (pose_taking_cycles):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- self.ztr = -self.gaitHeight - (j+1)*(223.0-self.gaitHeight)/pose_taking_cycles #149 223.0
- self.ztl = -self.gaitHeight - (j+1)*(223.0-self.gaitHeight)/pose_taking_cycles # 149 223.0
- self.ytr = -self.d10 - (pose_taking_cycles-(j+1))*self.amplitude/2 /pose_taking_cycles
- self.ytl = self.d10 - (pose_taking_cycles-(j+1))*self.amplitude/2 /pose_taking_cycles
- if j == pose_taking_cycles - 1:
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1, hands_on = False)
- else: angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- #if not self.falling_Flag ==0: return
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID,
- self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- start2 = self.pyb.millis()
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- time2 = self.pyb.elapsed_millis(start2)
- self.pyb.delay(self.frame_delay - time1)
- # returning xr, xl, yr, yl to initial value
- self.xr, self.xl, self.yr, self.yl = xr_old, xl_old, yr_old, yl_old
-
- def kick(self, first_Leg_Is_Right_Leg, small = False, oneoff_kick = True):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- gaitHeight = 210
- #gaitHeight = 180
- stepHeight = 55
- #stepHeight = 45
- stepLength = 64
- kick_size = 70
- if small : kick_size = -10
- tmp1 = self.first_Leg_Is_Right_Leg
- self.first_Leg_Is_Right_Leg = first_Leg_Is_Right_Leg
- tmp = self.gaitHeight
- self.gaitHeight = gaitHeight
- if oneoff_kick: self.walk_Initial_Pose()
- alpha = 0
- alpha01 = math.pi/self.fr1*2
- frameNumberPerCycle = 2*self.fr1+2*self.fr2
- framestep = self.simThreadCycleInMs//10
- dx0_typical = self.stepLength/(2*self.fr1+self.fr2+ 2 * framestep)*framestep
- xr_old, xl_old, yr_old, yl_old = self.xr, self.xl, self.yr, self.yl
- # correction of body tilt forward
- self.xr, self.xl = self.params['BODY_TILT_AT_KICK'], self.params['BODY_TILT_AT_KICK'] #
- # correction of sole skew depending on side angle of body when step pushes land
- self.yr, self.yl = - self.params['SOLE_LANDING_SKEW'], self.params['SOLE_LANDING_SKEW']
- for iii in range(0,frameNumberPerCycle,framestep):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- if 0<= iii <self.fr1 :
- alpha = alpha01 * (iii/2+0.5*framestep)
- S = (self.amplitude/2 )*math.cos(alpha)
- self.ytr = S - self.d10
- self.ytl = S + self.d10
- self.ztl = -gaitHeight
- self.ztr = -gaitHeight
- continue
- if self.fr1+self.fr2<=iii<2*self.fr1+self.fr2 :
- alpha = alpha01 * ((iii-self.fr2)/2+0.5*framestep)
- S = (self.amplitude/2)*math.cos(alpha)
- self.ytr = S - self.d10
- self.ytl = S + self.d10
- self.ztl = -gaitHeight
- self.ztr = -gaitHeight
- dx0 = dx0_typical
- self.xtl -= dx0
- self.xtr -= dx0
- if self.fr1<= iii <self.fr1+self.fr2:
- self.ztr = -gaitHeight + stepHeight
- dx = stepLength/2/self.fr2*2
- dx0 = stepLength/(2*self.fr1+self.fr2+4)*framestep
- if iii==self.fr1:
- self.xtr -= dx0
- self.ytr = S - 64
- elif iii == (self.fr1 +self.fr2 - 2):
- self.xtr -= dx0
- self.ytr = S - 64
- else:
- self.xtr += dx*self.fr2/(self.fr2-2 * framestep)
- self.ytr = S - 64
- if iii == self.fr1 +self.fr2 - 10: self.xtr += kick_size
- if iii == self.fr1 +self.fr2 - 4: self.xtr -= kick_size
- self.xtl -= dx0
- if 2*self.fr1+self.fr2<= iii :
- self.ztl = -gaitHeight + stepHeight
- dx0 = dx0_typical * 4 / self.fr2 # 8.75/6
- dx = (stepLength*(self.fr1+self.fr2)/(4*self.fr1)+2*dx0)/(self.fr2 - 2 * framestep) * framestep
- if iii== (2*self.fr1 + 2*self.fr2 - framestep):
- self.ztl = -gaitHeight
- self.ytl = S + self.d10
-
- if iii== (2*self.fr1 + self.fr2 ):
- self.xtl -= dx0
- self.ytl = S + 64
- elif iii== (2*self.fr1 + 2*self.fr2 - framestep):
- self.xtl -= dx0
- self.ytl = S + 64
- else:
- self.xtl += dx
- self.ytl = S + 64
- self.xtr -= dx0
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- if not self.falling_Flag ==0: return
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- #uprint(self.euler_angle)
- self.Dummy_HData.append(Dummy_Hposition)
- returnCode, self.Ballposition= self.sim.simxGetObjectPosition(self.clientID, self.BallHandle , -1, self.sim.simx_opmode_buffer)
- self.BallData.append(self.Ballposition)
- self.timeElapsed = self.timeElapsed +1
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- disp = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- disp.append((self.ACTIVEJOINTS[i],self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos -7500))
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- a=self.kondo.setServoPos (servoDatas,self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- self.pyb.delay(self.frame_delay - time1)
- # returning xr, xl, yr, yl to initial value
- self.xr, self.xl, self.yr, self.yl = xr_old, xl_old, yr_old, yl_old
- self.walk_Final_Pose_After_Kick()
- self.pause_in_ms(100)
- self.local.coord_shift[0] = self.first_step_yield/2000
- self.local.coord_shift[1] = 0
- self.local.coordinate_record(odometry = True, shift = True)
- self.gaitHeight = tmp
- self.first_Leg_Is_Right_Leg = tmp1
-
- def refresh_Orientation(self):
- if self.glob.SIMULATION == 2:
- yaw, roll, pitch = self.imu.euler()
- if 0<= yaw < 180: yaw = -yaw
- if 180<= yaw <=360: yaw = 360 -yaw
- self.euler_angle['yaw'] = math.radians(yaw)
- self.euler_angle['pitch'] = math.radians(90 - pitch)
- self.euler_angle['roll'] = math.radians(roll)
- self.euler_angle['yaw']-= self.direction_To_Attack
- print("euler_angle['yaw']: ", self.euler_angle['yaw'])
- else:
- returnCode, Dummy_Hquaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- Dummy_Hquaternion = self.from_vrep_quat_to_conventional_quat(Dummy_Hquaternion)
- self.euler_angle = self.quaternion_to_euler_angle(Dummy_Hquaternion)
- #self.euler_angle[0] = math.radians(self.euler_angle[0])
- self.euler_angle['yaw']-= self.direction_To_Attack
- self.head_quaternion = Dummy_Hquaternion
- self.euler_angle['yaw'] += self.imu_drift_speed * (self.utime.time() - self.start_point_for_imu_drift)
- self.euler_angle['yaw'] += self.glob.imu_drift_correction
-
-if __name__=="__main__":
- print('This is not main module!')
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/class_Motion_real.py b/src/azer_robocup_project/Soccer/Motion/class_Motion_real.py
deleted file mode 100644
index c99b9da..0000000
--- a/src/azer_robocup_project/Soccer/Motion/class_Motion_real.py
+++ /dev/null
@@ -1,1082 +0,0 @@
-# Walking engine for Starkit Kondo OpenMV
-# Copyright STARKIT Soccer team of MIPT
-
-import math, time, json
-import utility
-
-from class_Motion import Motion1
-#from ball_Approach_Steps_Seq import *
-
-def uprint(*text):
- print(*text )
-
-class Motion_real(Motion1):
-
- def __init__(self, glob, vision):
- super().__init__(glob, vision)
-
- def head_Tilt_Calibration(self):
- # ÐšÐ°Ð»Ð¸Ð±Ñ€Ð°Ñ†Ð¸Ñ Ðаклона камеры. УÑтановить мÑч на раÑÑтоÑнии (100 Ñм)по низу мÑча.
- # ÐŸÐ¾Ð»ÑƒÑ‡ÐµÐ½Ð½Ð°Ñ Ð²ÐµÐ»Ð¸Ñ‡Ð¸Ð½Ð° наклона камеры Ñквивалентна (69) градуÑа от вертикали.
- # Ð’Ñ‚Ð¾Ñ€Ð°Ñ Ð¿Ð¾Ð·Ð¸Ñ†Ð¸Ñ Ð³Ð¾Ð»Ð¾Ð²Ñ‹ 23 градуÑа к вертикали отличаетÑÑ Ð¾Ñ‚ первой на 1155
- return_value = 0
- if self.glob.SIMULATION == 2:
- uprint(' head_tilt_calibr')
- i= 400
- a = True
- self.kondo.setUserParameter(19,0)
- while(a):
- if (i < -1500) : a=False
- #clock.tick()
- i=i-1
- uprint ('i =', i)
- b=self.kondo.setUserParameter(20,i)
- for j in range(5):
- img = self.sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- for blob in img.find_blobs([self.vision.TH['orange ball']['th']], pixels_threshold=20, area_threshold=20, merge=True):
- #if blob.roundness() > 0.5:
- img.draw_rectangle(blob.rect())
- img.draw_cross(blob.cx(), blob.cy())
- uprint('blob.cy() =', blob.cy())
- #if (blob.y()+ blob.h()) <=120 : a=False
- if blob.cy() <=120 : a=False
- return_value = blob.cy()
-
- else:
- import reload as re
- i= 200
- a = True
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , 0*self.FACTOR[21], self.sim.simx_opmode_oneshot) #head pan to zero
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , 0*self.FACTOR[22], self.sim.simx_opmode_oneshot) #head tilt to zero
- while(a):
- if (i < -1500) : a=False
- #clock.tick()
- i=i-1
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , i * self.TIK2RAD * self.FACTOR[22], self.sim.simx_opmode_oneshot)
- self.sim_simxSynchronousTrigger(self.clientID)
- img1 = self.vision_Sensor_Get_Image()
- img = re.Image(img1)
- for blob in img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold=self.vision.TH['orange ball']['pixel'],
- area_threshold=self.vision.TH['orange ball']['area'],
- merge=True):
- img.draw_rectangle(blob.rect())
- if blob.cy()==120 : a=False
- return_value = blob.cy()
- self.vision_Sensor_Display(img.img)
- if 118 < return_value <=120 :
- self.neck_calibr = i
- self.neck_play_pose = int(self.neck_calibr - 120 * self.params['APERTURE_PER_PIXEL_VERTICAL'] / 0.03375)
- self.refresh_Orientation()
- data = {"neck_calibr": self.neck_calibr, "neck_play_pose": self.neck_play_pose, "head_pitch_with_horizontal_camera": self.euler_angle['pitch']}
- if self.glob.SIMULATION == 2:
- with open(self.glob.current_work_directory + "Init_params/Real/Real_calibr.json", "w") as f:
- json.dump(data, f)
- else:
- with open(self.glob.current_work_directory + "Init_params/Sim/" + self.glob.SIM_OPTION + "/Sim_calibr.json", "w") as f:
- json.dump(data, f)
- uprint( self.get_course_and_distance_to_ball(blob))
- return True
- else: return False
-
- def seek_Ball(self, fast_Reaction_On): # seeks ball in 360 dergree one time
- a, course, distance = self.seek_Ball_In_Pose(fast_Reaction_On)
- if a == True: return a, course, distance
- else:
- target_course1 = self.euler_angle['yaw'] +math.pi
- self.turn_To_Course(target_course1)
- a, course, distance = self.seek_Ball_In_Pose(fast_Reaction_On)
- if a == True: return a, course, distance
- else:
- target_course = target_course1 -170
- self.turn_To_Course(target_course)
- return False, 0, 0
-
- def seek_Ball_In_Pose(self, fast_Reaction_On, penalty_Goalkeeper = False, with_Localization = True, very_Fast = False, first_look_point= None):
- self.local.correct_yaw_in_pf()
- if self.robot_In_0_Pose == False:
- if self.glob.SIMULATION == 2:
- self.play_Soft_Motion_Slot(name = 'Initial_Pose')
- #self.kondo.motionPlay(1)
- #self.pyb.delay(500)
- else:
- self.simulateMotion(name = 'Initial_Pose')
- self.robot_In_0_Pose = True
- variants = []
- # U19 - Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # U20 - Ð¨ÐµÑ Ðаклон
- if first_look_point == None:
- course_to_ball = 0
- tilt_to_ball = (-self.neck_play_pose - self.neck_calibr) * self.TIK2RAD
- else:
- course_to_ball = self.norm_yaw(math.atan2((first_look_point[1] - self.glob.pf_coord[1]), (first_look_point[0] - self.glob.pf_coord[0])) - self.glob.pf_coord[2])
- distance_to_ball = math.sqrt((first_look_point[1] - self.glob.pf_coord[1])**2 + (first_look_point[0] - self.glob.pf_coord[0])**2)
- tilt_to_ball = math.pi/2 - math.atan (distance_to_ball * 1000/(self.params['HEIGHT_OF_CAMERA'] - self.params['DIAMETER_OF_BALL']/2))
- c = self.neck_play_pose
- head_pose = [(-2667,c), (-1333, c) , ( 0, c) , (1333, c) , (2667,c),
- (-2667, c-700),(-1333, c-700), (0, c-700), (1333,c-700),(2667, c-700),
- (-2667, c-1400), (-1333, c-1400), ( 0, c-1400), (1333, c-1400), (2667, c-1400)]
- #head_pose_seq = [2,7,12,13,8,3,1,6,11,10,5,0,4,9,14,2]
- head_pose_seq = [2,7,12,11,6,8,13,14,9,4,3,10,5,0,1,2]
- if penalty_Goalkeeper: head_pose_seq = [2,7,12,11,6,8,13]
- for i in range(len(head_pose_seq)+1):
- if i == 0:
- self.neck_pan = -int(course_to_ball/self.TIK2RAD)
- if abs(self.neck_pan) > 2667: self.neck_pan = int(math.copysign(2667, self.neck_pan))
- self.neck_tilt = int((-tilt_to_ball)/self.TIK2RAD + self.neck_calibr)
- else:
- x = head_pose[head_pose_seq[i-1]]
- self.neck_pan = x[0]
- self.neck_tilt = x[1]
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,self.neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,self.neck_tilt)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , self.neck_pan * self.TIK2RAD * self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , self.neck_tilt * self.TIK2RAD * self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for j in range(20):
- self.sim_simxSynchronousTrigger(self.clientID)
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return False, 0, 0, [0, 0]
- self.refresh_Orientation()
- a, course, dist, blob = self.seek_Ball_In_Frame(with_Localization)
- if a == True:
- variants.append ((course, dist *1000))
- if fast_Reaction_On == True and a== True: break
- course = 0
- distance = 0
- if len(variants)>0:
- for i in range (len(variants)):
- course = course + variants[i][0]
- distance = distance + variants[i][1]
- course1 = course /len(variants)
- distance1 = distance /len(variants)
- if very_Fast:
- if distance1 !=0:
- self.local.localisation_Complete()
- dist = distance1 / 1000
- course_global_rad = course1 + self.glob.pf_coord[2]
- self.glob.ball_coord = [dist * math.cos(course_global_rad) + self.glob.pf_coord[0],
- dist * math.sin(course_global_rad) + self.glob.pf_coord[1]]
- return(a, course1, dist, [0, 0])
- self.neck_pan =int( - course1/ self.TIK2RAD)
- D = self.params['HEIGHT_OF_CAMERA'] - self.params['HEIGHT_OF_NECK']- self.params['DIAMETER_OF_BALL']/2
- E = (2*distance1*D - math.sqrt(4*distance1**2*D**2 - 4*(distance1**2-self.params['HEIGHT_OF_NECK']**2)*(D**2 -self.params['HEIGHT_OF_NECK']**2)))/(2*(D**2-self.params['HEIGHT_OF_NECK']**2))
- alpha = math.atan(E)
- alpha_d = math.pi/2 - alpha
- self.neck_tilt = int((-alpha_d)/self.TIK2RAD + self.neck_calibr)
- #uprint('self.neck_pan =', self.neck_pan, 'self.neck_tilt =', self.neck_tilt)
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,self.neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,self.neck_tilt)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , self.neck_pan * self.TIK2RAD * self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , self.neck_tilt * self.TIK2RAD * self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for j in range(16):
- self.sim_simxSynchronousTrigger(self.clientID)
- self.refresh_Orientation()
- a, course, dist, speed = self.detect_Ball_Speed()
- #if with_Localization: self.local.localisation_Complete()
- self.local.localisation_Complete()
- if a == True:
- #if with_Localization: self.local.localisation_Complete()
- course_global_rad = course + self.glob.pf_coord[2]
- self.glob.ball_coord = [dist*math.cos(course_global_rad)+ self.glob.pf_coord[0],
- dist*math.sin(course_global_rad)+ self.glob.pf_coord[1]]
- return(a, course, dist, speed)
- else:
- if distance1 !=0:
- #if with_Localization: self.local.localisation_Complete()
- dist = distance1 / 1000
- course_global_rad = course1 + self.glob.pf_coord[2]
- self.glob.ball_coord = [dist * math.cos(course_global_rad) + self.glob.pf_coord[0],
- dist * math.sin(course_global_rad) + self.glob.pf_coord[1]]
- return(a, course1, dist, [0, 0])
- if with_Localization: self.local.localisation_Complete()
- return False, 0, 0, [0, 0]
-
- def watch_Ball_In_Pose(self, penalty_Goalkeeper = False):
- self.local.correct_yaw_in_pf()
- if self.robot_In_0_Pose == False:
- if self.glob.SIMULATION == 2:
- self.play_Soft_Motion_Slot(name = 'Initial_Pose')
- else:
- self.simulateMotion(name = 'Initial_Pose')
- self.robot_In_0_Pose = True
- # U19 - Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # U20 - Ð¨ÐµÑ Ðаклон
- c = self.neck_play_pose
- head_pose = [(-2667,c), (-1333, c) , ( 0, c) , (1333, c) , (2667,c),
- (-2667, c-700),(-1333, c-700), (0, c-700), (1333,c-700),(2667, c-700),
- (-2667, c-1400), (-1333, c-1400), ( 0, c-1400), (1333, c-1400), (2667, c-1400)]
- head_pose_seq = [2,7,12,11,6,8,13,14,9,4,3,10,5,0,1,2]
- if penalty_Goalkeeper: head_pose_seq = [2,7,12,11,6,8,13]
- for i in range(len(head_pose_seq)):
- if i != 0:
- x = head_pose[head_pose_seq[i]]
- self.neck_pan = x[0]
- self.neck_tilt = x[1]
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return False, 0, 0, [0, 0]
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,self.neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,self.neck_tilt)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , self.neck_pan * self.TIK2RAD * self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , self.neck_tilt * self.TIK2RAD * self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for j in range(20):
- self.sim_simxSynchronousTrigger(self.clientID)
- self.refresh_Orientation()
- a, course, dist, speed = self.detect_Ball_Speed()
- if a == True or (a== False and dist !=0): break
- if a == True or (a== False and dist !=0):
- course_global_rad = course + self.glob.pf_coord[2]
- self.glob.ball_coord = [dist*math.cos(course_global_rad)+ self.glob.pf_coord[0],
- dist*math.sin(course_global_rad)+ self.glob.pf_coord[1]]
- if len(self.glob.obstacles) == 0: self.glob.obstacles = [[0,0,0]]
- self.glob.obstacles[0] = [self.glob.ball_coord[0], self.glob.ball_coord[1], 0.15]
- distance = dist *1000
- self.neck_pan =int( - course/ self.TIK2RAD)
- D = self.params['HEIGHT_OF_CAMERA'] - self.params['HEIGHT_OF_NECK']- self.params['DIAMETER_OF_BALL']/2
- E = (2*distance*D - math.sqrt(4*distance**2*D**2 - 4*(distance**2-self.params['HEIGHT_OF_NECK']**2)*(D**2 -self.params['HEIGHT_OF_NECK']**2)))/(2*(D**2-self.params['HEIGHT_OF_NECK']**2))
- alpha = math.atan(E)
- alpha_d = math.pi/2 - alpha
- self.neck_tilt = int((-alpha_d)/self.TIK2RAD + self.neck_calibr)
- return(a, course, dist, speed)
- return False, 0, 0, [0, 0]
-
- def sorted_blobs(self, blobs, number_of_blobs=5):
- blobs_new = []
- for i in range(number_of_blobs):
- if blobs :
- lowest_blob_in_frame = blobs[0]
- for blob in blobs:
- if blob.cy() > lowest_blob_in_frame.cy():
- lowest_blob_in_frame = blob
- blobs.pop(blobs.index(lowest_blob_in_frame))
- blobs_new.append(lowest_blob_in_frame)
- return blobs_new
-
- def orange_ball_is_on_green_field(self, blob, img):
- x , y , w , h = blob.rect() # ball blob
- x1 = x + w # x1, y1, w1, h1-right rectangle
- y1 = y
- if x1 + w <= 320:
- w1 = w
- else:
- w1 = 320 - x1
- if x1 == 320:
- w1 = 1
- x1 = 319
- if y1 + 2 * h <= 240:
- h1 = 2 * h
- else:
- h1 = 240 - y1
- if y1 + h == 240:
- h1 = h
- y2 = y # x2, y2, w2, h2 - left rectangle
- if x - w > 0:
- x2 = x - w
- w2 = w
- else:
- x2 = 0
- w2 = x1 - x2
- if x1 == 0:
- x2 = 0
- w2 = 1
- y2 = y1
- h2 = h1
- x3 = x # x3, y3, w3, h3 - bottom rectangle
- y3 = y + h - 1
- w3 = w
- h3 = h1 - h + 1
- blob_p = [] # right blobs
- blob_l = [] # left blobs
- blob_n = [] # bottom blobs
- blob_p = img.find_blobs([self.vision.TH['green field']['th']],roi = [x1 , y1 , w1 , h1], pixels_threshold=7, area_threshold=7, merge=True)
- blob_l = img.find_blobs([self.vision.TH['green field']['th']],roi = [x2 , y1 , w2 , h1], pixels_threshold=7, area_threshold=7, merge=True)
- blob_n = img.find_blobs([self.vision.TH['green field']['th']],roi = [x3 , y3 , w3 , h3], pixels_threshold=7, area_threshold=7, merge=True)
- return len(blob_p) > 0 or len( blob_l ) > 0 or len( blob_n ) > 0
-
-
- def seek_Ball_In_Frame(self, with_Localization = True):
- tra = 0
- for number in range (2):
- if self.glob.SIMULATION == 2:
- img = self.sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold=self.vision.TH['orange ball']['pixel'],
- area_threshold=self.vision.TH['orange ball']['area'],
- merge=True, margin=10)
- blobs = self.sorted_blobs(blobs)
- for blob in blobs:
- tra = tra + 1
- if self.orange_ball_is_on_green_field(blob, img):
- img.draw_rectangle(blob.rect(), color = (255, 0, 0))
- self.green_led.on() # подмигивание зеленым Ñветодиодом
- time.sleep(50) #
- self.green_led.off() #
- break
- else: tra = 0
- if with_Localization and number == 0: self.local.read_Localization_marks(img)
- else:
- import reload as re
- if self.glob.SIMULATION != 3 :
- self.sim_simxSynchronousTrigger(self.clientID)
- img1 = self.vision_Sensor_Get_Image()
- img = re.Image(img1)
- self.vision_Sensor_Display(img.img)
- if with_Localization and number == 0: self.local.read_Localization_marks(img1)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold=self.vision.TH['orange ball']['pixel'],
- area_threshold=self.vision.TH['orange ball']['area'],
- merge=True)
- blobs = self.sorted_blobs(blobs)
- for blob in blobs:
- tra = tra + 1
- if self.orange_ball_is_on_green_field(blob, img):
- img.draw_rectangle(blob.rect())
- self.vision_Sensor_Display(img.img)
- break
- else: tra = 0
- if tra == 0: return False, 0, 0, 0
- else:
- course, distance = self.get_course_and_distance_to_ball(blob)
- return True, course, distance, blob
-
- def detect_Ball_Speed(self):
- tra = 0
- position = []
- for number in range (2):
- there_is_blob = False
- if self.glob.SIMULATION == 2:
- img = self.sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- #self.local.read_Localization_marks(img)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold=self.vision.TH['orange ball']['pixel'],
- area_threshold=self.vision.TH['orange ball']['area'],
- merge=True, margin=10)
- blobs = self.sorted_blobs(blobs)
- for blob in blobs:
- tra = tra + 1
- if self.orange_ball_is_on_green_field(blob, img):
- img.draw_rectangle(blob.rect(), color = (255, 0, 0))
- self.green_led.on() # подмигивание зеленым Ñветодиодом
- time.sleep(50) #
- self.green_led.off() #
- there_is_blob = True
- break
- else:
- tra = 0
- there_is_blob = False
- else:
- if self.glob.SIMULATION != 3 :
- self.sim_simxSynchronousTrigger(self.clientID)
- img1 = self.vision_Sensor_Get_Image()
- img = self.re.Image(img1)
- self.vision_Sensor_Display(img.img)
- blobs = img.find_blobs([self.vision.TH['orange ball']['th']],
- pixels_threshold = self.vision.TH['orange ball']['pixel'],
- area_threshold = self.vision.TH['orange ball']['area'],
- merge=True)
- there_is_blob = False
- blobs = self.sorted_blobs(blobs)
- for blob in blobs:
- tra = tra + 1
- if self.orange_ball_is_on_green_field(blob, img):
- img.draw_rectangle(blob.rect())
- self.vision_Sensor_Display(img.img)
- course, distance = self.get_course_and_distance_to_ball(blob)
- there_is_blob = True
- if there_is_blob:
- course, distance = self.get_course_and_distance_to_ball(blob)
- position.append([course,distance])
- n = len(position)
- speed = [0,0]
- if n > 1:
- front_speed = ( position[n-1][1] - position[0][1])/ distance/n
- tangential_speed = ( position[n-1][0] - position[0][0]) * distance/n
- speed = [tangential_speed, front_speed ]
- if tra < 1: return False, 0, 0, [0,0]
- elif tra < 2: return False, course, distance, speed
- else: return True, course, distance, speed
-
-
- def get_course_and_distance_to_ball(self, blob): # returns course in degrees and distance in mm
- #c = self.neck_calibr
- #if self.glob.SIMULATION == 2:
- # z,a = self.kondo.getUserParameter(19)
- # self.pyb.delay(200)
- # z,b = self.kondo.getUserParameter(20)
- # a = self.neck_pan
- # b = self.neck_tilt
- # # U19 - Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # # U20 - Ð¨ÐµÑ Ðаклон
- #else:
- # returnCode, position21= self.sim.simxGetJointPosition(self.clientID, self.jointHandle[21], self.sim.simx_opmode_blocking)
- # a = position21*self.FACTOR[21]*1698 # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # returnCode, position22= self.sim.simxGetJointPosition(self.clientID, self.jointHandle[22], self.sim.simx_opmode_blocking)
- # b = position22*self.FACTOR[22]*1698 # Ð¨ÐµÑ Ðаклон
- self.refresh_Orientation()
- y1 = self.euler_angle['pitch'] - self.head_pitch_with_horizontal_camera
- y = y1 - math.radians((120-blob.cy())* self.params['APERTURE_PER_PIXEL_VERTICAL']) # 0.19406 #
- vision_height = self.params['HEIGHT_OF_CAMERA'] - self.params['HEIGHT_OF_NECK'] + self.params['HEIGHT_OF_NECK']*math.cos(y1)- self.params['DIAMETER_OF_BALL']/2
- distance_in_mm1 = vision_height / math.tan(y) + self.params['HEIGHT_OF_NECK']*math.sin(y1)
- vision_dist = vision_height/math.sin(y)
- vision_shift = vision_dist * math.tan(math.radians((160 - blob.cx()) * self.params['APERTURE_PER_PIXEL']))
- distance_in_mm2 = math.sqrt(distance_in_mm1**2 + vision_shift**2)
- course1 = -self.neck_pan * self.TIK2RAD + math.atan(vision_shift/distance_in_mm1)
- return course1, distance_in_mm2/1000
-
- def get_course_and_distance_to_post(self, blob_cx, blob_y_plus_h): # returns course in degrees and distance in mm
- c = self.neck_calibr
- if self.glob.SIMULATION == 2:
- z,a = self.kondo.getUserParameter(19)
- z,b = self.kondo.getUserParameter(20)
- a = self.neck_pan
- b = self.neck_tilt
- # U19 - Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # U20 - Ð¨ÐµÑ Ðаклон
- else:
- returnCode, position21= self.sim.simxGetJointPosition(self.clientID, self.jointHandle[21], self.sim.simx_opmode_blocking)
- a = position21*self.FACTOR[21]*1698 # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode, position22= self.sim.simxGetJointPosition(self.clientID, self.jointHandle[22], self.sim.simx_opmode_blocking)
- b = position22*self.FACTOR[22]*1698 # Ð¨ÐµÑ Ðаклон
- x = -(b-c)*0.03375 -(120-blob_y_plus_h)* self.params['APERTURE_PER_PIXEL_VERTICAL']
- y1 = math.radians(-(b-c)*0.03375)
- y=math.radians(x)
- vision_height = self.params['HEIGHT_OF_CAMERA'] - self.params['HEIGHT_OF_NECK'] + self.params['HEIGHT_OF_NECK']*math.cos(y1)
- if y == 0:
- distance_in_mm1 = vision_dist = 4000
- else:
- distance_in_mm1 = vision_height / math.tan(y) + self.params['HEIGHT_OF_NECK']*math.sin(y1)
- vision_dist = vision_height/math.sin(y)
- vision_shift = vision_dist * math.tan(math.radians((160 - blob_cx) * self.params['APERTURE_PER_PIXEL']))
- distance_in_mm2 = math.sqrt(distance_in_mm1**2 + vision_shift**2)
- course1 = self.euler_angle['yaw'] + math.atan(vision_shift/distance_in_mm1)
- #uprint('distance_in_mm1 = ', distance_in_mm1, 'distance_in_mm2 = ', distance_in_mm2 )
- #uprint('course =', course, 'course1 = ', course1)
- return course1, distance_in_mm2/1000
-
- def get_cooord_of_point(self, point_x, point_y): # takes x,y of point in QVGA frame and returns x,y of point in m in local coordinates of robot
- try:
- head_to_horizon_angle = (self.neck_calibr - self.neck_tilt) * 0.03375
- #vision_line_angle = math.radians(head_to_horizon_angle +(point_y - 120) * APERTURE_PER_PIXEL) # 0.18925
- vision_line_angle = math.radians(head_to_horizon_angle +(point_y - 120) * self.params['APERTURE_PER_PIXEL_VERTICAL']) # 0.18925
- vision_height = self.params['HEIGHT_OF_CAMERA'] - self.params['HEIGHT_OF_NECK'] + self.params['HEIGHT_OF_NECK']*math.cos(math.radians(head_to_horizon_angle))
- distance_in_mm1 = vision_height / math.tan(vision_line_angle) + self.params['HEIGHT_OF_NECK']*math.sin(math.radians(head_to_horizon_angle))
- vision_dist = vision_height/math.sin(vision_line_angle)
- vision_shift = vision_dist * math.tan(math.radians((160 - point_x) * self.params['APERTURE_PER_PIXEL']))
- distance_in_mm2 = math.sqrt(distance_in_mm1**2 + vision_shift**2)
- course = math.atan(vision_shift/distance_in_mm1) - self.neck_pan * self.TIK2RAD
- x = distance_in_mm2/1000 * math.cos(course)
- y = distance_in_mm2/1000 * math.sin(course)
- except Exception: return False, 0, 0
- return True, x, y
-
- def turn_To_Course(self, course, accurate = False, one_Off_Motion = True):
- stepLength = 0
- sideLength = 0
- rotation = 0
- cycleNumber = 1
- cycle = 0
- target = course # + self.direction_To_Attack
- if one_Off_Motion: old_neck_pan, old_neck_tilt = self.head_Up()
- self.refresh_Orientation()
- rotation1 = target - self.euler_angle['yaw']
- if rotation1 > math.pi : rotation1 -= (2 * math.pi)
- if rotation1 < -math.pi : rotation1 += (2 * math.pi)
- if abs(rotation1)> 0.035 or accurate:
- cycleNumber = int(math.floor(abs(rotation1)/0.23))+1 # rotation yield 0.23 with rotation order 0.21
- if one_Off_Motion:
- self.walk_Initial_Pose()
- after_cycle = 0
- else: after_cycle = 1
- for cycle in range (cycleNumber):
- self.refresh_Orientation()
- rotation1 = target - self.euler_angle['yaw']
- if rotation1 > math.pi : rotation1 -= (2 * math.pi)
- if rotation1 < -math.pi : rotation1 += (2 * math.pi)
- if abs(rotation1)< 0.035 and not accurate: break
- if abs(rotation1)< 0.01: break
- rotation = rotation1/(cycleNumber - cycle)
- #uprint('self.euler_angle[0]=', self.euler_angle[0],'rotation =', rotation )
- self.walk_Cycle(stepLength, sideLength,rotation,cycle,cycleNumber + after_cycle)
- if one_Off_Motion: self.walk_Final_Pose()
- self.refresh_Orientation()
- self.local.coord_odometry[2] = self.euler_angle['yaw']
- self.local.coord_shift = [0,0,0]
- self.local.coordinate_record(odometry = True, shift = True)
- if one_Off_Motion: self.head_Return(old_neck_pan, old_neck_tilt)
-
- def head_Up(self):
- old_neck_pan = self.neck_pan
- old_neck_tilt = self.neck_tilt
- self.neck_pan = 0
- self.neck_tilt = 0 #self.neck_play_pose
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,0)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20, 0) #self.neck_play_pose)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , 0, self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , 0*0.000589*self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for i in range(16):
- self.sim_simxSynchronousTrigger(self.clientID)
- self.refresh_Orientation()
- return old_neck_pan, old_neck_tilt
-
- def head_Return(self, old_neck_pan, old_neck_tilt):
-
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,old_neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,old_neck_tilt)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , old_neck_pan*0.000589*self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , old_neck_tilt*0.000589*self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for i in range(16):
- self.sim_simxSynchronousTrigger(self.clientID)
- self.neck_pan = old_neck_pan
- self.neck_tilt = old_neck_tilt
- self.refresh_Orientation()
-
- def localisation_Motion(self):
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- if self.glob.SIMULATION == 2:
- self.kondo.motionPlay(1)
- self.pyb.delay(500)
- # U19 - Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- # U20 - Ð¨ÐµÑ Ðаклон
- c = self.neck_play_pose
- head_pose = [(-2667,c), (-1333, c) , ( 0, c) , (1333, c) , (2667,c),
- (-2667, c-700),(-1333, c-700), (0, c-700), (1333,c-700),(2667, c-700),
- (-2667, c-1400), (-1333, c-1400), ( 0, c-1400), (1333, c-1400), (2667, c-1400)]
- head_pose_seq = [2,7,6,8,9,4,3,5,0,1,2]
- for k in range(1):
- for i in range(len(head_pose_seq)):
- x = head_pose[head_pose_seq[i]]
- self.neck_pan = x[0]
- self.neck_tilt = x[1]
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,self.neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,self.neck_tilt)
- self.pyb.delay(200)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , self.neck_pan*0.000589*self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , self.neck_tilt*0.000589*self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for i in range(16):
- self.sim_simxSynchronousTrigger(self.clientID)
- self.refresh_Orientation()
- a, course, distance, blob = self.seek_Ball_In_Frame()
- a = self.local.localisation_Complete()
- return a
-
- def normalize_rotation(self, yaw, limit = 0.5):
- if abs(yaw) > 2 * math.pi: yaw %= (2 * math.pi)
- if yaw > math.pi : yaw -= (2 * math.pi)
- if yaw < -math.pi : yaw += (2 * math.pi)
- if yaw > limit : yaw = limit
- if yaw < -limit : yaw = -limit
- return yaw
-
- def near_distance_omni_motion(self, dist_mm, napravl, one_Off_Motion = True):
- if one_Off_Motion: old_neck_pan, old_neck_tilt = self.head_Up()
- dist = dist_mm/1000
- self.refresh_Orientation()
- initial_direction = self.imu_body_yaw()
- print('initial_direction', initial_direction)
- n = int(math.floor((dist_mm*math.cos(napravl)-self.first_step_yield)/self.cycle_step_yield)+1)+1 #calculating the number of potential full steps forward
- displacement = dist_mm*math.sin(napravl)
- first_Leg_Is_Right_Leg_Old = self.first_Leg_Is_Right_Leg
- if displacement > 0:
- self.first_Leg_Is_Right_Leg = False
- side_step_yield = self.side_step_left_yield
- else:
- side_step_yield = self.side_step_right_yield
- if self.first_Leg_Is_Right_Leg: invert = 1
- else: invert = -1
- m = int(math.floor(abs(displacement)/side_step_yield)+1)
- if n < m : n = m
- stepLength = dist_mm*math.cos(napravl)/(self.first_step_yield*1.25+self.cycle_step_yield*(n-1)+ self.cycle_step_yield*0.75)*64
- number_Of_Cycles = n+2
- sideLength = abs(displacement) /number_Of_Cycles*20/side_step_yield
- if stepLength > 15 and number_Of_Cycles > 4:
- deceleration = True
- number_Of_Cycles += 1
- else: deceleration = False
- self.local.correct_yaw_in_pf()
- if one_Off_Motion:
- self.walk_Initial_Pose()
- after_cycle = 0
- else: after_cycle = 1
- for cycle in range(number_Of_Cycles):
- self.refresh_Orientation()
- rotation = initial_direction - self.imu_body_yaw() * 1
- rotation = self.normalize_rotation(rotation)
- stepLength1 = stepLength
- if cycle == 0: stepLength1 = stepLength/4
- if cycle == 1: stepLength1 = stepLength/2
- if deceleration:
- if cycle == number_Of_Cycles - 1: stepLength1 = stepLength / 3
- if cycle == number_Of_Cycles - 2: stepLength1 = stepLength * 2 / 3
- self.walk_Cycle(stepLength1, sideLength, invert*rotation,cycle,number_Of_Cycles + after_cycle)
- if one_Off_Motion: self.walk_Final_Pose()
- self.first_Leg_Is_Right_Leg = first_Leg_Is_Right_Leg_Old
- self.local.coord_odometry[0] += dist * math.cos(napravl)
- self.local.coord_odometry[1] += dist * math.sin(napravl)
- #self.local.coordinate_record(odometry = True)
- if one_Off_Motion: self.head_Return(old_neck_pan, old_neck_tilt)
-
- def near_distance_ball_approach_and_kick(self, kick_direction, strong_kick = False, small_kick = False, very_Fast = False ):
- offset_of_ball = self.params['KICK_OFFSET_OF_BALL'] # self.d10 # module of local robot Y coordinate of ball im mm before kick
- a, napravl, dist= None, None, None
- if (self.glob.mqttc):
- a, napravl, dist = self.glob.update_coord_by_mqtt()
- else:
- a, napravl, dist, speed = self.seek_Ball_In_Pose(fast_Reaction_On = True, very_Fast = very_Fast,
- first_look_point = self.glob.ball_coord, with_Localization = False)
-
- dist_mm = dist *1000
- if a==False or self.falling_Flag != 0: return False
- if dist > 0.9 or a == False: return False
- if 0.02 < abs(dist * math.cos(napravl)) < 0.06 and dist * math.sin(napravl) < 0.03:
- # old_neck_pan, old_neck_tilt = self.head_Up()
- if napravl > 0: self.kick(first_Leg_Is_Right_Leg=False)
- else: self.kick(first_Leg_Is_Right_Leg=True)
- # self.head_Return(old_neck_pan, old_neck_tilt)
- if abs(napravl) > 1 :
- direction = math.copysign(2.55, napravl)
- self.near_distance_omni_motion( 180 , direction)
- else:
- forth_dist = dist_mm*math.cos(napravl)
- n = int(math.ceil((forth_dist - self.params['KICK_ADJUSTMENT_DISTANCE_1']
- -self.first_step_yield)/self.cycle_step_yield)+1) #calculating the number of potential full steps forward
- displacement = dist_mm * math.sin(napravl)- math.copysign(offset_of_ball, napravl)
- if displacement > 0:
- invert = -1
- self.first_Leg_Is_Right_Leg = False
- side_step_yield = self.side_step_left_yield
- else:
- invert = 1
- side_step_yield = self.side_step_right_yield
- m = int(math.ceil(abs(displacement)/side_step_yield)) # potential side steps number
- if n < m : n = m
- n += 2 # final steps number
- stepLength = (dist_mm*math.cos(napravl)-
- self.params['KICK_ADJUSTMENT_DISTANCE_1'])/(self.first_step_yield
- + self.cycle_step_yield * n) * 64
- number_Of_Cycles = n + 1
- if napravl > 0:
- kick_by_Right = False
- else:
- kick_by_Right = True
- sideLength = abs(displacement)/number_Of_Cycles*20/side_step_yield
- # old_neck_pan, old_neck_tilt = self.head_Up()
- self.local.correct_yaw_in_pf()
- init_yaw = kick_direction #= self.imu_body_yaw()
- #init_yaw = self.imu_body_yaw()
- stepLengthResidue = 0
- sideLengthResidue = 0
- stepLengthResidue_accumulated = 0
- #sideLengthResidue_accumulated = 0
- self.walk_Initial_Pose()
- cycle = 0
- while (cycle < number_Of_Cycles):
- rotation = (kick_direction - self.imu_body_yaw()) * 1
- rotation = self.normalize_rotation(rotation)
- stepLength1 = stepLength
- sideLength1 = sideLength
- if cycle == 0: stepLength1 = stepLength / 3
- if cycle == 1: stepLength1 = stepLength * 2 / 3
- stepLength1 += stepLengthResidue
- #sideLength1 += sideLengthResidue
- self.walk_Cycle(stepLength1, sideLength1, invert*rotation,cycle,number_Of_Cycles)
- self.refresh_Orientation()
- delta_yaw = self.norm_yaw(self.imu_body_yaw() - init_yaw)
- stepLengthResidue = stepLength1 * (1 - math.cos(delta_yaw)) - sideLength1 * math.sin(delta_yaw) * invert
- stepLengthResidue_accumulated += stepLengthResidue
- #sideLengthResidue = sideLength1 * (1 - math.cos(delta_yaw)) + stepLength1 * math.sin(delta_yaw) * invert
- #sideLengthResidue_accumulated += sideLengthResidue
- cycle += 1
- self.walk_Final_Pose()
- print('stepLengthResidue_accumulated =', stepLengthResidue_accumulated)
- self.first_Leg_Is_Right_Leg = True
- #self.stepHeight = 20
- # result, kick_by_Right = self.verify_ball_position(kick_by_Right, kick_direction)
- self.first_Leg_Is_Right_Leg = True
- #self.stepHeight = 32
- # self.head_Up()
- if strong_kick == True:
- if kick_by_Right == True:
- self.play_Motion_Slot(name = 'Soccer_Kick_Forward_Right_Leg')
- else:
- self.play_Motion_Slot(name = 'Soccer_Kick_Forward_Left_Leg')
- else:
- self.kick( first_Leg_Is_Right_Leg=kick_by_Right, small = small_kick)
- self.local.coord_odometry[0] += dist * math.cos(napravl)
- self.local.coord_odometry[1] += dist * math.sin(napravl)
- self.pause_in_ms(1000)
- # self.head_Return(old_neck_pan, old_neck_tilt)
- return True
-
- def verify_ball_position(self, kick_by_Right, kick_direction):
- def moving_direction(kick_by_Right):
- if self.relative_ball_position == [0,0]: return False , None, None, kick_by_Right
- ball_x, ball_y = self.relative_ball_position
- if ball_y > 20: kick_by_Right = False
- if ball_y < -20: kick_by_Right = True
- if kick_by_Right: side_motion = ball_y + self.params["KICK_OFFSET_OF_BALL"]
- else: side_motion = ball_y - self.params["KICK_OFFSET_OF_BALL"]
- front_motion = ball_x - self.params["KICK_ADJUSTMENT_DISTANCE_2"]
- return True, front_motion, side_motion, kick_by_Right
-
- self.move_head(0, -2000)
- front_motion_tolerance = 20
- side_motion_tolerance = 20
- self.glob.camera_ON = True
- self.check_camera()
- result, front_motion, side_motion, kick_by_Right = moving_direction(kick_by_Right)
- if not result:
- self.glob.camera_ON = False
- return result, kick_by_Right
- elif result and front_motion <= front_motion_tolerance and abs(side_motion) < side_motion_tolerance:
- self.glob.camera_ON = False
- return result, kick_by_Right
- if side_motion <= 0:
- self.first_Leg_Is_Right_Leg = True
- invert = 1
- else:
- self.first_Leg_Is_Right_Leg = False
- invert = -1
- self.params['BODY_TILT_AT_WALK'] -= 0.01
- if self.glob.SIMULATION == 2:
- self.glob.params['SOLE_LANDING_SKEW'] -= 0.02
- self.frame_delay = 25 # self.glob.params['FRAME_DELAY']
- self.stepHeight = 25
- self.walk_Initial_Pose()
- number_Of_Cycles = 50
- for cycle in range(number_Of_Cycles):
- self.refresh_Orientation()
- self.body_euler_angle_calc()
- rotation = (kick_direction - self.body_euler_angle['yaw']) * 1.1
- rotation = self.normalize_rotation(rotation)
- if front_motion > front_motion_tolerance: stepLength = 15
- else: stepLength = 0
- print('side_motion =', side_motion)
- if side_motion < -side_motion_tolerance : sideLength = 15 * invert
- elif side_motion > side_motion_tolerance : sideLength = -15 * invert
- else: sideLength = 0
- self.walk_Cycle(stepLength, sideLength, invert * rotation, cycle, number_Of_Cycles)
- result, front_motion, side_motion, kick_by_Right = moving_direction(kick_by_Right)
- if (result and front_motion <= front_motion_tolerance and abs(side_motion) < side_motion_tolerance) or not result:
- self.walk_Cycle(stepLength, sideLength, invert * rotation, cycle, cycle + 1)
- break
- else:
- self.walk_Cycle(stepLength, sideLength, invert * rotation, cycle, number_Of_Cycles)
- self.walk_Final_Pose()
- self.params['BODY_TILT_AT_WALK'] += 0.01
- if self.glob.SIMULATION == 2:
- self.glob.params['SOLE_LANDING_SKEW'] += 0.02
- self.frame_delay = self.glob.params['FRAME_DELAY']
- self.stepHeight = 32
- self.glob.camera_ON = False
- return result, kick_by_Right
-
- def kick_off_ride(self):
- angle = 30
- distance = 0.55
- sideLength = 0
- acceleration = False
- deceleration = False
- invert = round(utility.random(),0)*2 - 1
- self.walk_Initial_Pose()
- delta_yaw = invert * math.radians(angle) #0.52 #0.79
- number_Of_Cycles = math.ceil(abs(delta_yaw / 0.2))
- delta_yaw_step = delta_yaw / number_Of_Cycles
- stepLength = 0
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- self.refresh_Orientation()
- rotation = 0 + delta_yaw_step * (cycle + 1) - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- self.walk_Cycle(stepLength1, sideLength, rotation, cycle, number_Of_Cycles+1)
- delta_yaw = -2 * math.radians(angle) #-1.04 # -1.58
- number_Of_Cycles = math.ceil(abs(delta_yaw / 0.2))
- delta_yaw_step = delta_yaw / number_Of_Cycles
- R = distance / 2 / math.sin(math.radians(angle)) #0.45 # 0.32
- stepLength = R * abs(delta_yaw_step) * 1000 * 64 / self.cycle_step_yield * 1.1
- acceleration = True
- #number_Of_Cycles += 1
- deceleration = True
- #number_Of_Cycles += 1
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- if acceleration:
- if cycle == 0: stepLength1 = stepLength / 3
- if cycle == 1: stepLength1 = stepLength * 2 / 3
- if deceleration:
- if cycle == number_Of_Cycles - 1: stepLength1 = stepLength - (stepLength - 30) * 2 / 3
- if cycle == number_Of_Cycles - 2: stepLength1 = stepLength - (stepLength - 30) / 3
- self.refresh_Orientation()
- rotation = (math.radians(angle) + delta_yaw_step * (cycle)) * invert - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- self.walk_Cycle(stepLength1, sideLength, rotation, cycle + 1, number_Of_Cycles+2)
- stepLength_old = stepLength
- acceleration = False
- deceleration = False
- L = 0.6
- number_Of_Cycles = math.ceil(abs(L * 1000 / self.cycle_step_yield))
- #stepLength = L * 1000 / number_Of_Cycles * 64 / self.cycle_step_yield
- stepLength = 30
- if stepLength - stepLength_old > 22 :
- acceleration = True
- number_Of_Cycles += 1
- deceleration = True
- number_Of_Cycles += 1
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- if acceleration:
- if cycle == 0: stepLength1 = stepLength / 3
- if cycle == 1: stepLength1 = stepLength * 2 / 3
- if deceleration:
- if cycle == number_Of_Cycles - 1: stepLength1 = stepLength / 3
- if cycle == number_Of_Cycles - 2: stepLength1 = stepLength * 2 / 3
- #print('steplength = ', stepLength1)
- self.refresh_Orientation()
- rotation = -math.radians(angle) * invert - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- self.walk_Cycle(stepLength1, sideLength, rotation, cycle + 1, number_Of_Cycles+2)
- stepLength_old = stepLength
- acceleration = False
- deceleration = False
- self.walk_Final_Pose()
- self.first_Leg_Is_Right_Leg = True
-
- def far_distance_straight_approach(self, ball_coord, target_yaw, gap = 0.2, stop_Over = False):
- start_yaw = self.glob.pf_coord[2] #self.imu_body_yaw()
- old_neck_pan, old_neck_tilt = self.head_Up()
- self.local.correct_yaw_in_pf()
- sideLength = 0
- acceleration = False
- deceleration = False
- self.walk_Initial_Pose()
-# initial arc
- dest_yaw = target_yaw
- delta_yaw = self.norm_yaw(dest_yaw - start_yaw)
- number_Of_Cycles = math.ceil(abs(delta_yaw / 0.2))
- if number_Of_Cycles == 0: number_Of_Cycles = 1
- delta_yaw_step = delta_yaw / number_Of_Cycles
- stepLength = 0
- for cycle in range(number_Of_Cycles):
- self.refresh_Orientation()
- rotation = start_yaw + delta_yaw_step * (cycle + 1) - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- self.walk_Cycle(stepLength, sideLength, rotation, cycle, number_Of_Cycles+1)
- print('target yaw', target_yaw)
- #self.turn_To_Course(target_yaw, accurate = True)
- #self.walk_Initial_Pose()
- self.refresh_Orientation()
- print('imu_body_yaw', self.imu_body_yaw())
- acceleration = False
- deceleration = False
-# straight segment
- L = math.sqrt((ball_coord[0]-self.glob.pf_coord[0])**2 + (ball_coord[1]-self.glob.pf_coord[1])**2) - gap
- number_Of_Cycles = math.ceil(abs(L * 1000 / self.cycle_step_yield))
- stepLength = L * 1000 / number_Of_Cycles * 64 / self.cycle_step_yield
- if stepLength > 22 :
- acceleration = True
- number_Of_Cycles += 1
- if not stop_Over:
- if stepLength > 15:
- deceleration = True
- number_Of_Cycles += 1
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- if acceleration:
- if cycle == 0: stepLength1 = stepLength / 3
- if cycle == 1: stepLength1 = stepLength * 2 / 3
- if deceleration:
- if cycle == number_Of_Cycles - 1: stepLength1 = stepLength - (stepLength ) * 2 / 3
- if cycle == number_Of_Cycles - 2: stepLength1 = stepLength - (stepLength ) / 3
- self.refresh_Orientation()
- rotation = dest_yaw - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- self.walk_Cycle(stepLength1, sideLength, rotation, cycle + 1, number_Of_Cycles+1)
- else:
- number_Of_Cycles = math.ceil(number_Of_Cycles/2)
- if stepLength > 15:
- deceleration = True
- number_Of_Cycles += 1
- else: deceleration = False
- for cycle in range(1, number_Of_Cycles + 1, 1):
- stepLength1 = stepLength
- if acceleration:
- if cycle == 0: stepLength1 = stepLength / 3
- if cycle == 1: stepLength1 = stepLength * 2 / 3
- self.refresh_Orientation()
- rotation = dest_yaw - self.imu_body_yaw()
- rotation = self.normalize_rotation(rotation)
- if deceleration:
- if cycle == number_Of_Cycles: stepLength1 = stepLength / 3
- if cycle == number_Of_Cycles - 1: stepLength1 = stepLength * 2 / 3
- self.walk_Cycle(stepLength1, sideLength, rotation, cycle, number_Of_Cycles + 1)
- self.walk_Final_Pose()
- #self.head_Return(old_neck_pan, old_neck_tilt)
- return True
-
- def move_head(self, pan, tilt):
- if pan > 2500:
- pan = 2500
- if pan < -2500:
- pan = 2500
- if tilt > 0:
- tilt = 0
- if tilt < -2000:
- tilt = -2000
-
- self.neck_pan = pan
- self.neck_tilt = tilt
- if self.glob.SIMULATION == 2:
- self.kondo.setUserParameter(19,self.neck_pan)
- self.pyb.delay(200)
- self.kondo.setUserParameter(20,self.neck_tilt)
- self.pyb.delay(400)
- else:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[21] , self.neck_pan * self.TIK2RAD * self.FACTOR[21], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ð¿Ð¾Ð²Ð¾Ñ€Ð¾Ñ‚
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[22] , self.neck_tilt * self.TIK2RAD * self.FACTOR[22], self.sim.simx_opmode_oneshot) # Ð¨ÐµÑ Ðаклон
- for j in range(20):
- self.sim.simxSynchronousTrigger(self.clientID)
-
- def walk_Restart(self):
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- initPoses = int(self.fr1/2)
- self.ztr = - self.gaitHeight
- self.ztl = - self.gaitHeight
- for j in range (initPoses):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- self.ytr = -self.d10 + (initPoses-(j+1))*self.amplitude/2 /initPoses
- self.ytl = self.d10 + (initPoses-(j+1))*self.amplitude/2 /initPoses
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- #if not self.falling_Flag ==0: return
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID,
- self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- start2 = self.pyb.millis()
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- time2 = self.pyb.elapsed_millis(start2)
- self.pyb.delay(self.frame_delay - time1)
- self.robot_In_0_Pose = False
- if not self.falling_Test() == 0:
- self.local.quality =0
- if self.falling_Flag == 3: uprint('STOP!')
- else: uprint('FALLING!!!', self.falling_Flag)
- return[]
- for j in range (initPoses):
- if self.glob.SIMULATION == 2: start1 = self.pyb.millis()
- self.ytr = -self.d10 - j*self.amplitude/2 /initPoses
- self.ytl = self.d10 - j*self.amplitude/2 /initPoses
- angles = self.computeAlphaForWalk(self.SIZES, self.limAlpha1 )
- if len(angles)==0:
- self.exitFlag = self.exitFlag +1
- else:
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for i in range(len(angles)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- elif self.glob.SIMULATION == 0:
- returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[i] , angles[i]*self.FACTOR[i]+self.trims[i],
- self.sim.simx_opmode_oneshot)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- time.sleep(self.slowTime)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID,
- self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- elif self.glob.SIMULATION == 2:
- servoDatas = []
- for i in range(len(angles)):
- pos = int(angles[i]*1698 + 7500)
- servoDatas.append( self.kondo.ServoData(self.ACTIVESERVOS[i][0],self.ACTIVESERVOS[i][1],pos))
- servoDatas = self.reOrderServoData(servoDatas)
- a=self.kondo.setServoPos (servoDatas, self.frames_per_cycle)
- time1 = self.pyb.elapsed_millis(start1)
- self.pyb.delay(self.frame_delay - time1)
-
-if __name__=="__main__":
- print('This is not main module!')
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/class_Motion_sim.py b/src/azer_robocup_project/Soccer/Motion/class_Motion_sim.py
deleted file mode 100644
index 3b58379..0000000
--- a/src/azer_robocup_project/Soccer/Motion/class_Motion_sim.py
+++ /dev/null
@@ -1,343 +0,0 @@
-# Walking engine for Starkit Kondo OpenMV
-# Copyright STARKIT Soccer team of MIPT
-
-import sys, os
-import math, time, json
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if current_work_directory.find('Soccer') >= 0:
- current_work_directory = current_work_directory[:-14]
-if sys.version != '3.4.0':
- current_work_directory += '/'
- with open(current_work_directory + "simulator_lib_directory.txt", "r") as f:
- simulator_lib_directory = f.read()
- simulator_lib_directory = simulator_lib_directory.replace('\\', '/')
- sys.path.append(simulator_lib_directory)
- import random
- import sim, threading
-else:
- import starkit
- sys.path.append('/')
-
-sys.path.append( current_work_directory + 'Soccer/')
-sys.path.append( current_work_directory + 'Soccer/Motion/')
-sys.path.append( current_work_directory + 'Soccer/Vision/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/')
-sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
-
-
-from class_Motion import *
-from class_Motion_real import Motion_real
-from compute_Alpha_v3 import Alpha
-
-class Transfer_Data():
- def __init__(self):
- self.stop_Flag = False
- self.finish_Flag = False
- self.pause = False
- self.stop = 0
- self.finish = 0
-
-
-def sim_Enable(ip_address, port):
- simThreadCycleInMs = 2
- uprint ('Simulation started')
- #sim.simxFinish(-1) # just in case, close all opened connections
- clientID = sim.simxStart(ip_address, port, True, True, 5000, simThreadCycleInMs)
- if clientID != -1:
- uprint ('Connected to remote API server')
- else:
- uprint ('Failed connecting to remote API server')
- uprint ('Program ended')
- exit(0)
- return clientID
-
-def simulation_Trigger_Accumulator(clientIDs, events, transfer_Datas, lock):
- while(True):
- finish = False
- while(True):
- for transfer_Data in transfer_Datas:
- if transfer_Data.stop_Flag == True:
- finish = True
- for transfer_Data in transfer_Datas:
- if transfer_Data.finish_Flag == False: finish = False
- for transfer in transfer_Datas:
- transfer.stop_Flag = True
- break
- event_flag = True
- for i in range(len(events)):
- if (not events[i].is_set()): event_flag = False
- if transfer_Datas[i].finish_Flag == True:
- event_flag = True
- break
- if event_flag == True: break
- time.sleep(0.001)
- lock.acquire()
- lock.release()
- for clientID in clientIDs:
- sim.simxSynchronousTrigger(clientID)
- for event in events: event.clear()
- if finish == True:
- for clientID in clientIDs:
- sim.simxFinish(clientID)
- break
- pass
-
-
-class Motion_sim(Motion_real):
- def __init__(self, glob, vision, clientID , motion_EventID, lock, transfer_Data, numberOfRobots, robot_Number = ''):
- self.FRAMELENGTH = 0.02
- import random as random
- self.random = random
- import sim as vr
- self.sim = vr
- import numpy as np
- self.np = np
- import matplotlib.pyplot as plt
- self.plt = plt
- import cv2 as cv2
- self.cv2 = cv2
- import reload as re
- self.re = re
- import msvcrt as ms
- self.ms = ms
- import time
- self.utime = time
- self.Dummy_HData =[]
- self.BallData =[]
- self.timeElapsed = 0
- self.trims = []
- self.jointHandle = []
- self.Dummy_HHandle = 0
- self.Dummy_1Handle = 0
- self.BallHandle = 0
- self.VisionHandle = 0
- self.Ballposition = []
- self.transfer_Data = transfer_Data
- self.lock = lock
- self.motion_Event = motion_EventID
- self.clientID = clientID
- self.robot_Number = robot_Number
- self.sim_step_counter = 0
- self.numberOfRobots = numberOfRobots
- #self.SIM_OPTION = 'SURROGAT' # other variants: 'KONDOv23', 'ODE'
- super().__init__(glob, vision)
- with open(current_work_directory + "Init_params/Sim/" + self.glob.SIM_OPTION + "/Sim_calibr.json", "r") as f:
- data1 = json.loads(f.read())
- self.neck_calibr = data1['neck_calibr']
- self.neck_play_pose = data1['neck_play_pose']
- self.head_pitch_with_horizontal_camera = data1['head_pitch_with_horizontal_camera']
- self.neck_tilt = self.neck_calibr
- self.Vision_Sensor_Display_On = self.glob.params['Vision_Sensor_Display_On']
-
- def wait_sim_step(self):
- while True:
- self.sim.simxGetIntegerParameter(self.clientID, self.sim.sim_intparam_program_version, self.sim.simx_opmode_buffer)
- tim = self.sim.simxGetLastCmdTime(self.clientID)
- #print ('Simulation time: ', tim)
- if tim > self.sim_step_counter:
- self.sim_step_counter = tim
- break
- time.sleep(0.004)
- if self.transfer_Data.stop > 0:
- self.transfer_Data.stop += 1
- self.sim_Stop()
- while True:
- if self.transfer_Data.stop == self.numberOfRobots:
- self.sim_Disable()
- sys.exit(0)
- time.sleep(0.1)
-
- def getSimTime(self):
- inputInts=[]
- inputFloats=[]
- inputStrings=[]
- inputBuffer=bytearray()
- name = 'Telo_Surrogat'+ self.robot_Number
- self.sim.simxPauseSimulation(self.clientID, self.sim.simx_opmode_oneshot)
- res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(self.clientID,name,sim.sim_scripttype_childscript,
- 'getSimTime',inputInts,inputFloats,inputStrings,inputBuffer,sim.simx_opmode_blocking)
- self.sim.simxStartSimulation(self.clientID, self.sim.simx_opmode_oneshot)
- if len(retFloats) == 0: return 0
- return retFloats[0]
-
- def sim_simxSynchronousTrigger(self, clientID):
- if self.glob.SIMULATION == 1 :
- if self.transfer_Data.stop > 0:
- self.transfer_Data.stop += 1
- self.sim_Stop()
- while True:
- if self.transfer_Data.stop == self.numberOfRobots:
- self.sim_Disable()
- sys.exit(0)
- time.sleep(0.1)
- if self.glob.SIMULATION == 3 :
- self.wait_sim_step()
- return
- if self.motion_Event == 0:
- self.sim.simxSynchronousTrigger(clientID)
- else:
- self.motion_Event.set()
- while (self.motion_Event.is_set()): time.sleep(0.001)
-
- def vision_Sensor_Get_Image(self):
- returnCode, resolution, image_Data = self.sim.simxGetVisionSensorImage(self.clientID, self.VisionHandle, 0 ,self.sim.simx_opmode_buffer)
- nuimg = self.np.array(image_Data, dtype=self.np.uint8)
- nuimg.shape = (resolution[1],resolution[0],3)
- nuimg1 = self.cv2.cvtColor(nuimg, self.cv2.COLOR_RGB2BGR)
- img = self.np.flip(nuimg1, 1)
- return img
-
- def vision_Sensor_Display(self, img):
- if self.Vision_Sensor_Display_On:
- self.cv2.imshow('Vision Sensor'+ self.robot_Number, img)
- self.cv2.waitKey(10) & 0xFF
- #if self.robot_Number != '':
- # self.cv2.waitKey(10) & 0xFF
- #else:
- # res = self.cv2.waitKey(0)
- # if res == 115:
- # print('you have pressed "s"')
- # token = str(int(self.random.random()*10000))
- # filename = current_work_directory + "Soccer/CameraStill/VisionSensor" + token + '.png'
- # isWritten = self.cv2.imwrite(filename, img)
-
- def simulateMotion(self, number = 0, name = ''):
- #mot = [(0,'Initial_Pose'),(1,0),(2,0),(3,0),(4,0),(5,'Get_Up_Left'),
- # (6,'Soccer_Get_UP_Stomach_N'),(7,0),(8,'Soccer_Walk_FF'),(9,0),(10,0),
- # (11,0),(12,0),(13,0),(14,'Soccer_Small_Jump_Forward'),(15,0),
- # (16,0),(17,0),(18,'Soccer_Kick_Forward_Right_Leg'),(19,'Soccer_Kick_Forward_Left_Leg'),(20,0),
- # (21,'Get_Up_From_Defence'),(22,0),(23,'PanaltyDefenceReady_Fast'), (24,'PenaltyDefenceF'),(25,0),
- # (26,0),(27,0),(28,0),(29.0),(30,'Soccer_Walk_FF0'),
- # (31,'Soccer_Walk_FF1'), (32,'Soccer_Walk_FF2'), (33,'Soccer_Get_UP_Stomach'), (34,'Soccer_Get_UP_Face_Up'),
- # (35,'Get_Up_Right'), (36,'PenaltyDefenceR'), (37,'PenaltyDefenceL')]
- # start the simulation
- if number > 0 and name == '': name = self.MOTION_SLOT_DICT[number]
- with open(current_work_directory + "Soccer/Motion/motion_slots/" + name + ".json", "r") as f:
- slots = json.loads(f.read())
- mot_list = slots[name]
- i=0
- for i in range(len(mot_list)):
- if self.falling_Flag ==3: return
- activePoseOld = []
- for ind in range(len(self.activePose)): activePoseOld.append(self.activePose[ind])
- self.activePose =[]
- for j in range(len(self.ACTIVEJOINTS) - 2):
- self.activePose.append(0.017*mot_list[i][j+1]*0.03375)
- pulseNum = int(mot_list[i][0]*self.FRAMELENGTH * 1000 / self.simThreadCycleInMs)
- for k in range (pulseNum):
- if self.glob.SIMULATION == 3: self.wait_sim_step()
- self.sim.simxPauseCommunication(self.clientID, True)
- for j in range(len(self.ACTIVEJOINTS) - 2):
- tempActivePose = activePoseOld[j]+(self.activePose[j]-activePoseOld[j])*k/pulseNum
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- tempActivePose*self.FACTOR[j] +self.trims[j], self.sim.simx_opmode_streaming)
- self.sim.simxPauseCommunication(self.clientID, False)
- if self.glob.SIMULATION == 1:
- self.sim_simxSynchronousTrigger(self.clientID)
- return
-
-
- def sim_Start(self):
- #uprint ('Simulation started')
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- #self.sim.simxFinish(-1) # just in case, close all opened connections
- #self.clientID=self.sim.simxStart('127.0.0.1',19997,True,True,5000,self.simThreadCycleInMs) # Connect to V-REP
- #if self.clientID!=-1:
- # uprint ('Connected to remote API server')
- #else:
- # uprint ('Failed connecting to remote API server')
- # uprint ('Program ended')
- # exit(0)
- ## Collect Joint Handles and trims from model
- returnCode, self.Dummy_HHandle = self.sim.simxGetObjectHandle(self.clientID, 'Dummy_H'+ self.robot_Number, self.sim.simx_opmode_blocking)
- returnCode, self.Dummy_1Handle = self.sim.simxGetObjectHandle(self.clientID, 'Dummy1' + self.robot_Number, self.sim.simx_opmode_blocking)
- returnCode, self.BallHandle = self.sim.simxGetObjectHandle(self.clientID, 'Ball', self.sim.simx_opmode_blocking)
- returnCode, self.VisionHandle = self.sim.simxGetObjectHandle(self.clientID, 'Vision_sensor' + self.robot_Number, self.sim.simx_opmode_blocking)
- returnCode, self.Ballposition= self.sim.simxGetObjectPosition(self.clientID, self.BallHandle , -1, self.sim.simx_opmode_streaming)
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_streaming)
- returnCode, Dummy_Hquaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_streaming)
- returnCode, Dummy_1position= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_1Handle , -1, self.sim.simx_opmode_streaming)
- returnCode, Dummy_1quaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_1Handle , -1, self.sim.simx_opmode_streaming)
- returnCode, resolution, image_Data = self.sim.simxGetVisionSensorImage(self.clientID, self.VisionHandle, 0 ,self.sim.simx_opmode_streaming)
- returnCode, Camera_quaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.VisionHandle , -1, self.sim.simx_opmode_streaming)
- #uprint(Dummy_Hquaternion)
- for i in range(len(self.ACTIVEJOINTS)):
- returnCode, handle= self.sim.simxGetObjectHandle(self.clientID, self.ACTIVEJOINTS[i] + self.robot_Number, self.sim.simx_opmode_blocking)
- self.jointHandle.append(handle)
- returnCode, position= self.sim.simxGetJointPosition(self.clientID, handle, self.sim.simx_opmode_blocking)
- self.trims.append(position)
- self.activePose.append(position)
- if self.glob.SIMULATION == 1:
- self.sim.simxSynchronous(self.clientID,True)
- if self.glob.SIMULATION == 3:
- self.sim.simxGetIntegerParameter(self.clientID, self.sim.sim_intparam_program_version, self.sim.simx_opmode_streaming)
-
- def sim_Progress(self,simTime): # simTime in seconds
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- for i in range(int(simTime*1000//self.simThreadCycleInMs)):
- returnCode, Dummy_Hposition= self.sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- self.Dummy_HData.append(Dummy_Hposition)
- returnCode, self.Ballposition= self.sim.simxGetObjectPosition(self.clientID, self.BallHandle , -1, self.sim.simx_opmode_buffer)
- self.BallData.append(self.Ballposition)
- returnCode, Dummy_Hquaternion= self.sim.simxGetObjectQuaternion(self.clientID, self.Dummy_HHandle , -1, self.sim.simx_opmode_buffer)
- #uprint(quaternion_to_euler_angle(Dummy_Hquaternion))
- self.timeElapsed = self.timeElapsed +1
- #self.vision_Sensor_Display(self.vision_Sensor_Get_Image())
- if self.glob.SIMULATION == 1 : self.sim_simxSynchronousTrigger(self.clientID)
- if self.glob.SIMULATION == 3 :
- time.sleep(0.005)
- self.wait_sim_step()
-
- def sim_Stop(self):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 0 or self.glob.SIMULATION == 3:
- self.sim.simxStopSimulation(self.clientID,self.sim.simx_opmode_oneshot)
- # return to initial pose
- for j in range(len(self.ACTIVEJOINTS)):
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- returnCode = self.sim.simxSetJointTargetPosition(self.clientID,
- self.jointHandle[j] , self.trims[j], self.sim.simx_opmode_oneshot)
- else: returnCode = self.sim.simxSetJointPosition(self.clientID,
- self.jointHandle[j] , self.trims[j], self.sim.simx_opmode_oneshot)
- def print_Diagnostics(self):
- Dummy_HDataX =[]
- Dummy_HDataY =[]
- Dummy_HDataZ =[]
- for i in range (self.timeElapsed):
- Dummy_HDataX.append( self.Dummy_HData[i][0])
- Dummy_HDataY.append(self.Dummy_HData[i][1])
- Dummy_HDataZ.append(self.Dummy_HData[i][2])
- BallDataX =[]
- BallDataY =[]
- BallDataZ =[]
- for i in range (self.timeElapsed):
- BallDataX.append( self.BallData[i][0])
- BallDataY.append(self.BallData[i][1])
- BallDataZ.append(self.BallData[i][2])
- uprint('exitFlag' ,self.exitFlag)
- rng = self.np.arange(self.timeElapsed)
- fig,ax = self.plt.subplots(figsize=(10,6))
- self.plt.plot(rng,Dummy_HDataX, label = 'Body X')
- self.plt.plot(rng,Dummy_HDataY, label = 'Body Y')
- self.plt.plot(rng,Dummy_HDataZ, label = 'Body Z')
- self.plt.plot(rng,BallDataX, label = 'Ball X')
- self.plt.plot(rng,BallDataY, label = 'Ball Y')
- self.plt.plot(rng,BallDataZ, label = 'Ball Z')
- ax.legend(loc='upper left')
- ax.grid(True)
- if self.glob.SIMULATION == 1 or self.glob.SIMULATION == 3:
- self.plt.show()
- #break
- uprint('exitFlag' ,self.exitFlag)
-
- def sim_Disable(self): # Now close the connection to V-REP:
- time.sleep(0.2)
- #self.sim.simxFinish(self.clientID)
- self.transfer_Data.finish_Flag = True
-
-
-if __name__=="__main__":
- print('This is not main module!')
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v3.py b/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v3.py
deleted file mode 100644
index 96a4151..0000000
--- a/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v3.py
+++ /dev/null
@@ -1,181 +0,0 @@
-import math, time
-
-#@micropython.native
-class Alpha():
-
- def compute_Alpha_v3(self, xt,yt,zt,x,y,z,w, sizes, limAlpha):
- from math import sqrt,cos,sin,asin,fabs,tan,atan
- #t1_start =time.perf_counter_ns()
- a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 = sizes
- limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10 = limAlpha
- alpha5 = w
- cos5 = math.cos(alpha5)
- sin5 = math.sin(alpha5)
- nor = math.sqrt(x*x+y*y+z*z)
- x = x/nor
- y = y/nor
- z = z/nor
- xtp = xt * cos5 + (yt + a5) * sin5
- ytp = (yt + a5) * cos5 - xt * sin5
- ztp = zt
- xp = x * cos5 + y * sin5
- yp = y * cos5 - x * sin5
- zp = z
- var = [-1,1]
- angles = []
- lim1a= limAlpha6[0]*0.00058909
- lim2a = limAlpha6[1]*0.00058909
- ind = 1
- step1 = (lim2a-lim1a)/10
- testalpha6 =[]
- for i in range (11):
- alpha6 = lim1a+i*step1
- cos= math.cos(alpha6)
- sin= math.sin(alpha6)
- testalpha6.append( ((ytp+b5)*cos+ztp*sin -c10)*((yp*cos+zp*sin)**2-
- (zp*cos-yp*sin)**2 -xp*xp)-a10-b10*(yp*cos+zp*sin)/math.sqrt((zp*cos-yp*sin)**2+xp*xp))
- #print(testalpha6)
- points = []
- for i in range(10):
- if (testalpha6[i]>0 and testalpha6[i+1]<0)or(testalpha6[i]<0 and testalpha6[i+1]>0): points.append(i)
- k=0
- if len(points)==0:
- for i in range(11):
- if (math.fabs(testalpha6[i]) < math.fabs(testalpha6[k])): k=i
- if k==10: points.append(9)
- else:
- if (math.fabs(testalpha6[k-1]) < math.fabs(testalpha6[k+1])): points.append(k-1)
- else: points.append(k)
- alpha6m = []
- for j in range(len(points)):
- lim1=lim1a+points[j]*step1
- lim2=lim1+step1
- while (True):
- step = (lim2-lim1)/10
- testalpha6 =[]
- for i in range (11):
- alpha6 = lim1+i*step
- cos= math.cos(alpha6)
- sin= math.sin(alpha6)
- testalpha6.append( ((ytp+b5)*cos+ztp*sin-c10)*((yp*cos+zp*sin)**2-
- (zp*cos-yp*sin)**2 -xp*xp)-a10-b10*(yp*cos+zp*sin)/math.sqrt((zp*cos-yp*sin)**2+xp*xp))
- k=0
- for i in range(11):
- if (math.fabs(testalpha6[i]) < math.fabs(testalpha6[k])): k = i
- if k==0: k2=1
- elif k==10: k2 = 9
- else:
- if (math.fabs(testalpha6[k-1]) < math.fabs(testalpha6[k+1])): k2=k-1
- else: k2=k+1
- alpha6 = lim1+k*step
- if k>k2:
- lim1 = lim1+k2*step
- lim2 = lim1+ step
- else:
- lim1 = lim1+k*step
- lim2 = lim1+ step
- if (lim2-lim1 < 0.00025): break
- ind = ind + 1
- if ind> (limAlpha6[1]- limAlpha6[0]): break
- alpha6m.append(alpha6)
- alpha10m =[]
- kk=0
- #t1_stop =time.perf_counter_ns()
- #print('time t1 elapsed= ',(t1_stop-t1_start))
- for i in range (len(alpha6m)):
- tan6 = math.tan(alpha6m[i-kk])
- alpha10 = math.atan((-yp-zp*tan6)/math.sqrt((zp-yp*tan6)**2+xp*xp*(1+tan6*tan6)))
- if limAlpha10[0] < alpha10*1698 and alpha10*1698<limAlpha10[1]: alpha10m.append(alpha10)
- else:
- alpha6m.pop(i-kk)
- kk=kk+1
- #print('alpha10m = ', alpha10m)
- kk=0
- #t1_stop =time.perf_counter_ns()
- #print('time t2 elapsed= ',(t1_stop-t1_start))
- for ii in range (len(alpha6m)):
- cos6 = math.cos(alpha6m[ii-kk])
- sin6 = math.sin(alpha6m[ii-kk])
- alpha987 = math.atan(-xp/(zp*cos6- yp*sin6))
- sin987 = math.sin(alpha987)
- cos987 = math.cos(alpha987)
- K1 = a6*sin987+xtp*cos987+(ztp*cos6-(ytp+b5)*sin6)*sin987
- K2 = a9+a6*cos987+(ztp*cos6-(ytp+b5)*sin6)*cos987-xtp*sin987+b10/math.cos(alpha10m[ii-kk])+((ytp+b5)*cos6+ztp*sin6-c10)*math.tan(alpha10m[ii-kk])
- m = (K1*K1+K2*K2+a8*a8-a7*a7)/(2*a8)
-
-
- temp1 = K1*K1*m*m-(K1*K1+K2*K2)*(m*m-K2*K2)
- if temp1>=0 :
- temp2 = (-K1*m + math.sqrt(temp1))/(K1*K1+K2*K2)
- temp3 = (-K1*m - math.sqrt(temp1))/(K1*K1+K2*K2)
- if math.fabs(temp2) <= 1 and math.fabs(temp3) <= 1:
- alpha91 = math.asin(temp2)
- alpha92 = math.asin(temp3)
- else:
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
- else:
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
- alpha81 = math.atan((K1+a8*math.sin(alpha91))/(K2+a8*math.cos(alpha91))) - alpha91
- alpha82 = math.atan((K1+a8*math.sin(alpha92))/(K2+a8*math.cos(alpha92))) - alpha92
- alpha71 = alpha91+alpha81- alpha987
- alpha72 = alpha92+alpha82- alpha987
- #t1_stop =time.perf_counter_ns()
- #print('time t3 elapsed= ',(t1_stop-t1_start))
- temp71 = alpha71*1698<limAlpha7[0] or alpha71*1698>limAlpha7[1]
- temp72 = alpha72*1698<limAlpha7[0] or alpha72*1698>limAlpha7[1]
- temp81 = alpha81*1698<limAlpha8[0] or alpha81*1698>limAlpha8[1]
- temp82 = alpha82*1698<limAlpha8[0] or alpha82*1698>limAlpha8[1]
- temp91 = alpha91*1698<limAlpha9[0] or alpha91*1698>limAlpha9[1]
- temp92 = alpha92*1698<limAlpha9[0] or alpha92*1698>limAlpha9[1]
- if (temp71 and temp72) or (temp81 and temp82) or (temp91 and temp92) or ((temp71 or temp81 or temp91) and (temp72 or temp82 or temp92)):
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
- else:
- if not (temp71 or temp81 or temp91):
- ang =()
- ang =alpha10m[ii-kk],alpha91,alpha81,alpha71,alpha6m[ii-kk],alpha5
- angles.append(ang)
- if not (temp72 or temp82 or temp92):
- ang =()
- ang =alpha10m[ii-kk],alpha92,alpha82,alpha72,alpha6m[ii-kk],alpha5
- angles.append(ang)
- #t1_stop =time.perf_counter_ns()
- #print('time t4 elapsed= ',(t1_stop-t1_start))
- return angles
-
-if __name__ == "__main__":
- a5 = 21.5 # мм раÑÑтоÑние от оÑи Ñимметрии до оÑи Ñервы 5
- b5 = 18.5 # мм раÑÑтоÑние от оÑи Ñервы 5 до оÑи Ñервы 6 по горизонтали
- c5 = 0 # мм раÑÑтоÑние от оÑи Ñервы 6 до Ð½ÑƒÐ»Ñ Z по вертикали
- a6 = 42 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 7
- a7 = 65.5 # мм раÑÑтоÑние от оÑи Ñервы 7 до оÑи Ñервы 8
- a8 = 63.8 # мм раÑÑтоÑние от оÑи Ñервы 8 до оÑи Ñервы 9
- a9 = 35.5 # мм раÑÑтоÑние от оÑи Ñервы 9 до оÑи Ñервы 10
- a10= 25.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до центра Ñтопы по горизонтали
- b10= 16.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до низа Ñтопы
- c10 = 12 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 10 по горизонтали
- sizes = [ a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 ]
-
- limAlpha5 = [-2667, 2667]
- limAlpha6 = [-3000, 740]
- limAlpha7 = [-3555, 3260]
- limAlpha8 = [-4150, 1777]
- limAlpha9 = [-4000, 2960]
- limAlpha10 =[-2815, 600]
- limAlpha = [limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10]
-
- clock1 = time.clock()
- clock1.tick()
- compute_Alpha_v3(0,-54.3,-200,0,0,-1,0, sizes, limAlpha)
- print('time elapsed in compute_Alpha:', clock1.avg())
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v4.py b/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v4.py
deleted file mode 100755
index 84f3b28..0000000
--- a/src/azer_robocup_project/Soccer/Motion/compute_Alpha_v4.py
+++ /dev/null
@@ -1,199 +0,0 @@
-import math, time
-
-#@micropython.native
-class Alpha():
-
- def compute_Alpha_v4(self, xt,yt,zt,x,y,z,w, sizes, limAlpha):
- from math import sqrt,cos,sin,asin,fabs,tan,atan
- #t1_start =time.perf_counter_ns()
- a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 = sizes
- limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10 = limAlpha
- alpha5 = w
- cos5 = math.cos(alpha5)
- sin5 = math.sin(alpha5)
- nor = math.sqrt(x*x+y*y+z*z)
- x = x/nor
- y = y/nor
- z = z/nor
- xtp = xt * cos5 + (yt + a5) * sin5
- ytp = (yt + a5) * cos5 - xt * sin5
- ztp = zt
- xp = x * cos5 + y * sin5
- yp = y * cos5 - x * sin5
- zp = z
- var = [-1,1]
- angles = []
- lim1a= limAlpha6[0]*0.00058909
- lim2a = limAlpha6[1]*0.00058909
- ind = 1
- step1 = (lim2a-lim1a)/10
- testalpha6 =[]
- for i in range (11):
- alpha6 = lim1a+i*step1
- cos= math.cos(alpha6)
- sin= math.sin(alpha6)
- testalpha6.append( ((ytp+b5)*cos+ztp*sin -c10)*((yp*cos+zp*sin)**2-
- (zp*cos-yp*sin)**2 -xp*xp)-a10-b10*(yp*cos+zp*sin)/math.sqrt((zp*cos-yp*sin)**2+xp*xp))
- #print(testalpha6)
- points = []
- for i in range(10):
- if (testalpha6[i]>0 and testalpha6[i+1]<0)or(testalpha6[i]<0 and testalpha6[i+1]>0): points.append(i)
- k=0
- if len(points)==0:
- for i in range(11):
- if (math.fabs(testalpha6[i]) < math.fabs(testalpha6[k])): k=i
- if k==10: points.append(9)
- else:
- if (math.fabs(testalpha6[k-1]) < math.fabs(testalpha6[k+1])): points.append(k-1)
- else: points.append(k)
- alpha6m = []
- for j in range(len(points)):
- lim1=lim1a+points[j]*step1
- lim2=lim1+step1
- while (True):
- step = (lim2-lim1)/10
- testalpha6 =[]
- for i in range (11):
- alpha6 = lim1+i*step
- cos= math.cos(alpha6)
- sin= math.sin(alpha6)
- testalpha6.append( ((ytp+b5)*cos+ztp*sin-c10)*((yp*cos+zp*sin)**2-
- (zp*cos-yp*sin)**2 -xp*xp)-a10-b10*(yp*cos+zp*sin)/math.sqrt((zp*cos-yp*sin)**2+xp*xp))
- k=0
- for i in range(11):
- if (math.fabs(testalpha6[i]) < math.fabs(testalpha6[k])): k = i
- if k==0: k2=1
- elif k==10: k2 = 9
- else:
- if (math.fabs(testalpha6[k-1]) < math.fabs(testalpha6[k+1])): k2=k-1
- else: k2=k+1
- alpha6 = lim1+k*step
- if k>k2:
- lim1 = lim1+k2*step
- lim2 = lim1+ step
- else:
- lim1 = lim1+k*step
- lim2 = lim1+ step
- if (lim2-lim1 < 0.00025): break
- ind = ind + 1
- if ind> (limAlpha6[1]- limAlpha6[0]): break
- alpha6m.append(alpha6)
- alpha10m =[]
- kk=0
- #t1_stop =time.perf_counter_ns()
- #print('time t1 elapsed= ',(t1_stop-t1_start))
- for i in range (len(alpha6m)):
- tan6 = math.tan(alpha6m[i-kk])
- alpha10 = math.atan((-yp-zp*tan6)/math.sqrt((zp-yp*tan6)**2+xp*xp*(1+tan6*tan6)))
- if limAlpha10[0] < alpha10*1698 and alpha10*1698<limAlpha10[1]: alpha10m.append(alpha10)
- else:
- alpha6m.pop(i-kk)
- kk=kk+1
- #print('alpha10m = ', alpha10m)
- kk=0
- #t1_stop =time.perf_counter_ns()
- #print('time t2 elapsed= ',(t1_stop-t1_start))
- for ii in range (len(alpha6m)):
- cos6 = math.cos(alpha6m[ii-kk])
- sin6 = math.sin(alpha6m[ii-kk])
- alpha987 = math.atan(-xp/(zp*cos6- yp*sin6))
- sin987 = math.sin(alpha987)
- cos987 = math.cos(alpha987)
- K1 = a6*sin987+xtp*cos987+(ztp*cos6-(ytp+b5)*sin6)*sin987
- K2 = a9+a6*cos987+(ztp*cos6-(ytp+b5)*sin6)*cos987-xtp*sin987+b10/math.cos(alpha10m[ii-kk])+((ytp+b5)*cos6+ztp*sin6-c10)*math.tan(alpha10m[ii-kk])
- m = (K1*K1+K2*K2+a8*a8-a7*a7)/(2*a8)
-
-
- temp1 = K1*K1*m*m-(K1*K1+K2*K2)*(m*m-K2*K2)
- if temp1>=0 :
- temp2 = (-K1*m + math.sqrt(temp1))/(K1*K1+K2*K2)
- temp3 = (-K1*m - math.sqrt(temp1))/(K1*K1+K2*K2)
- if math.fabs(temp2) <= 1 and math.fabs(temp3) <= 1:
- alpha91 = math.asin(temp2)
- alpha92 = math.asin(temp3)
-####################################################### # Ver4 change start
- temp21 = (K1+temp2 + m)/(-K2)
- temp31 = (K1+temp3 + m)/(-K2)
- if math.fabs(temp21) <= 1:
- alpha911 = math.acos(temp21)
- if (alpha91 > 0 and alpha911 > alpha91) or (alpha91 < 0 and alpha911 < alpha91):
- alpha91 = alpha911
- if math.fabs(temp31) <= 1:
- alpha921 = math.acos(temp31)
- if (alpha92 > 0 and alpha921 > alpha92) or (alpha92 < 0 and alpha921 < alpha92):
- alpha92 = alpha921
-####################################################### # Ver4 change stop
- else:
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
- else:
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
-####################################################### # Ver4 change start
- #alpha81 = math.atan((K1+a8*math.sin(alpha91))/(K2+a8*math.cos(alpha91))) - alpha91
- #alpha82 = math.atan((K1+a8*math.sin(alpha92))/(K2+a8*math.cos(alpha92))) - alpha92
- alpha81 = math.atan2(-(K1+a8*math.sin(alpha91)),-(K2+a8*math.cos(alpha91))) - alpha91
- alpha82 = math.atan2(-(K1+a8*math.sin(alpha92)),-(K2+a8*math.cos(alpha92))) - alpha92
-####################################################### # Ver4 change stop
- alpha71 = alpha91+alpha81- alpha987
- alpha72 = alpha92+alpha82- alpha987
- #t1_stop =time.perf_counter_ns()
- #print('time t3 elapsed= ',(t1_stop-t1_start))
- temp71 = alpha71*1698<limAlpha7[0] or alpha71*1698>limAlpha7[1]
- temp72 = alpha72*1698<limAlpha7[0] or alpha72*1698>limAlpha7[1]
- temp81 = alpha81*1698<limAlpha8[0] or alpha81*1698>limAlpha8[1]
- temp82 = alpha82*1698<limAlpha8[0] or alpha82*1698>limAlpha8[1]
- temp91 = alpha91*1698<limAlpha9[0] or alpha91*1698>limAlpha9[1]
- temp92 = alpha92*1698<limAlpha9[0] or alpha92*1698>limAlpha9[1]
- if (temp71 and temp72) or (temp81 and temp82) or (temp91 and temp92) or ((temp71 or temp81 or temp91) and (temp72 or temp82 or temp92)):
- alpha6m.pop(ii-kk)
- alpha10m.pop(ii-kk)
- kk=kk+1
- continue
- else:
- if not (temp71 or temp81 or temp91):
- ang =()
- ang =alpha10m[ii-kk],alpha91,alpha81,alpha71,alpha6m[ii-kk],alpha5
- angles.append(ang)
- if not (temp72 or temp82 or temp92):
- ang =()
- ang =alpha10m[ii-kk],alpha92,alpha82,alpha72,alpha6m[ii-kk],alpha5
- angles.append(ang)
- #t1_stop =time.perf_counter_ns()
- #print('time t4 elapsed= ',(t1_stop-t1_start))
- return angles
-
-if __name__ == "__main__":
- a5 = 21.5 # мм раÑÑтоÑние от оÑи Ñимметрии до оÑи Ñервы 5
- b5 = 18.5 # мм раÑÑтоÑние от оÑи Ñервы 5 до оÑи Ñервы 6 по горизонтали
- c5 = 0 # мм раÑÑтоÑние от оÑи Ñервы 6 до Ð½ÑƒÐ»Ñ Z по вертикали
- a6 = 42 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 7
- a7 = 65.5 # мм раÑÑтоÑние от оÑи Ñервы 7 до оÑи Ñервы 8
- a8 = 63.8 # мм раÑÑтоÑние от оÑи Ñервы 8 до оÑи Ñервы 9
- a9 = 35.5 # мм раÑÑтоÑние от оÑи Ñервы 9 до оÑи Ñервы 10
- a10= 25.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до центра Ñтопы по горизонтали
- b10= 26.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до низа Ñтопы
- c10 = 12 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 10 по горизонтали
- sizes = [ a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 ]
-
- limAlpha5 = [-2667, 2667]
- limAlpha6 = [-3000, 740]
- limAlpha7 = [-3555, 3260]
- limAlpha8 = [-4500, 1777]
- limAlpha9 = [-4000, 3340]
- limAlpha10 =[-2815, 600]
- limAlpha = [limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10]
-
- #clock1 = time.clock()
- #clock1.tick()
- a = Alpha()
- angles = a.compute_Alpha_v4(-50,-54.3,-167,0,0,-1,0, sizes, limAlpha)
- #print('time elapsed in compute_Alpha:', clock1.avg())
- print('angles:', angles)
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/kondo_controller.py b/src/azer_robocup_project/Soccer/Motion/kondo_controller.py
deleted file mode 100644
index 2564ad9..0000000
--- a/src/azer_robocup_project/Soccer/Motion/kondo_controller.py
+++ /dev/null
@@ -1,856 +0,0 @@
-# UART Control for Kondo Rcb4 controller
-#
-openmv = True
-
-import time
-import struct
-# try:
-# from pyb import UART
-# except ImportError:
-# raise Exception("Trying to import MicroPython library using Python3")
-
-
-# from micropython import const
-
-class Serial:
- def __init__(self, uart):
- self.uart = uart
-
- def flushInput(self):
- if openmv:
- buf = self.uart.any()
- self.uart.read(buf)
- else:
- self.uart.flushInput()
-
- def read(self, rxLen):
- if openmv:
- msg = []
- for _ in range(rxLen):
- msg.append(self.uart.readchar())
- #print("read ",msg)
- return bytes(bytearray(msg))
- else:
- #print("read ",msg)
- return self.uart.read(rxLen)
-
- def write(self, buf):
- if openmv:
- tmp = bytes(bytearray(buf))
- #print("written ",buf)
- for b in tmp:
- self.uart.writechar(b)
- # time.sleep(1)
- else:
- #print("written ",buf)
- self.uart.write(buf)
-
-class Rcb4BaseLib:
- def __init__(self):
- self.maxMotionCount = (120)
- self.MotionSingleDataCount = 2048
- self.UserParameterSingleDataCount = 2
- self.UserParameterCount = 20
- self.IcsDeviceSize = 35
- self.IcsDeviceDataSize = 20
- self.CounterSingleDataCount = 1
- self.CounterCount = 10
- self.AdcCount = 11
- self.AdcDataCount = 22
- self.__isSynchronize = False
- self.__configData = 0
- self.comOpen = False
- self.com = None
-
-
-
- def open(self, uart=''):
- if self.comOpen == False:
- self.com = Serial(uart)
- self.com.flushInput()
- if self.checkAcknowledge() == True:
- confData = self.getConfig()
- if (confData == 0xFFFF):
- return False
- else:
- self.__configData = confData
- self.comOpen = True
- return True
- else:
- return False
-
- else:
- return False
-
- def close(self):
- try:
- self.com.deinit()
- self.comOpen = False
- return 0
- except:
- return -1
-
- @staticmethod
- def CheckSum(dataBytes):
- sum = 0
- for i in range(dataBytes[0] - 1):
- sum = sum + dataBytes[i]
- return sum & 0xff
-
- def __checkCheckSum(self, dataBytes):
- if dataBytes[0] == 0:
- return False
- else:
- if dataBytes[dataBytes[0] - 1] == self.CheckSum(dataBytes):
- return True
- else:
- return False
-
- class CmdOkType:
- Ok = 0
- Error = -1
-
- class AckType:
- Ack = 0x06
- Nack = 0x15
-
- class SubMoveCmd:
- RamToCom = 0x20
- ComToRam = 0x02
- DeviceToCom = 0x21
- ComToDevice = 0x12
-
- class CommandTypes:
- Move = 0x00
- Call = 0x0C
- AckCheck = 0xFE
- SingleServo = 0x0F
- ConstFrameServo = 0x10
- ServoParam = 0x12
-
- class RamAddr:
- ConfigRamAddress = 0x0000
- ProgramCounterRamAddress = 0x0002
- UserParameterRamAddress = 0x0462
- CounterRamAddress = 0x0457
- AdcRamAddress = 0x0022
- PioModeAddres = 0x0038
- PioAddress = 0x003A
-
- class RomAddr:
- StartupCmdRomAddress = 0x0444
- MainLoopCmd = 0x044B
- MotionRomAddress = 0x0b80
-
- class ConfigData:
- EnableSio = (0x0001)
- EnableRunEeprom = (0x0002)
- EnableServoResponse = (0x0004)
- EnableReferenceTable = (0x0008)
-
- class ServoData:
- def __init__(self, id=0, sio=0, data=0):
- self.id = id
- self.sio = sio
- self.data = data
-
- def icsNum2id(self):
- return self.id * 2 + (self.sio - 1)
-
- def itemAdd(self, x, y, z):
- self.id = x
- self.sio = y
- self.data = z
-
- def __lt__(self, other):
- return (self.id * 2 + (self.sio - 1)) < (other.id * 2 + (other.sio - 1))
-
- def __repr__(self):
- return "({0}, {1}, {2})".format(self.id, self.sio, self.data)
-
- class DeviceAddrOffset:
- CategoryAddressOffset = 0x00
- IDAddressOffset = 0x01
- TrimAddressOffset = 0x02
- MotorPositionAddressOffset = 0x04
- PositionAddressOffset = 0x06
- FrameAddressOffset = 0x08
- Mixing1AddressOffset = 0x0E
- Mixing1RatioAddressOffset = 0x10
- Mixing2AddressOffset = 0x11
- Mixing2RatioAddressOffset = 0x13
-
- def synchronize(self, txBuf, rxLen, writeOnly=False):
- sendbuf = [256]
- if self.__isSynchronize == False:
- sendbuf = []
-
- if (len(txBuf) == 0 or rxLen <= 3):
- rxbuf = []
- return rxbuf
-
- for i in range(len(txBuf)):
- if txBuf[i] < 0 or 255 < txBuf[i]:
- rxbuf = []
- return rxbuf
- sendbuf.append(txBuf[i])
-
- self.__isSynchronize = True
-
- self.com.flushInput()
- self.com.write(sendbuf)
- if not writeOnly:
- rxbuf = self.com.read(rxLen)
- self.com.flushInput()
- if rxbuf is not None:
- if len(rxbuf) == rxLen and rxbuf[0] == rxLen:
- if self.__checkCheckSum(rxbuf) == False:
- rxbuf = []
- else:
- rxbuf = []
- else:
- rxbuf = []
- else:
- self.com.flushInput()
- rxbuf = []
- self.__isSynchronize = False
-
- return rxbuf
- else:
- rxbuf = []
- return rxbuf
-
- @staticmethod
- def moveComToRamCmd(destAddr, destData):
- txbuf = []
- if len(destData) > 249:
- return 0, Rcb4BaseLib.CmdOkType.Error
- txbuf.append((len(destData) + 7) & 0xff)
- txbuf.append(Rcb4BaseLib.CommandTypes.Move)
- txbuf.append(Rcb4BaseLib.SubMoveCmd.ComToRam)
- txbuf.append(destAddr & 0xff)
- txbuf.append((destAddr >> 8) & 0xff)
- txbuf.append(0x00)
-
- if len(destData) == 1:
- txbuf.append(destData[0])
- else:
- for i in range(len(destData)):
- txbuf.append(destData[i])
-
- txbuf.append(Rcb4BaseLib.CheckSum(txbuf))
-
- returnDataSize = 4
-
- return returnDataSize, txbuf
-
- def moveComToRamCmdSynchronize(self, scrAddr, destData):
-
- readSize, sendData = self.moveComToRamCmd(scrAddr, destData)
- if readSize > 0:
- rxbuf = self.synchronize(sendData, readSize)
- if len(rxbuf) > 3 and Rcb4BaseLib.AckType.Ack == rxbuf[2]:
- return True
- else:
- return False
- else:
- return False
-
- @staticmethod
- def callCmd(romAddr):
- buf = []
- buf.append(0x07)
- buf.append(Rcb4BaseLib.CommandTypes.Call)
- buf.append(romAddr & 0xff)
- buf.append((romAddr >> 8) & 0xff)
- buf.append((romAddr >> 16) & 0xff)
- buf.append(0x00)
- buf.append(Rcb4BaseLib.CheckSum(buf))
- return 4, buf
-
- def synchronizeAck(self, txData):
- rxbuf = self.synchronize(txData, 4)
- if len(rxbuf) > 3 and self.AckType.Ack == rxbuf[2]:
- return True
- else:
- return False
-
- @staticmethod
- def acknowledgeCmd():
- txbuf = [0x04, Rcb4BaseLib.CommandTypes.AckCheck, Rcb4BaseLib.AckType.Ack, 0]
- txbuf[3] = Rcb4BaseLib.CheckSum(txbuf)
-
- returnDataSize = 4
-
- return returnDataSize, txbuf
-
- @staticmethod
- def moveRamToComCmd(scrAddr, scrDataSize):
- txbuf = []
- txbuf.append(0x0a)
- txbuf.append(Rcb4BaseLib.CommandTypes.Move)
- txbuf.append(Rcb4BaseLib.SubMoveCmd.RamToCom)
- txbuf.append(0x00)
- txbuf.append(0x00)
- txbuf.append(0x00)
- txbuf.append(scrAddr & 0xff)
- txbuf.append((scrAddr >> 8) & 0xff)
- txbuf.append(scrDataSize)
- txbuf.append(Rcb4BaseLib.CheckSum(txbuf))
-
- returnDataSize = scrDataSize + 3
-
- return returnDataSize, txbuf
-
- def moveRamToComCmdSynchronize(self, scrAddr, scrDataSize):
- readSize, sendData = self.moveRamToComCmd(scrAddr, scrDataSize)
- if readSize > 0:
- rxbuf = self.synchronize(sendData, readSize)
- if len(rxbuf) < readSize:
- rxbuf = []
- return False, rxbuf
-
- destData = []
- if scrDataSize == 1:
- destData.append(rxbuf[2])
-
- else:
- for i in range(scrDataSize):
- destData.append(rxbuf[i + 2])
-
- return True, bytes(destData)
-
- else:
- rxbuf = []
- return False, rxbuf
-
-
- def moveComToDeviceCmd (self, icsNum, offset, destData):
-
- buf =[]
- destSize = len(destData)
- if((icsNum < 0)or(self.IcsDeviceSize < icsNum)or (offset < 0) or(self.IcsDeviceDataSize <(offset+destSize))):
- return -1,buf
-
- buf.append (0x07 + len(destData))
- buf.append (self.CommandTypes.Move)
- buf.append (self.SubMoveCmd.ComToDevice)
- buf.append (offset)
- buf.append (icsNum)
- buf.append (0x00)
- if len(destData) == 1:
- buf.append (destData[0])
- else:
- for i in range(len(destData)):
- buf.append (destData[ i ])
- buf.append (Rcb4BaseLib.CheckSum(buf))
- return 4,buf
-
- def moveComToDeviceCmdSynchronize(self,icsNum, offset, destData):
- readSize, sendData = self.moveComToDeviceCmd (icsNum, offset, destData)
- if readSize > 0:
- rxbuf = self.synchronize(sendData, readSize)
- if len(rxbuf) > 3 and self.AckType.Ack ==rxbuf[2]:
- return True
- else:
- return False
- else:
- return False
-
-
- def getConfig(self):
- retf, rxbuf = self.moveRamToComCmdSynchronize(Rcb4BaseLib.RamAddr.ConfigRamAddress, 2)
-
- if (retf == False) or (len(rxbuf) != 2):
- return 0xFFFF
- else:
- return (rxbuf[1] * 256 + rxbuf[0])
-
- def checkAcknowledge(self):
- reSize, txbuf = self.acknowledgeCmd()
- rxbuf = self.synchronize(txbuf, reSize)
- if len(rxbuf) == 0:
- return False
- else:
- return True
-
- def getMotionPlayNum(self):
-
- retf, retbuf = self.moveRamToComCmdSynchronize(self.RamAddr.ProgramCounterRamAddress, 10)
-
- if (retf == False) or (len(retbuf) != 10):
- return -1
-
- eflfg = retbuf[3] + retbuf[4] + retbuf[5] + retbuf[6] + retbuf[7]
-
- pcunt = retbuf[0] + (retbuf[1] << 8) + (retbuf[2] << 16)
-
- mno = int(((pcunt - Rcb4BaseLib.RomAddr.MotionRomAddress) / self.MotionSingleDataCount) + 1)
-
- if eflfg == 0:
- return 0
- if pcunt < Rcb4BaseLib.RomAddr.MotionRomAddress:
- return 0
- if mno < 0 or mno > 120:
- return -2
-
- return mno
-
- def suspend(self):
-
- self.__configData &= ~Rcb4BaseLib.ConfigData.EnableRunEeprom
- self.__configData &= ~Rcb4BaseLib.ConfigData.EnableServoResponse
- self.__configData &= ~Rcb4BaseLib.ConfigData.EnableReferenceTable
- self.__configData &= ~Rcb4BaseLib.ConfigData.EnableSio
-
- txbuf = [self.__configData & 0xff, (self.__configData >> 8) & 0xff]
- return self.moveComToRamCmdSynchronize(Rcb4BaseLib.RamAddr.ConfigRamAddress, txbuf)
-
- def motionAddr2motionNum(self, motionNum):
- if 0 < motionNum <= self.maxMotionCount:
- return (motionNum - 1) * self.MotionSingleDataCount + Rcb4BaseLib.RomAddr.MotionRomAddress
- else:
- return -1
-
- def setMotionNum(self, motionNum):
- if 0 < motionNum <= self.maxMotionCount:
- _, buf = self.callCmd(self.motionAddr2motionNum(motionNum))
- return self.synchronizeAck(buf)
- else:
- return False
-
- def resetProgramCounter(self):
- buf = [Rcb4BaseLib.RomAddr.MainLoopCmd & 0xff, (Rcb4BaseLib.RomAddr.MainLoopCmd >> 8) & 0xff, 0, 0,
- 0, 0, 0, 0, 0, 0]
- return self.moveComToRamCmdSynchronize(Rcb4BaseLib.RamAddr.ProgramCounterRamAddress, buf)
-
- def resume(self):
- self.__configData |= Rcb4BaseLib.ConfigData.EnableRunEeprom
- self.__configData &= ~Rcb4BaseLib.ConfigData.EnableServoResponse
- self.__configData |= Rcb4BaseLib.ConfigData.EnableReferenceTable
- self.__configData |= Rcb4BaseLib.ConfigData.EnableSio
- buf = [self.__configData & 0xff, (self.__configData >> 8) & 0xff]
- return self.moveComToRamCmdSynchronize(Rcb4BaseLib.RamAddr.ConfigRamAddress, buf)
-
-
- def userParameterAddr(self, ParameterNum):
- if 0 < ParameterNum <= self.UserParameterCount:
- return Rcb4BaseLib.RamAddr.UserParameterRamAddress + (
- (ParameterNum - 1) * self.UserParameterSingleDataCount)
- else:
- return -1
-
- @staticmethod
- def setServoNo (servoDatas):
- ret = 0
-
- for idat in servoDatas:
- no = Rcb4BaseLib.icsNum2id(idat.id, idat.sio)
- sf = 0x1
- ret |= (sf << no)
- return ret << 24
-
- def userCounterAddr(self, counterNum):
- if (0 < counterNum <= self.CounterCount):
- return self.RamAddr.CounterRamAddress + ((counterNum - 1) * self.CounterSingleDataCount)
- else:
- return -1
-
- @staticmethod
- def runConstFrameServoCmd (servoDatas,frame):
- buf =[]
-
- sDatas = []
- if type(servoDatas) == Rcb4BaseLib.ServoData:
- sDatas.append(servoDatas)
- else:
- sDatas = servoDatas
-
- if Rcb4BaseLib().checkServoDatas(sDatas) == False:
- return -1,buf
-
- wk = Rcb4BaseLib.setServoNo (sDatas) >> 24
- buf.append (len(sDatas) * 2 + 9)
- buf.append ( Rcb4BaseLib.CommandTypes.ConstFrameServo)
- buf.append (wk & 0xff)
- buf.append ((wk >> 8) & 0xff)
- buf.append ((wk >> 16) & 0xff)
- buf.append ((wk >> 24) & 0xff)
- buf.append ((wk >> 32) & 0xff)
- buf.append (frame)
- servoDatas = sorted(sDatas)
- for idat in servoDatas:
- buf.append(idat.data & 0xff)
- buf.append((idat.data >> 8)& 0xff)
- buf.append(Rcb4BaseLib.CheckSum(buf))
- return 4,buf
-
- @staticmethod
- def runKondoMVFrameServoCmd (kondoMVServoDatas,frame):
- """Specific function for fast working sending
-
- Args:
- kondoServoDatas (list of servoData): ordered list. [servoData[id=1, sio=1],
- servoData[id=1, sio=2],
- servoData[id=2, sio=1],
- servoData[id=2, sio=2],
- ...
- ]
- frame ([type]): same as runConstFrameServoCmd()
-
- Returns:
- [type]: same as runConstFrameServoCmd()
- """
- buf = [
- 0x37, # message size
- Rcb4BaseLib.CommandTypes.ConstFrameServo, # set servo pos command
- 0xff, # 1 to 4 servos in both sio (8 servos)
- 0xff, # 5 to 8 servos in both sio (8 servos)
- 0x7f, # 9 to 12 servos in both sio (8 servos)
- 0x0, # 13 to 16 servos in both sio
- 0x0, # 13 to 16 servos in both sio
- frame
- ]
-
- sDatas = kondoMVServoDatas
-
- if Rcb4BaseLib().checkServoDatas(sDatas) == False:
- return -1,buf
-
- for idat in sDatas:
- buf.append(idat.data & 0xff)
- buf.append((idat.data >> 8)& 0xff)
- buf.append(Rcb4BaseLib.CheckSum(buf))
- return 4,buf
-
- def checkServoDatas(self, servoDatas):
- checkData = []
-
- for sData in servoDatas:
- if not (type(sData) is Rcb4BaseLib.ServoData):
- return False
- cData = sData.icsNum2id()
- if not (0 <= cData <= self.IcsDeviceSize):
- return False
-
- checkData.append(cData)
-
- if len(servoDatas) == len(set(checkData)):
- return True
- else:
- return False
-
- @staticmethod
- def checkSio(sio):
- return sio == 0x01 or sio == 0x02
-
- @staticmethod
- def icsNum2id(id, sio):
- return id * 2 + (sio - 1)
-
- @staticmethod
- def runSingleServoCmd(idNum, sioNum, pos, frame):
- buf = []
- buf.append(0x07)
- buf.append(Rcb4BaseLib.CommandTypes.SingleServo)
- buf.append(Rcb4BaseLib.icsNum2id(idNum, sioNum))
- buf.append(frame)
- buf.append(pos & 0xff)
- buf.append((pos >> 8) & 0xff)
- buf.append(Rcb4BaseLib.CheckSum(buf))
- return 4, buf
-
-
- def moveDeviceToComCmd(self, icsNum, offset, dataSize):
- buf =[]
-
- if((icsNum < 0)or(self.IcsDeviceSize < icsNum)or (offset < 0) or(self.IcsDeviceDataSize <(offset+dataSize))):
- return -1,buf
-
- buf.append (0x0a)
- buf.append ( self.CommandTypes.Move)
- buf.append ( self.SubMoveCmd.DeviceToCom)
- buf.append ( 0x00)
- buf.append ( 0x00)
- buf.append ( 0x00)
- buf.append ( offset)
- buf.append ( icsNum)
- buf.append (dataSize)
- buf.append (Rcb4BaseLib.CheckSum(buf))
-
- returnDataSize = dataSize+3
-
- return returnDataSize,buf
-
- def moveDeviceToComCmdSynchronize(self,icsNum, offset, dataSize):
-
- readSize, sendData = self.moveDeviceToComCmd(icsNum, offset, dataSize)
-
- if readSize > 0:
- rxbuf = self.synchronize(sendData, readSize)
-
- if len(rxbuf)< readSize:
- rxbuf = []
- return False,rxbuf
-
- destData = []
- if dataSize == 1:
- destData.append (rxbuf[2])
-
- else:
- for i in range(dataSize):
- destData.append (rxbuf[i+2])
-
- return True, bytes(destData)
-
- def adDataAddr(self, adPort):
- if adPort >= 0 and adPort < 11:
- return self.RamAddr.AdcRamAddress + adPort * 2
- else:
- return -1
-
- def setParametersBaseCmd(self, servoDatas, servoParameter):
- buf = []
-
- sDatas = []
- if type(servoDatas) == self.ServoData:
- sDatas.append(servoDatas)
- else:
- sDatas = servoDatas
-
- if self.checkServoDatas(sDatas) == False:
- return -1, buf
-
- wk = self.setServoNo(sDatas) >> 24
- buf.append(len(sDatas) + 9)
- buf.append(self.CommandTypes.ServoParam)
- buf.append(wk & 0xff)
- buf.append((wk >> 8) & 0xff)
- buf.append((wk >> 16) & 0xff)
- buf.append((wk >> 24) & 0xff)
- buf.append((wk >> 32) & 0xff)
- buf.append(servoParameter)
- servoDatas = sorted(sDatas)
- for idat in sDatas:
- if 0 < idat.data < 128:
- buf.append(idat.data & 0xff)
- else:
- buf = []
- return 0, buf
- buf.append(self.CheckSum(buf))
- return 4, buf
-
- def setSpeedCmd(self, servoDatas):
- return self.setParametersBaseCmd(servoDatas, 2)
-
- def setStretchCmd(self, servoDatas):
- return self.setParametersBaseCmd(servoDatas, 1)
-
-
- #runs uploaded motion
- def motionPlay(self, motionNum):
-
- if (motionNum <= 0) or (self.maxMotionCount < motionNum):
- return False
-
- if self.suspend() == False:
- return False
-
- if self.resetProgramCounter() == False:
- return False
-
- if self.setMotionNum(motionNum) == False:
- return False
-
- return self.resume()
-
-
- #sets U constants
- def setUserParameter(self, ParameterNum, data):
- addr = self.userParameterAddr(ParameterNum)
- try:
- buf = struct.pack('<h', data)
- except:
- return False
-
- if not (addr < 0):
- return self.moveComToRamCmdSynchronize(addr, buf)
- else:
- return False
-
-
-
- #gets U constants
- def getUserParameter(self, ParameterNum):
- addr = self.userParameterAddr(ParameterNum)
- if not (addr < 0):
- retf, retbuf = self.moveRamToComCmdSynchronize(addr, 2)
- if (retf == True) and (len(retbuf) == 2):
- paraData = struct.unpack('<h', retbuf)[0]
- return True, paraData
- else:
- return False, -1
- else:
- return False, -1
-
- def __del__(self):
- self.close()
-
-
- #sets C user counter
- def setUserCounter(self,counterNum,data):
-
- addr = self.userCounterAddr(counterNum)
- if (addr < 0):
- return False
-
- try:
- sendData = struct.pack('B',data)
- except:
- return False
-
- return self.moveComToRamCmdSynchronize(addr, sendData)
-
- #gets C user counter
- def getUserCounter(self,counterNum):
- addr = self.userCounterAddr(counterNum)
- if(addr < 0):
- return False,-1
- else:
- retf, retbuf = self.moveRamToComCmdSynchronize(addr , 1 )
-
- if(retf == False) or (len(retbuf) != 1):
- return False,-1
- else:
- countData = struct.unpack_from('B',retbuf,0)[0]
- return True, countData
-
-
- #moves single servo
- def setSingleServo(self, id, sio, pos, frame):
- if not Rcb4BaseLib.checkSio(sio):
- print('Wrong Sio. 1 / 2 for left / right side')
- return False
- _, txbuf = self.runSingleServoCmd(id, sio, pos, frame)
- if len(txbuf) == 0:
- return False
- return self.synchronizeAck(txbuf)
-
-
- #gets position of single servo
- def getSinglePos(self,id,sio):
- if not Rcb4BaseLib.checkSio(sio):
- return False, -1
- retf,retbuf = self.moveDeviceToComCmdSynchronize(self.icsNum2id(id, sio), Rcb4BaseLib.DeviceAddrOffset.MotorPositionAddressOffset, 2)
-
- if(retf == True) and (len(retbuf) ==2):
- posData = struct.unpack('<h',retbuf)[0]
- return True, posData
-
- else:
- return False, -1
-
- #gets trim of single servo
- def getSingleTrim(self,id,sio):
- if not Rcb4BaseLib.checkSio(sio):
- print('Wrong Sio. 1 for left, 2 for right side')
- return False, -1
- retf,retbuf = self.moveDeviceToComCmdSynchronize(self.icsNum2id(id, sio), Rcb4BaseLib.DeviceAddrOffset.TrimAddressOffset, 2)
-
- if(retf == True) and (len(retbuf) ==2):
- trimData = struct.unpack('<h',retbuf)[0]
- return True, trimData
-
- else:
- return False, -1
-
- #test Ram trim offset
- def setSingleTrim(self,id ,sio, trim):
- if not Rcb4BaseLib.checkSio(sio):
- print('Wrong Sio. 1 for left, 2 for right side')
- return False, -1
- buf =[trim]
- offset = Rcb4BaseLib.DeviceAddrOffset.TrimAddressOffset
- reFlag = self.moveComToDeviceCmdSynchronize(self.icsNum2id(id, sio), offset, buf)
- return reFlag
-
-
- # get analog port data
- def getAdData(self,adPort):
- if adPort >= 0 and adPort < 11:
- retf,rxbuf = self.moveRamToComCmdSynchronize(self.adDataAddr(adPort), 2 )
- if (retf == True) and (len(rxbuf) == 2):
- return rxbuf[1] * 256 + rxbuf[0]
-
- return 0xffff
-
- # get analog data from all ports
- def getAllAdData(self):
- retf,retbuf = self.moveRamToComCmdSynchronize(Rcb4BaseLib.RamAddr.AdcRamAddress ,self.AdcDataCount)
- redate = []
- if (retf == True) and (len(retbuf) == (self.AdcCount * 2)):
- for i in range(self.AdcCount):
- redate.append( retbuf[1+ i*2] * 256 + retbuf[0+ i*2])
- return retf,redate
-
- # get digital port data
- def getPio(self):
- retf,rxbuf = self.moveRamToComCmdSynchronize(self.RamAddr.PioAddress ,2)
- if(retf == True) and (len(rxbuf) == 2):
- return ((rxbuf[1] & 0x03) * 256) + rxbuf[0]
- else:
- return -1
-
- # set digital port data
- def setPio (self, pioData):
- buf=[pioData & 0xff,(pioData >> 8) & 0x03]
- return self.moveComToRamCmdSynchronize(self.RamAddr.PioAddress, buf)
-
- # change digital port mode (input is 0 or output is 1)
- def setPioMode (self, pioModeData):
- buf=[pioModeData & 0xff,(pioModeData >> 8) & 0x03]
- return self.moveComToRamCmdSynchronize(self.RamAddr.PioModeAddres, buf)
-
-
- # sets potitions of several servos using ServoData class
- def setServoPos (self,servoDatas,frame):
- _,txbuf = self.runConstFrameServoCmd(servoDatas,frame)
- return self.synchronizeAck(txbuf)
-
-
- # sets potitions of several servos using ServoData class
- def setKondoMVServoPos (self,servoDatas,frame):
- rxLen ,txbuf = self.runKondoMVFrameServoCmd(servoDatas,frame)
- return self.synchronize(txbuf, rxLen, writeOnly=True)
-
- def setServoSpeed(self, servoDatas):
- _, txbuf = self.setSpeedCmd(servoDatas)
- return self.synchronizeAck(txbuf)
-
- def setServoStretch(self, servoDatas):
- _, txbuf = self.setStretchCmd(servoDatas)
- return self.synchronizeAck(txbuf)
-
-
-
-
-
-
-
-if __name__ == "__main__":
- kondo = Rcb4BaseLib()
- if openmv:
- uart = UART(1, 115200, timeout=1000, parity=0)
- kondo.open(uart)
- else:
- kondo.open()
- #kondo.com.read(buf)
- #buf = kondo.com.write()
- #kondo.motionPlay(51)
- print(kondo.checkAcknowledge())
- #kondo.setSingleTrim(11,1, 1000)
-
-
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/kondo_controller2020.mpy b/src/azer_robocup_project/Soccer/Motion/kondo_controller2020.mpy
deleted file mode 100644
index 5f26dc2c217f2e7b0a7fac8ba550dde4684c850b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 9317
zcmbVSTTolqdR`L9;vykw@6AHSHpl`C0pcdh%FWZ-LfAOQ3C0jYoR-Ma#+D5dBMAr6
zldvU_FPPJ#bCRZMXPV)(dFV`MI$IZ#PTPlL8|U<)51F2J9MViC?M(a7cG?``={XOb
zY5%qM-qMzY{ZJUQ_g>fkw*LS7|K$rhZM$uAd~giuuMfXCFcs%x@qxgAKN?(G;3J9n
zz^jXV<icwM!B{jNAGi^Xgrc*-Xe1Gf&d>9){>7EI1_wve`g{1#-s^EbKX2>r8|Zfq
zxcV1ueXc&&kjpiMgo}sj65$0NYvAp`QZx=P=DMlV6O%ofLkAXDt{kqxni=Rb#1pZl
zU;=C02pg6bW8p|*(ycKYF7dH&VBVv}nn}0Tj3<IuJib7j9}8b0Z5GsGM$NnaBT0+K
zgKz_-(bAZ--(E-NCbDsKDPg~T&1_f<#KMUc!LzQ#bxVO*!cxzno0b}{X2xQ&=r~*O
z8XvrII(Q=zeS4k{UFE&V16?(oEzT#1V8#QnzyhD(V;|UmZS`trEP68QYGtQaL#FAV
z&kB8~=jTs{{DDM(t_EBl0+RI>J;#tv+OsCj&~ptTem<~bPmU6j>VdpQi3DY>OX0}X
zd44<^-%NS65^a|Efi*&lku%B@^_>0#`~Mgyft<eBXnEhO8PsC!J6IdAG;)n{hlOn9
zkrw!Xj?iQ66-y&w0GclbQB894uLa>0qw5pu26_>LZkjm|>EcqNKe3!Jn>^aH&FDVU
z$2EnP7C|}-SpoKD(C>(sPN12KZ~hjnfs!W??rYpM@qAj3KxG(Va}7qVplW1sYcMxA
zzZAcAE&{8%wHUqYhR#o5V(qEcCS=hGW|S4JU1TM6SL3R0(&+>iNuSBZmpSyVM!+3i
zZ=hcEAS?JdVftZ?LnlR6$C+b%AQZe7h<Oo+HTf^EzwvnZUsq><Kp%rn<vgG*x@tpg
zd5n)FJrfN2L@yi}a{#AgM3#(rkefYkLJvh2^!7Gb5l<G(!+O5|T$QLeaAj%kM}G4+
z+UM#w9cebrr0X{H>G~Ts_<ieM-97z{Q}ArQXH0i)Sks0LLt4L4zrmz&x(<GEx`BTy
zt>;gr>!BUqH`>yc>$dB*4P&|)e(PX8Q`!LiP4H|?8{t`>ZrD$f=q3^1Squ8WUE|SU
z^DwdMOG^vxTHH1}OKn_fnsn>T%<w|!)f<y5i@bEe4w#$Z2$a6{X719`l?&0EJPeeE
z)S0cr(S?OTBt(YArN(-5(?nq5bSM;)z8l~>s`_p;bHmZdT=*(E3(^pSnSrByGg@jf
znr(jmW;n=`bzYsDgM&wgx|=YLuS9~^V$n$W9iFge0@m6V2vyOA$tdg$)Va;rQ_*y(
ze3k`fH3b9n^JFxG533dhrpdZ2j5r)9`2ZM5nmJ`V*l98fdy)m3rR`Krs_onulM^oS
z>~6tT-o<Uj#CKeXCc@FkIGp=eKt~60Q+z27E;r<E#m)uVM`YoaUdfO{*a~8irjhT%
z*b(EQWjq$W8Ur~*mm+Wo-AAwin!$;?+pv8}?Y+o)gr|1kiGzt$2d_lkHrz(W$qOi>
zj>4#(*ir--i6_p$20R^4__yOWYEHC)C^1f!?SKhpsXZJe4V}=ygoANHY?w;(7_``7
zP061X*K^=bqCUw2ic;>vFIKmz1?|LKfcjA?sBX+jvLYgN_uwk$?e3*=FLUT_7jBxJ
z1@AjM>+S=ykX*VV%6@zp6h$T|Edz9nihTDVCeFU-pxr~bwfH?53op3c)Ep#-?><gP
z7ej;F1NJQ|0=zpoloM-p7ALa&|3b3R)zx)O`Wb--vJ1<i_3%PqSuz^Rggt{qYmd|*
zQB4#`yHcX{`7-HL^nV~)JIng%tUnR0t>=PqNqLvxl)^B3v4Jd38k3^cB@3}))eX@K
zUWM8&ecThR=F_1deg6>p;JrxUxM&4e7LKc|K!73O-HILthBO7E3k1Df6s-r~RdUm7
zR(mJ9c|>@zyQinOtJ`<%*a(jxOpNdPM+8QO8sPFpt6pR^oGL7N(TunsMwr<39~DFz
z&ZWcdMGtcJwf|k+^=pOkU{17}CPb@oBIqNZ^p8Xe#UM$~G2*O+v5uZ&XV54Iw3s+U
zh!01?3D^<Xs274S>(R_=@MR66p%mRso_v8T`t^EY%HBJOg!`f}mHb}tH3-uWvSMy(
z@4gi4zJpkowI^5q3!)qo0zLRv5_~*)vZpE>=!DBopWBDB!jvEppM}@t_fsOL&w4ea
zX(Bitl&IGy5mE&Y{bs0B9gPHB`|6EV+%OW0MPp<J8LmW@`ps))gKyyj;C_D(*S!Ls
zmkxF!P8D|QH|IM$%Vun0v*LDB5aQ$*@KcOc&f_N-(+;VLF8ewnMpg=4#?4q~r=xsn
z0oPpsFSLp4h9OYclkZ_O5JyAik1&?3mb$DAZmV#LKfxUBp+J#tIEJti7A>3az+R4W
zA=xC80bYpn;^XlY9xQj{r7ruOE4aVx;Hmqjv0XksrPe>jt#E8g-;^X5VwA{i+Dw)c
zWWVFb&Pxfv#!HLf;pG4hWZCbW$M`}Z92tv7Nss;a5w=O*qco_fIf*H9TpKrURcJ1W
znUP4~$~=E598KXi>3w1;GQuy$q6_QTA@>qsFu}(cA?)yXuu~q#&+#!n667bz3Ut38
zptcf<Fm7&kcKZ2KJb$WO&b8-40zLp+f_4dA3hXS!B>+uLfa{1J0DBr{XXg{Mv#>*L
z;!kTCWHAXp22TnY)CdRKn1_F4cXStBJ`fK_>&Ur!^I((3D0~Pf*qC|(f0Ezt(II33
zi(dExX}%uj>tVi0ny+2fu4vy$ay?BgCgEdp`pH}q%+0bjsVDFUULVtS8ls77d@N-{
zwknWRzxh)Tv-DHWc+YSk!C#HWR%H8<KvfdAo%5Hqq_CZYsj{|p>;V-=V-S+U1f-O8
z-oicmw%@^>6eW}m{$1?65MGAtWvHw<gWYoTL;yA@>-(`X#a-6?zB0vKJ%vd0_Sof9
zYOHur_;W}1@KA@(<?gUgj`Z~Q_H-S0ojKDn;_E>66zbIgsILqr5DflG=tU11bOS<x
zuseJ}$gO?l*FoD(At<-{jtSF`vfER_<KYGY{42aZR)*vu_kizWzVQ#>gCPiLM3zLN
z%BaHDaOe|~Opu6!-pzS6JLIhf{D0x%H^1=r3b19tqsi_1TZMu!1y2SdP9>Tzcr`$H
z4I$E}`U@dMUkFmlC6jANEc=$`q;%(2Xu?$qqfNpuI{NyK4-UHeM|&^0CdYjwtCG03
zCs)4o8wG(;XW?uqhhQa#-=htZ^h=s^dyhmPV2?KG*fI~yQfFK0RC#^2=-1_LQMHbE
z=a;H40Y*rhC3k^5vJpfsX!O~L?1&P5O>$0{&6o7@AD|bBs!}`!@x#RRok9+{fS?uP
z<B+F^V)TXhj?}61t<T6aT7-XhIl5o+jUbUbn9X^$gW88XdErY?qsVm?@?PyDAZgp9
zS^E;y$e36qDzYR9Zl|V{IIDsRP4Qri&|q2f549-?j79iWhZB_4?;HdL-OmEkw}Eo<
zWQ=S(OXHW^poorimPALtGKwI{7ZD^~K*t?|kWU1WrDA9T=S(O_j=Q;vw8@y!cE8}G
z+^&1I#84oq2IkCy$R1=5=1Lq%_Gd^e6EP-VcXAb(R%52&xiKGFXIQp{Ek<2N$C_A^
z&|CX6M#r^?UX8f++WP&hNa<Q1^Es#iLzGie22+Lw!3;W`PFKHk+)u2RD0u_8p~}|7
zdVR`=j_qTMSVC?jQXnHMpNbSoUrCDO3uL7jl1Vb4?AqSLyktUM7}Q5tCJ1=l6?anW
zQ4y@dv23Ojtf#kQq{Fe$_1g6g5NosAh-Scuj;f3(|JYv#C;rj+Hi)AaeV(HS;8P$8
zED4D69T9SZ&n!&;oVI<I+ACTAzY(tlMUGEB8ai28L{d^?W6C_F0=12q7bk~^NgEjp
zt3VpI8om&7zyBaRzB}AVP99i7>TB?d1P=#u$tm!-PjbT4o(v+B{uinf<Hn4yr@NOJ
zRF@Z#)j$NftXE6iah|wiimNB&CDQ1eqbiTqXU<Oy`6kC-ci8*w{bVT%1XcE_&}yTa
z?2WHs8=!e@Dz`qhmrKLHTk!Ap)ZRm209d*6-5!{B=DWQe2K{ycOjbcvY!xv2US#)A
zwSJY{^JpIBDN91I!01`{eGD55!EJ~ub;35}ZePOpd%<mr0oKjkR;5B<$pkze&*TXt
zPP1jX57Wr1YAtLf#`?Q|egl%5X;wqG+YfJ*kJuM`l@BRFI;ox25a$d*O!Csb+WR8+
z4428&vq#u&P%-5(`KF!OxAf7Hxqjjj60*~mAD^iM=Tec<egzKmjTt{HWRoE9qffvI
z9%Ya21y^NDVHRjP1<DY~Wgk~oL@{L+PYjK!mNv<Y9faei_*;=<sY@KxK6ofrIzb5E
z#ni}{GR!dh6=jT>x7$Y%)66!rWnda)1x^lrC2<MzPj3T=tsS(~&JHNTS+DK3%7q<M
z<_E_;$B$F}9|N6tvjg9%H9JtQql!*CbU!_{fHqo68z$efbJg{umQ4CY_sc4Q?Nbpb
z>H}zNWS^F~P-Q){-teTPLTj-EPqg@Yz^bZWnKD20Ifm@rBom%Je*_SVkt@Ob91AHx
zjwLAoc#`B!wvhks+xxz6SF;~|2UZ8`$c4ywa?;LJ7Q&2~4^LcFaa&fYkomOGa95p#
zQ3jt<^iZ+J1*PH#Fm`AB`t$pj+>Dt&8txc8?;9L(4UxzXvA@eP;34t<0mZ8yiyjSg
zkS%3IyZ%Ojpiu~$GjKk{hfIwJag}vDuwH<UI$@OrET)EQhlY=SxkIBRb-Jl1clVSK
zS5txDDzjjDTtS7;3T0@mP=p+0gThA&0HI%7>Ms&j8I%F9|1;2OCwfGNY_l|lQ!T%(
zI2yv_?kXi)UxnzTV0^S;E(S%+wb3{1mgdY~4m$?zT_=3!Pf(DXojre&!2XY6N3h!F
zZ-z<DCeL+y(T-4<zWf#SJLPpQN&R#nWq{Cde-&aCpicP9piO$mprAS_yqd-=O~oRq
zG4r=bOzw`<2x4j%jFDuNetT1+F7pKvAqRvEkT}N6y0ytKYTK#{r@GAF1Kt?|v_lMG
zrvNe{K#)8kSRY~4MGK`}#~DTNDe@;tKFIK#EL8el)EQl<hC2ia28>%TP~g=N4uPo}
zfK+%KSd7Mxrl0?!*0oOsw>k6eaQo2_Cm1XJp@?iJ3-PTLj!b*@@vsFj%3}ZwB3oTC
z%;on;TD3}cC4>+ZN1g;*D!GI7txg@(bs%{H0dr(v<rCl<Z6~%y`W0C)hZK(UYkQBj
zC9ct3gH*{jMCt8>nI!%c)){_X+xhH5^})=4UvQjLpdDb6L)l#3TQjKLc~pS(fFL_{
z=DJ<*YCi`90(4f8K;Gx$pHiIm^E?H4p8>I-Dj+XULEhicw!hd9<b6g#o@9MSqcM5d
z&Zx@f#;pd|7!>~g5mJ)uy~ln7=%R@7%8N+Zl*(%v0-^C}ERpit+f-%u`Ym?A*9*Tc
zQcV16xE@xk2-aDGa>j!YUFG`@F9HuFHSHHVRo!Kg8?;uOC&xuEqA;Qq?o;NotyR|m
zrmaqA$Hg;_bD)Uo_y)^995o|GzLMtZ7NIg}u044T^0^&Sjvc4v*gTjmF!wBMlLs{x
zkwIDfqD7<B^2>*-u8=KT$DIz>(7?zj5N82e9+30#g0j*6nL`flglVb?1?hIVVvySP
za{|M1hNTE)fCXZNsYeqeSr}i1KR%s6pORfzMlDUsJ&0-RH(_&h7K#v3XR7XOYsJk-
z&DMz1;R7kc$jXpOBnaZ6DkYLMafi%WU%B{1?g$kRDWs|5UwdkC;&8>4i)CxV=@=Os
z8W~iH1pES}9@fbvW$u;C1L`G7l6lYpAOb9jV0I1?Su<M@yfum-WZqMq|5QsERF!y5
zTLG8T-rs9SWd#qe=(@pqK=p@_|F=Si4ld=1QCItDIO+v&?V$FNe5B<prV2-X3OG_z
ztSb_~D#fd~V;U|$ui5~=^xKAP12(s9E*i7V^K%LKh=s3SOW5M!5WmJ<ZY{fsVz=g9
z_BV$HcmzUV3BA%Gd$zBF`;6j^9Z5d{gjN+D*=JM-Q^c=9*`vx9k8v-VU#hf1NdxHe
zAwqrmy;;lFibB4HYblZ6B62M<VJ&QR>GG-jw{R`ymZ$CClJfXe*K{pgw_WZb$t|9s
zZgBvP;E>3v94DALJ$DZt=7850a_#^-z_~LBc`4n^lXREnzr@UChq*QyrfMo)0oHG2
zp6l@Tv>yZWt_KA76z40lWfx(@=I;~amxaxLLr%CwD4oa`XQ?N<{(f-2Q2uHYrgsPs
z%q0upz~w}ozV1Qbldg)sCRasya0`Ue<zch>Qqr{bhc2LPe_X0mC`9f#010%u3Q<4z
Ee?*ebbN~PV
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_2.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_2.json
deleted file mode 100644
index 6e753e6..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_2.json
+++ /dev/null
@@ -1,31 +0,0 @@
-{
-
- "Dance_2": [
- [ 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ],
- [ 20, 0, -1500, 0, -3400, 0, 0, 0, 0, 0, -1600, 0, 0, 1500, 0, 3400, 0, 0, 0, 0, 0, 1600, 0, 0 ],
- [ 20, 0, -900, 0, -3400, 0, 0, 1200, 0, -700, -1200, 0, 0, 900, 0, 3400, 0, 0, -1200, 0, 700, 1200, 0, 0 ],
- [ 10, 0, 700, 0, -3400, 0, 0, 1500, 0, -700, -1200, 0, 0, -700, 0, 3400, 0, 0, -1500, 0, 700, 1200, 0, 0 ],
- [ 20, 0, 2000, -4500, -3400, 0, 0, 1500, 0, -700, -1200, 0, 0, -2000, 4500, 3400, 0, 0, -1500, 0, 700, 1200, 0, 0 ],
- [ 10, 0, -200, -1320, 2700, 0, 0, 1500, 0, -700, -1200, 0, 0, 200, 1320, -2700, 0, 0, -1500, 0, 700, 1200, 0, 0 ],
- [ 10, 0, -800, -4500, -3400, 0, 0, 1800, 0, -700, -2500, 0, 0, 800, 4500, 3400, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 300, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -300, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, -1000, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -1600, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 1600, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, 1000, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, -1000, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -1600, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 1600, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, 1000, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, -1000, 0, 0, 1800, 0, -700, -2500, -1000, 0, 0, 0, -1600, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 1600, 0, 0, 1800, 0, -700, -2500, 1000, 0, 0, 0, 1000, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, -1000, 0, 0, 1800, 0, -700, -2500, -1000, 0, 0, 0, -1600, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 1600, 0, 0, 1800, 0, -700, -2500, 1000, 0, 0, 0, 1000, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 300, -2000, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -300, 2000, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 300, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -300, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 300, -2000, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -300, 2000, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, 300, 0, 0, 1800, 0, -700, -2500, 0, 0, 0, 0, -300, 0, 0, -1800, 0, 700, 2500, 0, 0 ],
- [ 20, 0, 0, 0, -1400, 0, 0, 1800, 0, -700, -2800, 0, 0, 0, 0, 1400, 0, 0, -1800, 0, 700, 2800, 0, 0 ],
- [ 3, 0, 1500, -2000, 3000, 0, 0, 2500, 0, 0, -6000, 0, 0, -1500, 2000, -3000, 0, 0, -2500, 0, 0, 6000, 0, 0 ],
- [ 10, 0, 2400, -2000, 3000, 0, 0, 1500, 0, 0, -6000, 0, 0, -2400, 2000, -3000, 0, 0, -1500, 0, 0, 6000, 0, 0 ],
- [ 10, 0, 3000, -2000, 3000, 0, 0, 3000, 0, 0, -4500, 0, 0, -3000, 2000, -3000, 0, 0, -3000, 0, 0, 4500, 0, 0 ],
- [ 50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
- ],
- "pageNames": [ "page 0", "page 1", "page 2", "page 3", "page 4", "page 4 clone 1", "page 5", "page 6", "page 7", "page 8", "page 9", "page 10", "page 11", "page 12", "page 13", "page 14", "page 15", "page 16", "page 17", "page 18", "page 19", "page 20", "page 21", "page 22", "page 23" ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_3.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_3.json
deleted file mode 100644
index 6310c48..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_3.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_3": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, -1500, 0, -3400, 0, 0, 0, 0, 0, -1600, 0, 0, 1500, 0, 3400, 0, 0, 0, 0, 0, 1600, 0, 0], [20, 0, -900, 0, -3400, 0, 0, 1200, -800, -200, -1200, 0, 0, 900, 0, 3400, 0, 0, -1200, 800, 200, 1200, 0, 0], [10, 0, 700, 0, -3400, 0, 0, 1500, -800, -200, -1200, 0, 0, -700, 0, 3400, 0, 0, -1500, 800, 200, 1200, 0, 0], [20, 0, 2000, -4500, -3400, 0, 0, 1500, -800, -200, -1200, 0, 0, -2000, 4500, 3400, 0, 0, -1500, 800, 200, 1200, 0, 0], [20, 0, -800, -4500, -3400, 0, 0, 1800, -800, 0, -2700, 0, 0, 800, 4500, 3400, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 300, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -300, 0, 0, -1800, 800, 0, 2700, 0, 0], [5, 0, 0, 0, 800, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -800, 0, 0, -1800, 800, 0, 2700, 0, 0], [7, 0, 0, 0, 300, 0, 0, 500, 800, -5000, -2200, 0, 0, 0, 0, -300, 0, 0, -500, -800, 5000, 2200, 0, 0], [100, 0, 0, 0, 300, 0, 0, 500, 800, -5000, -2200, 0, 0, 0, 0, -300, 0, 0, -500, -800, 5000, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -1600, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, 1000, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -1600, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, 1000, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 1800, -800, 0, -2700, -1000, 0, 0, 0, -1600, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 1800, -800, 0, -2700, 1000, 0, 0, 0, 1000, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 1800, -800, 0, -2700, -1000, 0, 0, 0, -1600, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 1800, -800, 0, -2700, 1000, 0, 0, 0, 1000, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 300, -2000, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -300, 2000, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 300, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -300, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 300, -2000, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -300, 2000, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, 300, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, -300, 0, 0, -1800, 800, 0, 2700, 0, 0], [20, 0, 0, 0, -1400, 0, 0, 1800, -800, 0, -2700, 0, 0, 0, 0, 1400, 0, 0, -1800, 800, 0, 2700, 0, 0], [3, 0, 1500, -2000, 3000, 0, 0, 2500, 0, 0, -6000, 0, 0, -1500, 2000, -3000, 0, 0, -2500, 0, 0, 6000, 0, 0], [10, 0, 2400, -2000, 3000, 0, 0, 1500, 0, 0, -6000, 0, 0, -2400, 2000, -3000, 0, 0, -1500, 0, 0, 6000, 0, 0], [10, 0, 3000, -2000, 3000, 0, 0, 3000, 0, 0, -4500, 0, 0, -3000, 2000, -3000, 0, 0, -3000, 0, 0, 4500, 0, 0], [50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 6 clone 1", "page 6 clone 2", "page 6 clone 3", "page 7", "page 8", "page 9", "page 10", "page 11", "page 12", "page 13", "page 14", "page 15", "page 16", "page 17", "page 18", "page 19", "page 20", "page 21", "page 22", "page 23"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_4.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_4.json
deleted file mode 100644
index 7a016a6..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_4.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_4": [[10, 0, 0, 0, 0, 0, 0, 0, 0, -222, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 222, 0, 0, 0], [50, 0, -100, 0, 0, 0, 0, 0, 0, -2500, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 2500, 0, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 0, -2500, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 2500, 0, 0, 0], [50, 0, -100, 0, 0, 0, 0, 0, 0, 0, -5000, 0, 0, 100, 0, 0, 0, 0, 0, 0, 0, 5000, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 0, 0, -5000, 0, 0, 100, 0, 0, 0, 0, 0, 0, 0, 5000, 0, 0], [50, 0, -100, 0, 0, 0, 0, 2600, 0, 300, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, 0, -300, 2500, 0, 0], [20, 0, -100, 0, 0, 0, 0, 2600, 0, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, 0, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 1300, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -1300, -600, 2500, 0, 0], [5, 0, -100, 0, 0, 0, 0, 2600, 500, 600, -2500, 0, 0, 100, 0, 0, 0, 0, -2600, -500, -600, 2500, 0, 0], [20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 1 clone 1", "page 2", "page 2 clone 1", "page 4", "page 4 clone 1", "page 5", "page 6", "page 7", "page 8", "page 9", "page 10", "page 10 clone 1", "page 9 clone 1", "page 10 clone 1", "page 9 clone 2", "page 10 clone 2", "page 9 clone 3", "page 10 clone 3", "page 11"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_5.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_5.json
deleted file mode 100644
index be915b0..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_5.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_5": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 100, -100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, 0, 0, 0, 0, 0, 2200, 0], [10, 100, -100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, -500, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, 500, 0, 0, 0, 0, 2200, 0], [10, 100, -100, 0, 0, 0, -500, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, 500, 0, 0, 0, 0, 2200, 0], [10, 100, -100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, -500, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, 500, 0, 0, 0, 0, 2200, 0], [10, 100, -100, 0, 0, 0, -500, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, 500, 0, 0, 0, 0, 2200, 0], [10, 100, -100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, -100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, 500, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, -500, 0, 0, 0, 0, 2200, 0], [10, 100, 100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100, 100, 0, 0, 0, 0, 0, 0, 0, 0, 2200, 0], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 0 clone 1", "page 0 clone 2", "page 0 clone 3", "page 0 clone 4", "page 0 clone 5", "page 0 clone 6", "page 0 clone 7", "page 0 clone 8", "page 0 clone 9", "page 0 clone 10", "page 0 clone 11", "page 0 clone 12", "page 0 clone 13"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6.json
deleted file mode 100644
index 6005293..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_6": [[10, 0, 0, 0, 0, 0, 0, 0, 0, -222, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 222, 0, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [20, -303, 1628, -3196, -1567, -303, 0, 2500, 2500, 300, -700, 0, -303, -1628, 3196, 1567, -303, 0, -1300, -2500, -300, 700, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [20, 303, 1628, -3196, -1567, 303, 0, 1300, 2500, 300, -700, 0, 303, -1628, 3196, 1567, 303, 0, -2500, -2500, -300, 700, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [30, -303, 1628, -3196, -1567, -303, 0, 2500, 2500, 300, -700, 0, -303, -1628, 3196, 1567, -303, 0, -1300, -2500, -300, 700, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [30, 303, 1628, -3196, -1567, 303, 0, 1300, 2500, 300, -700, 0, 303, -1628, 3196, 1567, 303, 0, -2500, -2500, -300, 700, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [30, -303, 1628, -3196, -1567, -303, 0, 2500, 2500, 300, -700, 0, -303, -1628, 3196, 1567, -303, 0, -1300, -2500, -300, 700, 0, 0], [20, 3, 1454, -2858, -1404, 3, 0, 1800, 2500, -400, -700, 0, -3, -1454, 2858, 1404, -3, 0, -1800, -2500, 400, 700, 0, 0], [30, 303, 1628, -3196, -1567, 303, 0, 1300, 2500, 300, -700, 0, 303, -1628, 3196, 1567, 303, 0, -2500, -2500, -300, 700, 0, 0], [20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 1 clone 1", "page 4", "page 1 clone 2", "page 2 clone 1", "page 1 clone 3", "page 4 clone 1", "page 1 clone 3", "page 2 clone 2", "page 1 clone 4", "page 4 clone 2", "page 5"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6_1.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6_1.json
deleted file mode 100644
index 6bfded7..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_6_1.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_6_1": [[20, 0, 0, 0, 0, 0, 0, 1800, 2500, -400, -700, 0, 0, 0, 0, 0, 0, 0, -1800, -2500, 400, 700, 0, 0], [20, 0, 0, 0, 0, 400, 0, 2500, 2500, 300, -700, 0, 0, -900, 1800, 900, 400, 0, -1300, -2500, -300, 700, 0, 0], [20, 0, 0, 0, 0, 0, 0, 1800, 2500, -400, -700, 0, 0, 0, 0, 0, 0, 0, -1800, -2500, 400, 700, 0, 0], [20, 0, 900, -1800, -900, -400, 0, 1300, 2500, 300, -700, 0, 0, 0, 0, 0, -400, 0, -2500, -2500, -300, 700, 0, 0]], "pageNames": ["page 1", "page 2", "page 1 clone 1", "page 4"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-1.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-1.json
deleted file mode 100644
index 5253ee1..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-1.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_7-1": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 3300, 1500, 700, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 3300, 1500, 700, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 1500, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [50, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -1500, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -2000, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -2200, -1800, 0, 6000, 0, 0], [5, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -2200, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 6000, 0, 0], [5, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, -854, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 600, 0, 1000, 0, 0, 1100, 1800, 0, 854, 0, 0, -600, 0, -1000, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 600, 0, 1000, 0, 0, 1100, 1800, 0, 854, 0, 0, -600, 0, -1000, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, -854, 0, 0], [10, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0], [20, 0, 500, 0, 1000, 0, 0, 0, 1800, 0, 854, 0, 0, -500, 0, -1000, 0, 0, 0, -1800, 0, -854, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0], [20, 0, 500, 0, 1000, 0, 0, 0, 1800, 0, 854, 0, 0, -500, 0, -1000, 0, 0, 0, -1800, 0, -854, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 7", "page 8", "page 9", "page 10", "page 11", "page 12", "page 13", "page 14", "page 15", "page 16", "page 17", "page 18", "page 19", "page 20", "page 21", "page 22", "page 23", "page 24", "page 25", "page 26", "page 27", "page 28", "page 29", "page 30", "page 31", "page 32"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-2.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-2.json
deleted file mode 100644
index 062db90..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7-2.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_7-2": [[10, 0, -100, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 100, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2000, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 1200, 3000, 700, -700, 0, 0, 0, 0, 0, 0, 0, -1200, -1600, -900, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1200, 3000, 700, -700, 0, 0, 0, 0, 0, 0, 0, -1200, -1600, -900, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 3000, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 0, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 3000, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 200, 0, 2500, 2408, -2500, 0, 0, 0, -600, 1200, 600, 200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 400, 0, 2500, 2408, -2500, 0, 0, -2, -924, 1822, 897, 400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 600, -1200, -600, -200, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 924, -1822, -897, -400, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 200, 0, 2500, 2408, -2500, 0, 0, 0, -600, 1200, 600, 200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 400, 0, 2500, 2408, -2500, 0, 0, -2, -924, 1822, 897, 400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 600, -1200, -600, -200, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 924, -1822, -897, -400, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 33", "page 34", "page 35", "page 36", "page 37", "page 38", "page 39", "page 40", "page 41", "page 42", "page 42 clone 1", "page 43", "page 43 clone 1", "page 44", "page 44 clone 1", "page 45", "page 46", "page 47", "page 48", "page 49", "page 50", "page 51", "page 52", "page 53", "page 54", "page 55", "page 56", "page 57", "page 58", "page 59", "page 60"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7.json
deleted file mode 100644
index 1af16ea..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Dance_7.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Dance_7": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 3300, 1500, 700, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 3300, 1500, 700, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1500, 0, -700, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 1500, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [50, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -3300, -1500, -700, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, 0, -1500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2200, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -1500, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -2000, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -6000, 0, 0, 0, 0, 0, 0, 0, -2200, -1800, 0, 6000, 0, 0], [5, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -2200, -1800, 0, 6000, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 6000, 0, 0], [5, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 700, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, -78, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -700, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, 78, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, -854, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 600, 0, 1000, 0, 0, 1100, 1800, 0, 854, 0, 0, -600, 0, -1000, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 600, 0, 1000, 0, 0, 1100, 1800, 0, 854, 0, 0, -600, 0, -1000, 0, 0, -1100, -1800, 0, -854, 0, 0], [20, 0, 0, 0, 0, 0, 0, 1100, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, -1100, -1800, 0, -854, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 1800, 0, 854, 0, 0, 0, 0, 0, 0, 0, 0, -1800, 0, -854, 0, 0], [10, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0], [20, 0, 500, 0, 1000, 0, 0, 0, 1800, 0, 854, 0, 0, -500, 0, -1000, 0, 0, 0, -1800, 0, -854, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0], [20, 0, 500, 0, 1000, 0, 0, 0, 1800, 0, 854, 0, 0, -500, 0, -1000, 0, 0, 0, -1800, 0, -854, 0, 0], [20, 0, -100, 0, 0, 0, 0, 0, 1800, 0, -1500, 0, 0, 100, 0, 0, 0, 0, 0, -1800, 0, 1500, 0, 0], [10, 0, -100, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 100, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2000, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 1200, 3000, 700, -700, 0, 0, 0, 0, 0, 0, 0, -1200, -1600, -900, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 1200, 3000, 700, -700, 0, 0, 0, 0, 0, 0, 0, -1200, -1600, -900, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 0, 3000, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 0, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 3000, 0, -700, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 0, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2000, 3000, -600, -700, 0, 0, 0, 0, 0, 0, 0, -2000, -2500, 600, 700, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, 0, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2500, -2500, -5200, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 5200, 0, 0], [5, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 200, 0, 2500, 2408, -2500, 0, 0, 0, -600, 1200, 600, 200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 400, 0, 2500, 2408, -2500, 0, 0, -2, -924, 1822, 897, 400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 600, -1200, -600, -200, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 924, -1822, -897, -400, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 200, 0, 2500, 2408, -2500, 0, 0, 0, -600, 1200, 600, 200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 400, 0, 2500, 2408, -2500, 0, 0, -2, -924, 1822, 897, 400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 600, -1200, -600, -200, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -200, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 924, -1822, -897, -400, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, -400, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 2500, 2408, -2500, 0, 0, 0, 0, 0, 0, 0, 0, -2500, -2500, 2500, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [10, 3, 1628, -3196, -1567, 3, 0, 0, 0, -500, -700, 0, -3, -1628, 3196, 1567, -3, 0, 0, 0, 500, 700, 0, 0], [10, 3, 1628, -3196, -1567, 3, 0, 0, 0, -500, -700, 0, -3, -1628, 3196, 1567, -3, 0, 0, 0, 500, 700, 0, 0], [30, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 7", "page 8", "page 9", "page 10", "page 11", "page 12", "page 13", "page 14", "page 15", "page 16", "page 17", "page 18", "page 19", "page 20", "page 21", "page 22", "page 23", "page 24", "page 25", "page 26", "page 27", "page 28", "page 29", "page 30", "page 31", "page 32", "page 33", "page 34", "page 35", "page 36", "page 37", "page 38", "page 39", "page 40", "page 41", "page 42", "page 42 clone 1", "page 43", "page 43 clone 1", "page 44", "page 44 clone 1", "page 45", "page 46", "page 47", "page 48", "page 49", "page 50", "page 51", "page 52", "page 53", "page 54", "page 55", "page 56", "page 57", "page 58", "page 59", "page 60", "page 61", "page 62", "page 63", "page 64"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence.json
deleted file mode 100644
index df751dc..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Get_Up_From_Defence": [[5, -2800, 0, 0, 0, -2700, 0, 221, 4, -3000, -2000, 0, 2800, 0, 0, 0, 2700, 0, -221, -4, 3000, 2000], [20, -2800, 2700, 0, 0, -2700, 0, 221, 4, -4800, -2000, 0, 2800, -2700, 0, 0, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1786, 2700, -4500, -2700, -2700, 0, 221, 4, -4800, -2000, 0, -700, -2700, 4500, 2700, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1786, 2700, -4500, -2700, 0, 0, 0, 0, 0, 0, 0, -700, -2700, 4500, 2700, 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], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 0 clone 1", "page 0 clone 2", "page 0 clone 3", "page 0 clone 4"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence21.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence21.json
deleted file mode 100644
index 4cf71af..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence21.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Get_Up_From_Defence21": [[5, -2800, 0, 0, 0, -2700, 0, 221, 4, -3000, -2000, 0, 2800, 0, 0, 0, 2700, 0, -221, -4, 3000, 2000], [20, -2800, 2700, 0, 0, -2700, 0, 221, 4, -4800, -2000, 0, 2800, -2700, 0, 0, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1165, 2700, 0, 0, 0, 0, 221, 4, -4800, -2000, 0, -3961, -2700, 0, 0, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1165, 2700, 0, 0, 0, 0, 221, 4, -4800, -2000, 0, -3961, -2700, 0, 0, 0, 0, -221, -4, 4800, 2000, 0, 0], [20, 0, 0, 0, 0, 0, 0, 221, 4, -5049, 0, 0, 0, 0, 0, 0, 0, 0, -221, -4, 4800, 0, 0, 0], [20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 0 clone 1", "page 0 clone 2", "page 0 clone 3", "page 0 clone 4", "page 0 clone 5"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence22.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence22.json
deleted file mode 100644
index a6a41c0..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_From_Defence22.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Get_Up_From_Defence22": [[5, -2800, 0, 0, 0, -2700, 0, 221, 4, -3000, -2000, 0, 2800, 0, 0, 0, 2700, 0, -221, -4, 3000, 2000], [20, -2800, 2700, 0, 0, -2700, 0, 221, 4, -4800, -2000, 0, 2800, -2700, 0, 0, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1786, 2700, -4500, -2700, -2700, 0, 221, 4, -4800, -2000, 0, -700, -2700, 4500, 2700, 2700, 0, -221, -4, 4800, 2000, 0, 0], [20, 1786, 2700, -4500, -2700, 0, 0, 0, 0, 0, 0, 0, -700, -2700, 4500, 2700, 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], [10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 0 clone 1", "page 0 clone 2", "page 0 clone 3", "page 0 clone 4"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Left.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Left.json
deleted file mode 100644
index 3fbdfd2..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Left.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
- "Get_Up_Left" : [
- [40, 0, 0, 0, 0, 0,0, 2000,0,-500,500,0,0, 0, 0, 0, 0,0, -2500, 0, 2500, 2500],
- [20, 0, 0, 0, 0, 0,0, 2000,0,-500,500,0,0, 0, 0, 0, 0,0, -2500, 0, 2500, 2500],
- [60, 0, 0, 0, 0, 0,0, 2000,0,-200,800,0,0, 0, 0, 0, 0,0, -2000, 0, 200, -800],
- [ 20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Right.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Right.json
deleted file mode 100644
index f9da5ff..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Get_Up_Right.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
- "Get_Up_Right" : [
- [40, 0, 0, 0, 0, 0,0, 2500,0,-2500,-2500,0,0, 0, 0, 0, 0,0, -2000, 0, 500, -500],
- [20, 0, 0, 0, 0, 0,0, 2500,0,-2500,-2500,0,0, 0, 0, 0, 0,0, -2000, 0, 500, -500],
- [60, 0, 0, 0, 0, 0,0, 2000,0, -200, 800,0,0, 0, 0, 0, 0,0, -2000, 0, 200, -800],
- [20, 0, 0, 0, 0, 0,0, 0,0, 0, 0,0,0, 0, 0, 0, 0,0, 0, 0, 0, 0]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Initial_Pose.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Initial_Pose.json
deleted file mode 100644
index 173e876..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Initial_Pose.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
- "Initial_Pose" : [
- [20, 0, 0, 0, 0,0,0, 0, 0, 0, 0,0,0, 0, 0, 0,0,0, 0, 0, 0, 0]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Motion_slot_Random_259.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Motion_slot_Random_259.json
deleted file mode 100644
index fcfa52f..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Motion_slot_Random_259.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Motion_slot_Random_259": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, -1500, 0, -3400, 0, 0, 0, 0, 0, -2000, 0, 0, 1500, 0, 3400, 0, 0, 0, 0, 0, 2000, 0, 0], [20, 0, -900, 0, -3400, 0, 0, 1500, 0, 0, -1200, 0, 0, 900, 0, 3400, 0, 0, -1500, 0, 0, 1200, 0, 0], [10, 0, 0, 0, -3400, 0, 0, 1500, 0, 0, -1200, 0, 0, 0, 0, 3400, 0, 0, -1500, 0, 0, 1200, 0, 0], [10, 0, -800, -4500, -3400, 0, 0, 3000, 0, 0, -1200, 0, 0, 800, 4500, 3400, 0, 0, -3000, 0, 0, 1200, 0, 0], [10, 0, -800, -4500, -3400, 0, 0, 3000, 0, 0, -2200, 0, 0, 800, 4500, 3400, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 300, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -300, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -1600, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, 1000, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -1600, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, 1000, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 3000, 0, 0, -2200, -1000, 0, 0, 0, -1600, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 3000, 0, 0, -2200, 1000, 0, 0, 0, 1000, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 3000, 0, 0, -2200, -1000, 0, 0, 0, -1600, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 1600, 0, 0, 3000, 0, 0, -2200, 1000, 0, 0, 0, 1000, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 300, -2000, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -300, 2000, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 300, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -300, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 300, -2000, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -300, 2000, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, 300, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, -300, 0, 0, -3000, 0, 0, 2200, 0, 0], [20, 0, 0, 0, -1000, 0, 0, 3000, 0, 0, -2200, 0, 0, 0, 0, 1000, 0, 0, -3000, 0, 0, 2200, 0, 0], [5, 0, 1500, -2000, 3000, 0, 0, 3000, 0, 0, -4500, 0, 0, -1500, 2000, -3000, 0, 0, -3000, 0, 0, 4500, 0, 0], [10, 0, 3000, -2000, 3000, 0, 0, 3000, 0, 0, -4500, 0, 0, -3000, 2000, -3000, 0, 0, -3000, 0, 0, 4500, 0, 0], [50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 0 clone 1", "page 0 clone 2", "page 0 clone 3", "page 0 clone 4", "page 0 clone 5", "page 0 clone 6", "page 0 clone 7", "page 0 clone 8", "page 0 clone 8", "page 0 clone 9", "page 0 clone 9", "page 0 clone 10", "page 0 clone 10", "page 0 clone 11", "page 0 clone 12", "page 0 clone 13", "page 0 clone 13", "page 0 clone 14", "page 0 clone 15", "page 0 clone 16", "page 0 clone 17", "page 0 clone 18"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady.json
deleted file mode 100644
index f8e2e76..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady.json
+++ /dev/null
@@ -1,6 +0,0 @@
-{
- "PanaltyDefenceReady" : [
- [50, 0,-200, 0, 0, 0,0, 0, 0, 0,-2000,0, 0, 200, 0, 0, 0,0, 0, 0, 0,2000],
- [100,-1000,1900,-4000,-2400,-1000,0,156,-90,670,-1119,0,1000,-1900,4000,2400,1000,0,-156,90,-670,1119]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady_Fast.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady_Fast.json
deleted file mode 100644
index ff98562..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/PanaltyDefenceReady_Fast.json
+++ /dev/null
@@ -1,6 +0,0 @@
-{
- "PanaltyDefenceReady_Fast" : [
- [10, 0,-200, 0, 0, 0,0, 0, 0, 0,-2000,0, 0, 200, 0, 0, 0,0, 0, 0, 0,2000],
- [30,-1000,1900,-4000,-2400,-1000,0,156,-90,670,-1119,0,1000,-1900,4000,2400,1000,0,-156,90,-670,1119]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceF.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceF.json
deleted file mode 100644
index 399fcc8..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceF.json
+++ /dev/null
@@ -1,7 +0,0 @@
-{
- "PenaltyDefenceF" : [
- [10,-2800, 0, 0, 0,-2700,0,221,4, 89,-1544,0,2800, 0, 0, 0,2700,0,-221,-4, -89, 1544],
- [10,-2800, 0, 0, 0,-2700,0,221,4, 89,-1544,0,2800, 0, 0, 0,2700,0,-221,-4, -89, 1544],
- [ 5,-2800, 0, 0, 0,-2700,0,221,4, -3000,-2000,0,2800, 0, 0, 0,2700,0,-221,-4,3000, 2000]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceL.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceL.json
deleted file mode 100644
index 50778fe..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceL.json
+++ /dev/null
@@ -1,9 +0,0 @@
-{
- "PenaltyDefenceL": [
- [ 10, -1000, 0, 0, 0, -1200, 0, 0, 0, -500, -5000, 0, -600, 0, 0, 0, -970, 0, 0, 0, -70, 5000 ],
- [ 50, -1000, 0, 0, 0, -1200, 0, 0, 0, -500, -5000, 0, -600, 0, 0, 0, -970, 0, 0, 0, -70, 5000 ],
- [ 10, -1000, 0, 0, 0, -1200, 0, 0, 0, -500, -5000, 0, -600, 0, 0, 0, -970, 0, 0, 0, -650, 1370 ],
- [ 50, -1000, 0, 0, 0, -1200, 0, 0, 0, -500, -5000, 0, -600, 0, 0, 0, -970, 0, 0, 0, -650, 1370 ],
- [ 10, -1000, 0, 0, 0, -1200, 0, 0, 0, -500, -5000, 0, -600, 0, 0, 0, -970, 0, 0, 0, -70, 5000 ]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceR.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceR.json
deleted file mode 100644
index 09146dc..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/PenaltyDefenceR.json
+++ /dev/null
@@ -1,9 +0,0 @@
-{
- "PenaltyDefenceR": [
- [ 10, 600, 0, 0, 0, 970, 0, 0, 0, 70, -5000, 0, 1000, 0, 0, 0, 1200, 0, 0, 0, 500, 5000 ],
- [ 50, 600, 0, 0, 0, 970, 0, 0, 0, 70, -5000, 0, 1000, 0, 0, 0, 1200, 0, 0, 0, 500, 5000 ],
- [ 10, 600, 0, 0, 0, 970, 0, 0, 0, 650, -1370, 0, 1000, 0, 0, 0, 1200, 0, 0, 0, 500, 5000 ],
- [ 50, 600, 0, 0, 0, 970, 0, 0, 0, 650, -1370, 0, 1000, 0, 0, 0, 1200, 0, 0, 0, 500, 5000 ],
- [ 10, 600, 0, 0, 0, 970, 0, 0, 0, 70, -5000, 0, 1000, 0, 0, 0, 1200, 0, 0, 0, 500, 5000 ]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Quick_sit_v1.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Quick_sit_v1.json
deleted file mode 100644
index ec6c8ab..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Quick_sit_v1.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Quick_sit_v1": [[7, 3, 2106, -4095, -1989, 3, 0, 0, 0, -222, 0, 0, -3, -2106, 4095, 1989, -3, 0, 0, 0, 222, 0, 0, 0, 0, 0], [10, 3, 2106, -4095, -1989, 3, 0, 0, 0, -222, 0, 0, -3, -2106, 4095, 1989, -3, 0, 0, 0, 222, 0, 0, 0, 0, 0], [40, 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]], "pageNames": ["page 0", "page 1", "page 2"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Face_Up.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Face_Up.json
deleted file mode 100644
index 4ab7d81..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Face_Up.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Soccer_Get_UP_Face_Up": [[10, 0, 0, 0, 0, 0, 0, 3500, 0, -1500, 1400, 0, 0, 0, 0, 0, 0, 0, -3500, 0, 1500, -1400, 0, 0], [7, 0, 0, 0, 0, 0, 0, 3500, 0, -200, 1400, 0, 0, 0, 0, 0, 0, 0, -3500, 0, 200, -1400, 0, 0], [15, 0, 0, 0, -2000, 0, 0, 1000, -3000, -200, 1400, 0, 0, 0, 0, 2000, 0, 0, -1000, 3000, 200, -1400, 0, 0], [15, 0, -3500, 1200, -2800, 0, 0, 100, 0, -200, 1400, 0, 0, 3500, -1200, 2800, 0, 0, -100, 0, 200, -1400, 0, 0], [28, 0, -3500, 1300, -3400, 0, 0, 500, 0, -200, 200, 0, 0, 3500, -1300, 3400, 0, 0, -500, 0, 200, -200, 0, 0], [10, 0, -3000, 1200, -3400, 0, 0, 500, 0, -200, -800, 0, 0, 3000, -1200, 3400, 0, 0, -500, 0, 200, 800, 0, 0], [10, 0, -2400, 1200, -3400, 0, 0, 500, 0, -200, -800, 0, 0, 2400, -1200, 3400, 0, 0, -500, 0, 200, 800, 0, 0], [5, 0, -2400, 1200, -3400, 0, 0, 500, 0, -200, -800, 0, 0, 2400, -1200, 3400, 0, 0, -500, 0, 200, 800, 0, 0], [20, 0, -2400, 1000, -3400, 0, 0, 3500, 0, -1000, 800, 0, 0, 2400, -1000, 3400, 0, 0, -3500, 0, 1000, -800, 0, 0], [20, 0, -2000, 1000, -2389, 0, 0, 3500, 0, -1000, 800, 0, 0, 2000, -1000, 2389, 0, 0, -3500, 0, 1000, -800, 0, 0], [60, 0, -150, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 150, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 5 clone 1", "page 6", "page 7", "page 8", "page 9", "page 10"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N.json
deleted file mode 100644
index e4b9770..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N.json
+++ /dev/null
@@ -1,14 +0,0 @@
-{
-
- "Soccer_Get_UP_Stomach_N": [
- [ 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ],
- [ 10, 0, 0, 0, 0, 0, 0, 4427, 0, 0, 2600, 0, 0, 0, 0, 0, 0, 0, -3800, 0, 0, -2600, 0, 0 ],
- [ 30, 0, 2408, -4600, -3400, 0, 0, 3800, 0, 0, -1010, 0, 0, -2408, 4600, 3400, 0, 0, -3800, 0, 0, 1010, 0, 0 ],
- [ 10, 0, 2408, -4600, -3400, 0, 0, 0, 0, 0, -2000, 0, 0, -2408, 4600, 3400, 0, 0, 0, 0, 0, 2000, 0, 0 ],
- [ 30, 0, -500, 0, -3400, 0, 0, 0, 0, 0, -2700, 0, 0, 500, 0, 3400, 0, 0, 0, 0, 0, 2700, 0, 0 ],
- [ 20, 0, -1500, 0, -3400, 0, 0, 0, 0, 0, -2400, 0, 0, 1500, 0, 3400, 0, 0, 0, 0, 0, 2400, 0, 0 ],
- [ 100, 0, -200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ],
- [ 50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
- ],
- "pageNames": [ "page 0", "page 1", "page 2", "page 3", "page 4", "page 4 clone 1", "page 5", "page 5 clone 1" ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N_old.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N_old.json
deleted file mode 100644
index cb85e18..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Get_UP_Stomach_N_old.json
+++ /dev/null
@@ -1,11 +0,0 @@
-{
- "Soccer_Get_UP_Stomach_N" : [
- [ 20, 0, 600, -1000, 2964, 0, 0, 2904, 0, -900, -1500, 0, 0, -600, 1000, -2893, 0, 0, -2900, 0, 900, 1500 ],
- [ 20, 0, 600, -1000, 2964, 0, 0, 2904, 0, -900, -1500, 0, 0, -600, 1000, -2893, 0, 0, -2900, 0, 900, 1500 ],
- [ 20, 0, -400, 339, -3478, 0, 0, 3500, 0, 0, -1500, 0, 0, 400, -412, 3519, 0, 0, -3500, 0, 0, 1500 ],
- [ 20, 0, -841, -412, -3459, 0, 0, -121, 0, 0, -2831, 0, 0, 833, 332, 3440, 0, 0, 121, 0, 0, 2831 ],
- [ 40, 0, -1700, -412, -3459, 0, 0, -121, 0, 0, -2831, 0, 0, 1700, 412, 3440, 0, 0, 121, 0, 0, 2831 ],
- [ 60, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
-
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Left_Leg.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Left_Leg.json
deleted file mode 100644
index cca6c01..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Left_Leg.json
+++ /dev/null
@@ -1,19 +0,0 @@
-{
- "Soccer_Kick_Forward_Left_Leg" : [
- [14, 0, 550, -1000, -500, 0, 0, 1000, 0, -200, 500, 0, 0, -550, 1000, 500, 0, 0, -1000, 0, 200, -500],
- [40, 420,1550, -2650, -1140,-300, 0, 1000, 0, -200, 500, 0, 417, -474, 1000, 500, -200, 0, -1000, 0, 200, -500],
- [20, 420,1550, -2650, -1140,-300, 0, 2000, 0, -700,1500, 0, 117,-1459, 1718,-723, -400, 0, -2000, 0, 200, 0],
- [ 3, 420,1550, -2650, -1140,-400, 0, 2000, 0, -700,1500, 0, 0,-1493, 3017, 887, -400, 0, -2000, 0, 200, 0],
- [ 3, 440,2093, -2619, -719,-400, 0, 2000, 0, -700, 0, 0, 41, 1445, 0,2000, -400, 0, -2000, 0, 200,-1500],
- [ 2, -52,3024, -4634, -2839, 0, 0, 221, 0, -299,-1476,0,-132, 3102,-1340,2888, 0, 0, 417, 0, 167, 1825],
- [50, -52,3024, -4634, -2839, 0, 0, 221, 0, -299,-1476,0,-132, 3102,-1340,2888, 0, 0, 417, 0, 167, 1825],
- [50,-138,1448, -1718, -269, 0, 0, 221, 0, -299,-1476,0,-132, 2024,-1782, 139, 0, 0, 417, 0, 167, 1825],
- [50, -52,1938, -2091, -269,-400, 0, 221, 0, -299, 34,0,-132, 637, 796,1558, -400, 0, 417, 0, 167,-1228],
- [50,-312,2103, -1153, 881, 0, 0, 221, 0, -299,-1476,0,-132, -517, 2785,2298, 0, 0, 417, 0, 167, 1825],
- [50,-400,1967, -1153, 520,-400, 0, 221, 0, -299,-1476,0,-400, -517, 2785,2583, -400, 0, 417, 0, 167, 1825],
- [ 6,-400,2995, -4662, -1942,-400, 0, 221, 0, -299,-1476,0,-400, -517, 2785,2583, -400, 0, 417, 0, 167, 1825],
- [ 6,-244,1100, -2936, -1844,-400, 0, 221, 0, -299,-1476,0,-400, -973, 2506,1558, -400, 0, 417, 0, 167, 1825],
- [40, 0, 550, -1000, -500, 0, 0, 1000, 0, -200, 500,0, 0, -550, 1000, 500, 0, 0, -1000, 0, 200, -500],
- [15, 0, 600, -1000, -500, 0, 0, 2000, 0, -200, 800,0, 0, -600, 1000, 500, 0, 0, -2000, 0, 200, -800]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Right_Leg.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Right_Leg.json
deleted file mode 100644
index fd7cfa3..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Kick_Forward_Right_Leg.json
+++ /dev/null
@@ -1,19 +0,0 @@
-{
- "Soccer_Kick_Forward_Right_Leg": [
- [ 14, 0, 550, -1000, -500, 0, 0, 1000, 0, -200, 500, 0, 0, -550, 1000, 500, 0, 0, -1000, 0, 200, -500 ],
- [ 40, -417, 474, -1000, -500, 200, 0, 1000, 0, -200, 500, 0, -430, -1550, 2650, 1140, 300, 0, -1000, 0, 200, -500 ],
- [ 20, -117, 1459, -1718, 723, 400, 0, 2000, 0, -200, 0, 0, -430, -1550, 2650, 1140, 300, 0, -2000, 0, 700, -1500 ],
- [ 3, 0, 1493, -3017, -887, 400, 0, 2000, 0, -200, 0, 0, -430, -1550, 2650, 1140, 400, 0, -2000, 0, 700, -1500 ],
- [ 3, -41, -1445, 0, -2000, 400, 0, 2000, 0, -200, 1500, 0, -450, -2093, 2619, 719, 400, 0, -2000, 0, 700, 0 ],
- [ 2, 132, -3102, 1340, -2888, 0, 0, -417, 0, -167, -1825, 0, 52, -3024, 4634, 2839, 0, 0, -221, 0, 299, 1476 ],
- [ 50, 132, -3102, 1340, -2888, 0, 0, -417, 0, -167, -1825, 0, 52, -3024, 4634, 2839, 0, 0, -221, 0, 299, 1476 ],
- [ 50, 132, -2024, 1782, -139, 0, 0, -417, 0, -167, -1825, 0, 138, -1448, 1718, 269, 0, 0, -221, 0, 299, 1476 ],
- [ 50, 132, -637, -796, -1558, 400, 0, -417, 0, -167, 1228, 0, 52, -1938, 2091, 269, 400, 0, -221, 0, 299, -34 ],
- [ 50, 132, 517, -2785, -2298, 0, 0, -417, 0, -167, -1825, 0, 312, -2103, 1153, -881, 0, 0, -221, 0, 299, 1476 ],
- [ 50, 400, 517, -2785, -2583, 400, 0, -417, 0, -167, -1825, 0, 400, -1967, 1153, -520, 400, 0, -221, 0, 299, 1476 ],
- [ 6, 400, 517, -2785, -2583, 400, 0, -417, 0, -167, -1825, 0, 400, -2995, 4662, 1942, 400, 0, -221, 0, 299, 1476 ],
- [ 6, 400, 973, -2506, -1558, 400, 0, -417, 0, -167, -1825, 0, 244, -1100, 2936, 1844, 400, 0, -221, 0, 299, 1476 ],
- [ 40, 0, 550, -1000, -500, 0, 0, 1000, 0, -200, 500, 0, 0, -550, 1000, 500, 0, 0, -1000, 0, 200, -500 ],
- [ 15, 0, 600, -1000, -500, 0, 0, 2000, 0, -200, 800, 0, 0, -600, 1000, 500, 0, 0, -2000, 0, 200, -800 ]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Small_Jump_Forward.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Small_Jump_Forward.json
deleted file mode 100644
index 3dd3174..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Soccer_Small_Jump_Forward.json
+++ /dev/null
@@ -1,11 +0,0 @@
-{
- "Soccer_Small_Jump_Forward" : [
- [10,50,1481,-2963,-1481,100,0,3000,0,-300, 1000,0,-50,-1481,2963,1481,-100,0,-3000,0,300,-1000],
- [ 8,50,1900,-4000,-2400,100,0,3000,0,-300, 0,0,-50,-1900,4000,2400,-100,0,-3000,0,300, 0],
- [ 4,50,2000,-2500,-1000,100,0,3000,0,-300,-1500,0,-50,-2000,2500,1000,-100,0,-3000,0,300, 1500],
- [ 1,50,2000,-4000,-1932,100,0,3000,0,-300, 1000,0,-50,-2000,4000,1932,-100,0,-3000,0,300,-1000],
- [ 3,50,2000,-4000,-2100,100,0,3000,0,-300, 1000,0,-50,-2000,4000,2100,-100,0,-3000,0,300,-1000],
- [10,50,1381,-2963,-1481,100,0,3000,0,-300, 1000,0,-50,-1381,2963,1481,-100,0,-3000,0,300,-1000],
- [10,50, 500,-1000, -500, 0,0,2000,0,-300, 800,0,-50, -500,1000, 500, 0,0,-2000,0,300, -800]
- ]
-}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/Spot_walk1.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/Spot_walk1.json
deleted file mode 100755
index 065de4c..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/Spot_walk1.json
+++ /dev/null
@@ -1 +0,0 @@
-{"Spot_walk1": [[20, 0, 0, 0, 0, 0, 0, 0, 0, -444, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 444, 0, 0, 0], [20, -452, 1589, -3119, -1530, -452, 0, 0, 0, -444, 0, 0, -539, -1417, 2786, 1369, -539, 0, 0, 0, 444, 0, 0, 0], [20, -349, 2019, -3936, -1917, -762, 0, 0, 0, -444, 0, 0, -581, -1385, 2724, 1339, -581, 0, 0, 0, 444, 0, 0, 0], [20, -452, 1589, -3119, -1530, -452, 0, 0, 0, -444, 0, 0, -539, -1417, 2786, 1369, -539, 0, 0, 0, 444, 0, 0, 0], [40, 539, 1417, -2786, -1369, 539, 0, 0, 0, -444, 0, 0, 452, -1589, 3119, 1530, 452, 0, 0, 0, 444, 0, 0, 0], [20, 581, 1385, -2724, -1339, 581, 0, 0, 0, -444, 0, 0, 349, -2019, 3936, 1917, 761, 0, 0, 0, 444, 0, 0, 0], [20, 539, 1417, -2786, -1369, 539, 0, 0, 0, -444, 0, 0, 452, -1589, 3119, 1530, 452, 0, 0, 0, 444, 0, 0, 0], [20, 0, 0, 0, 0, 0, 0, 0, 0, -444, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 444, 0, 0, 0]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 3 clone 1", "page 2 clone 1", "page 3 clone 2", "page 0 clone 1"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/TripleJumpForFIRA2019full.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/TripleJumpForFIRA2019full.json
deleted file mode 100644
index d3f8cad..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/TripleJumpForFIRA2019full.json
+++ /dev/null
@@ -1 +0,0 @@
-{"TripleJumpForFIRA2019full": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -667, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, -200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, 200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3866, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3866, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [2, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [6, 0, 2600, -4600, -2000, 0, 0, -600, 0, -300, 2666, 0, 0, -2600, 4600, 2000, 0, 0, 600, 0, 300, -2666, 0, 600], [50, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -666, 0, 600], [20, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -667, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, -200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, 200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3866, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3866, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [2, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [6, 0, 2600, -4600, -2000, 0, 0, -600, 0, -300, 2666, 0, 0, -2600, 4600, 2000, 0, 0, 600, 0, 300, -2666, 0, 600], [50, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -666, 0, 600], [20, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 1000, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -1000, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, 200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, -200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3876, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3876, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, -3860, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, 3860, 0, -600], [6, 0, 333, -4600, -3380, 0, 0, -600, 0, -300, -3860, 0, 0, -333, 4600, 3380, 0, 0, 600, 0, 300, 3860, 0, -600], [6, 0, 1000, -4600, -3380, 0, 0, -600, 0, -300, -2666, 0, 0, -1000, 4600, 3380, 0, 0, 600, 0, 300, 2666, 0, -600], [15, 0, 1000, -4000, -2250, 0, 0, 3000, 0, -300, -2500, 0, 0, -1000, 4000, 2250, 0, 0, -3000, 0, 300, 2500, 0, -600]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 7", "page 8", "page 9", "page 1 clone 1", "page 2 clone 1", "page 3 clone 1", "page 4 clone 1", "page 5 clone 1", "page 6 clone 1", "page 7 clone 1", "page 8 clone 1", "page 9 clone 1", "page 10", "page 11", "page 12", "page 13", "page 14", "page 15", "page 16", "page 17", "page 18"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019.json
deleted file mode 100644
index b37945f..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019.json
+++ /dev/null
@@ -1 +0,0 @@
-{"TropleJumpForFIRA2019": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -667, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, -200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, 200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3866, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3866, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [2, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [6, 0, 2600, -4600, -2000, 0, 0, -600, 0, -300, 2666, 0, 0, -2600, 4600, 2000, 0, 0, 600, 0, 300, -2666, 0, 600], [50, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -666, 0, 600]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 7", "page 8", "page 9"]}
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019v1.json b/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019v1.json
deleted file mode 100644
index 4795909..0000000
--- a/src/azer_robocup_project/Soccer/Motion/motion_slots/TropleJumpForFIRA2019v1.json
+++ /dev/null
@@ -1 +0,0 @@
-{"TropleJumpForFIRA2019v1": [[10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [20, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -667, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, -200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, 200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3866, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3866, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [2, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [8, 0, 2500, -4000, -2000, 0, 0, 0, 0, -300, 666, 0, 0, -2500, 4000, 2000, 0, 0, 0, 0, 300, -666, 0, 600], [4, 0, 2166, -2500, -1000, 0, 0, 0, 0, -300, -1500, 0, 0, -2166, 2500, 1000, 0, 0, 0, 0, 300, 1500, 0, 600], [2, -600, -200, -200, 1300, 0, 0, 0, 0, -300, -2500, 0, 600, 200, 200, -1300, 0, 0, 0, 0, 300, 2500, 0, 600], [1, -1000, -960, 0, 1500, 0, 0, 405, 0, -300, -3866, 0, 1000, 960, 0, -1500, 0, 0, -405, 0, 300, 3866, 0, 600], [1, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [2, 0, 2233, -4600, -3380, 0, 0, -600, 0, -300, 2333, 0, 0, -2233, 4600, 3380, 0, 0, 600, 0, 300, -2333, 0, 600], [6, 0, 2600, -4600, -2000, 0, 0, -600, 0, -300, 2666, 0, 0, -2600, 4600, 2000, 0, 0, 600, 0, 300, -2666, 0, 600], [50, 0, 2000, -4000, -2250, 0, 0, 0, 0, -300, 666, 0, 0, -2000, 4000, 2250, 0, 0, 0, 0, 300, -666, 0, 600]], "pageNames": ["page 0", "page 1", "page 2", "page 3", "page 4", "page 5", "page 6", "page 7", "page 2 clone 1", "page 3 clone 1", "page 4 clone 1", "page 5 clone 1", "page 6 clone 1", "page 7 clone 1", "page 8", "page 9"]}
\ No newline at end of file
diff --git "a/src/azer_robocup_project/Soccer/Motion/motion_slots/\320\274\320\276\320\264\321\203\320\273\321\2142.py" "b/src/azer_robocup_project/Soccer/Motion/motion_slots/\320\274\320\276\320\264\321\203\320\273\321\2142.py"
deleted file mode 100644
index 719483f..0000000
--- "a/src/azer_robocup_project/Soccer/Motion/motion_slots/\320\274\320\276\320\264\321\203\320\273\321\2142.py"
+++ /dev/null
@@ -1,252 +0,0 @@
-import class_Motion as mt
-from ball_Approach_Steps_Seq import *
-import math, random
-import class_Local as lc
-import sys, json
-
-
-
-
-def uprint(*text):
- with open("output.txt",'a') as f:
- print(*text, file = f)
- print(*text )
-class GoalKeeper:
- def __init__(self, simulation, motion, local):
- self.motion = motion
- self.local = local
- if simulation == 2:
- with open("live_params.json", "r") as f:
- params = json.loads(f.read())
- else:
- with open("C:/Users/a/Documents/VREP/remoteApiBindings/python/python/sim_params.json", "r") as f:
- params = json.loads(f.read())
- self.side_step_yield = params['SIDE_STEP_YIELD']
- self.first_step_yield = params['FIRST_STEP_YIELD']
- self.cycle_step_yield = params['CYCLE_STEP_YIELD']
-
-
- def near_distance_ball_approach_and_kick(self):
- self.motion.turn_To_Course(self.motion.direction_To_Attack)
- self.local.post_data_in_pose = []
- self.local.quality = 0
- a, napravl, dist_mm = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True)
- self.local.localisation_Complete()
- if a==False and self.motion.falling_Flag != 0: return False
- if dist_mm > 900 or a == False: return False
- if 20 < abs(dist_mm*math.cos(math.radians(napravl))) < 90 and dist_mm*math.sin(math.radians(napravl)) < 50:
- self.motion.head_Up()
- if napravl > 0: self.motion.kick(first_Leg_Is_Right_Leg=False)
- else: self.motion.kick(first_Leg_Is_Right_Leg=True)
- self.motion.head_Return()
- if abs(napravl) > 60 :
- self.motion.head_Up()
- self.motion.walk_Initial_Pose()
- self.motion.walk_Cycle(stepLength = -15, sideLength = - math.copysign(20, napravl),rotation = 0,cycle = 0, number_Of_Cycles = 3)
- self.motion.walk_Cycle(stepLength = -30, sideLength = - math.copysign(20, napravl),rotation = 0,cycle = 1, number_Of_Cycles = 3)
- self.motion.walk_Cycle(stepLength = -60, sideLength = - math.copysign(20, napravl),rotation = 0,cycle = 2, number_Of_Cycles = 3)
- self.motion.walk_Final_Pose()
- compo_step = self.first_step_yield*15/64 + self.cycle_step_yield*30/64 + self.cycle_step_yield*60/64
- self.local.coord_odometry[0] +=int(-compo_step*math.cos(math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coord_odometry[1] +=int(-compo_step*math.sin(math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coordinate_record(odometry = True)
- self.motion.head_Return()
-
- else:
- n = int(math.floor((dist_mm-100-self.first_step_yield)/self.cycle_step_yield)+1)+1 #calculating the number of potential full steps forward
- displacement = dist_mm*math.sin(math.radians(napravl))
- m = int(math.floor(abs(abs(displacement)-53.4)/self.side_step_yield)+1)
- if n < m : n = m
- stepLength = (dist_mm-100)/(self.first_step_yield*1.25+self.cycle_step_yield*(n-1)+ self.cycle_step_yield*0.75)*64
- number_Of_Cycles = n+2
- if napravl > 0:
- sideLength = -(displacement - 53.4)/number_Of_Cycles*20/self.side_step_yield
- kick_by_Right = False
- else:
- sideLength = -(displacement + 53.4)/number_Of_Cycles*20/self.side_step_yield
- kick_by_Right = True
- self.motion.head_Up()
- self.motion.walk_Initial_Pose()
- for cycle in range(number_Of_Cycles):
- rotation = self.motion.direction_To_Attack-(self.motion.neck_pan*0.03375 + self.motion.euler_angle[0])*1
- if rotation > 30 : rotation = 30
- if rotation < -30 : rotation = -30
- stepLength1 = stepLength
- if cycle == 0: stepLength1 = stepLength/4
- if cycle == 1: stepLength1 = stepLength/2
- self.motion.walk_Cycle(stepLength1, sideLength,rotation,cycle,number_Of_Cycles)
- self.motion.walk_Final_Pose()
- self.motion.kick(first_Leg_Is_Right_Leg=kick_by_Right)
- self.local.coord_odometry[0] += int(dist_mm*math.cos(math.radians(napravl)))
- self.local.coord_odometry[1] += int(dist_mm*math.sin(math.radians(napravl)))
- self.local.coordinate_record(odometry = True)
- self.motion.head_Return()
-
- return True
-
-
- def goto_Center(self):#Function for reterning to center position
- uprint('Function for reterning to center position')
- #x1, y1, z1 = self.motion.Dummy_HData[len(self.motion.Dummy_HData)-1] # reading position from simulation service
- if self.local.quality < 1: self.motion.localisation_Motion()
- player_X_m = self.local.coordinate[0]/1000
- player_Y_m = self.local.coordinate[1]/1000
- ball_X_m, ball_Y_m = -1.6, 0
- self.motion.head_Up()
- ball_Approach(self.motion, self.local, ball_X_m, ball_Y_m, player_X_m, player_Y_m)
- self.motion.head_Return()
-
- def find_Ball(self):
- #dist = float(input('dist ='))
- #napravl = float(input('napravl ='))
- a, napravl, distance = self.motion.seek_Ball_In_Pose(fast_Reaction_On=True)
- self.local.localisation_Complete()
- dist = distance/1000
- uprint ( 'dist = ', dist, 'napravl =', napravl)
- return dist, napravl
- def ball_Speed_Dangerous(self):
- pass
- def fall_to_Defence(self):
- uprint('fall to defence')
- def get_Up_from_defence(self):
- uprint('up from defence')
- def scenario_A1(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- uprint('The robot knock out the ball to the side of the enemy')
- for i in range(10):
- if dist > 0.5 :
- player_X_m, player_Y_m = self.local.coordinate[0]/1000 , self.local.coordinate[1]/1000
- napravl_rad = math.radians(napravl)
- ball_X_m = dist* math.cos(napravl_rad) + player_X_m
- ball_Y_m = dist* math.sin(napravl_rad) + player_Y_m
- ball_Approach(self.motion, self.local, ball_X_m, ball_Y_m, player_X_m, player_Y_m)
- a = self.near_distance_ball_approach_and_kick()
- else: a = self.near_distance_ball_approach_and_kick()
- if a==False and self.motion.falling_Flag != 0: return
- if a == False : break
- a, napravl, distance = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True)
- self.local.localisation_Complete()
- dist = distance / 1000
- if dist > 1 : break
- target_course1 = self.motion.euler_angle[0] +180
- self.motion.turn_To_Course(target_course1)
- a, napravl, distance = self.motion.seek_Ball_In_Pose(fast_Reaction_On = False)
- self.local.localisation_Complete()
- self.goto_Center()
-
-
-
- def scenario_A2(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- uprint('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_A3(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- uprint('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_A4(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- uprint('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_B1(self):#the robot moves to the left and stands on the same axis as the ball and the opponents' goal
- uprint('the robot moves to the left 4 steps')
- self.motion.first_Leg_Is_Right_Leg = True
- self.motion.walk_Initial_Pose()
- stepLength, sideLength, rotation, cycle, number_Of_Cycles = 0, -20 , 0 , 0 , 4
- for cycle in range(number_Of_Cycles): self.motion.walk_Cycle(stepLength, sideLength,rotation,cycle,number_Of_Cycles)
- self.local.coord_odometry[0] += int(-self.side_step_yield * 4 * math.sin( math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coord_odometry[1] += int(self.side_step_yield * 4 * math.cos( math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coordinate_record(odometry = True)
- self.motion.walk_Final_Pose()
- self.motion.turn_To_Course(self.motion.direction_To_Attack)
-
- def scenario_B2(self):#the robot moves to the left and stands on the same axis as the ball and the opponents' goal
- uprint('the robot moves to the left 4 steps')
- self.scenario_B1()
-
- def scenario_B3(self):#the robot moves to the right and stands on the same axis as the ball and the opponents' goal
- uprint('the robot moves to the right 4 steps')
- self.motion.first_Leg_Is_Right_Leg = True
- self.motion.walk_Initial_Pose()
- stepLength, sideLength, rotation, cycle, number_Of_Cycles = 0, 20 , 0 , 0 , 4
- for cycle in range(number_Of_Cycles): self.motion.walk_Cycle(stepLength, sideLength,rotation,cycle,number_Of_Cycles)
- self.motion.walk_Final_Pose()
- self.local.coord_odometry[0] += int(self.side_step_yield * 4 * math.sin( math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coord_odometry[1] += int(-self.side_step_yield * 4 * math.cos( math.radians(self.motion.euler_angle[0] - self.motion.direction_To_Attack + self.motion.neck_pan*0.03375)))
- self.local.coordinate_record(odometry = True)
- self.motion.turn_To_Course(self.motion.direction_To_Attack)
-
- def scenario_B4(self):#the robot moves to the right and stands on the same axis as the ball and the opponents' goal
- uprint('the robot moves to the right 4 steps')
- self.scenario_B3()
-
-def main():
- SIMULATION = 1
- motion = mt.Motion(SIMULATION)
- local = lc.Local(motion)
- motion.local = local
- g = GoalKeeper(SIMULATION, motion, local)
- if SIMULATION == 2:
- import pyb
- motion.push_Button()
- pyb.delay(2000)
- else:
- motion.sim_Enable()
- motion.Vision_Sensor_Display_On = False
- motion.sim_Start()
- local.coord_odometry = [-1800, 0, 0]
- local.coordinate_record(odometry = True)
-
- motion.falling_Flag = 0
- while (True):
- Ballposition = motion.Ballposition
- #Ballposition[0] = random.uniform(-1.7, 0)
- #Ballposition[1] = random.uniform(-1.3, 1.3)
- if input('continue? (y/n)') == 'n': break
- #returnCode = motion.vrep.simxSetObjectPosition(motion.clientID, motion.BallHandle , motion.ResizableFloorHandle,Ballposition, motion.vrep.simx_opmode_oneshot)
- dist = -1.0
- if motion.falling_Flag != 0:
- motion.falling_Flag = 0
- motion.turn_To_Course(motion.direction_To_Attack)
- #goto_Center()
- while(dist < 0):
- dist,napravl = g.find_Ball()
- print(Ballpisition, local.ball_coord)
- #if dist == 0 and napravl == 0:
- # if -1850 < local.coord_odometry[0] < -1700 and -50 < local.coord_odometry[1] < 50: break
- # g.goto_Center()
- # break
- #if g.ball_Speed_Dangerous():
- # g.fall_to_Defence()
- # time.sleep(3.0)
- # g.get_Up_from_defence()
- #motion.head_Up()
- #if (dist <= 0.9 and 0 <= napravl <= 45): g.scenario_A1( dist, napravl)
- #if (dist <= 0.9 and 45 < napravl <= 90): g.scenario_A2( dist, napravl)
- #if (dist <= 0.9 and 0 >= napravl >= -45): g.scenario_A3( dist, napravl)
- #if (dist <= 0.9 and -45 > napravl >= -90): g.scenario_A4( dist, napravl)
- #if ((1.8 >= dist > 0.9) and (10 <= napravl <= 45)): g.scenario_B1()
- #if ((1.8 >= dist > 0.9) and (45 < napravl <= 90)): g.scenario_B2()
- #if ((1.8 >= dist > 0.9) and (-10 >= napravl >= -45)): g.scenario_B3()
- #if ((1.8 >= dist > 0.9) and (-45 > napravl >= -90)): g.scenario_B4()
- #motion.head_Return()
-
-
- motion.sim_Progress(2)
- motion.sim_Stop()
- #motion.print_Diagnostics()
- motion.sim_Disable()
-
-
-
-
-if __name__=="__main__":
- #try:
- main()
-
- #except Exception as e:
- #with open("output.txt",'a') as f:
- # sys.print_exception(e,f)
- # sys.print_exception(e,sys.stdout)
-
-
-
diff --git a/src/azer_robocup_project/Soccer/Motion/path_planning.py b/src/azer_robocup_project/Soccer/Motion/path_planning.py
deleted file mode 100644
index 5eb63c8..0000000
--- a/src/azer_robocup_project/Soccer/Motion/path_planning.py
+++ /dev/null
@@ -1,794 +0,0 @@
-
-import sys
-import os
-import math
-import array
-import json
-
-
-if sys.version == '3.4.0':
- # will be running on openMV
- pass
-else:
- # will be running on desktop computer
- import wx
- import random
-
-
-
-goalPostRadius = 0.15 # Radius to walk around a goal post (in m).
-ballRadius = 0.1 # Radius to walk around the ball (in m).
-uprightRobotRadius = 0.2 # Radius to walk around an upright robot (in m).
-roundAboutRadiusIncrement = 0.15
-
-def uprint(*text):
- #with open("output.txt",'a') as f:
- # print(*text, file = f)
- print(*text )
-
-class Glob:
- def __init__(self):
- self.COLUMNS = 18
- self.ROWS = 13
- self.pf_coord = [0.276, 0.749, 2] #[-0.4, 0.0 , 0] # [0.5, 0.5 , -math.pi * 3/4]
- self.ball_coord = [-0.132, 0.957] #[0, 0]
- self.obstacles = [[0.4, 0.025, 0.2], [0.725, -0.475, 0.2]] #[[0, 0, 0.15], [0.4, 0.025, 0.2], [0.725, -0.475, 0.2]]
- #self.ball_coord = [2, 0]
- #self.obstacles = [[0.4, 0.025, 0.2], [0.725, -0.475, 0.2], [0.8, 0.55, 0.2], [1.175, 0, 0.2], [1.625, -0.4, 0.2], [1.7, 0.425, 0.2]]
- self.landmarks = {"post1": [[ 1.8, -0.6 ]], "post2": [[ 1.8, 0.6 ]], "post3": [[ -1.8, 0.6 ]], "post4": [[ -1.8, -0.6 ]],
- "unsorted_posts": [[ 1.8, 0.6 ],[ 1.8, -0.6 ],[ -1.8, 0.6 ],[ -1.8, -0.6 ]],
- "FIELD_WIDTH": 2.6, "FIELD_LENGTH": 3.6 }
- self.params = {'CYCLE_STEP_YIELD': 103.5}
- self.cycle_step_yield = 103.5
- current_work_directory = os.getcwd()
- current_work_directory = current_work_directory.replace('\\', '/')
- current_work_directory += '/'
- self.strategy_data = array.array('b',(0 for i in range(self.COLUMNS * self.ROWS * 2)))
- self.import_strategy_data(current_work_directory)
-
- def import_strategy_data(self, current_work_directory):
- with open(current_work_directory + "Init_params/strategy_data.json", "r") as f:
- loaded_Dict = json.loads(f.read())
- if loaded_Dict.get('strategy_data') != None:
- strategy_data = loaded_Dict['strategy_data']
- for column in range(self.COLUMNS):
- for row in range(self.ROWS):
- index1 = column * self.ROWS + row
- power = strategy_data[index1][2]
- yaw = int(strategy_data[index1][3] * 40) # yaw in radians multiplied by 40
- self.strategy_data[index1*2] = power
- self.strategy_data[index1*2+1] = yaw
-
-class Forward:
- def __init__(self, glob):
- self.glob = glob
-
- def direction_To_Guest(self):
- if self.glob.ball_coord[0] < 0:
- return 0
- elif self.glob.ball_coord[0] > 0.8 and abs(self.glob.ball_coord[1]) > 0.6:
- return math.atan(-self.glob.ball_coord[1]/(1.8-self.glob.ball_coord[0]))
- elif self.glob.ball_coord[0] < 1.5 and abs(self.glob.ball_coord[1]) < 0.25:
- if (1.8-self.glob.ball_coord[0]) == 0: return 0
- else:
- if abs(self.glob.ball_coord[1]) < 0.2:
- return math.atan((math.copysign(0.5, self.glob.ball_coord[1])-
- self.glob.ball_coord[1])/(1.8-self.glob.ball_coord[0]))
- else:
- return math.atan((0.5* (round(random.random(),0)*2 - 1)-
- self.glob.ball_coord[1])/(1.8-self.glob.ball_coord[0]))
- else:
- return math.atan(-self.glob.pf_coord[1]/(2.4-self.glob.pf_coord[0]))
-
-class Forward_Vector_Matrix:
- def __init__(self, glob):
- self.glob = glob
- #self.direction_To_Guest = 0
- self.kick_Power = 1
-
-
- def direction_To_Guest(self):
- if abs(self.glob.ball_coord[0]) > self.glob.landmarks["FIELD_LENGTH"] / 2:
- ball_x = math.copysign(self.glob.landmarks["FIELD_LENGTH"] / 2, self.glob.ball_coord[0])
- else: ball_x = self.glob.ball_coord[0]
- if abs(self.glob.ball_coord[1]) > self.glob.landmarks["FIELD_WIDTH"] / 2:
- ball_y = math.copysign(self.glob.landmarks["FIELD_WIDTH"] / 2, self.glob.ball_coord[1])
- else: ball_y = self.glob.ball_coord[1]
- col = math.floor((ball_x + self.glob.landmarks["FIELD_LENGTH"] / 2) / (self.glob.landmarks["FIELD_LENGTH"] / self.glob.COLUMNS))
- row = math.floor((- ball_y + self.glob.landmarks["FIELD_WIDTH"] / 2) / (self.glob.landmarks["FIELD_WIDTH"] / self.glob.ROWS))
- if col >= self.glob.COLUMNS : col = self.glob.COLUMNS - 1
- if row >= self.glob.ROWS : row = self.glob.ROWS -1
- direction_To_Guest = self.glob.strategy_data[(col * self.glob.ROWS + row) * 2 + 1] / 40
- self.kick_Power = self.glob.strategy_data[(col * self.glob.ROWS + row) * 2]
- print('direction_To_Guest = ', math.degrees(direction_To_Guest))
- return direction_To_Guest
-
-
-class PathPlan:
- def __init__(self, glob):
- self.glob = glob
- self.posts = [self.glob.landmarks["post1"][0], self.glob.landmarks["post2"][0], self.glob.landmarks["post3"][0], self.glob.landmarks["post4"][0]]
- self.goal_bottoms = [[[self.posts[0][0]+0.10, self.posts[0][1]],[self.posts[1][0]+0.10, self.posts[1][1]]],
- [[self.posts[2][0]-0.10, self.posts[2][1]],[self.posts[3][0]-0.10, self.posts[3][1]]],
- [[self.posts[0][0], self.posts[0][1]],[self.posts[0][0]+0.35, self.posts[0][1]]],
- [[self.posts[1][0], self.posts[1][1]],[self.posts[1][0]+0.35, self.posts[1][1]]],
- [[self.posts[2][0], self.posts[2][1]],[self.posts[2][0]-0.35, self.posts[2][1]]],
- [[self.posts[3][0], self.posts[3][1]],[self.posts[3][0]-0.35, self.posts[3][1]]]]
-
-
- def coord2yaw(self, x, y):
- if x == 0:
- if y > 0 : yaw = math.pi/2
- else: yaw = -math.pi/2
- else: yaw = math.atan(y/x)
- if x < 0:
- if yaw > 0: yaw -= math.pi
- else: yaw += math.pi
- return yaw
-
- def intersection_line_segment_and_line_segment(self, x1, y1, x2, y2, x3, y3, x4, y4 ):
- """
- x = x1 + (x2 - x1) * t1 t1 - paramentric coordinate
- y = y1 + (y2 - y1) * t1
- x = x3 + (x4 - x3) * t2 t2 - paramentric coordinate
- y = y3 + (y4 - y3) * t2
- x1 + (x2 - x1) * t1 = x3 + (x4 - x3) * t2
- y1 + (y2 - y1) * t1 = y3 + (y4 - y3) * t2
- t1 = (x3 + (x4 - x3) * t2 - x1) / (x2 - x1)
- t1 = (y3 + (y4 - y3) * t2 - y1) / (y2 - y1)
- y1 + (y2 - y1) * (x3 + (x4 - x3) * t2 - x1) / (x2 - x1) = y3 + (y4 - y3) * t2
- (y2 - y1) * (x4 - x3)/ (x2 - x1) * t2 - (y4 - y3) * t2 = y3 - y1 - (y2 - y1) * (x3 - x1) / (x2 - x1)
- t2 = (y3 - y1 - (y2 - y1) * (x3 - x1) / (x2 - x1)) /((y2 - y1) * (x4 - x3)/ (x2 - x1) - (y4 - y3))
- if t1 == 0:
- t2 = (y1 - y3)/ (y4 - y3)
- t2 = (x1 - x3)/ (x4 - x3)
- """
- if x2 == x1:
- if x4 == x3:
- if x1 == x3 and max(y1,y2) >= min(y3,y4) and max(y3, y4) >= min(y1,y2):
- return True
- else: return False
- elif y2 == y1:
- if x3 == x4:
- if y3 == y4:
- if x1 == x3 and y1 == y3:
- return True
- else: return False
- else:
- t2 = (y1 - y3)/ (y4 - y3)
- if x1 == x3 and 0 <= round(t2, 4) <= 1:
- return True
- else: return False
- else:
- dt = (y1 - y3)/ (y4 - y3) - (x1 - x3)/ (x4 - x3)
- if round(dt, 4) == 0:
- return True
- else: return False
- else:
- t2 = (x1 - x3)/(x4 - x3)
- t1 = (y3 - y1 + (y4 - y3) * t2) / (y2 - y1)
- if 0 <= round(t1, 4) <= 1 and 0 <= round(t2, 4) <= 1:
- return True
- else: return False
- else:
- if (y2 - y1) * (x4 - x3) == (y4 - y3) * (x2 - x1):
- if max(x1,x2) >= min(x3,x4) and max(x3, x4) >= min(x1,x2):
- return True
- else: return False
- else:
- t2 = (y3 - y1 - (y2 - y1) * (x3 - x1) / (x2 - x1)) /((y2 - y1) * (x4 - x3)/ (x2 - x1) - (y4 - y3))
- t1 = (x3 + (x4 - x3) * t2 - x1) / (x2 - x1)
- if 0 <= round(t1, 4) <= 1 and 0 <= round(t2, 4) <= 1:
- return True
- else: return False
-
-
- def intersection_line_segment_and_circle(self, x1, y1, x2, y2, xc, yc, R):
- """
- x = x1 + (x2 - x1) * t t - paramentric coordinate
- y = y1 + (y2 - y1) * t
- R**2 = (x - xc)**2 + (y - yc)**2
- (x1 + (x2 - x1) * t - xc)**2 + (y1 + (y2 - y1) * t - yc)**2 - R**2 = 0
- ((x2 - x1) * t)**2 + (x1 - xc)**2 + 2 * (x2 - x1) * (x1 - xc) * t +
- ((y2 - y1) * t)**2 + (y1 - yc)**2 + 2 * (y2 - y1) * (y1 - yc) * t - R**2 = 0
- ((x2 - x1)**2 + (y2 - y1)**2) * t**2 + (2 * (x2 - x1) * (x1 - xc) + 2 * (y2 - y1) * (y1 - yc)) * t +
- (x1 - xc)**2 + (y1 - yc)**2 - R**2 = 0
- a * t**2 + b * t + c = 0
- a = (x2 - x1)**2 + (y2 - y1)**2
- b = 2 * (x2 - x1) * (x1 - xc) + 2 * (y2 - y1) * (y1 - yc)
- c = (x1 - xc)**2 + (y1 - yc)**2 - R**2
- """
- a = (x2 - x1)**2 + (y2 - y1)**2
- b = 2 * (x2 - x1) * (x1 - xc) + 2 * (y2 - y1) * (y1 - yc)
- c = (x1 - xc)**2 + (y1 - yc)**2 - R**2
- successCode, t1, t2 = self.square_equation(a,b,c)
- if successCode:
- if 0 <= round(t1, 4) <= 1 or 0 <= round(t2, 4) <= 1 or (t1 > 1 and t2 < 0) or (t2 > 1 and t1 < 0):
- return True
- return False
-
- def intersection_circle_segment_and_circle(self, x1, y1, x2, y2, x0, y0, CW, xc, yc, R):
- R0sq = (x1 - x0)**2 + (y1 - y0)**2
- if yc == y0:
- xp1 = xp2 = (R0sq - R**2 + xc**2 - x0**2)/(2 * (xc - x0))
- if R0sq - (xp1 - x0)**2 < 0: return False
- yp1 = y0 + math.sqrt(R0sq - (xp1 - x0)**2)
- yp2 = y0 - math.sqrt(R0sq - (xp1 - x0)**2)
- else:
- A = (x0 - xc)/(yc - y0)
- B = (R0sq - R**2 + xc**2 + yc**2 - x0**2 - y0**2)/(2*(yc - y0))
- a = 1 + A**2
- b = 2 * (-A * y0 + A * B - x0)
- c = x0**2 - R0sq + (B - y0)**2
- succsessCode, xp1, xp2 = self.square_equation(a, b, c)
- if not succsessCode: return False
- yp1 = A * xp1 + B
- yp2 = A * xp2 + B
- alpha2 = self.coord2yaw(x2 - x0, y2 - y0)
- alpha1 = self.coord2yaw(x1 - x0, y1 - y0)
- alphap2 = self.coord2yaw(xp2 - x0, yp2 - y0)
- alphap1 = self.coord2yaw(xp1 - x0, yp1 - y0)
- if CW:
- if alpha1 < alpha2:
- alpha1 += math.pi * 2
- if alphap1 < 0 : alphap1 += math.pi * 2
- if alphap2 < 0 : alphap2 += math.pi * 2
- if alpha2 <= alphap1 <= alpha1 or alpha2 <= alphap2 <= alpha1:
- return True
- else:
- if alpha2 < alpha1:
- alpha2 += math.pi * 2
- if alphap1 < 0 : alphap1 += math.pi * 2
- if alphap2 < 0 : alphap2 += math.pi * 2
- if alpha1 <= alphap1 <= alpha2 or alpha1 <= alphap2 <= alpha2:
- return True
- return False
-
- def norm_yaw(self, yaw):
- yaw %= 2 * math.pi
- if yaw > math.pi: yaw -= 2* math.pi
- if yaw < -math.pi: yaw += 2* math.pi
- return yaw
-
- def delta_yaw(self, start_yaw, dest_yaw, CW):
- s = math.degrees(start_yaw)
- d = math.degrees(dest_yaw)
- if CW:
- if start_yaw < dest_yaw:
- start_yaw += math.pi * 2
- else:
- if dest_yaw < start_yaw:
- dest_yaw += math.pi * 2
- delta_yaw = dest_yaw - start_yaw
- #print('CW = ', CW, 'start_yaw = ', s, 'dest_yaw = ', d, 'delta_yaw = ', math.degrees(delta_yaw))
- return delta_yaw
-
- def path_calc_optimum(self, start_coord, target_coord):
- dest, centers, number_Of_Cycles = self.path_calc(start_coord, target_coord)
- if len(centers) > 0:
- x1, y1, x2, y2, cx, cy, R, CW = centers[0]
- if R <= 0.08:
- start_yaw = start_coord[2]
- dest_yaw = self.coord2yaw(dest[1][0] - dest[0][0], dest[1][1] - dest[0][1])
- delta_yaw = self.delta_yaw(start_yaw, dest_yaw, CW)
- if abs(delta_yaw) > math.pi: centers[0][7] = not CW
- x1, y1, x2, y2, cx, cy, R, CW = centers[len(centers)-1]
- if R <= 0.08:
- start_yaw = self.coord2yaw(dest[len(dest)-1][0] - dest[len(dest)-2][0], dest[len(dest)-1][1] - dest[len(dest)-2][1])
- dest_yaw = target_coord[2]
- delta_yaw = self.delta_yaw(start_yaw, dest_yaw, CW)
- if abs(delta_yaw) > math.pi: centers[len(centers)-1][7] = not CW
- return dest, centers, number_Of_Cycles
-
- def path_calc(self, start_coord, target_coord):
- x1, y1, yaw1 = start_coord
- x2, y2, yaw2 = target_coord
- dest1, centers1, number_Of_Cycles1 = self.arc_path_internal( x1, y1, yaw1, x2, y2, yaw2)
- dest2, centers2, number_Of_Cycles2 = self.arc_path_external( x1, y1, yaw1, x2, y2, yaw2)
- if number_Of_Cycles1 < number_Of_Cycles2: return dest1, centers1, number_Of_Cycles1
- else: return dest2, centers2, number_Of_Cycles2
- return dest1, centers1
-
- def arc_path_external(self, x1, y1, yaw1, x2, y2, yaw2):
- number_Of_Cycles_min = 1000
- for i in range(10):
- for j in range(10):
- R1 = i * 0.05
- R2 = j * 0.05
- #R1 = 0.2
- #R2 = 0.2
- if (y2-y1) < 0:
- xc1 = x1 - R1 * math.sin(yaw1)
- yc1 = y1 + R1 * math.cos(yaw1)
- xc2 = x2 - R2 * math.sin(yaw2)
- yc2 = y2 + R2 * math.cos(yaw2)
- CW1 = False
- CW2 = False
- else:
- xc1 = x1 + R1 * math.sin(yaw1)
- yc1 = y1 - R1 * math.cos(yaw1)
- xc2 = x2 + R2 * math.sin(yaw2)
- yc2 = y2 - R2 * math.cos(yaw2)
- CW1 = True
- CW2 = True
- successCode, xp1, yp1 = self.external_tangent_line(True, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW1 )
- if successCode:
- successCode, xp2, yp2 = self.external_tangent_line(False, R2, R1, x2, y2, xc2, yc2, xc1, yc1, CW2 )
- if successCode:
- dest = [[xp1,yp1], [xp2,yp2]]
- centers = [[x1, y1, xp1, yp1, xc1, yc1, R1, CW1], [xp2, yp2, x2, y2, xc2, yc2, R2, CW2]]
- nearestObstacle = self.check_Obstacle(xp1, yp1, xp2, yp2)
- if nearestObstacle >= 0 :
- roundAboutRadius = self.glob.obstacles[nearestObstacle][2] / 2 + roundAboutRadiusIncrement
- #if self.intersection_line_segment_and_circle(xp1, yp1, xp2, yp2,
- # self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- for variant in range(2):
- if variant == 0:
- CW = CW1
- successCode1, xp1, yp1 = self.external_tangent_line(True,
- R1, roundAboutRadius, x1, y1, xc1, yc1, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW1)
- successCode2, xp2, yp2 = self.external_tangent_line(False,
- roundAboutRadius, R1, x2, y2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc1, yc1, CW)
- successCode3, xp3, yp3 = self.external_tangent_line(True,
- roundAboutRadius, R2, x1, y1, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc2, yc2, CW)
- successCode4, xp4, yp4 = self.external_tangent_line(False,
- R2, roundAboutRadius, x2, y2, xc2, yc2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW2)
- if not (successCode1 and successCode2 and successCode3 and successCode4) : continue
- if variant == 1:
- CW = not CW1
- successCode5, xp1, yp1 = self.internal_tangent_line(True,
- R1, roundAboutRadius, x1, y1, xc1, yc1, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW1)
- successCode6, xp2, yp2 = self.internal_tangent_line(False,
- roundAboutRadius, R1, xp2, yp2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc1, yc1, CW)
- successCode7, xp3, yp3 = self.internal_tangent_line(True,
- roundAboutRadius, R2, xp2, yp2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc2, yc2, CW)
- successCode8, xp4, yp4 = self.internal_tangent_line(False,
- R2, roundAboutRadius, x2, y2, xc2, yc2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW2)
- if not (successCode5 and successCode6 and successCode7 and successCode8) : continue
- dest = [[xp1,yp1], [xp2,yp2], [xp3,yp3], [xp4,yp4]]
- centers = [[x1, y1, xp1, yp1, xc1, yc1, R1, CW1],
- [xp2, yp2, xp3, yp3, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], roundAboutRadius, CW],
- [xp4, yp4, x2, y2, xc2, yc2, R2, CW2]]
- price = self.check_Price(x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers)
- number_Of_Cycles = self.number_Of_Cycles_count(dest, centers, yaw1, yaw2) + price
- if number_Of_Cycles < number_Of_Cycles_min:
- number_Of_Cycles_min = number_Of_Cycles
- dest_min = dest
- centers_min = centers
- else:
- price = self.check_Price(x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers)
- number_Of_Cycles = self.number_Of_Cycles_count(dest, centers, yaw1, yaw2) + price
- if number_Of_Cycles < number_Of_Cycles_min:
- number_Of_Cycles_min = number_Of_Cycles
- dest_min = dest
- centers_min = centers
- if number_Of_Cycles_min == 1000: return [], [], number_Of_Cycles_min
- else: return dest_min, centers_min, number_Of_Cycles_min
-
- def check_Obstacle(self, xp1, yp1, xp2, yp2):
- nearestObstacle = -1
- obstacles = []
- distances = []
- for j in range(len(self.glob.obstacles)):
- roundAboutRadius = self.glob.obstacles[j][2] / 2 + roundAboutRadiusIncrement
- if self.intersection_line_segment_and_circle(xp1, yp1, xp2, yp2,
- self.glob.obstacles[j][0], self.glob.obstacles[j][1], roundAboutRadius):
- obstacles.append(j)
- distances.append(math.sqrt((xp1 - self.glob.obstacles[j][0])**2 + (yp1 - self.glob.obstacles[j][1])**2))
- if obstacles != []:
- nearestObstacle = obstacles[distances.index(min(distances))]
- return nearestObstacle
-
- def check_Limits(self, x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest):
- permit = True
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_circle(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- self.glob.ball_coord[0], self.glob.ball_coord[1],
- ballRadius + roundAboutRadiusIncrement): permit = False
- ind = len(dest) - 1
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- self.glob.ball_coord[0], self.glob.ball_coord[1],
- ballRadius + roundAboutRadiusIncrement): permit = False
- if self.intersection_circle_segment_and_circle(x1, y1, dest[0][0], dest[0][1], xc1, yc1, CW1,
- self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius): permit = False
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius): permit = False
-
- for j in range(4):
- goalPostX, goalPostY = self.glob.landmarks["unsorted_posts"][j]
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- goalPostX, goalPostY, goalPostRadius): permit = False
- if self.intersection_circle_segment_and_circle(x1, y1, dest[0][0], dest[0][1], xc1, yc1, CW1,
- goalPostX, goalPostY, goalPostRadius): permit = False
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_circle(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- goalPostX, goalPostY, goalPostRadius): permit = False
- #if permit == False: print('permit denied')
- return permit
-
- def check_Price(self, x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers):
- price = 0
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_circle(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- self.glob.ball_coord[0], self.glob.ball_coord[1],
- ballRadius + roundAboutRadiusIncrement): price += 200
- ind = len(dest) - 1
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- self.glob.ball_coord[0], self.glob.ball_coord[1],
- ballRadius + roundAboutRadiusIncrement): price += 200
- for j in range(len(self.glob.obstacles)):
- roundAboutRadius = self.glob.obstacles[j][2] / 2 + roundAboutRadiusIncrement
- if self.intersection_circle_segment_and_circle(x1, y1, dest[0][0], dest[0][1], xc1, yc1, CW1,
- self.glob.obstacles[j][0], self.glob.obstacles[j][1], roundAboutRadius): price += 200
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- self.glob.obstacles[j][0], self.glob.obstacles[j][1], roundAboutRadius): price += 100
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_circle(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- self.glob.obstacles[j][0], self.glob.obstacles[j][1], roundAboutRadius): price += 300 - i * 100
- for j in range(4):
- goalPostX, goalPostY = self.posts[j]
- if self.intersection_circle_segment_and_circle(dest[ind][0], dest[ind][1], x2, y2, xc2, yc2, CW2,
- goalPostX, goalPostY, goalPostRadius): price += 200
- if self.intersection_circle_segment_and_circle(x1, y1, dest[0][0], dest[0][1], xc1, yc1, CW1,
- goalPostX, goalPostY, goalPostRadius): price += 300
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_circle(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- goalPostX, goalPostY, goalPostRadius): price += 300 - i * 100
-
- for j in range(6):
- for i in range(0, len(dest), 2):
- if self.intersection_line_segment_and_line_segment(dest[i][0], dest[i][1], dest[i + 1][0], dest[i + 1][1],
- self.goal_bottoms[j][0][0], self.goal_bottoms[j][0][1], self.goal_bottoms[j][1][0], self.goal_bottoms[j][1][1]):
- price += 300 # - i * 100
- for i in range(len(centers)):
- if self.intersection_line_segment_and_circle(self.goal_bottoms[j][0][0], self.goal_bottoms[j][0][1],
- self.goal_bottoms[j][1][0], self.goal_bottoms[j][1][1],
- centers[i][4], centers[i][5], centers[i][6]): price += 300 #- i * 100
-
- return price
-
- def number_Of_Cycles_count(self, dest, centers, yaw1, yaw2):
- prop_yaw_glob1 = self.coord2yaw(dest[1][0] - dest[0][0], dest[1][1] - dest[0][1])
- x1, y1, x2, y2, cx, cy, R, CW = centers[0]
- prop_yaw_local1 = self.delta_yaw(yaw1, prop_yaw_glob1, CW)
- number_Of_Cycles = math.ceil(abs(prop_yaw_local1 / 0.2))
- for i in range(0,len(dest),2):
- distance = math.sqrt((dest[i+1][0] - dest[i][0])**2 + (dest[i+1][1] - dest[i][1])**2)
- number_Of_Cycles += math.ceil(abs(distance / (self.glob.cycle_step_yield/1000)))
- x1, y1, x2, y2, cx, cy, R, CW = centers[int(i/2+1)]
- if len(dest) == i+2 :
- prop_yaw_local2 = self.delta_yaw(prop_yaw_glob1, yaw2, CW)
- else:
- prop_yaw_glob2 = self.coord2yaw(dest[i+3][0] - dest[i+2][0], dest[i+3][1] - dest[i+2][1])
- prop_yaw_local2 = self.delta_yaw(prop_yaw_glob1, prop_yaw_glob2, CW)
- prop_yaw_glob1 = prop_yaw_glob2
- number_Of_Cycles += math.ceil(abs(prop_yaw_local2 / 0.2))
- return number_Of_Cycles
-
-
- def external_tangent_line(self, start, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW ):
- if R1 == 0: return True, x1, y1
- L = math.sqrt((xc2 - xc1)**2 + (yc2 - yc1)**2)
- if R1 == R2:
- xp1 = [xc1 + (yc2-yc1) * R1 /L, xc1 - (yc2-yc1) * R1/L]
- yp1 = [yc1 - (xc2-xc1) * R1/L, yc1 + (xc2-xc1) * R1/L]
- else:
- L1 = L * R1 / abs(R1 - R2)
- L2 = L * R2 / abs(R1 - R2)
- x0 = (R2 * xc1 - R1 * xc2) / (R2 - R1)
- y0 = (R2 * yc1 - R1 * yc2) / (R2 - R1)
- if yc1 == y0:
- xp = (L1**2 - 2 * R1**2 + xc1**2 - x0**2)/(2 * xc1 - 2 * x0)
- xp1 = [xp, xp]
- tmp = R1**2 - (xp - xc1)**2
- if tmp < 0 : return False, 0, 0
- yp1 = [yc1 + math.sqrt(tmp), yc1 - math.sqrt(tmp)]
- else:
- A = (x0 - xc1) / (yc1 - y0)
- B = (L1**2 - 2 * R1**2 + xc1**2 + yc1**2 - x0**2 - y0**2) / (2 * yc1 - 2 * y0)
- ap = 1 + A**2
- bp = 2 * (-A * yc1 + A * B - xc1)
- cp = xc1**2 + (B - yc1)**2 - R1**2
- successCode, xp10, xp11 = self.square_equation(ap, bp, cp)
- if not successCode:
- return False, 0, 0
- xp1 =[xp10, xp11]
- yp1 = [A * xp1[0] + B, A * xp1[1] + B]
- #alpha_1 = self.coord2yaw(x1 - xc1, y1 - yc1)
- al0 = self.coord2yaw(xp1[0] - xc1, yp1[0] - yc1)
- al1 = self.coord2yaw(xp1[1] - xc1, yp1[1] - yc1)
- if CW: da = -math.pi/2
- else: da = math.pi/2
- directToOtherEnd = self.coord2yaw(xc2 - xc1, yc2 - yc1)
- alpha_p1 = [abs(self.norm_yaw(da + al0 - directToOtherEnd)),
- abs(self.norm_yaw(da + al1 - directToOtherEnd))]
- if start:
- ind = alpha_p1.index(min(alpha_p1))
- else: ind = alpha_p1.index(max(alpha_p1))
- return True, xp1[ind], yp1[ind]
-
- def arc_path_internal(self, x1, y1, yaw1, x2, y2, yaw2):
- number_Of_Cycles_min = 1000
- for i in range(10):
- for j in range(10):
- R1 = i * 0.05
- R2 = j * 0.05
- #R1 = 0.2
- #R2 = 0.2
- if (y2-y1) < 0:
- xc1 = x1 + R1 * math.sin(yaw1)
- yc1 = y1 - R1 * math.cos(yaw1)
- xc2 = x2 - R2 * math.sin(yaw2)
- yc2 = y2 + R2 * math.cos(yaw2)
- CW1 = True
- CW2 = False
- else:
- xc1 = x1 - R1 * math.sin(yaw1)
- yc1 = y1 + R1 * math.cos(yaw1)
- xc2 = x2 + R2 * math.sin(yaw2)
- yc2 = y2 - R2 * math.cos(yaw2)
- CW1 = False
- CW2 = True
- successCode, xp1, yp1 = self.internal_tangent_line(True, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW1)
- if successCode:
- successCode, xp2, yp2 = self.internal_tangent_line(False, R2, R1, x2, y2, xc2, yc2, xc1, yc1, CW2)
- if successCode:
- dest = [[xp1,yp1], [xp2,yp2]]
- centers = [[x1, y1, xp1, yp1, xc1, yc1, R1, CW1], [xp2, yp2, x2, y2, xc2, yc2, R2, CW2]]
- nearestObstacle = self.check_Obstacle(xp1, yp1, xp2, yp2)
- if nearestObstacle >= 0 :
- roundAboutRadius = self.glob.obstacles[nearestObstacle][2] / 2 + roundAboutRadiusIncrement
- #if self.intersection_line_segment_and_circle(xp1, yp1, xp2, yp2,
- # self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- for variant in range(2):
- if variant == 0:
- CW = CW1
- successCode1, xp1, yp1 = self.external_tangent_line(True,
- R1, roundAboutRadius, x1, y1, xc1, yc1, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW1)
- successCode2, xp2, yp2 = self.external_tangent_line(False,
- roundAboutRadius, R1, x2, y2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc1, yc1, CW)
- successCode3, xp3, yp3 = self.internal_tangent_line(True,
- roundAboutRadius, R2, xp2, yp2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc2, yc2, CW)
- successCode4, xp4, yp4 = self.internal_tangent_line(False,
- R2, roundAboutRadius, x2, y2, xc2, yc2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW2)
- if not (successCode1 and successCode2 and successCode3 and successCode4) : continue
- if variant == 1:
- CW = not CW1
- successCode5, xp1, yp1 = self.internal_tangent_line(True,
- R1, roundAboutRadius, x1, y1, xc1, yc1, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW1)
- successCode6, xp2, yp2 = self.internal_tangent_line(False,
- roundAboutRadius, R1, xp2, yp2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc1, yc1, CW)
- successCode7, xp3, yp3 = self.external_tangent_line(True,
- roundAboutRadius, R2, xp2, yp2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], xc2, yc2, CW)
- successCode8, xp4, yp4 = self.external_tangent_line(False,
- R2, roundAboutRadius, x2, y2, xc2, yc2, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], CW2)
- if not (successCode5 and successCode6 and successCode7 and successCode8) : continue
- dest = [[xp1,yp1], [xp2,yp2], [xp3,yp3], [xp4,yp4]]
- centers = [[x1, y1, xp1, yp1, xc1, yc1, R1, CW1],
- [xp2, yp2, xp3, yp3, self.glob.obstacles[nearestObstacle][0], self.glob.obstacles[nearestObstacle][1], roundAboutRadius, CW],
- [xp4, yp4, x2, y2, xc2, yc2, R2, CW2]]
- price = self.check_Price(x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers)
- number_Of_Cycles = self.number_Of_Cycles_count(dest, centers, yaw1, yaw2) + price
- if number_Of_Cycles < number_Of_Cycles_min:
- number_Of_Cycles_min = number_Of_Cycles
- dest_min = dest
- centers_min = centers
- else:
- price = self.check_Price(x1, y1, x2, y2, xp1, yp1, xp2, yp2, xc1, yc1, CW1, xc2, yc2, CW2, dest, centers)
- number_Of_Cycles = self.number_Of_Cycles_count(dest, centers, yaw1, yaw2) + price
- if number_Of_Cycles < number_Of_Cycles_min:
- number_Of_Cycles_min = number_Of_Cycles
- dest_min = dest
- centers_min = centers
- if number_Of_Cycles_min == 1000: return [], [], number_Of_Cycles_min
- else: return dest_min, centers_min, number_Of_Cycles_min
-
- def internal_tangent_line(self, start, R1, R2, x1, y1, xc1, yc1, xc2, yc2, CW ):
- if R1 == 0: return True, x1, y1
- L = math.sqrt((xc2 - xc1)**2 + (yc2 - yc1)**2)
- L1 = L * R1/(R1 +R2)
- x3 = xc1 + (xc2 - xc1) * R1 / (R1 + R2)
- y3 = yc1 + (yc2 - yc1) * R1 / (R1 + R2)
- if round(y3, 4) != round(yc1, 4):
- A = - (x3 - xc1) / (y3 - yc1)
- B = ( 2 * R1**2 - L1**2 - xc1**2 + x3**2 - yc1**2 + y3**2)/ 2 /(y3 - yc1)
- a = 1 + A**2
- b = 2 * A *(B - yc1) - 2 * xc1
- c = xc1**2 + (B - yc1)**2 - R1**2
- succsessCode, xp10, xp11 = self.square_equation(a, b , c)
- if not succsessCode:
- return False, 0, 0
- xp1 =[xp10, xp11]
- yp1 = [A * xp1[0] + B, A * xp1[1] + B]
- else:
- tmp1 = ( R1**2 - L1**2 - xc1**2 + x3**2 - yc1**2 + y3**2)/ 2 /(x3 - xc1)
- xp1 = [tmp1,tmp1]
- ttt = R1**2 - (tmp1 - xc1)**2
- if ttt < 0: return False, 0, 0
- tmp2 = math.sqrt(ttt)
- yp1 = [yc1 + tmp2, yc1 - tmp2]
- #alpha_1 = self.coord2yaw(x1 - xc1, y1 - yc1)
- al0 = self.coord2yaw(xp1[0] - xc1, yp1[0] - yc1)
- al1 = self.coord2yaw(xp1[1] - xc1, yp1[1] - yc1)
- if CW: da = -math.pi/2
- else: da = math.pi/2
- directToOtherEnd = self.coord2yaw(xc2 - xc1, yc2 - yc1)
- alpha_p1 = [abs(self.norm_yaw(da + al0 - directToOtherEnd)),
- abs(self.norm_yaw(da + al1 - directToOtherEnd))]
- if start:
- ind = alpha_p1.index(min(alpha_p1))
- else: ind = alpha_p1.index(max(alpha_p1))
- return True, xp1[ind], yp1[ind]
-
-
- def square_equation(self, a,b,c):
- D = b**2 - 4 * a * c
- if D < 0: return False, 0, 0
- return True, (-b + math.sqrt(D))/(2 * a), (-b - math.sqrt(D))/(2 * a)
-
-
-
-
-if __name__ == '__main__':
- class Example(wx.Frame):
-
- def __init__(self, *args, **kw):
- #super().__init__(*args, **kw)
- super(Example, self).__init__(*args, **kw)
- self.glob = Glob()
- self.p = PathPlan(self.glob)
- self.f = Forward_Vector_Matrix(self.glob)
- self.isLeftDown = False
- self.InitUI()
-
- def InitUI(self):
-
- self.Bind(wx.EVT_PAINT, self.OnPaint)
- self.Bind(wx.EVT_LEFT_DOWN, self.OnLeftDown)
- self.Bind(wx.EVT_LEFT_UP, self.OnLeftUp)
- self.Bind(wx.EVT_MOTION, self.OnMove)
-
- self.SetTitle('Lines')
- self.Centre()
-
- def OnPaint(self, e):
- self.dc = wx.PaintDC(self)
- #dc.SetBackground(wx.Brush('#1ac500'))
- #dc.SetPen(wx.Pen('#d4d4d4'))
- self.SetClientSize(800, 600)
- self.dc.SetBrush(wx.Brush('#1ac500'))
- self.dc.DrawRectangle(0, 0, 800, 600)
- pen = wx.Pen('#ffffff', 10, wx.SOLID)
- pen.SetJoin(wx.JOIN_MITER)
- self.dc.SetPen(pen)
- self.dc.DrawRectangle(40, 40, 720, 520)
- self.dc.DrawRectangle(0, 200, 40, 200)
- self.dc.DrawRectangle(760, 200, 40, 200)
- self.dc.DrawRectangle(40, 160, 40, 280)
- self.dc.DrawRectangle(720, 160, 40, 280)
- self.dc.DrawCircle(400,300,60)
- self.dc.DrawLine(400,45,400,555)
- self.dc.DrawLine(390,300,410,300)
- self.dc.DrawLine(210,300,230,300)
- self.dc.DrawLine(220,290,220,310)
- self.dc.DrawLine(570,300,590,300)
- self.dc.DrawLine(580,290,580,310)
-
- self.dc.SetAxisOrientation(True, True)
- self.dc.SetDeviceOrigin(400, 300)
-
- pen1 = wx.Pen('#000000', 1, wx.SOLID)
- self.dc.SetPen(pen1)
- self.dc.SetBrush(wx.Brush('#ffffff'))
- self.dc.DrawCircle(self.glob.pf_coord[0] *200, self.glob.pf_coord[1] *200 , 0.1 *200) #robot
- self.dc.SetBrush(wx.Brush('#000000'))
- for obstacle in self.glob.obstacles:
- self.dc.DrawCircle(obstacle[0] *200, obstacle[1] *200, obstacle[2] *100) # obstacle
- self.dc.SetBrush(wx.Brush('#ff0000'))
- self.dc.DrawCircle(self.glob.ball_coord[0] *200, self.glob.ball_coord[1] *200, 0.04 *200) # ball
- target_yaw = self.f.direction_To_Guest()
- dest = []
- centers = []
- number_Of_Cycles = 1000
- #for i in range(4):
- # target_x = self.glob.ball_coord[0] - (0.11 + i * 0.05) * math.cos(target_yaw)
- # target_y = self.glob.ball_coord[1] - (0.11 + i * 0.05) * math.sin(target_yaw)
- # target_coord = [target_x, target_y, target_yaw]
- # dest1, centers1, number_Of_Cycles1 = self.p.path_calc_optimum(self.glob.pf_coord, target_coord)
- # if number_Of_Cycles1 <= number_Of_Cycles:
- # dest = dest1
- # centers = centers1
- # number_Of_Cycles = number_Of_Cycles1
- for i in range(5):
- for j in range(2):
- target_x = self.glob.ball_coord[0] - (0.21 + j * 0.05) * math.cos(target_yaw - 0.8 + i * 0.4)
- target_y = self.glob.ball_coord[1] - (0.21 + j * 0.05) * math.sin(target_yaw - 0.8 + i * 0.4)
- target_coord = [target_x, target_y, target_yaw]
- dest1, centers1, number_Of_Cycles1 = self.p.path_calc_optimum(self.glob.pf_coord, target_coord)
- if i != 2: number_Of_Cycles1 += 50
- if number_Of_Cycles1 <= number_Of_Cycles:
- dest = dest1
- centers = centers1
- number_Of_Cycles = number_Of_Cycles1
- print('number_Of_Cycles= ', number_Of_Cycles)
- #print('centers =', centers)
- if len(dest)==0:
- print('Impossible')
- return
- #print(d)
- for i in range(0,len(dest),2):
- self.dc.DrawLine((dest[i][0] *200, dest[i][1] *200), (dest[i+1][0] *200, dest[i+1][1] *200))
- if self.p.intersection_line_segment_and_circle(dest[0][0], dest[0][1], dest[1][0], dest[1][1], self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- print('intersection with line')
- for i in range(len(centers)):
- x1, y1, x2, y2, cx, cy, R, CW = centers[i]
- self.draw_arc(x1*200, y1*200, x2*200, y2*200, cx*200, cy*200, CW)
- if len(dest) == 2:
- if self.p.intersection_circle_segment_and_circle(dest[1][0], dest[1][1], target_x, target_y,
- centers[1][0], centers[1][1], CW, self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- print('intersection with circle')
- if len(dest) == 4:
- if self.p.intersection_circle_segment_and_circle(dest[3][0], dest[3][1], target_x, target_y,
- centers[1][0], centers[1][1], CW, self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- print('intersection with circle')
- if self.p.intersection_circle_segment_and_circle(self.glob.pf_coord[0], self.glob.pf_coord[1], dest[0][0],
- dest[0][1], centers[0][0], centers[0][1], CW,
- self.glob.obstacles[0][0], self.glob.obstacles[0][1], uprightRobotRadius):
- print('intersection with circle')
-
- #dc.Bind()
-
- def draw_arc(self, x1, y1, x2, y2, cx, cy, CW):
- self.dc.SetBrush(wx.Brush('#ff0000', wx.BRUSHSTYLE_TRANSPARENT))
- if CW: self.dc.DrawArc(x2, y2, x1, y1, cx, cy)
- else: self.dc.DrawArc(x1, y1, x2, y2, cx, cy)
-
- def OnLeftDown(self, event):
- #dc = wx.ClientDC(self.staticBMP)
- pos = event.GetLogicalPosition(self.dc)
- self.isLeftDown = True
- if math.sqrt((pos[0]-self.glob.pf_coord[0]*200)**2 + (pos[1]-self.glob.pf_coord[1]*200)**2) <= 20:
- self.moving_object = -1 #'robot'
- self.dx = self.glob.pf_coord[0]*200 - pos[0]
- self.dy = self.glob.pf_coord[1]*200 - pos[1]
- elif math.sqrt((pos[0]-self.glob.ball_coord[0]*200)**2 + (pos[1]-self.glob.ball_coord[1]*200)**2) <= 8:
- self.moving_object = -2 #'ball'
- self.dx = self.glob.ball_coord[0]*200 - pos[0]
- self.dy = self.glob.ball_coord[1]*200 - pos[1]
- else: self.isLeftDown = False
- if not self.isLeftDown:
- for obstacle in self.glob.obstacles:
- if math.sqrt((pos[0]-obstacle[0]*200)**2 + (pos[1]-obstacle[1]*200)**2) <= 20:
- self.isLeftDown = True
- self.moving_object = self.glob.obstacles.index(obstacle) #'obstacle'
- self.dx = obstacle[0]*200 - pos[0]
- self.dy = obstacle[1]*200 - pos[1]
- break
- #dc.DrawCircle(pos[0], pos[1], 5)
- a = 1
-
- def OnLeftUp(self, event):
- self.isLeftDown = False
-
- def OnMove(self, event):
- if self.isLeftDown:
- pos = event.GetLogicalPosition(self.dc)
- if self.moving_object == -1: #'robot'
- self.glob.pf_coord = [(pos[0] + self.dx)/200, (pos[1] + self.dy)/200, self.glob.pf_coord[2]]
- self.Refresh()
- if self.moving_object == -2: # 'ball'
- self.glob.ball_coord = [(pos[0] + self.dx)/200, (pos[1] + self.dy)/200]
- #self.glob.obstacles[0] = [self.glob.ball_coord[0], self.glob.ball_coord[1], 0.15]
- self.Refresh()
- if self.moving_object >= 0: # 'obstacle'
- self.glob.obstacles[self.moving_object] = [(pos[0] + self.dx)/200, (pos[1] + self.dy)/200, self.glob.obstacles[self.moving_object][2]]
- self.Refresh()
-
- def main():
-
- app = wx.App()
- ex = Example(None)
- ex.Show()
- app.MainLoop()
-
- main()
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Vision/__pycache__/class_Vision.cpython-39.pyc b/src/azer_robocup_project/Soccer/Vision/__pycache__/class_Vision.cpython-39.pyc
deleted file mode 100644
index cdf9e7f7de04322fc2b066440f878550797dc0b1..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 1732
zcmb7E&2Jnv6t`zQv$MPTf<_G~5)GfyT(Tu_KnNjFtF}R9k(w05sK}ZfI~ynSk=iqj
zx|+SPxpU*dX>;U);2!|~ggyJpY5xGu@a)aUrsRT0e)H`2;pg{$&++173&A-4WsBWz
zAoQ16%nuh9pTSfg0bz*Y6h-I(Vh(dp5OYUPgt^1Wp(8)!n0X@{xtwtKz+?V88et3;
zJ1A^S!5VcFL+8hZ_xE6zqX;t;ISe!B1Vt`$8G%1xZ5FWR364D00$&T-|5_+f<cTxq
ziU1$LybDwP38cm~qNqj(=m0Ye@$Hw6UAfFzvyuO>F?32Y_G)MBpQ2Nv-}naZNZ=c}
z809PPBlX1{gzfh&P)3-S23d0m(%J?6X1;sj)&Cz}$N%x#n)k|($ys-5E;x91I~OHA
zie;Rs&SM^@meAdO$<=<5GPQZ2id<tUF{p=F-{@5$Wi%Bzr;7J8o|kIy_Pm!Jk#%6c
z>?3Sz(oc(?ZtZM;`Q*{(yW3xNH7SmGu5Tull;DUS7xIuYA$d|3@`Wb$Y%eWhrZkZ}
zX4=_(7_G`US8;MR2I)b3(e3L*REexg;}WVG5=|6O_oNTn8t+-FpOIXF<=+&@-H)F>
zQCzA{+__gImAzi)fhZqVz0TvJR~(A6GZ{~QpZo^%r^&CAKep~{eXzlVI@}Oh+~=L0
zB1yRH%uw_+Nn@qxY_WOtLI;!zlW}SwrB;C;?BfNziZ`7FCve~otHJV_#kqE3Zg)Nm
zZ{oLTG?CY6Y!K-HYRD;p>f&yQqvl{g&igwR&qGfxftq$j)`vV6$4WPnBCWDqMJuA@
z(&k1%LNig?T+GNT#waNFMV^bie*;#^cEx3U=d$S=Ae@@O?q3PD4LePkHq;-1nQNKv
zYr-68om<QudO2Za=rKI>nKx|Ic<k8gc(q?QPQ78!N5f_bEnAX07-8m*+<WM|tzoNf
zLW>1;YaJQN+%<H@;R~)|xIQ_B=i=F;d<Bx_UnXnVtecRqpexO5u{$*=w@{y&muqUO
zG&Pd2VId>o6+_YT8OqM_uRYl4bM2-8W<NdxOf!qDux%4BP3od0P&%aLd1pmSwuka6
zo8>)fS70bDs#Vmo3!|Z;CgabAEm5=z>OD{@_UL8ZqNa?G&+HbjG^WPl<#MhvUP60W
z^@|3+3Gu00KoEd$2^0GT*#Cz(1faR)tl+`xXOO(kz8ku74XovBK<<aWfq5QhoKoGQ
zG%Hw@8h(M&XH}feB=S{bU{02A8e)JvM-29)L7U0GGH?%YupGE{&-l+`ky9$yK{99;
ywg4NwI88%en$lT4`4(^*KiA%o6nQBZjo_MtbnwY6$WKlEm3is|XXza{5&sK%P_#P$
diff --git a/src/azer_robocup_project/Soccer/Vision/__pycache__/ransac_line_segments_simulation.cpython-39.pyc b/src/azer_robocup_project/Soccer/Vision/__pycache__/ransac_line_segments_simulation.cpython-39.pyc
deleted file mode 100644
index 9061391fb306883c22ca4baa6526676ebb78bdc1..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 3476
zcmcInO>7&-6`t8YE|))|C0e2+$6dI87NA(R>>`HY6!8z-#6<+dPFsMm%Pu)9a!qnc
z&Mqy{m~D$H$gP3UV^N?|(L>uqk=$}hdn$VLd5<knR96GN<WTgRSyGk?BR~%ov2W(h
zym|B9ykB{xl0)#f^T$o5FB0;1iJbg1fV_h*`UrrLI$@M)JzA$~)ashA9q5OoZurK5
z$#iBMYM9ff7Bjm#f8ZO;cniCVI#;(bTD^QfU(Zt#*3_y~chp#@7t~l(R9df8FR>i6
zKP2@sE3+aieMswbOnXGu=DfdBLY(&pY~1tRAaa=>1?|vhZgb+^x^&xXdj2zan-BW#
z`|myYz<m^Zyc@)B?0bFZUJ$Y%Y`eW6^douT;jOJlx9+%JuRY+CxYKuo(7nC!1GgE3
z9-p{D-)sBMI0C%kIL&d;V-0z>5&7-DAI8x|mwB<bu3#eyp8M-A_rh)??r=Zq40_Cc
z+r4^uU0v7+$9<gcLzuVSZ>>3wJnFvbj)O414q+f6=ibEK3F3|$b-bbPUJTba*Vf&k
z*Xm{p?ygUO1w3*G+wM*$h<$e$?E1Y(F;fz_?oBrYTin+4xuhFM{`R=nQ$jlbB_0Tw
zAN#Es{Ige4*DIzo7Ck?T+_>X~0C?~z<DCF3n=nB;9B`jG>Rn!`2*#PN5a9TRS2C%)
z9Onp*MYg`6kH4TB&&{isu3fskMpFwyvO&Kk{cra71|~Utcko3|vBPKr&q_>u7QURw
z39U=^HEuI4v6-IaMV=X5gWr!yO!tV;t7MNRjv&kwPL(igi7b;OKe_<gf+#53pPiyD
zD%#>PZ9zDq*wy%!JTcmuaRQgpj1u#*D2Xzte#df&9X^OPk>8_$g*{p$7)uH(OKg=S
zbJ<R|UX}Y9HTJ9IHS`tedM4I|zDMtpCx1)k)y?xW8KoBclwa7VBSXv!r>pJLNX{TP
z)^DhF`2sirSr<;9PO8EN)iyL(sFCDs*b#GLp-Mz$iHIsz7R4N+iql785&Q3o1yL21
z8WD4=kTzKoi_`s2#L{dAiO+`T#iCe(e#jJ<ErREg#Fxk+#VeeREpc{_3JMESZiq9?
zdW$4yl(c6=rqjQLlbE8ISmI1#h@7IxjVek5Q)!b~!BX?`Y<*^Dg=24u7cwisUmpCQ
z>|`g%amg~vCo33NXExiXPcFQ&+G}l4RaVsaKlioKXRsZzDddzhYdW*{bzVL8r3L9r
zUt%qtU;9#P>HLRNYhm_3S<8SNA1f&rL=Cpn`LD!6QiE44!w+@-2Vl$akQKG}nW!c!
zz*g1XUx2NOD%0UxQ;(FkxrrEt)>6Zlu+a*>)wYqG3x{G^)Zl5WVx>m9=Dxu%i5enr
zIVs2RKT*s46fvGx`0_Dc2rr5i<b&0ufSr|@zk;#^eDwq_V>d6#q9AV+=Y+8>YmT$U
zQFF+>qn`pE{X_yy0=f9X(XZun2VgCi>d56>rmfW6?hU*+HId5Neroi+xRV;uh{vh!
zHKVmRQd6e?D`}qj&jK$F24Pyw(qm(=-9TdJYB2%<=u~D;ou(hfji`luTQ6mEuRrYh
zjVq_o&AK_{gC>Zc`8*C<UQbo6RGVC>+tKt0u+3v^Gu3vl;M+{8m(pfxs!LKDq;xl>
zlazK+&AVFHnpf*)KL{JU7`$CI0GnjkBrSyg&a5{6p75{YA^yby?%a6%y~yX$hPQEd
z&>E}Kx$&JKes|p5csOVdP@p$n9K87a#i#gw`Qnoof7-mfdDUe>)Xi$}#-l;2<@1g2
z2N7QCF&;JYTBl1fo~S?WsYkmsgeX%QDBt=DM2LP1Kn#OsKSzx=Ez^0-<X8c;Xc51%
z=F-bpEz%WQ1FX=BR-q2H09WXF&=!Hs_{bBW08Mr(TbDEyP*!Q})Y*BgWxpD&YVvLa
zbn{v^R&a`c9daI>m%s$Lcd80juwvm;Rqz+^KSZm6dS?oqb5tcOCQm6d05xX*TuXAX
z&az5zFR?MVRMj|#SRf+5OoY8m4va&Bs_5`*l#CvZt_d5tl?uCxNJK15=PV~9b96EM
zGqZ7v&QPW9k|&#}oCH;|!1PsAS8!DGMKynFFdKc~kA(%A!ZJZdw?@F2(<S`#HhDyp
zv^lJx)@^GusST3k5_Es+o=p1tC#1i5Qu;Pwh2K!z@^UISGW@aJGTSS;aYIhs{&MsW
zVI{>jDmL=5rgXTEtYD&hFoY?rJ>#*Kl#@B+ldKkXC@RwEV^KP=MEQ_XB8p<}I!TJ6
zi25e0)$NP|RJTP5*`m0`WutN=d+VbEfZ!b%3T=(a;>plI+QkyLC6M_eAm0U<Oc`ss
zT6hJJPqc8Dn$LYch*DGaiM#|#q>yl;{3Nx!;m{A+S_P@6-%iW$`V-w}T0Y&CrG+ub
zxeA<!&r2B=B&bTJhD<bEIv$q+!wV9~wyR!1GaJkH6%A!NuUO6+;D$PMK~V)^F935X
zf(rc%d<}c)!fSeGnMb4B0Aw0V1|lVkCI@2`F;S!!WVC7LKp}Uk_#};tmLj4?Mx_l(
zejQx!e&4{rzX@;;50Yy%*r3&Dq<S=oQVmaNwf(rY!_vjpm~*u6jhzASHdw&X|Dyj-
zbKD>Hyq2HlkT+$bNlhp{obWd#UzOJN;iQ>bksn3_o*KwEO<tCpGH8_)Cp4Gok1O@<
gn|<~F%D1J(qZ)uws%SQJkn$t@KoeWH4f{v`0r!G)SO5S3
diff --git a/src/azer_robocup_project/Soccer/Vision/__pycache__/reload.cpython-39.pyc b/src/azer_robocup_project/Soccer/Vision/__pycache__/reload.cpython-39.pyc
deleted file mode 100644
index fd85957da0d93e60b528a1f6eb51d35dc4605416..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 7448
zcmbtZTWlOx8J=_Iva?q^j^j8@D4{K(Ta`F*=|xp(oj8e8H8_>+Htj0avfeY^9ea0n
zeP&#H&8%9<MMxDzK|%=i$p#6L7X&=<RNfFzRm1~A0}=ur(7vHcJaYN|GrQ}{ZlIyF
zI{%!@KmWP?_bp|!DGiU<epUSFVNLrhH4Z*H8Z#)q1Q42FHLVc8S%IlGFL3qN3&xx#
zxX|CzgucTICR&Ct(K6Ap(6U4Vtpr*Lv}}<?D~XmZQX>7HR!BN2ky+!?bCNq6!(5A6
zKHI^(+I*Sd!H1(UgW@j%I2x@5>tKR|jxKaz;EmaZg|{gZ!p7SYNs+=kA<`m)w=J?F
zhj&tph*7*#;+Po2J1vf5=JCk5Tx%?sElkjRs7^D_qWF&hge=sGTF3!er~~rQ0MtVh
z&=A_9c9+fP&4_OmBi=&UM5z`dqa>YD-ii$0sjWx@l%z?3CXfjNHo#p?((U}?O@P_w
zZ@=a{(w{0#U1^jz>W&xqQ*&-`ePelQp|RXpbAzdSyZ1i2_j8mF@BQrFucj|dKXq2P
z{@Pi$UaB}#i;Z&GkyEd`zT5C_OQ+T-iSx}?WEYFB=LW?h&Fd?6U{?3HouxB4-@_;~
zp!6Q9Q&ERO3lFtbY*-S@<eA#kT_%%gsa?b%jb_BQa(MZ4k{a8#)D&9}LR+0x7GDog
zZXFw-+<Fko>H>yR-W(sGy!jxM)vXStTs=8JxjI0Zcto8vEhC__2J5YY<xguTwH=7c
zT|VET6V2<9?s`GYRQKHuFK<0`VAh1XgYw`d{Lg9Sn3_}4vSJ>-??!pK^|`@02e0SQ
zoWxYU&El=E7(Es<hRV&b6E>t%4$dB!I;plxy#wd`UpSgXQVyeUHQg<z=0AF1hP0X?
z?w`+RNXWcW-6<BMRIylZh>aT6GsWWiMyVE$$PCdZ$&e!iMhOr*%M%192oR^pNdhFN
z2LQ>Hr0b^}_vub-W+!a@;7^U0F?8smk7iK(D*&HelR|TjM#c@-b3Uu)j1`F)>01|z
zk=`P(HBB`DE2ug_shGE7{X%RdVZJo^c>-q%e1X9JDX9aZ6aEs)fR>~6{$E;7%{+`J
z_}0bqG!q>!lw3m1uyyf4_)TqSDDBq8ivzS<4@6s?(@@%5(_a~&y*2$H>pqL`hsp_W
zO+Pz8yY)b{)vXR&x2ZVd`2*xr>d001!0JD>iW$aP!=^WC;GnM_m_4axr{4K|N}i%4
zA%!bX6Ht==2-Qeu$j1nLk-#|u#GwZ`SMji7-!m8>44;(5VfmIcHXz%y%^4K`I>3Nz
zA6Fh3gP){)G5ATbA7+V+$ifdwh@2R~+ZN->Gh*<ROvv|%$b6YRin8KuP^O3KRAy1s
z`a-s=?Ll=H;hObSHjj*Q11^W|)+>3gWH&NUFCN%#Ur;X*B{c=x&Z!Ce5_(ONnw~F{
zM(#bN6{)Qj@Oz9`w6+~4ga(gtoUBMY={=*YNkXtw8b)HsPr-nGlcTMx-WVEj?+QlS
z$&g*q-g>{C3RsvTEMbnr<f0BNVV%@aOT@Jmy`2vDu8tm1C%iqv1C=hG-N(xW#;zGA
z17p?N<LxY3i7*?c!_1!E&V}$*vF?x2Ylpc#h8}x2Nj0Xe&xbaxVmGy?wMT+<Xzwt=
z){Hw0`?TQ$V>hQXtaz+FitjVwNH~faM((h64ee|=B2t(&hk1^L$7m1jv2bj4glM*p
z<J883QNhCFI~+8=(>@W7g(rZ2Or6C<I1%zS6LVb&@Rb@lP9xp=_<{O~1N8}2pV%?m
zC)J(kpz!PPq@k@+8#6x(?kS#BJ(Ka>sGiAi9Q!^=`^NgOgyZ19^n8b|zoP>vMv%u*
z``e0ojG4~6G)S!9AwO}cLtK$RC7-}cxhauRF9lVwqT|*gz5MocWS8F#W*fDJjIy&g
zZ`@ocUY=W+zH#YtlyJR;l2>t}^s*aly1rBN8bM^0noY+OG7pN8UUR(t;gTn!Gv$T{
z^A|W`woz|3JaW}<xj}U?C<T6G&AwiIYI-p;{c5S{M5a{qGqbbB#idJ2#Y+oUFDXME
z^}B9dy|xskdk9N6U&SiB*tcH3vUEMl^_thOzC3q*Y2Po(PbwKJ=t0$wMq_WJ7*wU>
zR~t1E<tPd{&^GIi#9pm>Nmg7hvRv<NM+T8D8*XIQOa7W)upq6}<UzGy)f$`lx{#>4
zl`1NcRVyuHoX*!@E2LIjuT(2iJ!199YHS3}jR5;r$H4vV$XrHn;TMwl%&8T<jk=0@
z3byBL7HQ~+R)~nAP$SA&@h{{2HD)lVLjDZkIrccu;b*fH&lnk&W20=8j~X^laf9br
zj;GL<!7uku!%DF+{BnGIG*&MA#1APp3Sh{oQhMbhRN#C=-XO<ZX#u7*f&neSgo$3%
zpapoqb~$>i7!O*2DIF2xkrs&Y^g!Q5#JFk(y3zv%+6lyvJk%kdW@xV3Z7WEwrl1i>
zUs;M`Vns(^S|mT<?L?4)%p<CXv_1@tXYOX9cA@u355zFESM(RO^$)`Yq$f2W%M5uZ
zN^s;9)h-ZtlE9Y<kbjY9ktu>!)5+&z_x>pwA@4vwP2d><Bvn1BS(v*_QX`+EftP52
zd(Ft)c4Whk%nd{(&&e4YBf-fhW8dv-gr|d*B7^uy&J)5->f}CH3cR{e3nJzoWd>^*
z>?K<*U@CuC({cT{@Eo`J!?^4-d^;0gY>arw2OuNUHx@rZBk0Fdl)@0k2S%HrPp3<T
z@<SrwG!hb&NWh_6Z4Nc5Z{H5-&==Z9Xo%D)?Y7Z2+g3Y4mk=hPzHf^ZjLt-0;Y#h$
zn#8q)=A?ESMxmVy2+Kl`9VVd`Q=tw;mr@G;`CFPdW@tgG^+Sdbn`v)q9>1W4daV4W
zvEI)20n}C9WrYkBi3~QRv*_Zi=@L5>j&&X<Kwcy=5W1{3lzR~+`wF0tt-BuiDK{Li
z0zppIOIy^OD>W65<kRY+BXhRod9C=W)7Kjtl`3KRugW(-JK{}O5@#T(5tKaTh)D9o
zqU6=BveOKZZ2%z>qsVkb#Q`gjTjWP0@!giZpr|@x$LA3s4s(klr4|VBC?||5$)%B^
z?^IM~<b4qF$s^FrapR0`=p5<+yul_o6apLL+mi=`{?Mm~_=IAxKA*e+XC%;eRoqRy
z0i#MhVt_}Gc<OQuM~@kDaE-N^P+S9^0XOO37?|koe}(!Ln6kG%A>kHDC<2sj4Lrm(
z;-G+xePWL(C*a%Q5=vdAR}65N-A=Ys?erbyT?SUFkM#>+WC-&J))~y3fs<hMXr6}a
z($0!xm^CzATVwK%A`SS7XE1P`NW#^~z-h>Z*$O9aq;Ey0H`ll}sojqfO!^Z<3-6gO
zwahSTJz8odIjkfXuLR$%A^}L>zuR9;cF1bdy%ls<qfTK6wO@9recYv%ZXczFcK;6I
z8twpnfa+f28ucY0qay*%=2PNBvUe<|g~|a_Qhg0lJr1cJxx-rvhvXV5jeK@qmcVRs
znSfHkWvY>9(;@RC1wdYx6{@Q+p-MG!gkp*RJ8JF>+WFC=>N&UX)XXhhdQ-YUjZ*u9
ztmIg(5h*3ex)1BGmm7Yp_DPtPSVv}4QfjG=4$PuN+Nj7<OL~OTAkZYBBAES^Dv^GS
z1|B6qkq+gkM4acKU`DPJP?3%Tq@TOyUa5v3qwT*4pc#-}V-n`vfRTR)CjT62V{8&`
zK#rYdXViPF`|h<buxUvD_+eA82p=-_bj}GBW$H=G#xV?}j<@wTw0po*GK!OVUZGU4
zxq=KF$^Hi>64r=w^2R(aNx7&01_9Y7KpGjw6pGg^uR-(qKv74e%qUsVULr&F4%2`~
zjwUH&X;W#<-{NbZoLvn&+#ZRwT7UGe9<M;L34yXssNIM5+TSnP^^jq&YNv2Ix|$AY
zZ3BBnN*gYY5hC0{3%lOy?zoo#q`gE*%BjJjtvSkC<P))tr?gdMmdV2&n&3LJyPR5o
zpVaU0b$5#ywmp7ei+z;55r>4==8<`R7x?#yxnIMO{5k<iASCb(fgJ+-%TP5HiINSS
zLFvWZdg^KWn0>~6(l#WGl~E2o9IY7?pCa;sn7jYa1X#t2tVPH38}bkn()V;pvqXbT
zSo|M@@+T3+!jr`R3;Q9p5!ov)LJ%YuV`B{u9bUbN&p(c_3&|eJ&_MeSG*iq%7FZ}=
zQ$N-|WDkjj`impMjWf}^d{5<|<CLAQ4n{3Nd3r;0LALLeng~OJKhf;8X=KTIbN9yZ
z*U8Y-$+v(#^tRr^K!l>UgRsDm)N|;LC?@PPJuyH4#||bDsft~Oh#H>yE|1MQ{eQG)
zw!!Wynuaro$R7Us9&Q0~__@H`#eZ5MTTSe77)lszVJnkq?xsW%<~ub5qaPYUTBLUv
z9721&xo+dD%pRONr7kk_+c{ZTE-oWVa)R@#%}U<tJc?B*YtW%YaOahNllI;D4K)S8
zQis?yN;H;NA$5M`8{<En`}6kqUW(m#(mWAc&u7&UNOfe2{deell}J`13x2^hY{uT5
z5x8~4q<+nDno(l2<OZ)eNY^x^h>i)z4`ictsaCrM-QC#q;Z<6`V&M3$muN<V0=I&(
z=ClUFH-v?*_ZJ}f695`+$JWQV%|{am%n-I^n8D=_hzPRfgj+0jJy2zq<acSvM3~rY
z#TJXWCiXW;661Z+_c6~5k><#LA<n(ONQ|tO0VYxLWA1G3q1-##>6~ppY_tCW6Qr%}
diff --git a/src/azer_robocup_project/Soccer/Vision/class_Vision.py b/src/azer_robocup_project/Soccer/Vision/class_Vision.py
deleted file mode 100644
index 91a2e84..0000000
--- a/src/azer_robocup_project/Soccer/Vision/class_Vision.py
+++ /dev/null
@@ -1,42 +0,0 @@
-import json
-
-class Vision:
- def __init__(self, glob):
- self.glob = glob
- if self.glob.SIMULATION == 2:
- with open(self.glob.current_work_directory + "Init_params/Real/Real_Thresholds.json", "r") as f:
- self.TH = json.loads(f.read())
- #from zhangSuen_Thinning import zhangSuen
- from ransac_line_segments import build_line_segments
- #self.zhangSuen = zhangSuen
- self.build_line_segments = build_line_segments
- else:
- with open(self.glob.current_work_directory + "Init_params/Sim/Sim_Thresholds.json", "r") as f:
- self.TH = json.loads(f.read())
- from ransac_line_segments_simulation import build_line_segments
- self.build_line_segments = build_line_segments
-
- def thinning(self, img, rows, columns, iterations_limit):
- return self.zhangSuen(img, rows, columns, iterations_limit)
-
- def detect_line_segments( self, img, rank_threshold = 30, line_num_limit = 5, upper_lines = False):
- if self.glob.SIMULATION == 2:
- if upper_lines == True: upperlines = 1
- else: upperlines = 0
- deviation = 2
- number_of_iterations = 10
- raw_lines = img.ransac_segments(rank_threshold, line_num_limit,
- upperlines, deviation , number_of_iterations)
- line_segments = []
- for i in range(0,len(raw_lines),4):
- line_segments.append([raw_lines[i],raw_lines[i+1],raw_lines[i+2],raw_lines[i+3]])
- else:
- line_segments = self.build_line_segments( img, rank_threshold, line_num_limit, upper_lines )
- return line_segments
-
-
-
-if __name__=="__main__":
- v = Vision(1)
- print(v.TH['orange ball'])
-
diff --git a/src/azer_robocup_project/Soccer/Vision/ransac_line_segments.py b/src/azer_robocup_project/Soccer/Vision/ransac_line_segments.py
deleted file mode 100644
index af62dc8..0000000
--- a/src/azer_robocup_project/Soccer/Vision/ransac_line_segments.py
+++ /dev/null
@@ -1,294 +0,0 @@
-"""
-Module is designed by A.Babaev from MIPT Starkit team
-Finding lines by RANSAC algorythm in B/W binary image
-usage:
-
-build_line_segments( data, data_size, rank_threshold = 30, line_num_limit = 5)
-
-data - uint8 bytearray with shape (n,2), packed by x,y pairs of white pixels
-data_size < n - number of usefull data
-rank_threshold - detected lines with pixel number less than this number will be ignored
-line_num_limit - limit of detected number of lines
-
-"""
-import sys, os
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if sys.version != '3.4.0':
- current_work_directory += '/'
- import numpy as np
-else:
- import pyb, sensor
- import ulab as np
-
-
-import math
-
-#from urandom import getrandbits
-
-
-#def randrange( start, stop=None):
-##helper function for working with random bit sequence
-# if stop is None:
-# stop = start
-# start = 0
-# upper = stop - start
-# bits = 0
-# pwr2 = 1
-# while upper > pwr2:
-# pwr2 <<= 1
-# bits += 1
-# while True:
-# r = getrandbits(bits)
-# if r < upper:
-# break
-# return r + start
-
-#def random():
-# #getting a random number from 0 to 1
-# return randrange(10000) / 10000
-
-#@micropython.native
-#def ransac_calc(data1, deviation, number_of_iterations, data_size):
-# best_score = 0
-# best_sample_1 = 0
-# best_sample_2 = 0
-# probe = 0
-# while(probe < number_of_iterations):
-# vertical_line = False
-# y1 = 128
-# while(y1 > 127):
-# sample_1 = int(random()*data_size)
-# y1 = tuple(data1[sample_1])[1]
-# y2 = 128
-# while(y2 > 127):
-# sample_2 = int(random()*data_size)
-# y2 = tuple(data1[sample_2])[1]
-# if sample_1 == sample_2:
-# y2 = 128
-# x1 = tuple(data1[sample_1])[0]
-# x2 = tuple(data1[sample_2])[0]
-# if x1 == x2: vertical_line = True
-# else:
-# a = float((y1-y2)/(x1-x2))
-# b = float((y2*x1 - y1*x2)/(x1-x2))
-# score = 0
-# for i in range (data_size):
-# if tuple(data1[i])[1] > 127 : continue
-# if i == sample_1 or i == sample_2: continue
-# x = tuple(data1[i])[0]
-# y = tuple(data1[i])[1]
-# if vertical_line: h = x1 - x
-# else:
-# a3 = a * x + b - y
-# if a == 0:
-# h = a3
-# else:
-# b3 = x - (y - b) / a
-# if a3 == 0 and b3 == 0: h = 0
-# else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
-# if abs(h) < deviation:
-# score += 1
-# if score > best_score:
-# best_sample_1 = sample_1
-# best_sample_2 = sample_2
-# best_score = score
-# probe += 1
-# x1 = tuple(data1[best_sample_1])[0]
-# x2 = tuple(data1[best_sample_2])[0]
-# y1 = tuple(data1[best_sample_1])[1]
-# y2 = tuple(data1[best_sample_2])[1]
-# if x1 == x2: vertical_line = True
-# else:
-# a = float((y1-y2)/(x1-x2))
-# b = float((y2*x1 - y1*x2)/(x1-x2))
-# min_x = 159
-# max_x = 0
-# min_y = 119
-# max_y = 0
-# for i in range (data_size):
-# x = tuple(data1[i])[0]
-# y = tuple(data1[i])[1]
-# if y > 127 : continue
-# if i != best_sample_1 and i != best_sample_2:
-# if vertical_line: h = x1 - x
-# else:
-# a3 = a * x + b - y
-# if a == 0:
-# h = a3
-# else:
-# b3 = x - (y - b) / a
-# if a3 == 0 and b3 == 0: h = 0
-# else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
-# if abs(h) > deviation: continue
-# if x < min_x : min_x = x
-# if x > max_x : max_x = x
-# if y < min_y : min_y = y
-# if y > max_y : max_y = y
-# data1[i,1] = y + 128
-# if vertical_line: new_segment = [x1, min_y, x1, max_y]
-# elif a == 0: new_segment = [min_x, y1, max_x, y1]
-# else:
-# if (max_x - min_x) > (max_y - min_y):
-# x1 = min_x
-# x2 = max_x
-# y1 = int(a * min_x + b)
-# y2 = int(a * max_x + b)
-# else:
-# y1 = min_y
-# y2 = max_y
-# x1 = int((min_y - b) / a)
-# x2 = int((max_y - b) / a)
-# new_segment = [x1, y1, x2, y2]
-# return new_segment, best_score
-
-#@micropython.native
-#def build_line_segments( data, data_size, rank_threshold = 30, line_num_limit = 5):
-# deviation = 2
-# number_of_iterations = 10
-# if data_size < 10: return []
-# rest_number = data_size
-# line_segments_data = []
-# for i in range(line_num_limit):
-# new_segment, inlier_rank= ransac_calc(data, deviation, number_of_iterations, data_size)
-# #print('inlier_rank = ', inlier_rank)
-# if inlier_rank < rank_threshold : break
-# line_segments_data.append(new_segment)
-# rest_number -= inlier_rank
-# if rest_number < 10 : break
-# return line_segments_data
-
-#@micropython.native
-def ransac_calc(data1, deviation, number_of_iterations, data_size):
- def random():
- #getting a random number from 0 to 1
- return pyb.rng()/1073741824
- best_score = 0
- best_sample_1 = 0
- best_sample_2 = 0
- probe = 0
- while(probe < number_of_iterations):
- vertical_line = False
- y1 = 128
- while(y1 > 127):
- sample_1 = int(random()*data_size)
- y1 = data1[2*sample_1+1]
- y2 = 128
- while(y2 > 127):
- sample_2 = int(random()*data_size)
- y2 = data1[2*sample_2+1]
- if sample_1 == sample_2:
- y2 = 128
- x1 = data1[2*sample_1]
- x2 = data1[2*sample_2]
- if x1 == x2: vertical_line = True
- else:
- a = float((y1-y2)/(x1-x2))
- b = float((y2*x1 - y1*x2)/(x1-x2))
- score = 0
- for i in range (data_size):
- if data1[2*i+1] > 127 : continue
- if i == sample_1 or i == sample_2: continue
- x = data1[2*i]
- y = data1[2*i+1]
- if vertical_line: h = x1 - x
- else:
- a3 = a * x + b - y
- if a == 0:
- h = a3
- else:
- b3 = x - (y - b) / a
- if a3 == 0 and b3 == 0: h = 0
- else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
- if abs(h) < deviation:
- score += 1
- if score > best_score:
- best_sample_1 = sample_1
- best_sample_2 = sample_2
- best_score = score
- probe += 1
- x1 = data1[2*best_sample_1]
- x2 = data1[2*best_sample_2]
- y1 = data1[2*best_sample_1+1]
- y2 = data1[2*best_sample_2+1]
- if x1 == x2: vertical_line = True
- else:
- a = float((y1-y2)/(x1-x2))
- b = float((y2*x1 - y1*x2)/(x1-x2))
- min_x = 159
- max_x = 0
- min_y = 119
- max_y = 0
- for i in range (data_size):
- x = data1[2*i]
- y = data1[2*i+1]
- if y > 127 : continue
- if i != best_sample_1 and i != best_sample_2:
- if vertical_line: h = x1 - x
- else:
- a3 = a * x + b - y
- if a == 0:
- h = a3
- else:
- b3 = x - (y - b) / a
- if a3 == 0 and b3 == 0: h = 0
- else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
- if abs(h) > deviation: continue
- if x < min_x : min_x = x
- if x > max_x : max_x = x
- if y < min_y : min_y = y
- if y > max_y : max_y = y
- data1[2*i+1] = y + 128
- if vertical_line: new_segment = [x1, min_y, x1, max_y]
- elif a == 0: new_segment = [min_x, y1, max_x, y1]
- else:
- if (max_x - min_x) > (max_y - min_y):
- x1 = min_x
- x2 = max_x
- y1 = int(a * min_x + b)
- y2 = int(a * max_x + b)
- else:
- y1 = min_y
- y2 = max_y
- x1 = int((min_y - b) / a)
- x2 = int((max_y - b) / a)
- new_segment = [x1, y1, x2, y2]
- return new_segment, best_score
-
-#@micropython.native
-def build_line_segments( img, rank_threshold = 30, line_num_limit = 5, upper_lines = False):
- deviation = 2
- number_of_iterations = 10
- if upper_lines:
- data_size = 160
- data = sensor.alloc_extra_fb(1, 1, 320)
- for x in range(160):
- for y in range(120):
- if img[x + y * 160] > 0: break
- data[2 * x] = x
- data[2 * x + 1] = y
- else:
- data_size = 0
- for i in range(19200):
- data_size += img[i]
- if data_size < 4: return []
- data = sensor.alloc_extra_fb(1, 1, data_size*2)
- data_size = 0
- for x in range(160):
- for y in range(120):
- if img[x + y * 160] > 0:
- data[2 * data_size] = x
- data[2 * data_size + 1] = y
- data_size += 1
- rest_number = data_size
- line_segments_data = []
- for i in range(line_num_limit):
- new_segment, inlier_rank= ransac_calc(data, deviation, number_of_iterations, data_size)
- #print('inlier_rank = ', inlier_rank)
- if inlier_rank < rank_threshold : break
- line_segments_data.append(new_segment)
- rest_number -= inlier_rank
- if rest_number < 2 : break
- sensor.dealloc_extra_fb()
- return line_segments_data
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Vision/ransac_line_segments_simulation.py b/src/azer_robocup_project/Soccer/Vision/ransac_line_segments_simulation.py
deleted file mode 100644
index 3467fa6..0000000
--- a/src/azer_robocup_project/Soccer/Vision/ransac_line_segments_simulation.py
+++ /dev/null
@@ -1,165 +0,0 @@
-"""
-Module is designed by A.Babaev from MIPT Starkit team
-Finding lines by RANSAC algorythm in B/W binary image
-usage:
-
-build_line_segments( data, data_size, rank_threshold = 30, line_num_limit = 5)
-
-data - uint8 bytearray with shape (n,2), packed by x,y pairs of white pixels
-data_size < n - number of usefull data
-rank_threshold - detected lines with pixel number less than this number will be ignored
-line_num_limit - limit of detected number of lines
-
-"""
-import sys, os
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if sys.version != '3.4.0':
- current_work_directory += '/'
- import numpy as np
- from random import random
-else:
- import pyb, sensor
- import ulab as np
-
-
-import math
-
-
-
-def ransac_calc(data1, deviation, number_of_iterations, data_size):
- #def random():
- # #getting a random number from 0 to 1
- # return pyb.rng()/1073741824
- best_score = 0
- best_sample_1 = 0
- best_sample_2 = 0
- probe = 0
- while(probe < number_of_iterations):
- vertical_line = False
- y1 = 128
- while(y1 > 127):
- sample_1 = int(random()*data_size)
- y1 = data1[2*sample_1+1]
- y2 = 128
- while(y2 > 127):
- sample_2 = int(random()*data_size)
- y2 = data1[2*sample_2+1]
- if sample_1 == sample_2:
- y2 = 128
- x1 = data1[2*sample_1]
- x2 = data1[2*sample_2]
- if x1 == x2: vertical_line = True
- else:
- a = (float(y1)-float(y2))/(float(x1)-float(x2))
- b = (float(y2)*float(x1) - float(y1)*float(x2))/(float(x1)-float(x2))
- score = 0
- for i in range (data_size):
- if data1[2*i+1] > 127 : continue
- if i == sample_1 or i == sample_2: continue
- x = data1[2*i]
- y = data1[2*i+1]
- if vertical_line: h = x1 - x
- else:
- a3 = a * x + b - y
- if a == 0:
- h = a3
- else:
- b3 = x - (y - b) / a
- if a3 == 0 and b3 == 0: h = 0
- else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
- if abs(h) < deviation:
- score += 1
- if score > best_score:
- best_sample_1 = sample_1
- best_sample_2 = sample_2
- best_score = score
- probe += 1
- x1 = data1[2*best_sample_1]
- x2 = data1[2*best_sample_2]
- y1 = data1[2*best_sample_1+1]
- y2 = data1[2*best_sample_2+1]
- if x1 == x2: vertical_line = True
- else:
- a = (float(y1)-float(y2))/(float(x1)-float(x2))
- b = (float(y2)*float(x1) - float(y1)*float(x2))/(float(x1)-float(x2))
- min_x = 159
- max_x = 0
- min_y = 119
- max_y = 0
- for i in range (data_size):
- x = data1[2*i]
- y = data1[2*i+1]
- if y > 127 : continue
- if i != best_sample_1 and i != best_sample_2:
- if vertical_line: h = float(x1) - float(x)
- else:
- a3 = a * x + b - y
- if a == 0:
- h = a3
- else:
- b3 = x - (y - b) / a
- if a3 == 0 and b3 == 0: h = 0
- else: h = (a3 * b3)/math.sqrt((a3 * a3 + b3 * b3))
- if abs(h) > deviation: continue
- if x < min_x : min_x = x
- if x > max_x : max_x = x
- if y < min_y : min_y = y
- if y > max_y : max_y = y
- data1[2*i+1] = y + 128
- if vertical_line: new_segment = [x1, min_y, x1, max_y]
- else:
- if (max_x - min_x) >= (max_y - min_y):
- x1 = min_x
- x2 = max_x
- y1 = int(a * min_x + b)
- y2 = int(a * max_x + b)
- else:
- y1 = min_y
- y2 = max_y
- x1 = int((min_y - b) / a)
- x2 = int((max_y - b) / a)
- new_segment = [x1, y1, x2, y2]
- return new_segment, best_score
-
-
-def build_line_segments( img, rank_threshold = 30, line_num_limit = 5, upper_lines = False):
- deviation = 2
- number_of_iterations = 10
- if upper_lines:
- data_size = 160
- #data = sensor.alloc_extra_fb(1, 1, 320)
- data = np.zeros((320), dtype = np.uint8)
- for x in range(160):
- for y in range(120):
- int = img[y][x].any()
- if int : break
- data[2 * x] = x
- data[2 * x + 1] = y
- else:
- data_size = 0
- for i in range(19200):
- if img[int(i/160)][int(i%160)].any != 0:
- data_size += img[i]
- if data_size < 4: return []
- #data = sensor.alloc_extra_fb(1, 1, data_size*2)
- data = data = np.zeros((1,data_size*2), dtype = np.uint8)
- data_size = 0
- for x in range(160):
- for y in range(120):
- if img[y][x] != [0,0,0]:
- data[2 * data_size] = x
- data[2 * data_size + 1] = y
- data_size += 1
- rest_number = data_size
- line_segments_data = []
- for i in range(line_num_limit):
- new_segment, inlier_rank= ransac_calc(data, deviation, number_of_iterations, data_size)
- #print('inlier_rank = ', inlier_rank)
- if inlier_rank < rank_threshold : break
- line_segments_data.append(new_segment)
- rest_number -= inlier_rank
- if rest_number < 2 : break
- #sensor.dealloc_extra_fb()
- return line_segments_data
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Vision/reload.py b/src/azer_robocup_project/Soccer/Vision/reload.py
deleted file mode 100644
index a609701..0000000
--- a/src/azer_robocup_project/Soccer/Vision/reload.py
+++ /dev/null
@@ -1,257 +0,0 @@
-import cv2
-import math
-import time
-import numpy as np
-
-class Blob:
- def __init__ (self, x_, y_, w_, h_, area):
- self.x_ = x_
- self.y_ = y_
- self.w_ = w_
- self.h_ = h_
- self.area = area
-
- def x (self):
- return self.x_
-
- def y (self):
- return self.y_
-
- def w (self):
- return self.w_
-
- def h (self):
- return self.h_
-
- def cx (self):
- return int (self.x_ + self.w_ / 2)
-
- def cy (self):
- return int (self.y_ + self.h_ / 2)
-
- def rect (self):
- return (self.x_, self.y_, self.w_, self.h_)
-
- def pixels(self):
- return self.area
-
-class Line:
- def __init__ (self, x1_, y1_, x2_, y2_, theta_):
- self.x1_ = x1_
- self.y1_ = y1_
- self.x2_ = x2_
- self.y2_ = y2_
- self.theta_ = theta_
-
- def x1 (self):
- return self.x1_
-
- def y1 (self):
- return self.y1_
-
- def x2 (self):
- return self.x2_
-
- def y2 (self):
- return self.y2_
-
- def theta (self):
- return self.theta_
-
- def line (self):
- return (self.x1_, self.y1_, self.x2_, self.y2_)
-
-class Image:
- def __init__ (self, img_):
- self.img = img_.copy ()
-
- def find_blobs (self, ths, pixels_threshold, area_threshold, merge=False, margin=0, invert = False, roi= None):
- masks = []
- if roi == None:
- imgroi = self.img
- else:
- imgroi = self.img[roi[1]:(roi[1] + roi[3]), roi[0]:(roi[0] + roi[2])]
- for th in ths:
- low_th = (int(th[0] * 2.55), th[2] + 128, th[4] + 128)
- high_th = (math.ceil(th[1] * 2.55), th[3] + 128, th[5] + 128)
-
- labimg = cv2.cvtColor (imgroi, cv2.COLOR_BGR2LAB)
-
- mask = cv2.inRange (labimg, low_th, high_th)
-
- if (invert == True):
- mask = cv2.bitwise_not (mask)
-
- masks.append (mask)
-
- #cv2.imshow ("a", mask)
-
- final_mask = masks [0].copy ()
- if (len (masks) > 1):
- for m in masks [1:]:
- final_mask = cv2.bitwise_and (final_mask, m)
-
- output = cv2.connectedComponentsWithStats (final_mask, 8, cv2.CV_32S)
-
- #labels_count = output [0]
- #labels = output [1]
- stats = output [2]
- #labels_count, labels, stats = output[:3]
- sz = stats.shape[0]
-
- blobs = []
-
- for label_num in range (1, sz):
- area = stats [label_num, cv2.CC_STAT_AREA]
-
- if (area >= pixels_threshold):
- new_blob = Blob (stats [label_num, cv2.CC_STAT_LEFT],
- stats [label_num, cv2.CC_STAT_TOP],
- stats [label_num, cv2.CC_STAT_WIDTH],
- stats [label_num, cv2.CC_STAT_HEIGHT], area)
-
- #print ("append", area)
- if roi != None:
- new_blob.x_ += roi[0]
- new_blob.y_ += roi[1]
- blobs.append (new_blob)
-
- return blobs
-
- def binary (self, th):
- #low = (th [0], th [2], th [4])
- #high = (th [1], th [3], th [5])
- low = (int(th[0] * 2.55), th[2] + 128, th[4] + 128)
- high = (math.ceil(th[1] * 2.55), th[3] + 128, th[5] + 128)
- labimg = cv2.cvtColor (self.img, cv2.COLOR_RGB2LAB)
- mask = cv2.inRange(labimg, low, high)
-
- sh = mask.shape
-
- result = np.zeros((sh[0], sh[1], 3), dtype=np.uint8)
-
- for i in range(0, 3):
- result[:, :, i] = mask.copy()
-
- return result
-
- def find_line_segments (self):
- # - Почему Колумб приплыл в Ðмерику, а не в Индию?
- # - Потому что он плавал по одометрии
-
- #gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
- edges = cv2.Canny(self.img, 50, 150, apertureSize=3)
-
- #cv2.imshow ("a", edges)
-
- lines = cv2.HoughLinesP(edges, rho=1, theta = np.pi / 20, threshold = 50, minLineLength =40, maxLineGap =30)
- #lines = cv2.HoughLinesP(edges, rho=1, theta = np.pi / 20, threshold = 10, minLineLength =40, maxLineGap =30)
-
- resultant_lines = []
-
- #print (lines)
- try:
- for line in lines:
- x1, y1, x2, y2 = line [0]
- if x1 == x2: theta = 0
- else: theta = math.atan ((y2 - y1) / (x2 - x1))
-
- new_line = Line (x1, y1, x2, y2, theta)
-
- resultant_lines.append (new_line)
- except Exception: pass
-
- return resultant_lines
-
- def find_lines (self):
- # - Почему Колумб приплыл в Ðмерику, а не в Индию?
- # - Потому что он плавал по одометрии
-
- gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
- edges = cv2.Canny(gray, 50, 150, apertureSize=3)
-
- #cv2.imshow ("a", edges)
-
- lines = cv2.HoughLines(edges, rho=2, theta = np.pi / 6, threshold = 20)
-
- resultant_lines = []
- #print (lines)
- try:
- for line in lines:
- #print(line)
- rho, theta = line[0][0], line[0][1]
- if math.sin(theta) == 0: x1, x2, y1, y2 = rho, rho, 0, 240
- elif math.cos(theta) == 0: x1, x2, y1, y2 = 0, 320, rho, rho
- else:
- ind = []
- y = int(rho/math.sin(theta))
- if 0 <= y <= 240 :
- x = 0
- ind.append([x,y])
- y = int((rho - 320* math.cos(theta))/math.sin(theta))
- if 0 <= y <= 240:
- x = 320
- ind.append([x,y])
- x = int(rho/math.cos(theta))
- if 0 <= x <= 320:
- y = 0
- ind.append([x,y])
- x = int((rho - 240 * math.sin(theta))/math.cos(theta))
- if 0 <= x <= 320:
- y =240
- ind.append([x,y])
- new_line = Line (ind[0][0], ind[0][1], ind[1][0], ind[1][1], theta)
- print(ind[0][0], ind[0][1], ind[1][0], ind[1][1], rho, theta)
- resultant_lines.append (new_line)
- except Exception: pass
-
- return resultant_lines
-
- def draw_line (self, line):
- (x1, y1, x2, y2) = line.line()
- cv2.line (self.img, (x1, y1), (x2, y2), (0, 255, 255), thickness = 1)
-
- def draw_rectangle (self, rect, color = (255, 0, 0) ):
- (x, y, w, h) = rect
- cv2.rectangle (self.img, (x, y), (x+w, y+h), color, 2)
-
-class Sensor:
- def __init__ (self, filename_):
- self.filename = filename_
- self.img = cv2.imread (self.filename)
-
- def snapshot (self):
- return Image (self.img.copy ())
-
-def main ():
- sensor = Sensor ("rgb_basket.jpg")
-
- while (True):
- #print ("a")
- img = sensor.snapshot ()
-
- #blobs = img.find_blobs ((40, 80, -28, 72, -28, 72), 200, 20, True, 10)
- blobs = img.find_blobs ((35, 50, 15, 75, 50, 135), 200, 20, True, 10)
-
- for blob in blobs:
- #print ("a")
- img.draw_rectangle (blob.rect ())
-
- #lines = img.find_lines ()
-
- #for line in lines:
- # img.draw_line (line.line ())
-
- cv2.imshow ("objects", img.img)
-
- time.sleep (0.02)
-
- keyb = cv2.waitKey (1) & 0xFF
-
- if (keyb == ord('q')):
- break
-
- cv2.destroyAllWindows()
-
-if __name__ == "__main__":
- main ()
\ No newline at end of file
diff --git a/src/azer_robocup_project/Soccer/Vision/zhangSuen_Thinning.py b/src/azer_robocup_project/Soccer/Vision/zhangSuen_Thinning.py
deleted file mode 100644
index f798490..0000000
--- a/src/azer_robocup_project/Soccer/Vision/zhangSuen_Thinning.py
+++ /dev/null
@@ -1,115 +0,0 @@
-"""
-The Zhang-Suen Thinning Algorithm
-Designed by A.Babaev for Phystech Liceum 2020
-to be used at OpenMV smart camera
-
-Usage:
-
-iterations_number = zhangSuen(img, rows, columns, iterations_limit)
-
-img - binary Image object
-rows - number of pixel rows in frame
-columns - number of pixels in one row
-iterations_limit - Typical number from 10 to 20. If limit if very big
- then iterations will be stopped automatically.
- Actual number of iterations will be returned
-
-This algorithm is used for thinning binary image. Two steps will be subsequently applied to the image.
-Here is the order of eight neighbors of P1 arranged into a clockwise order:
-
- P9 P2 P3
- P8 P1 P4
- p7 P6 P5
-
-P2+P3+P4+P5+P6+P7+P8+P9 = the number of non-zero pixel neighbours of P1
-s = the number of transitions from 0 to 1, (0 -> 1) in the sequence P2,P3,...,P8,P9,P2.
-
-Step 1:
-All pixels are tested and pixels satisfying all the following conditions (simultaneously) are
-just noted at this stage.
-
-Condition 0: The pixel P1 is set to 1 and has eight neighbours
-Condition 1: P2 * P4 * P6 = 0
-Condition 2: P4 * P6 * P8 = 0
-Condition 3: 2 < = P2+P3+P4+P5+P6+P7+P8+P9 < = 6
-Condition 4: s = 1
-
-Step 2:
-After iterating over one line of image and collecting all the pixels satisfying
-all step 1 conditions, the noted pixels from previous line are set to 0.
-
-"""
-
-@micropython.viper
-def zhangSuen(img: object, rows: int, columns: int, iterations_limit: int)->int:
- c = [[],[]]
- iterations = 0
- while(True):
- iterations += 1
- bank_num = 1
- changes = 0
- for x in range(1, rows - 1):
- for y in range(1, columns - 1):
- xw = x * 160
- if int(img[y + xw]) == 1:
- x_1w = (x - 1) * 160
- y_1 = y - 1
- x1w = (x + 1) * 160
- y1 = y + 1
- P2 = img[y + x_1w]
- P4 = img[y1 + xw]
- P6 = img[y + x1w]
- P8 = img[y_1 + xw]
- P46 = P4 * P6
- if int(P2 * P46) == 0:
- if int(P46 * P8) == 0:
- P3 = img[y1 + x_1w]
- P5 = img[y1 + x1w]
- P7 = img[y_1 + x1w]
- P9 = img[y_1 + x_1w]
- if 2 <= int(P2+P3+P4+P5+P6+P7+P8+P9) <= 6:
- m = [(P2,P3),(P3,P4),(P4,P5),(P5,P6),
- (P6,P7),(P7,P8),(P8,P9),(P9,P2)]
- transitions = 0
- for s in m:
- if s == (0,1): transitions += 1
- if int(transitions) == 1:
- c[bank_num].append((x,y))
- changes += 1
- bank_num = 1 - bank_num
- for x0,y0 in c[bank_num]: img[int(y0) + int(x0) * 160] = 0
- c[bank_num].clear()
- bank_num = 1 - bank_num
- for x0,y0 in c[bank_num]: img[int(y0) + int(x0) * 160] = 0
- c[bank_num].clear()
- if changes == 0: break
- if iterations >= iterations_limit: break
- return iterations
-
-
-if __name__=="__main__":
-
- import sensor, image, time, pyb
-
- sensor.reset()
- sensor.set_framesize(sensor.QQVGA)
- sensor.set_pixformat(sensor.RGB565)
- sensor.skip_frames(time = 2000)
- clock = time.clock()
-
- white_threshold = (43, 100, -35, 0, -99, 43) # L A B
-
- img = sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- #pyb.delay(2000)
- img.binary([white_threshold], to_bitmap=True)
- clock.tick()
- #img.to_grayscale()
- #print(img.compressed_for_ide(), end = "")
- #pyb.delay(2000)
- img.to_bitmap()
- iterations = zhangSuen(img, 120, 160, 10)
- #img.to_grayscale()
- #print(img.compressed_for_ide(), end = "")
- print('time =', clock.avg())
- print('iterations =', iterations)
-
diff --git a/src/azer_robocup_project/Soccer/__pycache__/strategy.cpython-39.pyc b/src/azer_robocup_project/Soccer/__pycache__/strategy.cpython-39.pyc
deleted file mode 100644
index ac4c889825dc37c406cf56ac3470d2f82447c75d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 21870
zcmcJ133Oc7dEUG?`<n&(%0(bW(S#~<lPs$WC2k-{Mi8hHpk<9^JrD5);0%Bn;Jyb@
zL<Y8Es)?IK37fiZ;<^ralK7NP>?|iIOP5G?l*CExR!wu9_%-#3mm?Lek5B3)Q4~k&
z_x<<H1`H%Qr=2<Tzq`NxZvXwaJNZn;3gNT2_#WqXJ{k&rjgjV07?BhBxm5rs<QNN~
zoPHWPL*i!6lxH{>mS-dvk!Lg)bxbGxOehyS8*(B}^qG(oy>8~>h{c>ZVsXS0h$WmP
zVoAi3h*?ewu@qufA?2j!P4$#xOoc`>8^9nmIb-q>e285Hzti}+mjSB45H;?s8hA$3
zPA^n49CJr#&0M~Z;Z#V+%2_Y8ZdQ$IXh-O#v1UrWkb0tOO1<f-$<p_sUfI&&cOqOh
zfk${!8F<qM@fZ#Jk;S}s#W$CVekAYZ%SbVv@{MbL_*(wzt&mEh)OV)=PT&9dCAXm5
zL-|8vm6?^rLfLZ<oh^FjRxTg9Sh-x8FM5YwTz~O*U;H)vKKkOXzWChH!$<EPaf<Hz
zNO3VgS2#3PnVBi5$6Zh5y~5nJgG<-^<V>ZaoTGRZ-{W=ygp7pIYh>}|(=t>FX*0}d
z{t)>S__@yj6hfRzOe2O0(-n5YP6SU(nG?e^>cpJ{o-rrsSa`-AObea~C*x%COgbG-
zC!Utm<#gkja(bLzJkw5}(~oDy8E^*i%sNBPFrFRG4reExoz5<2H=bS2Z5Y-O-#S~#
zFFaHzEEUuYC$;urnE1*h??iypW{0YnG~1|#0f9Lnmfuw9mZ_)bR>Y4jR=i@R>_-<W
zGx-HSGPh8<>~}jwRhVJAJzcTSt`uAkMI&xuVODjZZN)KET?8Cb4Y)=8gl!kgMbEZ-
z5pxp&AtNT=)t(0C2WzEfxJYUrhGY0i7bXy^hDuzS#t_$QNbN`5Tn{_OO~XJ6I)gnl
z)m+NNYJS>_2KB;2q3ebdk+j%)ypf9aT`PmO2W54$HQhd7gl{N+E~vrx$L2#f4Ar}-
z?YK@2T1ybqLA|$L(D;=f^zddU;Uzt5J#`a#*G(tc8*;*fp<a!7!U3MBr7;TH;C|bR
z5qrz_T6$tNu-vXEtT&p{Q~vTtf0mz(;~O0Am+zL(*N(sZh0pWzz2h(I*?6TEc=32p
z<4eaC*VoIl@`;@oP0IrC6HBwUY%p0Fe%4!2vT9CORur~!H|qG|{AE{#5mHe`t;_j^
z1zpLHs>({)@#8CAaiQp4^J6Msb}EavFxV`=X?b-^>*M*ELVaNlqnNu7AY^nJX)Lvf
zvCr6#@Bw2dzSirek$pX3W{rrs-&oz%v<SDh)v_G96zj{8)Xd}bVPSZfnvz)xJLXUb
zODVhqgnNLKX_i0@hIB}BnTM$byl$@PS`jCP5=|{IcR(+Ow)F?FJl9Mwx*iK^#rY1l
z+9SIgZ;i7}oiac|9j=BQEKT*BvOU61LYC=%;GZP^z%;R*bfR43p4Hka=|!VOU>!j%
zt6nRGmQz@`k((y!wrQ_?4w$A}TWNj6O>>P)JyFUa-JCP4QBcsdlX*7MikU3h?Bwd`
z_)1ww(a3B?ji`cGP~~EIZp5pM%;0z^sF9_LTNE0W9zTC(V$7a6GkJFUoUB$wN+>6h
z8bBym%70W_+<$re)tz&or}IKjM_QNefy1M(?rX{|7xK!+NrUqX%dfInF4~%QZv0Oy
z?eSZtAD+uE`AOHSEZG;HEU3|}A6-(#vgd~vi)FoAk_%Ygi+MHgsy#%Fn*xM&S$S$V
z<3e3;V~8h`KeX*j{U{I8qyFyMW~_MZ73Z#ac7AC|Rq``eMq{#%{0?WuyJkOS*T+gH
zTx%p`q(rTMel~~(=YZeUawLqQwat4oHCOQ}_Gy`uyMVkq0>FdzKI52i&`jf)O<;rW
z1?<KzYwj}!jn)394cXMdmYvA+O&<&hIEL(|(7FMVYGS4LJK<n|;Zz2>j?5eC&Y)DJ
z6a}Lbeb#t9vSzM@*CJSdW;KE{F*+LoVT?^qt5GyMF6-o$;djpFUC+K)$m<jELK%a|
z1JB<fbgoziXJLD41%+@hg6-hq)2E@#Dd*7datnod`xFV?`Lg{;#VrI=p&n#*+Fb#k
z;=1<fic|38<@}P$KZ)a)<L^h^r9#2kJif`<V%d=vPoM=?I8qS1ZZI(Y=4x-#IMxfc
zj3Vhhe=>?3PVTT<#-x7F9Z1;x#$KV*Q$)rd%a`X0pw;d<6m)qKgpAeOnqJzj?3VY}
zIg~W&*XXJ2baQgGV-};ueeD!x3)o{g0}i8@YKNMXwXxpDblD$7!5UV>O<1wXrefbG
zLMuxnv#PRKLrAgjZPDqOD|q&$C7Y$~0R4$3ly*0v)TZbR$w2KBM}Wtq%+p5@sEv~c
z&hxr47J6?E6kb>%K(H!nk!r*VgP}hrv^$E`A6Cb}B9S@LIp>Vf@`B{X$gj#?;BGLm
z7Yv*g5!J~O%bh3_FD0DBP{>P`tbx#4jJ4KN)fi8UYG^%u6N^Y@EIIk=JEpG`MilsR
zZ)Cn)nVBD{tau|H(zqBnlMK5>r!Z2Pm2jb4SiH8urK@+_w0Zzxq0K_CM>8_z;0k29
z$xhnWN&{t`)ySI-YHH6HXXZ7Zk>+&caInW0@^k70n-(TjNGr!!aosDSug4G?jcYE0
zL#<8$_(oBkWMm8=m-X_Bvo#~E%#qF8Bjw@%%By0<K6zvk<y~?tBzWm$)))q<>xX>N
zjb{syYpT>T?L@Cm<OUI`O*;nN;8DpVSGFve_lJ;w%hcQBpD-NW)R8y9_2^|(aQQ0Y
zx(#ke|6#bPi)d9%5lj<oV&9rZ?Cx3QzZn*H|Nq9~p3+;u;+`#3qs|t79Mv21R5D5?
znL=zCL$in2SU)W>NyjFQsM$s?;vvb8tYgzU+|)rmN(WvHj9nCx0Oq}X7xJW3qqXfz
zW5p!u+cY-$>h0WydYdj*o-DYvow-n$^+xU)alOKltM{O=OFX<~H{MF>6()N~j<yDm
zat{kNw$y$`+Y#!<l%G0MMj<x~z=bym7QNHhZ{D@d5<GQe%dpo^4{o%^u#*c2XcD%@
zyOB2<Y>gbkZ-&*;r%>ok7UHR+TX3l_fnR9BrOgrush?>~noRkX#?;A@V7^`g<cxzQ
zK$(C`01`$lSOkY_<)oA>sZ`Tq+ghV#A;c$N-M7s`P~;g~R>GVX6TkGFlGPyVg2S&K
zdkaH)ca=jLOmtXJ^a1nmb`yQ~mce~X6a6?|Qk&@az6GrA`4F*sW4d?hp2^W}Dly9W
z#e!}7mJJ@?Sy^B>ZQIK$`30S$`uIj!^~V@GLvWVhJOSAeMZ&Bu5xk4wQ36?!?_o$#
zeuAL}k(3ZmM@9aq2S1lPE@32MiS9&iB9SoV+nb2U*8)sS*%=gT`EUqN;3uN-o9nK?
z2Pgdactu^!D`!RqDQJ%2`h0L5P@LONv7|C6pg8l2*f(`edc{F|7Qa@-P-pPk?X^N=
ziFii44j5Q0{${PvuxMJ$8%BAoDTkV%S&tNzWgD8HSq~enk-Cj0$Sl9o)Rxx$+_$MU
zt^2vJRg)xo)lX`S%0IEGMglm!!uR7Ba}o_KZc^wD@T_ZsMDJDC=$OzXkwhfCxCi0=
zW;;!i^mE(#`|CZoI3DP^)*M+{YZYjmMEq}Pi&Q^GT^|i(Nz@PlH7;rg!2tp;4z&wl
zG}R;oQSYNh2;`6;L+dAIDofYg;#^tnWyb9U`v~M%X?J?0A$8U@J^unlU2ZvQdMs#p
z4p7HqLi`D|JXW}-@3F$r_4L-(RdXX-XbsVmwZ(3MV1{^@U_K0JzC)!j*<Ny8Y8=_O
zBFe#|NlKS)w!vIT!!M$2gWwF;(AlbYn!FWLHXD8)L7qV9i@Z#TOgkEJ8RdQgKQ{)T
zX@p|h41O&i&d>?`z8_l97hr3@3yR>1vLDWSs`%9QlwmL0-<C39B(<)#W*3kpH5%Ql
zOy+3%7WsKd%vTF)TO#sF_J$;-rd^I1tGnA`+@7dJQIkKJ_*D=UPoJaEkyB~R>DLS#
zwR70Er-IGXlqzMUJTzKHIo*h_8THnni>S8-tzFRCji%Ou)Sx!XqcyYM8sgCXfn0EC
zn{g6-V}On1#CK>TO;}2sb-o~Izg`}3DW3?+QxC$H<MUA}xOLcx42B?MQXy`X5|VRJ
zhb2d+Cxe;^>H`PRw_eeMQdzzO@32W;j8dCY#K2Bb12aogQCusg)!Q&1nwU^k=clwX
z@6q#P)8{rwXrOQ&VUY$mdX!P_U&STwM`ssc=<(yHFHBr|cyh{*T)gn;l%J`Yf@}vG
zGm#V36%@znBO95Ei9q>O@PtWfrn2CNRpsjD!;72o*YI;mC_=^vG+F5Y9yAU^;2Vb8
zdAGT`xBWW&e@28<0k3bz)e6n|3^E%u=RkX0+E5@h%U0WwnK@+KVygj}i6=T0)3%Tq
z&cq28d=lWzS&qP5wo7c81`K9U0>jXiaI#JZo=K<E>B7@;x}6?8Q%<kbhiBU9cLwmx
zID^g*o>^zu*@0(=vlEMHw;y|CA%6|VG4c$x55pvar}1+sxdfC7OCQEDEN2^PXuxn4
z>JCyWz^K3&RoQmDegc3?im1v$!S8bmGZiQwmK3yj(2reS@w}QAux=vrjXB?#^?PTF
z3d+LPqBY%v0y>H#rd$G9-Sq~VCZx_l{Q-)()JHaGUtnSD&|Pm&@<8N5oFN&5*AT$=
z1Q!9dQ8zUV!Kp`es;TxN#jK}LwRA61!u6C0QYMf>LLg=YX)B7<Cy*Memy03ALkcZc
zjdF3M{uolp0X0$+NL@#2vL!W%)L%!cRd3Tm$_q$=HBHWGSizCvBZVZm)?TKRg;hMP
zUPW5RdS_6!W4((ZQD0>yeQU0+`}ez-pvzf+j+~l0Z4bz$sk)-}kWW88tGQ#pqgHh4
zN~Hn=gr86=W!r-;IG1#nDxQ56Eg&eBVGYY=RK*h|u`ZNh4ux)s`Y~6G1X};qt+*ci
z{sv}8?8SVsY|mVq!K&L;dvycLrtI86Gr~<7es_R-Q<^L{f4J6Q(3hqHeow7i&56DB
z9yX=sdh4BSO7#a@`rVS#G6c;zU3xT{l4MKx*3|ijFHM}BK7V0SxQ1N9EiT$qu(jk;
z42>1t{N;s0JG$s6mp~HiIf!&GVsKpwF(D}C`0bZpzdRX(mBL)@4E9mIS_{+2t;Y>2
z8)+}L6zvQSPHp_)6@UeYbI*w!!-*J#qsL^kHMSJD(i$v*VhO`G;-Z9U8P-K;cLzo1
zifsha0~-ZMl5sN%?d&95TaVY4{ifDLEO6q&u7<y_c0cik?P@W_Dy5($Hf1|mrL>cP
zW;j_ML`}w$!d+OU47dR5jFVornxu2ePvzDu58JP5apOT#o$|V?@hZ3r80HPwHPeVE
zs_}X}<MmY2PWrl8O>qPf%LFlu;iK5N5qz(Qp%Cjn0)xE!{=*nKejmdRbu!>BQchN2
zhrmvOT>`rW_6Y11*e4KN7HW|*N;&-k2Luj+ml^t4b`3|2*Q;B<ZlI4l20{|YdnCRy
zh~teC-xb7#`$PHNK^$+8{M&*!aFh5*5C@hL-xI`vuf+ETabPa-+k-gzAn|=c9KDhF
z9YGv@k@)@~j$TRpZ9yFUllTG5S%=KoDCTT<&BA>3RXezrwv4&yeye57xhDI)fzqIp
zh8;ZKi&@rWdA;8;Yr@m$os((;TShEwbIT7{fA;fBUpVgfUc4|ZJKBCs47V50pFKD2
z_iRd?I5R$7tMi;t$<;mQ>0`2#SNTQv5Daw-0)m8tCAU&u?QAYGRa`u@+Ia|%<}$va
zHh3cXU8gRLJ!VgzpP05!PTP;3oOsCZnz}G?#-2DiId*>XtUdM6nMXI3dg%P=hy0$0
zFOVSFQ|Hc)PutUHrl#$8w@ZA?-!XN5?2J7%edZBcdZ`OvJTrA^V%i^WWU{j@d0iJT
zP3nXrho%0M-$*)IOBy6P_Q|myyfiia@R`YJd+hww^vTK7XZ#+?xiCIHb!OVWFm9hZ
zIWghC9kkRQyGW$ZPugQ=&R#roX38GBbdjCC*M1m@51%<Xb?M?6Ha4176f+eyH|i$|
zev04*!9OSXX#z^;>ca%ov#EbZ@Gl8ILh!EuRtJ8_+Ph___ufCUI&hb@cguj+GFob|
zH_RADjZN!B#+7+PTk|-qwX8OCwtd!P%-Tcrwx2&18EGl9c@|lMecUpaTQX`h%8cEp
zvE95fpZ#nzyUe;9S>M+jvyy48`Nq}JAvs_DnBHFC0t&O>aj)1HR1vm);bZ)CadE}I
zTyd`1@T&8>$ne0-4KIxRg4nv%!<cX1lJwKUxpYW)9L959!jHgmUVy240lpWS>-`bd
z>7Q9q3h%M6R@6M)E$DwxQP<@Bw=X=x?dC@+;9C3$_c{I6@{S9pypVUad`L}%ABNQD
zN3W_P_%kv0aifd%Z?se7KEJcPvUs_m><hEDVCec5d9sN@c@D0M78w*B&h=^b9X+)d
z3wd`%73h};t}o!~{HQQzx&A;T*i<mr=Upi(ha=Tmf3~;)o^!FFR*2ir0_1vYZQ9q0
zg$2hJ#eA-3Q!4E?IZMxvT`pYBSsV)V7^ct762o;g`xX4$KLH5AUx42<`5b&B_|Z(s
z@YXooBh5kNXKAT{1J%L6lTS+<o8Y%&*{os0*$NI<rumv_TCbU5^Y6{D_4j7P{GJ)H
zzGp_wzcZuO-<dJ<RWoM2YR1jKHRIObLZ|(TnXq0lljh%;N$YP+%lvE8vi{mkncp>2
z)_2Xc`5iNDeaFn0-!?PWx6Q2itw=}OxYO*13qruF)rW|DcoX;7;t5L`FoNG{{M^F;
z;sI;Aop@62*$pWIX|-bu><x6P^vZoUu)V@rM5`!%9F?_RlycRmp{S99HG;#feprX8
zUhuQ6Qs3t3`5?+1KofyRoO;cOvAQGZR6AaK;{>X-*hML{C-9SAEupbD=@t4kZ@LFj
zigMij=yTnGhBQiXkk067c{8LQXGn}w;s8*-vo$U;dN>N>*_uZwOSIexJBfBF$$i|h
zz$T}lS~A3E`O{7YEVBhxm~5sgDkHOGsn0ptwP-owq=k`%`++dMVJH1gsHMDgH7tzs
zDi~#REnbZ|lnGyhS2BH=8HYc#D6=HRF?yNG9%TaN*<8;u1PwTC0cNSg0qX|!Rc9|;
z@zw+vFs%tNtUBC9rWK58tQr<p{3U6vTiQS`N4uI<#Rm0?8`PC-a8fozWqtsm=?&4Y
zaJ_7BY3SucwrJyZd=yK+j2}LKb$ql(J%fN=Pn0%teOEx??CIiyXP?e56feWSS^ReA
zp@pdUmUczsy78m%r36)3XML&@-~df3lea-S`!y|_D9qXCUHhW=iXoyt#=<1p>Z1U;
zKB4Ue7`_x-EaqVUf^+bU-?=&2?=Dx=BG6ea*tH#~{tcV?IKXI9dnNn|6SzSX^+@WM
z0Q^|73>P_1eTtD(gYq6xzsxc+R02fr!5wvw+vF4iTKyjZRZ3v9Buq=F6P>vukwiEx
z^bP5ZBac33EUT&9AczU5#&C^l1cLi^1cMmVx7N=f0$ESXdGxxGjih!uRD)2FB$6Ne
z&Z4q-Yto%4QeX<wzB}izXv4A`1G&!;)f2er=uIv`aL~aLqy$)7f>fc@mY^w1@C4u1
zWadvys^0{D+SifPX<Ypl<J6_9-v+>Ba2=`tNYFan>R&VMIfC|cC(|Yq$hlL`6L8|x
zX9(I&7#Ht*IAJvH)Q%n4C^#vgahQ^@MRB{Cgm0%4*h0d?Xi|vX7y_6D=-W%!<UbIC
z51@phvEScF#|4ETJr#n(#fe4^$O&?=f#Em6vjGI0ZEHTk)QgwXziB!839fn`=oNO6
zB|_-fAT+?MH62{7L<{nq08$1){uEwCOJT5d;809O09}2NM%z=2dDp3<B=v&JtejEM
zyMbaQ*I~sAL@l9@?1<3wGV;^)P(bq>(9N<I<+Iv(OutRq>Y&XKL-tk1);pcR)eCPy
znAaM3`XR4|!I!-*Iub)wu^XFUD>mJZMa-6e_eOa6LCNod|9m(wFPNnsYU~g*neVWd
zrriS=TkoyW=TaZu3&%9L8>gR*z;QFW7F&x8H4c?@4+3n^4s?#dt)Iew5R0u3NK1y3
zl{Y;tSjhNRP4u=y9HO6IeiG^N^+A^PhMZ1sczp*`N;_{FP8Zb-NQHJ3t?zP(A>MsG
zg0X<ZwY2+9I4?tkino3fxfr{kEPQ|*(;F`B#)eMOy<5iPN$<8GpDy6ins(d{QmKZ}
zsR%(O!ZiR~V2XiGbbX|n#CeottFYbTq^l+)Q0Ss3W@VbGOq&mtBPMzS5kBek&PQ&T
zZW*CIDdhq0{`t^a3eN%Nsh6rLlz0j8L5Y9FiLa$`?ix_c46pBLVAH^Gu!du;54&_9
zV7AJjlv&!VYgIE)<P1HFF~lfac+0osU9X`hw}X_$=+(sXGG;R3IFF6BEYjkoeOSMd
z8)3B%_%OvR-682av0t)S7sJnDMJzuldfXk8<C-P=4QwnScf~8%l|l%oM~z$}KLe2h
zF1w1xo8k@Tkv_tf`yd{<uB%1wiao*GFU3`TrY3PRU9-pG1q@$cQQgC^G!Iv4`f^*3
zo*zGNpP#~Mx<Ou26dSMhX-TE|#>~hABYxb$*;**Lt8sB19(iC@%uOQ?1P6WOL@n^V
zqJcfQUGc@^Vq}K{>>(uLwiFIXRe@h4O{0)aaN;g#x5pQlzh58oap}oYa7E|e6Stou
zm?NOpQVSXsv()bp{5t?YwX_1^?vyrP&<>Hyhyk`TI}7o{DTtqXZU}Dg3Mv;4femG{
zO##qR#aT~_S0eAFmhvlZ0T+PmMOR&5<9$sY)8dK?NwVOn6Y}D##o1z@kMXmruvEbd
zpls*(YBM`IbWM=|h@ox*o+m<|#X(+umeF?*+za4m=irPfH%i1&S`2+^5;>zC4OYbO
zso8GCQe+Faq<)``JVJ1ifQ*JBofcP6zjsNO6$f9j7=rry13@Z!j0}uSW!7e<zq2WW
z&g3$pHQ#%g)J+ccr%+4%7rsuI9s$Dnd;n=HJY^OT5a&|(ds8N}pt-h;7=$OvXuSHu
zR35K%I!z!CqQvW2GXbmQUWBRR?nTJNIn@t2E%rLOBVLlBA6~?`$Lu#vX@#+FV|6It
zk+zx+TGZimRN$G$gVTKhJK{$}vpAA*NiMNwe2DZ6o)mJfg&D&yf?xDHVs;E}`;rPz
zM>;J(A?4xq2!4LzMr8SUhh=g8v6g0bShtiEC;u3n{6ENiT8D5(1gAu~pJ^ptRfz|V
z&T@J{Zopfp^2CLh2xxXUB!#4$1l`0#=aTZMughC9P7-w70?Y7KX)((yeb3@#D~~kg
zXQVB`6E-JUomhh9*CAC1-9L<$*;1#xYZh<q(xH0qv#@d2>!FpNMy%FKfI+X$Sw1Xr
z8Cf~eNZ}3jeo2qf-u1Y-mcY3YBPW2IzknlnzYYbsV<dPAsKYqT2d~#~?hbN?kUQk{
zlzMT3!3Q-s!4AW2nyBIqDroaXV(;}eT9c5>%pm(8Ey?%=spvmXG{j=1ew=95heGQE
zRqzQg5a2C1(S~rMS@jd`B2$@GKhaWE%NgP+<t`vJjQ*Mxh5qfpiI&FGVxIa&HH{MA
zKs+h&4+F~#PPG2D4Eni~{q%a)2OC&5Fb%Ml9t5d!4&!9YA|5IY=~~q+j1s%#WIHTp
z+ADZx*u@H1ABKb&<;jM3?QR)U^qOk{cyyqZSZRmg7gk4sUk6IxRx4fFDf#_Qf3?FI
zc^21ImJg#=*xMy<-|g%X-z|)c1G9j}h^xAh%TWW51BmNeok0nVe^rO21?JLSbi4X;
z>8uMQbz4v?!*{S%TsrHLS~9yjMJ}CnSG%3tF)I-jTRyye7_q3to<#5V)q1zy?L;^Z
zUSDgwt!)DD9@M&{UTXxc?T4F5PcXJy+FL%%S#52l^$nQCUf_*ctaetraP6!YdYyjf
zZM=5I^crRuZA$b9+FzmWA(5*BP!6Q5fvV#4jlqUAI2f9jYtlhF?f^E@T`=zT^a!?<
z<jqIbG#GafJlDgMruL_B&05EG>#!-JUAf>LT&|9R<sl2Bg*yr^!tIPkv|pXL5b(HA
zivXixwZw1*0JoFnUPY6Yl!uHzEGJs+e6ou!d}V^`KA+~<!$K6|290+EuAU4|hW1A4
zgU945vHRe`?OUC0$f<Ms3hM72m#ZLiKh*o_vtL>L@NwO@&l9_jN3>}^xau>3D_Rli
zP3QDg)Gt1_xx~fdZ>PXZ>874}PIxJQ@R7V2_=8qpHG=k9c)U&RkNZ7m^=+PU-sTC)
z3!67Sr?1I==egBAjm~erH@H;1^XQ?wM};fhc#WzZvUaO?G}UOiv<F_HT)(iVzQD3?
zZz{X#R^nDhb9D7kQ}MTQTLyCOf5vMx!|^tTXcO1&oOd$xj|lD}css!89<4fP$5P5-
z69qOIKK0)S_5k?ZSM%Ns7)!FM!3{X!`?QjtY+SC7T#`Linf2_4<rca<wNUXCHPuZF
zV5|CGvwgw}w(@@Ji>!8qfDE5HLU5GeZi0INM*B1q_!mt0GXhEg?fAXMmQjDsB3~l#
z2@VnbcLL$o-pSB?1pk=eeu5t$5DxARhJ=k<L+Df7O0=JajC3q*^g=IA#*M66Cu03z
z#i*L6vb`HjTv}K)vU3bWxsL6Z4fje7vT;;v-H*Br7A|4jgR6j=iA$gy**ILZ2uFe|
zm}DB;T*2J+hTFM?*-HcYx&osclt+6yQbfZqEXoRsk6{0t@Em+1kZP2ople|@8HA~H
zK6b-g&gu&<wx~PmPDAiS1oMbH!BFSMMD&*%o8l7@E+wSJ2pynth*(e%;H{;^bso7E
zoTF3X1QaP{AiyUnz(YVMP5o0agJdfIlsK$YqzpspYmvS4QYEN;ok*R@8S%P;e7H&L
z_VpG0AT?D_MJN!{@kTH;z-Ar(4Pa{GY?Ut7U~1w4Q)5)_XDZX`ObvV=psWTn>P{n+
zArn{00p~23nk1eb%u}DQCQ;(^h^Hi8#RywqYNBACQtMPqU<{lZUL6dHn9kH%A3Ag&
za15qU-YC)26ZuuLRh`1jj8qeN%b&};zK)*sK{+n>yjfnxOvY8jcB9mfRKzh;UC+|*
zCAwxTkI2#4Jvkka$!{EQkoj@-dssWG895UIVR%G+4rxk+J2}NfS^39I`%eTUn~Fz(
z5JebSNVG-=#K~Dxq)ukGBS9yaK|0h#NGPG`mkIe0y7GqNq$Vk(Ch$F_%08FU>HwFH
zUs_^3$wp5R)P=?ENyaAG?rBtuqvP&51VWI`sLigCgqrkbVe9u}Grxi4hWtQsgERc<
zo?ts~@7dL|rK!E79)sw+e~d9|zoofc;1dZ~XK<`~TT`|Ot1_2I3D-)oy-q-*_Xna5
zH*C@C)!mf>>8)z0GRXrMkKV-C<`GC(BhrcmvY{`ha?|6F0=yI!JvdJ=SG=V9B1+@W
z4Dggxk=34;ns7pLEJ|L>8lN;|wOSmvCa}@d++48mnR=Vi)(5=Pkl3VL5Cc(QD~zpY
zoKEtpK~7hYLwqSw9fm}O5cF%N(=FTbn0_~HOOU>xuS8DIyn(XMU`!%H1{hOb2gpD#
z$bjUP!GV@Zk|OI}yqJEInz35V7iwjL`s~B6OIlx0|AnUd-J9y03WP1F8AZ)+vL?t9
z-yMat1Z`pc-91fp`+=FM#u}xAHYi~o4O01jwlUV!MsKYRXiLqSET^uZZ((S3;eO<>
z79=<Iaj7M3IfEdQwb3!v4e6a8omx4%l1JA2N%UJ<{%o6;UukL?t<=ZORR2p_*5hU>
zT~4<=u&L#++PS6WuzHuYd~eYI2G&tX!{>rjJ+ig3x^>YnL!PURNLa15Y5g}^T5n)Z
zJ?JNb*7cZ&l`gAWA0qB(&k*S|&egMA4oLCPBwVMkukrb-=Nhyal=LYi&3*6I|M`=L
zFTXF~wf_f_)RzhVlHe-@!vtR?_!>a*Ly&*e*BKQeJ<8A@5|DZa?D$K^#gl)7G<I}{
zTu$3et371-eU;15DZ;7s<dc;m#EqH4dG|tD)P|ai+|P!O5xhh&0+8zzj&U=y=*Pu2
zdTFU)y=i;-PPoAw8+#AIdkLOkvni>6u|QUPG@(g-T9bM$3J7ucjl8DxW2ob&EAY=K
z7jRW&DPQ(8^%(xg#@npKdp|1N)lBGNxHL7d7bxmL`pKL1Yh@msEri?CoS(T9u`GDK
z6Zj*IG5n)g&Ugaf)qR@?{n@(ba<e%T6uthTu8wDr(E2eT1X@g6Ja5Z&^6;iei`(XK
z+=I;=22T0^FC8PB7B11=69u&gTLFfLG(HYL?ig4ehJzfiKlI}U`%s1G3YH^*IBf0m
zmo<{ckZ29)?H#9m9~9FP)+Da~VgQsRMz%~SE6<xa{xGKghN=m-O8$`nX(d|_d%l$F
zfQ|}pr6HA`8z@6IFu;?#GQkzJ8OFbOnQTnF)Bzg_%2yNh@;uu}F>{jN$<qz7dz}RA
zBUvyz^feFOD!vPGo3@{LscQ{-k+p0Y{_==*;<RGDa@%?w(OSK3)U)7d8pcTV2<(NH
zDGd4rEoQG=?E!wUnqXc!ygm;um(VqJP`$t}U2xYroyb}jyeqoB!D^@2=O)+49LCo*
zjfhu!svR^hOWN>F{C!U^XOXO69G0(MjEoNNtaeqqq34434|O_eaK2$Ie_6`y5?V=H
zB5)UmFFiR&ht3DAKRhmfq@v9+5OzZ2u)kz0<pP|nW&;sVq{?Y6-TxMfs1m^FZrQoK
zd*i}&3xAd)5~Z+d!rcj**1_VRC3pvch;<tbeT;zYTrMqMW*6ZC1Z#!Jd}8m$Z4El(
zi_g8dRB87rU%RbKGJUFas=iDV$pZ~P40DH8s*La*v~q}pL|cn_AYF2E37x?IM}VI2
z?<iE>DXzG=^h^c5ZTeqjo@$A~X(PX|bS0nb<iDrUi>~Hem+Nk-6#Pw0uA?c9{}`kG
zH{baUfLv<k3RO4uLSfF$bu_vy{$%QT78#}|TCqH%{n+fhC-)m<bhLxUC)wIB5qyf^
zmkBnpt`xT4MT+)r1fUv<YU2Y<6J%hAL9fWT;yxzA@M?l|%Xkb_k9019P;-p`%{2%b
zNSeqRY0x~&SnaDzrp;EJEhcrcutL&yV$o^#wPZT<)HdPIyc;HUn0~9~dQ^PS!!16|
zokv3My)7Qi(6`~%9mo_J&oxM*YcY80!^LS0<bkGGVRw05MVpWDR|_z=atn!63L=sO
zX-VLpwGy%kt2DTRV8Fv1b^dKNP6<%rbcy;8I_}^WT|iVo3<H9N|5^cAh{BW#eId4O
zrW%7>7Znc?GK}%{Of`#5tHar9x|+nMO~Gpf8@8HRi!I-fQXR5osn$$QuHJU2#WPy&
zo-}(!s~^K?`f2=q6b*7PXyRW1aSeBNwm`Q`EKSWv{3=RqH~@OLi{$p>h{C<{=b0+o
z_9jEWK=6wMpCI@V0@=#{l_A-?Z@jMp)1AxWqzk!Ba7RRYK&tOB<GajAFVNV=!<?Rw
zvW#yrQ`YjYFeD55GYDy(fyLS_?|+ICKgs2N5CKYNcgdpV0<A65hG!A~K+?kUO+)&^
zJ=DQq(QdiOG6}wm!-gTU@lfw0$P@Gt{3yY7f)5h>Fu^kfLh?Sq5Vc6^X##Q-T0AAH
zO}kU}sl~KqfZxYY?ki%;S&6R1*+k5GJN`%@mbfpGOWcvTGjX4k<6j=gH6wd84FY`4
z_03A~k9{(IKgR}dCrA;b2`&?myOb7mml{-}j)$K(rvF0WK?7xL|7Re>38caaD|;=w
PE4wrMfe^p53FH3)itmyf
diff --git a/src/azer_robocup_project/Soccer/__pycache__/utility.cpython-39.pyc b/src/azer_robocup_project/Soccer/__pycache__/utility.cpython-39.pyc
deleted file mode 100644
index 41a3bb3c024494924642507cb730ca89b607c72a..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 1543
zcmaJ>&2Aev5GHrIk}Us3sSPBEfr3C$6lxL4Q34b&+BATJra)o0sM8#T0K#gyme$hl
z$|Tpegw@IEOY~$Pb1B*vX!{60^V*Ydz4YFXlpQ!dlnc(zkVA6jo1buXwN5Zje(8u-
zK*(QKxIKIb&oEUVM362KRCr_BrS9)_y_g>Q!XJ5Z3oC5Q(x4kqB7$xuTfs_YTzyGI
zwObJlQ4>pVXtyTnV)+f}R>g`~#eYexi97h$g||<dcV|D<cXLsUV?L4jaUx>HRi4JY
z(8)N_3B;3O5)FA2W?U8-KS}hEs}LG8R>fHJFcVI$t7#VMQ{K;^kYs!}iDW*R>S3Po
z{D6NqiL>2TP8*v_<5(yK-}&BY)UV(!*_#EATTfbBZ>d=x#9Bh9pJ?^6Ne%t<>&UY@
z*Q0{Jm3J`JE07Wh6+|{lG6!OF$R)W?<{o(9HD{$)vJsQpB@_O)1Tt1XCv*RZYf}1W
zbRLvJ>5YioKH59l22bu0;KOE5>l|Ykk0-I@2Spa?1RfpalAq*qlw^YiB-YqG<*=8l
z_^61pD0T<;B!zWXmPl)Gvtj~xFSTJx=MxhY6I^VVb=0uQiR>6E-x9e3-GBB$cD7&t
zsA8$wVf%R=6=|Gl)qauaZ;F2VhrFMU65YN$zx@63H_V?ef4%&pv(<UbMWRMLNy9<h
z-p`{bmai8Powg=Zvt+Hs9K;{s1@!=guo`9fZfK3xy_%Ec8g?VAa(lc3-x{isre_Gi
zriIL#J#?NUg4)6p@@s6eBJEoYC2-o}I_Hm^Uefpkuu%7mbNd!q#pR~5IR7zt<#K?@
zJK*2T2h<X&V!C{I4n~tRqUoWx0n;{+2b!My!cxTF?%gLBSihiiA8UyG1ucE(v67Um
z#Lji}zjP*k^uNBm2n(eW)CVxvGU)^8Jagx+thobBJN~?8{{E_2aSj-khI(kcNrGHB
zLPq%Gd|>=2SIxk9X@NvZ25D&6X$LC7pd&+J=<L7n1Oxm2XLv!?>^rPMAJ8=_H^IM$
zku}EFjl<{(7){PmC4#P#WzbDmL+iw%f*n+jCWpYpR@P$Gh1Z{T7nq<WjH6;4YV-&_
zbp4|r_HEag+JZlIHS*}8NK*a|A=f?97+9r+>jlT5Y;otNyHqw{)(v3KnoQn>Z2b6i
zBG<u5o32gI&{KKOiZ=Ha#3f42A%RF(=WbHvLwkhoHJ44b*Ascv>lt57)#B#rF)9nu
zGSy;{WYUMaX%s3JJ!@=l(YIW@78z<`mG;tGuRA*yCVY{)Zu-;~yxIU^m4;WRE3|@g
MVywPitJIeM1+pV?@Bjb+
diff --git a/src/azer_robocup_project/Soccer/pid.py b/src/azer_robocup_project/Soccer/pid.py
deleted file mode 100644
index 5fbe52e..0000000
--- a/src/azer_robocup_project/Soccer/pid.py
+++ /dev/null
@@ -1,56 +0,0 @@
-'''
-Example:
-from pid import PID
-pid1 = PID(p=0.07, i=0, imax=90)
-while(True):
- error = 50 #error should be caculated, target - mesure
- output=pid1.get_pid(error,1)
- #control value with output
-'''
-
-from pyb import millis
-from math import pi, isnan
-
-class PID:
- _kp = _ki = _kd = _integrator = _imax = 0
- _last_error = _last_derivative = _last_t = 0
- _RC = 1/(2 * pi * 20)
- def __init__(self, p=0, i=0, d=0, imax=0):
- self._kp = float(p)
- self._ki = float(i)
- self._kd = float(d)
- self._imax = abs(imax)
- self._last_derivative = float('nan')
-
- def get_pid(self, error, scaler):
- tnow = millis()
- dt = tnow - self._last_t
- output = 0
- if self._last_t == 0 or dt > 1000:
- dt = 0
- self.reset_I()
- self._last_t = tnow
- delta_time = float(dt) / float(1000)
- output += error * self._kp
- if abs(self._kd) > 0 and dt > 0:
- if isnan(self._last_derivative):
- derivative = 0
- self._last_derivative = 0
- else:
- derivative = (error - self._last_error) / delta_time
- derivative = self._last_derivative + \
- ((delta_time / (self._RC + delta_time)) * \
- (derivative - self._last_derivative))
- self._last_error = error
- self._last_derivative = derivative
- output += self._kd * derivative
- output *= scaler
- if abs(self._ki) > 0 and dt > 0:
- self._integrator += (error * self._ki) * scaler * delta_time
- if self._integrator < -self._imax: self._integrator = -self._imax
- elif self._integrator > self._imax: self._integrator = self._imax
- output += self._integrator
- return output
- def reset_I(self):
- self._integrator = 0
- self._last_derivative = float('nan')
diff --git a/src/azer_robocup_project/Soccer/strategy.py b/src/azer_robocup_project/Soccer/strategy.py
deleted file mode 100644
index 4eca2c9..0000000
--- a/src/azer_robocup_project/Soccer/strategy.py
+++ /dev/null
@@ -1,680 +0,0 @@
-import sys
-import os
-import math
-import json
-import time
-import utility
-
-
-def coord2yaw(x, y):
- if x == 0:
- if y > 0 : yaw = math.pi/2
- else: yaw = -math.pi/2
- else: yaw = math.atan(y/x)
- if x < 0:
- if yaw > 0: yaw -= math.pi
- else: yaw += math.pi
- return yaw
-
-class GoalKeeper:
- def __init__(self, motion, local, glob):
- self.motion = motion
- self.local = local
- self.glob = glob
- self.direction_To_Guest = 0
-
- def turn_Face_To_Guest(self):
- if self.glob.pf_coord[0] < 0:
- self.motion.turn_To_Course(0)
- self.direction_To_Guest = 0
- return
- elif self.glob.pf_coord[0] > 0.8 and abs(self.glob.pf_coord[1]) > 0.6:
- self.direction_To_Guest = math.atan(-self.glob.pf_coord[1]/(1.8-self.glob.pf_coord[0]))
- self.motion.turn_To_Course(self.direction_To_Guest)
- elif self.glob.pf_coord[0] < 1.5 and abs(self.glob.pf_coord[1]) < 0.25:
- if (1.8-self.glob.ball_coord[0]) == 0: self.direction_To_Guest = 0
- else: self.direction_To_Guest = math.atan((0.4* (round(utility.random(),0)*2 - 1)-
- self.glob.ball_coord[1])/(1.8-self.glob.ball_coord[0]))
- self.motion.turn_To_Course(self.direction_To_Guest)
- return
- else:
- self.direction_To_Guest = math.atan(-self.glob.pf_coord[1]/(2.8-self.glob.pf_coord[0]))
- self.motion.turn_To_Course(self.direction_To_Guest)
-
- def goto_Center(self): #Function for reterning to center position
- print('Function for reterning to center position')
- duty_x_position = min((-self.glob.landmarks['FIELD_LENGTH']/2 + 0.4),(self.glob.ball_coord[0]-self.glob.landmarks['FIELD_LENGTH']/2)/2)
- duty_y_position = self.glob.ball_coord[1] * (duty_x_position + self.glob.landmarks['FIELD_LENGTH']/2) / (self.glob.ball_coord[0] + self.glob.landmarks['FIELD_LENGTH']/2)
- duty_distance = math.sqrt((duty_x_position - self.glob.pf_coord[0])**2 + (duty_y_position - self.glob.pf_coord[1])**2)
- #print('duty_x_position =', duty_x_position, 'duty_y_position =', duty_y_position)
- if duty_distance < 0.2 : return
- elif duty_distance < 0.5: # 0.6 :
- print('goalkeeper turn_To_Course(0)')
- self.motion.turn_To_Course(0)
- duty_direction = coord2yaw(duty_x_position - self.glob.pf_coord[0], duty_y_position - self.glob.pf_coord[1])
- print('goalkeeper near_distance_omni_motion')
- self.motion.near_distance_omni_motion(duty_distance * 1000, duty_direction)
- print('goalkeeper turn_To_Course(0)')
- self.motion.turn_To_Course(0)
- else:
- direction_To_Duty = math.atan2((duty_y_position - self.glob.pf_coord[1]), (duty_x_position - self.glob.pf_coord[0]))
- self.motion.far_distance_straight_approach([duty_x_position , duty_y_position], direction_To_Duty, gap = 0, stop_Over = False)
- self.motion.turn_To_Course(0)
-
- def find_Ball(self):
- fast_Reaction_On=True
- if self.local.coordinate_trust_estimation() < 0.5: fast_Reaction_On = False
- if self.glob.ball_coord[0] <= 0: fast_Reaction_On=True
- success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On=fast_Reaction_On)
- #print ( 'dist = ', dist, 'napravl =', napravl)
- return success_Code, dist, napravl, speed
-
- def ball_Speed_Dangerous(self):
- pass
- def fall_to_Defence(self):
- print('fall to defence')
- def get_Up_from_defence(self):
- print('up from defence')
- def scenario_A1(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- print('The robot knock out the ball to the side of the enemy')
- for i in range(10):
- #if dist > 0.5 :
- # if dist > 1: stop_Over = True
- # else: stop_Over = False
- # #self.motion.far_distance_plan_approach(self.glob.ball_coord, self.direction_To_Guest, stop_Over = stop_Over)
- # direction_To_Ball = math.atan2((self.glob.ball_coord[1] - self.glob.pf_coord[1]), (self.glob.ball_coord[0] - self.glob.pf_coord[0]))
- # self.motion.far_distance_straight_approach(self.glob.ball_coord, direction_To_Ball, stop_Over = stop_Over)
- # self.go_Around_Ball(dist, napravl)
- #self.turn_Face_To_Guest()
- success_Code = self.motion.near_distance_ball_approach_and_kick(self.direction_To_Guest)
- if success_Code == False and self.motion.falling_Flag != 0: return
- if success_Code == False : break
- success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True)
- if dist > 1 : break
- target_course1 = self.glob.pf_coord[2] +math.pi
- self.motion.turn_To_Course(target_course1)
- self.goto_Center()
-
- def scenario_A2(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- print('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_A3(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- print('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_A4(self, dist, napravl):#The robot knock out the ball to the side of the enemy
- print('The robot knock out the ball to the side of the enemy')
- self.scenario_A1( dist, napravl)
-
- def scenario_B1(self):#the robot moves to the left and stands on the same axis as the ball and the opponents' goal
- print('the robot moves to the left 4 steps')
- if self.glob.ball_coord[1] > self.glob.pf_coord[1]:
- if self.glob.ball_coord[1] > 0.4:
- if self.glob.pf_coord[1] < 0.4:
- self.motion.near_distance_omni_motion( 1000*(0.4 - self.glob.pf_coord[1]), math.pi/2)
- else:
- self.motion.near_distance_omni_motion( 1000*(self.glob.ball_coord[1] - self.glob.pf_coord[1]), math.pi/2)
- self.turn_Face_To_Guest()
-
- def scenario_B2(self):#the robot moves to the left and stands on the same axis as the ball and the opponents' goal
- print('the robot moves to the left 4 steps')
- self.scenario_B1()
-
- def scenario_B3(self):#the robot moves to the right and stands on the same axis as the ball and the opponents' goal
- print('the robot moves to the right 4 steps')
- #self.motion.first_Leg_Is_Right_Leg = True
- #self.motion.near_distance_omni_motion( 110, -math.pi/2)
- if self.glob.ball_coord[1] < self.glob.pf_coord[1]:
- if self.glob.ball_coord[1] < -0.4:
- if self.glob.pf_coord[1] > -0.4:
- self.motion.near_distance_omni_motion( 1000*(0.4 + self.glob.pf_coord[1]), -math.pi/2)
- else:
- self.motion.near_distance_omni_motion( 1000*(-self.glob.ball_coord[1] + self.glob.pf_coord[1]), -math.pi/2)
- self.turn_Face_To_Guest()
-
- def scenario_B4(self):#the robot moves to the right and stands on the same axis as the ball and the opponents' goal
- print('the robot moves to the right 4 steps')
- self.scenario_B3()
-
-class Forward:
- def __init__(self, motion, local, glob):
- self.motion = motion
- self.local = local
- self.glob = glob
- self.direction_To_Guest = 0
-
- def dir_To_Guest(self):
- if self.glob.ball_coord[0] < 0:
- self.direction_To_Guest = 0
- elif self.glob.ball_coord[0] > 0.8 and abs(self.glob.ball_coord[1]) > 0.6:
- self.direction_To_Guest = math.atan(-self.glob.ball_coord[1]/(1.8-self.glob.ball_coord[0]))
- elif self.glob.ball_coord[0] < 1.5 and abs(self.glob.ball_coord[1]) < 0.25:
- if (1.8-self.glob.ball_coord[0]) == 0: self.direction_To_Guest = 0
- else:
- if abs(self.glob.ball_coord[1]) > 0.2:
- self.direction_To_Guest = math.atan((math.copysign(0.2, self.glob.ball_coord[1])-
- self.glob.ball_coord[1])/(1.8-self.glob.ball_coord[0]))
- else:
- self.direction_To_Guest = math.atan((0.2* (round(utility.random(),0)*2 - 1)-
- self.glob.ball_coord[1])/(1.8-self.glob.ball_coord[0]))
- else:
- self.direction_To_Guest = math.atan(-self.glob.pf_coord[1]/(2.8-self.glob.pf_coord[0]))
- return self.direction_To_Guest
-
- def turn_Face_To_Guest(self):
- self.dir_To_Guest()
- self.motion.turn_To_Course(self.direction_To_Guest)
-
-class Forward_Vector_Matrix:
- def __init__(self, motion, local, glob):
- self.motion = motion
- self.local = local
- self.glob = glob
- self.direction_To_Guest = 0
- self.kick_Power = 1
-
- def dir_To_Guest(self):
- if abs(self.glob.ball_coord[0]) > self.glob.landmarks["FIELD_LENGTH"] / 2:
- ball_x = math.copysign(self.glob.landmarks["FIELD_LENGTH"] / 2, self.glob.ball_coord[0])
- else: ball_x = self.glob.ball_coord[0]
- if abs(self.glob.ball_coord[1]) > self.glob.landmarks["FIELD_WIDTH"] / 2:
- ball_y = math.copysign(self.glob.landmarks["FIELD_WIDTH"] / 2, self.glob.ball_coord[1])
- else: ball_y = self.glob.ball_coord[1]
- col = math.floor((ball_x + self.glob.landmarks["FIELD_LENGTH"] / 2) / (self.glob.landmarks["FIELD_LENGTH"] / self.glob.COLUMNS))
- row = math.floor((- ball_y + self.glob.landmarks["FIELD_WIDTH"] / 2) / (self.glob.landmarks["FIELD_WIDTH"] / self.glob.ROWS))
- if col >= self.glob.COLUMNS : col = self.glob.COLUMNS - 1
- if row >= self.glob.ROWS : row = self.glob.ROWS -1
- self.direction_To_Guest = self.glob.strategy_data[(col * self.glob.ROWS + row) * 2 + 1] / 40
- self.kick_Power = self.glob.strategy_data[(col * self.glob.ROWS + row) * 2]
- #print('direction_To_Guest = ', math.degrees(self.direction_To_Guest))
- return row, col
-
- def turn_Face_To_Guest(self):
- self.dir_To_Guest()
- self.motion.turn_To_Course(self.direction_To_Guest)
-
-class Player():
- def __init__(self, role, first_pressed_button, second_pressed_button, glob, motion, local):
- self.role = role #'goalkeeper', 'penalty_Goalkeeper', 'forward', 'penalty_Shooter'
- self.second_pressed_button = second_pressed_button
- self.glob = glob
- self.motion = motionyc
- self.local = local
- self.g = None
- self.f = None
- self.first_pressed_button = first_pressed_button
-
- def play_game(self):
- if self.role == 'goalkeeper': self.goalkeeper1_main_cycle()
- if self.role == 'penalty_Goalkeeper': self.penalty_Goalkeeper_main_cycle()
- if self.role == 'side_to_side': self.side_to_side_main_cycle()
- if self.role == 'forward': self.forward_main_cycle(self.second_pressed_button)
- if self.role == 'penalty_Shooter': self.penalty_Shooter_main_cycle()
- if self.role == 'run_test': self.run_test_main_cycle(self.second_pressed_button)
- if self.role == 'spot_walk': self.spot_walk_main_cycle(self.second_pressed_button)
- if self.role == 'side_turn': self.side_turn_main_cycle(self.second_pressed_button)
- if self.role == 'rotation_test': self.rotation_test_main_cycle()
- if self.role == 'sidestep_test': self.sidestep_test_main_cycle()
- if self.role == 'sprint': self.sprint_main_cycle(self.second_pressed_button)
- if self.glob.SIMULATION != 2:
- self.motion.sim_Stop()
- #self.motion.print_Diagnostics()
- self.motion.sim_Disable()
-
- def rotation_test_main_cycle(self):
- number_Of_Cycles = 10
- stepLength = 0
- sideLength = 0
- self.motion.params['ROTATION_YIELD_RIGHT'] = 0.23
- self.motion.params['ROTATION_YIELD_LEFT'] = 0.23
- self.motion.refresh_Orientation()
- first_yaw_measurement = self.motion.imu_body_yaw()
- rotation = -0.23
- self.motion.walk_Initial_Pose()
- for cycle in range(number_Of_Cycles):
- self.motion.walk_Cycle(stepLength,sideLength, rotation, cycle, number_Of_Cycles)
- self.motion.walk_Final_Pose()
- self.motion.refresh_Orientation()
- second_yaw_measurement = self.motion.imu_body_yaw()
- time.sleep(2)
- rotation = 0.23
- self.motion.walk_Initial_Pose()
- for cycle in range(number_Of_Cycles):
- self.motion.walk_Cycle(stepLength,sideLength, rotation, cycle, number_Of_Cycles)
- self.motion.walk_Final_Pose()
- self.motion.refresh_Orientation()
- third_yaw_measurement = self.motion.imu_body_yaw()
- if self.motion.glob.SIMULATION == 2:
- filename = self.glob.current_work_directory + "Init_params/Real/Real_params.json"
- else:
- filename = self.glob.current_work_directory + "Init_params/Sim/" + self.glob.SIM_OPTION + "/Sim_params.json"
- with open(filename, "r") as f:
- params = json.loads(f.read())
- rotation_yield_right = abs(second_yaw_measurement - first_yaw_measurement) / 10
- rotation_yield_left = abs(third_yaw_measurement - second_yaw_measurement) / 10
- #rotation_yield = (rotation_yield_right + rotation_yield_left)/2
- params['ROTATION_YIELD_RIGHT'] = round(rotation_yield_right, 3)
- params['ROTATION_YIELD_LEFT'] = round(rotation_yield_left, 3)
- jsonstring = '{\n"BODY_TILT_AT_WALK": ' + str(params["BODY_TILT_AT_WALK"]) \
- + ',\n"SOLE_LANDING_SKEW": ' + str(params["SOLE_LANDING_SKEW"]) \
- + ',\n"BODY_TILT_AT_KICK": ' + str(params["BODY_TILT_AT_KICK"]) \
- + ',\n"ROTATION_YIELD_RIGHT": ' + str(params["ROTATION_YIELD_RIGHT"]) \
- + ',\n"ROTATION_YIELD_LEFT": ' + str(params["ROTATION_YIELD_LEFT"]) \
- + ',\n"MOTION_SHIFT_TEST_X": ' + str(params["MOTION_SHIFT_TEST_X"]) \
- + ',\n"MOTION_SHIFT_TEST_Y": ' + str(params["MOTION_SHIFT_TEST_Y"]) \
- + ',\n"SIDE_STEP_RIGHT_TEST_RESULT": ' + str(params["SIDE_STEP_RIGHT_TEST_RESULT"]) \
- + ',\n"SIDE_STEP_LEFT_TEST_RESULT": ' + str(params["SIDE_STEP_LEFT_TEST_RESULT"]) \
- + ',\n"RUN_TEST_10_STEPS": ' + str(params["RUN_TEST_10_STEPS"]) \
- + ',\n"RUN_TEST_20_STEPS": ' + str(params["RUN_TEST_20_STEPS"]) \
- + ',\n"KICK_ADJUSTMENT_DISTANCE_1": ' + str(params["KICK_ADJUSTMENT_DISTANCE_1"]) \
- + ',\n"KICK_ADJUSTMENT_DISTANCE_2": ' + str(params["KICK_ADJUSTMENT_DISTANCE_2"]) \
- + ',\n"KICK_OFFSET_OF_BALL": ' + str(params["KICK_OFFSET_OF_BALL"]) \
- + ',\n"IMU_DRIFT_IN_DEGREES_DURING_6_MIN_MEASUREMENT": ' + str(params["IMU_DRIFT_IN_DEGREES_DURING_6_MIN_MEASUREMENT"]) \
- + '\n}'
- with open(filename, "w") as f:
- f.write(jsonstring)
- #json.dump(params_new, f)
- self.motion.turn_To_Course(math.pi/3*2)
- self.motion.turn_To_Course(0)
-
-
-
- def spot_walk_main_cycle(self, pressed_button):
- if pressed_button == 2 or pressed_button ==3 :
- self.rotation_test_main_cycle(pressed_button)
- return
- self.run_test_main_cycle(1, stepLength = 0)
-
- def run_test_main_cycle(self, pressed_button, stepLength = 64):
- if pressed_button == 2 or pressed_button ==3 :
- self.sidestep_test_main_cycle(pressed_button)
- return
- if pressed_button == 5 or pressed_button ==6 :
- self.rotation_test_main_cycle()
- return
- if pressed_button == 8:
- #execfile("Head_Tilt_Calibration.py")
- if self.motion.head_Tilt_Calibration():
- self.motion.kondo.motionPlay(25) # zummer
- else:
- self.motion.kondo.motionPlay(25) # zummer
- self.motion.utime.sleep(0.2)
- self.motion.kondo.motionPlay(25) # zummer
- return
- #stepLength = 64
- #self.motion.move_head(0, self.motion.neck_play_pose - 1400)
- #self.motion.params['BODY_TILT_AT_WALK'] -= 0.01
- #self.glob.camera_ON = True
- #self.motion.stepHeight = 20
- #stepLength = 20
- if pressed_button == 9: stepLength = 0
- number_Of_Cycles = 20
- if pressed_button == 1: number_Of_Cycles = 10
- sideLength = 0
- self.motion.first_Leg_Is_Right_Leg = True
- if self.motion.first_Leg_Is_Right_Leg: invert = -1
- else: invert = 1
- #self.motion.stepHeight = 20.0
- self.motion.walk_Initial_Pose()
- number_Of_Cycles += 1
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- if cycle ==0 : stepLength1 = stepLength/3
- if cycle ==1 : stepLength1 = stepLength/3 * 2
- self.motion.refresh_Orientation()
- self.motion.body_euler_angle_calc()
- rotation = 0 + invert * self.motion.body_euler_angle['yaw'] * 1.0
- #if rotation > 0: rotation *= 1.5
- rotation = self.motion.normalize_rotation(rotation)
- #rotation = 0
- self.motion.walk_Cycle(stepLength1,sideLength, rotation,cycle, number_Of_Cycles)
- self.motion.walk_Final_Pose()
-
- def sidestep_test_main_cycle(self, pressed_button):
- number_Of_Cycles = 20
- stepLength = 0 #64
- sideLength = 20
- if pressed_button == 3:
- self.motion.first_Leg_Is_Right_Leg = False
- if self.motion.first_Leg_Is_Right_Leg: invert = -1
- else: invert = 1
- self.motion.walk_Initial_Pose()
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- #if cycle ==0 : stepLength1 = stepLength/3
- #if cycle ==1 : stepLength1 = stepLength/3 * 2
- self.motion.refresh_Orientation()
- rotation = 0 + invert * self.motion.imu_body_yaw() * 1.0
- rotation = self.motion.normalize_rotation(rotation)
- #rotation = 0
- self.motion.walk_Cycle(stepLength1,sideLength, rotation,cycle, number_Of_Cycles)
- self.motion.walk_Final_Pose()
-
- def norm_yaw(self, yaw):
- yaw %= 2 * math.pi
- if yaw > math.pi: yaw -= 2* math.pi
- if yaw < -math.pi: yaw += 2* math.pi
- return yaw
-
- def forward_main_cycle(self, pressed_button):
- self.glob.with_pf = False
- second_player_timer = self.motion.utime.time()
- self.f = Forward_Vector_Matrix(self.motion, self.local, self.glob)
- if self.glob.SIMULATION == 2:
- self.motion.kondo.motionPlay(25) # zummer
- # pressed_button = self.motion.push_Button()
- #if pressed_button == 1 and self.first_pressed_button == 1:
- # self.motion.kick_off_ride()
- # first_look_point = None
- #else:
- # first_look_point= self.glob.ball_coord
- while (True):
- if (self.motion.utime.time() - self.motion.start_point_for_imu_drift) and (self.glob.SIMULATION == 2) > 360:
- self.motion.turn_To_Course(0)
- self.motion.turn_To_Course(0, accurate = True)
- if self.glob.SIMULATION == 2:
- for i in range(5):
- self.motion.kondo.motionPlay(25)
- self.motion.pause_in_ms(400)
- break
- if self.motion.falling_Flag != 0:
- if self.motion.falling_Flag == 3: break
- self.motion.falling_Flag = 0
- self.local.coordinate_fall_reset()
- print('coord =', self.glob.pf_coord, 'ball =', self.glob.ball_coord)
- success_Code = None
- napravl = None
- dist = None
- if (self.glob.mqttc):
- success_Code, napravl, dist = self.glob.update_coord_by_mqtt()
- else:
- success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True, with_Localization = False,
- very_Fast = True)
-
- # add mqtt connetction
- print("past mqttc check")
- first_look_point = self.glob.ball_coord
- if self.glob.SIMULATION == 2 and self.glob.wifi_params['WIFI_IS_ON']: self.local.report_to_WIFI()
- if pressed_button == 4 and (self.motion.utime.time() - second_player_timer) < 10 : continue
- self.f.dir_To_Guest()
- print('direction_To_Guest = ', math.degrees(self.f.direction_To_Guest), 'degrees')
- print('coord =', self.glob.pf_coord, 'ball =', self.glob.ball_coord)
- if dist == 0 and success_Code == False:
- self.motion.turn_To_Course(self.glob.pf_coord[2]+ 2 * math.pi / 3)
- continue
- player_from_ball_yaw = coord2yaw(self.glob.pf_coord[0] - self.glob.ball_coord[0],
- self.glob.pf_coord[1] - self.glob.ball_coord[1]) - self.f.direction_To_Guest
- player_from_ball_yaw = self.norm_yaw(player_from_ball_yaw)
- player_in_front_of_ball = -math.pi/2 < player_from_ball_yaw < math.pi/2
- player_in_fast_kick_position = (player_from_ball_yaw > 2.5 or player_from_ball_yaw < -2.5) and dist < 0.6
- if dist > 0.35 and not player_in_fast_kick_position:
- if dist > 1.5: stop_Over = True
- else: stop_Over = False
- direction_To_Ball = math.atan2((self.glob.ball_coord[1] - self.glob.pf_coord[1]), (self.glob.ball_coord[0] - self.glob.pf_coord[0]))
- print('napravl :', napravl)
- print('direction_To_Ball', direction_To_Ball)
- #self.motion.far_distance_plan_approach(self.glob.ball_coord, self.f.direction_To_Guest, stop_Over = stop_Over)
- self.motion.far_distance_straight_approach(self.glob.ball_coord, direction_To_Ball, stop_Over = stop_Over)
- #self.go_Around_Ball(dist, napravl)
- continue
- if player_in_front_of_ball or not player_in_fast_kick_position:
- self.go_Around_Ball(dist, napravl)
- self.motion.turn_To_Course(self.f.direction_To_Guest)
- small_kick = False
- if self.f.kick_Power > 1: small_kick = True
- success_Code = self.motion.near_distance_ball_approach_and_kick(self.f.direction_To_Guest, strong_kick = False, small_kick = small_kick, very_Fast = False)
-
- def goalkeeper1_main_cycle(self):
- def ball_position_is_dangerous(row, col):
- danger = False
- danger = (col <= (round(self.glob.COLUMNS / 3) - 1))
- if ((row <= (round(self.glob.ROWS / 3) - 1) or row >= round(self.glob.ROWS * 2 / 3)) and col == 0) or (col == 1 and (row == 0 or row == (self.glob.ROWS -1))):
- danger = False
- return danger
- second_player_timer = self.motion.utime.time()
- self.f = Forward_Vector_Matrix(self.motion, self.local, self.glob)
- #self.motion.near_distance_omni_motion(400, 0) # get out from goal
- fast_Reaction_On = True
- while (True):
- if self.motion.falling_Flag != 0:
- if self.motion.falling_Flag == 3: break
- self.motion.falling_Flag = 0
- self.local.coordinate_fall_reset()
- if self.glob.ball_coord[0] <= 0.15:
- success_Code, napravl, dist, speed = self.motion.watch_Ball_In_Pose()
- else:
- success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On = fast_Reaction_On)
- if abs(speed[0]) > 0.02 and dist < 1 : # if dangerous tangential speed
- fast_Reaction_On = True
- if speed[0] > 0:
- if self.glob.pf_coord[1] < 0.35:
- self.motion.play_Soft_Motion_Slot(name ='PenaltyDefenceL')
- else:
- if self.glob.pf_coord[1] > -0.35:
- self.motion.play_Soft_Motion_Slot(name ='PenaltyDefenceR')
- self.motion.pause_in_ms(3000)
- continue
- if speed[1] < - 0.01 and dist < 1.5 : # if dangerous front speed
- fast_Reaction_On = True
- self.motion.play_Soft_Motion_Slot(name = 'PanaltyDefenceReady_Fast')
- self.motion.play_Soft_Motion_Slot(name = 'PenaltyDefenceF')
- self.motion.pause_in_ms(3000)
- self.motion.play_Soft_Motion_Slot(name = 'Get_Up_From_Defence')
- continue
- if (self.motion.utime.time() - second_player_timer) < 10 : continue
- row, col = self.f.dir_To_Guest()
- #print('direction_To_Guest = ', math.degrees(self.f.direction_To_Guest), 'degrees')
- #print('goalkeeper coord =', self.glob.pf_coord, 'ball =', self.glob.ball_coord, 'row =', row, 'col =', col, 'ball_position_is_dangerous =', ball_position_is_dangerous(row,col))
- if dist == 0 and success_Code == False:
- if self.glob.pf_coord[0] > -1.3:
- print('goalkeeper turn_To_Course(pi*2/3)')
- self.motion.turn_To_Course(self.glob.pf_coord[2]+ 2 * math.pi / 3)
- continue
- if ball_position_is_dangerous(row, col):
- fast_Reaction_On = True
- player_from_ball_yaw = coord2yaw(self.glob.pf_coord[0] - self.glob.ball_coord[0], self.glob.pf_coord[1] - self.glob.ball_coord[1]) - self.f.direction_To_Guest
- player_from_ball_yaw = self.norm_yaw(player_from_ball_yaw)
- player_in_front_of_ball = -math.pi/2 < player_from_ball_yaw < math.pi/2
- player_in_fast_kick_position = (player_from_ball_yaw > 2 or player_from_ball_yaw < -2) and dist < 0.6
- if dist > 0.35 and not player_in_fast_kick_position:
- if dist > 1: stop_Over = True
- else: stop_Over = False
- print('goalkeeper far_distance_plan_approach')
- direction_To_Ball = math.atan2((self.glob.ball_coord[1] - self.glob.pf_coord[1]), (self.glob.ball_coord[0] - self.glob.pf_coord[0]))
- self.motion.far_distance_straight_approach(self.glob.ball_coord, direction_To_Ball, stop_Over = stop_Over)
- #self.f.turn_Face_To_Guest()
- continue
- if player_in_front_of_ball or not player_in_fast_kick_position:
- self.go_Around_Ball(dist, napravl)
- print('goalkeeper turn_To_Course(direction_To_Guest)')
- self.motion.turn_To_Course(self.f.direction_To_Guest)
- small_kick = False
- if self.f.kick_Power > 1: small_kick = True
- print('goalkeeper near_distance_ball_approach_and_kick')
- success_Code = self.motion.near_distance_ball_approach_and_kick(self.f.direction_To_Guest, strong_kick = False, small_kick = small_kick)
-
- else:
- fast_Reaction_On = False
- duty_x_position = min((-self.glob.landmarks['FIELD_LENGTH']/2 + 0.4),(self.glob.ball_coord[0]-self.glob.landmarks['FIELD_LENGTH']/2)/2)
- duty_y_position = self.glob.ball_coord[1] * (duty_x_position + self.glob.landmarks['FIELD_LENGTH']/2) / (self.glob.ball_coord[0] + self.glob.landmarks['FIELD_LENGTH']/2)
- duty_distance = math.sqrt((duty_x_position - self.glob.pf_coord[0])**2 + (duty_y_position - self.glob.pf_coord[1])**2)
- #print('duty_x_position =', duty_x_position, 'duty_y_position =', duty_y_position)
- if duty_distance < 0.2 : continue
- elif duty_distance < 3: # 0.6 :
- print('goalkeeper turn_To_Course(0)')
- self.motion.turn_To_Course(0)
- duty_direction = coord2yaw(duty_x_position - self.glob.pf_coord[0], duty_y_position - self.glob.pf_coord[1])
- print('goalkeeper near_distance_omni_motion')
- self.motion.near_distance_omni_motion(duty_distance * 1000, duty_direction)
- print('goalkeeper turn_To_Course(0)')
- self.motion.turn_To_Course(0)
- else:
- direction_To_Duty = math.atan2((duty_y_position - self.glob.pf_coord[1]), (duty_x_position - self.glob.pf_coord[0]))
- self.motion.far_distance_straight_approach([duty_x_position , duty_y_position], direction_To_Duty, gap = 0, stop_Over = False)
- self.motion.turn_To_Course(0)
-
-
- def penalty_Shooter_main_cycle(self):
- self.f = Forward(self.motion, self.local, self.glob)
- first_shoot = True
- first_look_point = [0.9, 0]
- while (True):
- if self.motion.falling_Flag != 0:
- if self.motion.falling_Flag == 3: break
- self.motion.falling_Flag = 0
- self.local.coordinate_fall_reset()
- success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True, with_Localization = False,
- very_Fast = False, first_look_point=first_look_point )
- first_look_point = self.glob.ball_coord
- self.f.dir_To_Guest()
- print('ball_coord = ', self.glob.ball_coord)
- print('direction_To_Guest = ', math.degrees(self.f.direction_To_Guest), 'degrees')
- if dist == 0 and success_Code == False:
- self.motion.turn_To_Course(self.glob.pf_coord[2]+ 2 * math.pi / 3)
- continue
- player_from_ball_yaw = coord2yaw(self.glob.pf_coord[0] - self.glob.ball_coord[0],
- self.glob.pf_coord[1] - self.glob.ball_coord[1]) - self.f.direction_To_Guest
- player_from_ball_yaw = self.norm_yaw(player_from_ball_yaw)
- player_in_front_of_ball = -math.pi/2 < player_from_ball_yaw < math.pi/2
- player_in_fast_kick_position = (player_from_ball_yaw > 2.5 or player_from_ball_yaw < -2.5) and dist < 0.6
- if dist > 0.35 and not player_in_fast_kick_position:
- if dist > 1: stop_Over = True
- else: stop_Over = False
- direction_To_Ball = math.atan2((self.glob.ball_coord[1] - self.glob.pf_coord[1]), (self.glob.ball_coord[0] - self.glob.pf_coord[0]))
- self.motion.far_distance_straight_approach(self.glob.ball_coord, direction_To_Ball, stop_Over = stop_Over)
- continue
- if player_in_front_of_ball or not player_in_fast_kick_position:
- self.go_Around_Ball(dist, napravl)
- self.motion.turn_To_Course(self.f.direction_To_Guest)
- #if first_shoot:
- success_Code = self.motion.near_distance_ball_approach_and_kick(self.f.direction_To_Guest, strong_kick = False)
- first_shoot = False
-
-
- def penalty_Goalkeeper_main_cycle(self):
- self.g = GoalKeeper(self.motion, self.local, self.glob)
- self.glob.obstacleAvoidanceIsOn = False
- first_Get_Up = True
- while (True):
- dist = -1.0
- if self.motion.falling_Flag != 0:
- if self.motion.falling_Flag == 3: break
- self.motion.falling_Flag = 0
- self.local.coordinate_fall_reset()
- self.g.turn_Face_To_Guest()
- if first_Get_Up:
- first_Get_Up = False
- self.g.goto_Center()
- while(dist < 0):
- a, napravl, dist, speed = self.motion.watch_Ball_In_Pose(penalty_Goalkeeper = True)
- #print('speed = ', speed, 'dist =', dist , 'napravl =', napravl)
- if abs(speed[0]) > 0.002 and dist < 1 : # if dangerous tangential speed
- if speed[0] > 0:
- self.motion.play_Motion_Slot(name ='PenaltyDefenceL')
- else:
- self.motion.play_Motion_Slot(name ='PenaltyDefenceR')
- continue
- if speed[1] < - 0.01 and dist < 1.5 : # if dangerous front speed
- self.motion.play_Motion_Slot(name = 'PanaltyDefenceReady_Fast')
- self.motion.play_Motion_Slot(name = 'PenaltyDefenceF')
- self.motion.pause_in_ms(5000)
- self.motion.play_Motion_Slot(name = 'Get_Up_From_Defence')
-
- if (dist == 0 and napravl == 0) or dist > 2.5:
- continue
- old_neck_pan, old_neck_tilt = self.motion.head_Up()
- if (dist <= 0.5 and 0 <= napravl <= math.pi/4): self.g.scenario_A1( dist, napravl)
- if (dist <= 0.5 and math.pi/4 < napravl <= math.pi/2): self.g.scenario_A2( dist, napravl)
- if (dist <= 0.5 and 0 >= napravl >= -math.pi/4): self.g.scenario_A3( dist, napravl)
- if (dist <= 0.7 and -math.pi/4 > napravl >= -math.pi/2): self.g.scenario_A4( dist, napravl)
- if ((0.5 < dist < self.glob.landmarks['FIELD_LENGTH']/2) and (math.pi/18 <= napravl <= math.pi/4)): self.g.scenario_B1()
- if ((0.5 < dist < self.glob.landmarks['FIELD_LENGTH']/2) and (math.pi/4 < napravl <= math.pi/2)): self.g.scenario_B2()
- if ((0.5 < dist < self.glob.landmarks['FIELD_LENGTH']/2) and (-math.pi/18 >= napravl >= -math.pi/4)): self.g.scenario_B3()
- if ((0.7 < dist < self.glob.landmarks['FIELD_LENGTH']/2) and (-math.pi/4 > napravl >= -math.pi/2)): self.g.scenario_B4()
- self.motion.head_Return(old_neck_pan, old_neck_tilt)
-
- def go_Around_Ball(self, dist, napravl):
- turning_radius = 0.18 # meters
- #first_look_point= self.glob.ball_coord
- #success_Code, napravl, dist, speed = self.motion.seek_Ball_In_Pose(fast_Reaction_On = True, with_Localization = False,
- # very_Fast = True, first_look_point=first_look_point)
- #if success_Code != True: return
- if dist > 0.5: return
- correction_x = dist * math.cos(napravl)
- correction_y = dist * math.sin(napravl)
- alpha = self.f.direction_To_Guest - self.glob.pf_coord[2]
- alpha = self.motion.norm_yaw(alpha)
- initial_body_yaw = self.glob.pf_coord[2]
- correction_napravl = math.atan2(correction_y, (correction_x - turning_radius))
- correction_dist = math.sqrt(correction_y**2 + (correction_x - turning_radius)**2)
- old_neck_pan, old_neck_tilt = self.motion.head_Up()
- self.motion.walk_Initial_Pose()
- if napravl * alpha > 0:
- self.motion.turn_To_Course(self.glob.pf_coord[2] + napravl, one_Off_Motion = False)
- self.motion.walk_Restart()
- self.motion.near_distance_omni_motion((dist - turning_radius) * 1000 , 0, one_Off_Motion = False)
- alpha = self.motion.norm_yaw(alpha - napravl)
- initial_body_yaw += napravl
- else:
- self.motion.near_distance_omni_motion(correction_dist*1000, correction_napravl, one_Off_Motion = False)
- if alpha >= 0:
- if self.motion.first_Leg_Is_Right_Leg == False:
- change_legs = True
- else:
- change_legs = False
- self.motion.walk_Restart()
- self.motion.first_Leg_Is_Right_Leg = True
- side_step_yield = self.motion.side_step_right_yield
- invert = 1
- else:
- if self.motion.first_Leg_Is_Right_Leg == True:
- change_legs = True
- else:
- change_legs = False
- self.motion.walk_Restart()
- self.motion.first_Leg_Is_Right_Leg = False
- side_step_yield = self.motion.side_step_left_yield
- invert = -1
- #print('6self.motion.first_Leg_Is_Right_Leg:', self.motion.first_Leg_Is_Right_Leg)
- yaw_increment_at_side_step = 2 * math.copysign(2 * math.asin(side_step_yield / 2 / (turning_radius * 1000)), alpha)
- number_Of_Cycles = int(round(abs(alpha / yaw_increment_at_side_step)))+1
- stepLength = 0
- sideLength = 40
- for cycle in range(number_Of_Cycles):
- self.motion.refresh_Orientation()
- rotation = initial_body_yaw + cycle * yaw_increment_at_side_step - self.motion.imu_body_yaw() * 1
- rotation = self.motion.normalize_rotation(rotation)
- #rotation = 0
- self.motion.walk_Cycle(stepLength, sideLength, invert*rotation, cycle, number_Of_Cycles)
- self.motion.walk_Final_Pose()
- self.motion.first_Leg_Is_Right_Leg = True
-
- def sprint_main_cycle(self, second_pressed_button):
- with open(self.glob.current_work_directory + "/Init_params/Sprint_params.json", "r") as f:
- sprint_params = json.loads(f.read())
- proportional = sprint_params['proportional']
- differential = sprint_params['differential']
- last_heading = 0
- stepLength = 64
- number_Of_Cycles = 100
- sideLength = 0
- if self.glob.SIMULATION == 2:
- pressed_button = self.motion.push_Button()
- self.motion.walk_Initial_Pose()
- number_Of_Cycles += 1
- for cycle in range(number_Of_Cycles):
- stepLength1 = stepLength
- if cycle ==0 : stepLength1 = stepLength/3
- if cycle ==1 : stepLength1 = stepLength/3 * 2
- self.motion.refresh_Orientation()
- heading = self.motion.imu_body_yaw()
- rotation = 0 + heading * proportional + (heading - last_heading) * differential
- rotation = self.motion.normalize_rotation(rotation)
- #rotation = 0
- self.motion.walk_Cycle(stepLength1, sideLength, rotation, cycle, number_Of_Cycles)
- last_heading = heading
- self.motion.walk_Final_Pose()
-
-if __name__=="__main__":
- pass
diff --git a/src/azer_robocup_project/Soccer/utility.py b/src/azer_robocup_project/Soccer/utility.py
deleted file mode 100644
index 7597954..0000000
--- a/src/azer_robocup_project/Soccer/utility.py
+++ /dev/null
@@ -1,58 +0,0 @@
-"""
-Module provides some utilities which can run with same result and same
-synatx both in Micropython of OpenMV and in standard CPython
-"""
-
-
-import sys, math
-
-if sys.version == '3.4.0':
- from urandom import getrandbits
- used_with_OpenMV = True
-else:
- from random import *
- used_with_OpenMV = False
-
-
-
-def randrange( start, stop=None):
- """
- helper function for working with random bit sequence
- """
- if stop is None:
- stop = start
- start = 0
- upper = stop - start
- bits = 0
- pwr2 = 1
- while upper > pwr2:
- pwr2 <<= 1
- bits += 1
- while True:
- r = getrandbits(bits)
- if r < upper:
- break
- return r + start
-
-def random():
- """
- getting a random number from 0 to 1
- """
- return randrange(10000) / 10000
-
-def gauss( mu, sigma):
- """
- #getting a random number from Gaussian distribution
- """
- x2pi = random() * math.pi * 2
- g2rad = math.sqrt(-2.0 * math.log(1.0 - random()))
- z = math.cos(x2pi) * g2rad
- return mu + z * sigma
-
-def gaussian( x, sigma):
- """
- # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
- """
- return math.exp(-(x ** 2) / 2*(sigma ** 2)) / math.sqrt(2.0 * math.pi * (sigma ** 2))
-
-
diff --git a/src/azer_robocup_project/System Volume Information/IndexerVolumeGuid b/src/azer_robocup_project/System Volume Information/IndexerVolumeGuid
deleted file mode 100644
index 12eb93e6c3a5c2768557deb099871e879acdc72d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 76
zcmXYnQ3`-C48?Mxg4<N|b93`j;@uZPO7mVrAChyB=~ONm6gHYJSULYzy_-I`wL8Q8
KzPen@!OH|)9|;fu
diff --git a/src/azer_robocup_project/System Volume Information/WPSettings.dat b/src/azer_robocup_project/System Volume Information/WPSettings.dat
deleted file mode 100644
index 79062b3d946a99056fb90324d1fef7998cd82df7..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 12
Tcmd;KU|_hNt}j!OQ{Dyu5Q77I
diff --git a/src/azer_robocup_project/Treshold_Utility.py b/src/azer_robocup_project/Treshold_Utility.py
deleted file mode 100644
index 41fe92f..0000000
--- a/src/azer_robocup_project/Treshold_Utility.py
+++ /dev/null
@@ -1,57 +0,0 @@
-# Single Color RGB565 Blob Tracking Example
-#
-# This example shows off single color RGB565 tracking using the OpenMV Cam.
-
-import sensor, image, time, math, pyb
-
-colorN = 1 # 0 for orange, 1 for blue, 2 for yellow, 3 or 5 for white, 4 for green
-
-# Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max)
-# The below thresholds track in general red/green/blue things. You may wish to tune them...
-
-
-th_index = ["orange ball", "blue posts", "yellow posts", "white posts", "green field", "white marking"]
-thresholds ={
- "orange ball": [48, 87, 4, 63, 39, 79],
- "blue posts": [11, 49, 3, 45, -61, 1],
- "yellow posts": [57, 100, -25, -3, 14, 76],
- "white posts": [46, 100, -17, 23, -20, 15],
- "green field": [5, 71, -65, -16, -5, 58],
- "white marking": [60, 100, -37, 4, -19, 13]
- }
-
-sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QVGA)
-sensor.skip_frames(time = 500)
-sensor.set_auto_exposure(False)
-sensor.set_auto_whitebal(False)
-sensor.skip_frames(time = 500)
-#sensor.set_auto_gain(False, gain_db=0 ) # 9.172951)
-#sensor.skip_frames(time = 500)
-#sensor.set_auto_exposure(False, exposure_us=2000) #6576)
-#sensor.skip_frames(time = 500)
-sensor.set_auto_whitebal(False, rgb_gain_db = (-6.0, -3.0, 3.0))
-
-clock = time.clock()
-
-
-while(True):
- clock.tick()
- img = sensor.snapshot().lens_corr(strength = 1.45, zoom = 1.0)
- for blob in img.find_blobs([thresholds[th_index[colorN]]], pixels_threshold=50, area_threshold=50, merge=True):
- # These values depend on the blob not being circular - otherwise they will be shaky.
- if blob.elongation() > 0.5:
- img.draw_edges(blob.min_corners(), color=(255,0,0))
- img.draw_line(blob.major_axis_line(), color=(0,255,0))
- img.draw_line(blob.minor_axis_line(), color=(0,0,255))
- # These values are stable all the time.
- img.draw_rectangle(blob.rect())
- img.draw_cross(blob.cx(), blob.cy())
- # Note - the blob rotation is unique to 0-180 only.
- #img.draw_keypoints([(blob.cx(), blob.cy(), int(math.degrees(blob.rotation())))], size=20)
- pyb.delay(1000)
- img2 = img.binary([thresholds[th_index[colorN]]],to_bitmap=True, copy=True)
- print(img2.compressed_for_ide(), end="")
- pyb.delay(1000)
- print(clock.fps())
diff --git a/src/azer_robocup_project/button_test.py b/src/azer_robocup_project/button_test.py
deleted file mode 100644
index 4b14371..0000000
--- a/src/azer_robocup_project/button_test.py
+++ /dev/null
@@ -1,152 +0,0 @@
-# Untitled - By: a - Ð’Ñ Ð¸ÑŽÐ½ 21 2020
-
-import sensor
-import pyb
-from pyb import Pin, ExtInt, LED
-
-class Button_Test:
- def __init__(self):
- from pyb import Pin, ExtInt, LED
- self.red_led = LED(1)
- self.green_led = LED(2)
- self.blue_led = LED(3)
- PinMapperDict = { 'TopRight' : Pin.board.P2, 'BottomRight': Pin.board.P3,
- 'BottomLeft': Pin.board.P9 }
- Pin.dict(PinMapperDict)
-
- self.TopRightPin = Pin('TopRight', Pin.IN, Pin.PULL_UP)
- self.BottomRightPin = Pin('BottomRight', Pin.IN, Pin.PULL_UP)
- self.BottomLeftPin = Pin('BottomLeft', Pin.IN, Pin.PULL_UP)
-
- self.TopRightButtonON = False
- self.BottomRightButtonON = False
- self.BottomLeftButtonON = False
-
- self.extint1 = ExtInt(self.TopRightPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, None)
- self.extint2 = ExtInt(self.BottomRightPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, None)
- self.extint3 = ExtInt(self.BottomLeftPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, None)
- self.extint1 = ExtInt(self.TopRightPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, self.callback00)
- self.extint2 = ExtInt(self.BottomRightPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, self.callback01)
- self.extint3 = ExtInt(self.BottomLeftPin, ExtInt.IRQ_RISING_FALLING, Pin.PULL_UP, self.callback02)
-
- def deactivate_P2_P3(self):
- self.TopRightPin = Pin('TopRight', 2, 2, af = 5)
- self.BottomRightPin = Pin('BottomRight', 1, 1, af = 0)
-
- def callback00(self, line):
- if self.TopRightPin.value() == 0:
- self.TopRightButtonON = True
- else: self.TopRightButtonON = False
-
- def callback01(self, line):
- if self.BottomRightPin.value() == 0:
- self.BottomRightButtonON = True
- else: self.BottomRightButtonON = False
-
- def callback02(self, line):
- if self.BottomLeftPin.value() == 0:
- self.BottomLeftButtonON = True
- else: self.BottomLeftButtonON = False
-
- def white_led_blink(self):
- pyb.delay(50)
- self.red_led.toggle()
- self.green_led.toggle()
- self.blue_led.toggle()
-
- def white_led_blink_long(self):
- pyb.delay(200)
- self.red_led.toggle()
- self.green_led.toggle()
- self.blue_led.toggle()
-
- def white_led_off(self):
- self.red_led.off()
- self.green_led.off()
- self.blue_led.off()
-
- def wait_for_button_pressing(self):
- returnCode = 0
-
- TR_short_pressed = False
- TR_long_pressed = False
- BR_short_pressed = False
- BR_long_pressed = False
- BL_short_pressed = False
- BL_long_pressed = False
-
- TR_pressed = False
- BR_pressed = False
- BL_pressed = False
-
- buttons_pressed = 0
-
- TopRightTimerStart = pyb.millis()
- BottomRightTimerStart = pyb.millis()
- BottomLeftTimerStart = pyb.millis()
-
- while True:
- if TR_pressed == False and BR_pressed == False and BL_pressed == False:
- self.white_led_blink_long() # ready
- if self.TopRightButtonON == True and TR_pressed == False:
- TopRightTimerStart = pyb.millis()
- TR_pressed = True
- buttons_pressed += 1
- TR_time_elapsed = pyb.elapsed_millis(TopRightTimerStart)
- if TR_time_elapsed > 1000 and TR_pressed == True:
- self.white_led_blink()
- if TR_time_elapsed > 50: # button bounce cancellation
- if self.TopRightButtonON == False and TR_pressed == True:
- if TR_time_elapsed > 1000: TR_long_pressed = True
- else: TR_short_pressed = True
- self.white_led_off()
- break
- if self.BottomRightButtonON == True and BR_pressed == False:
- BottomRightTimerStart = pyb.millis()
- BR_pressed = True
- buttons_pressed += 1
- BR_time_elapsed = pyb.elapsed_millis(BottomRightTimerStart)
- if BR_time_elapsed > 1000 and BR_pressed == True:
- self.white_led_blink()
- if BR_time_elapsed > 50: # button bounce cancellation
- if self.BottomRightButtonON == False and BR_pressed == True:
- if BR_time_elapsed > 1000: BR_long_pressed = True
- else: BR_short_pressed = True
- self.white_led_off()
- break
- if self.BottomLeftButtonON == True and BL_pressed == False:
- BottomLeftTimerStart = pyb.millis()
- BL_pressed = True
- buttons_pressed += 1
- BL_time_elapsed = pyb.elapsed_millis(BottomLeftTimerStart)
- if BL_time_elapsed > 1000 and BL_pressed == True:
- self.white_led_blink()
- if BL_time_elapsed > 50: # button bounce cancellation
- if self.BottomLeftButtonON == False and BL_pressed == True:
- if BL_time_elapsed > 1000: BL_long_pressed = True
- else: BL_short_pressed = True
- self.white_led_off()
- break
-
- if buttons_pressed > 1:
- if TR_pressed and BR_pressed: returnCode = 7
- if TR_pressed and BL_pressed: returnCode = 8
- if BL_pressed and BR_pressed: returnCode = 9
- else:
- if TR_short_pressed: returnCode = 1
- if BR_short_pressed: returnCode = 2
- if BL_short_pressed: returnCode = 3
- if TR_long_pressed: returnCode = 4
- if BR_long_pressed: returnCode = 5
- if BL_long_pressed: returnCode = 6
-
- return returnCode
-
-if __name__ == "__main__":
- b = Button_Test()
- returnCode = b.wait_for_button_pressing()
- print(returnCode)
-
-
-
-
diff --git a/src/azer_robocup_project/main.py b/src/azer_robocup_project/main.py
deleted file mode 100644
index b69a3bf..0000000
--- a/src/azer_robocup_project/main.py
+++ /dev/null
@@ -1,182 +0,0 @@
-import sys
-import os
-import math
-import json
-import time
-
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/')
-if sys.version == '3.4.0':
- # will be running on openMV
- SIMULATION = 2 # 2 - live on openMV
-else:
- # will be running on desktop computer
- current_work_directory += '/'
- import threading
- SIMULATION = 1 # 0 - Simulation without physics,
- # 1 - Simulation synchronous with physics,
- # 3 - Simulation streaming with physics
-
-if SIMULATION == 2:
- try:
- from button_test import Button_Test
- button = Button_Test()
- first_pressed_button = button.wait_for_button_pressing()
- del button
- if first_pressed_button == 9:
- execfile("threshold_tuner_server.py")
- except Exception as e:
- f = open("Error.txt",'w')
- sys.print_exception(e,f)
- f.close()
- sys.print_exception(e,sys.stdout)
-
-
-
-try:
- sys.path.append( current_work_directory + 'Soccer/')
- sys.path.append( current_work_directory + 'Soccer/Motion/')
- sys.path.append( current_work_directory + 'Soccer/Vision/')
- sys.path.append( current_work_directory + 'Soccer/Localisation/')
- sys.path.append( current_work_directory + 'Soccer/Localisation/PF/')
- sys.path.append( current_work_directory)
- #print('sys.path:', sys.path )
- from class_Glob import Glob
- from class_Vision import Vision
- from class_Local import *
- from strategy import Player
-
- if SIMULATION == 2:
- from class_Motion_real import Motion_real as Motion
- with open(current_work_directory + "Init_params/Real/Real_landmarks.json", "r") as f:
- landmarks = json.loads(f.read())
- subrole = ' '
- if first_pressed_button == 1:
- role = 'forward'
- initial_coord = [-0.5, 0, 0]
- if first_pressed_button == 2:
- role = 'forward'
- subrole = ' Rignt'
- x = -(landmarks['FIELD_LENGTH'] / 2 - 0.9)
- y = -landmarks['FIELD_WIDTH'] / 2
- initial_coord = [x, y, math.pi/2]
- if first_pressed_button == 3:
- role = 'forward'
- subrole = ' Left'
- x = -(landmarks['FIELD_LENGTH'] / 2 - 0.9)
- y = landmarks['FIELD_WIDTH'] / 2
- initial_coord = [x, y, -math.pi/2]
- if first_pressed_button == 4:
- role = 'penalty_Shooter'
- initial_coord = [0.2, 0, 0]
- if first_pressed_button == 5:
- role = 'penalty_Goalkeeper'
- initial_coord = [-landmarks['FIELD_LENGTH'] / 2, 0, 0]
- if first_pressed_button == 6:
- role = 'goalkeeper'
- initial_coord = [-landmarks['FIELD_LENGTH'] / 2, 0, 0]
- if first_pressed_button == 7:
- role = 'run_test'
- initial_coord = [0.0, 0.0, 0]
- if first_pressed_button == 8:
- role = 'sprint'
- initial_coord = [0.0, 0.0, 0]
- glob = Glob(SIMULATION, current_work_directory, particles_number = 100)
- glob.pf_coord = initial_coord
-
- vision = Vision(glob)
- motion = Motion(glob, vision)
- motion.falling_Flag = 0
- if role == 'forward' or role == 'sprint':
- second_pressed_button = 1
- else: second_pressed_button = motion.push_Button()
- if role == 'dance':
- local = None
- else:
- motion.direction_To_Attack = -initial_coord[2]
- motion.activation()
- local = Local(motion, glob, vision, coord_odometry = initial_coord)
- motion.local = local
- local.coordinate_record(odometry = True)
- print( role, subrole, ' initial_coord = ', initial_coord)
- player = Player(role, first_pressed_button, second_pressed_button, glob, motion, local)
- player.play_game()
- else:
- from class_Motion import *
- from class_Motion_sim import*
- from class_Motion_sim import Motion_sim as Motion
- from class_Motion_sim import sim_Enable, Transfer_Data, simulation_Trigger_Accumulator
-
- clientID = []
- transfer_Datas =[]
- motion =[]
- local =[]
- glob = []
- vision = []
- robots_Number = 1
- robot_IDs = [ '', '#0', '#1', '#2']
- initial_coord = [[-0.5, 0, 0], [ -1.8, 0, 0], [-1.1, 0, 0], [-1.8, 0, 0]]
- clientID.append(sim_Enable('127.0.0.1', -19997))
- print('clientID1 =', clientID[0])
- if robots_Number > 1:
- clientID.append(sim_Enable('127.0.0.2', -19998))
- print('clientID2 =', clientID[1])
- if robots_Number > 2:
- clientID.append(sim_Enable('127.0.0.3', -19999))
- print('clientID3 =', clientID[2])
- if robots_Number > 3:
- clientID.append(sim_Enable('127.0.0.4', -20000))
- print('clientID4 =', clientID[3])
-
- events = []
- t = []
- m =[]
- lock = threading.Lock()
- transfer_Data = Transfer_Data()
- for i in range(robots_Number):
- glob.append(Glob(SIMULATION, current_work_directory, particles_number = 100))
- glob[i].pf_coord = initial_coord[i]
- events.append(threading.Event())
- transfer_Datas.append(transfer_Data)
- vision.append(Vision(glob[i]))
- motion.append(Motion(glob[i], vision[i], clientID[i] , events[i], lock, transfer_Datas[i], robots_Number, robot_Number = robot_IDs[i]))
- motion[i].sim_Start()
- motion[i].direction_To_Attack = -initial_coord[i][2]
- motion[i].activation()
- local.append(Local(motion[i], glob[i], vision[i], coord_odometry = initial_coord[i]))
- motion[i].local = local[i]
- local[i].coordinate_record(odometry = True)
- motion[i].falling_Flag = 0
- #goalkeeper, forward_v2, run_test, penalty_Shooter, rotation_test, penalty_Goalkeeper, spot_walk, dance, quaternion_test, balancing_test, side_turn, marathon
- if i == 0 or i == 3:
- first_pressed_button = 8
- second_pressed_button = 4
- m.append(Player('forward', first_pressed_button, second_pressed_button, glob[i], motion[i], local[i]))
- if i == 1:
- first_pressed_button = 6
- second_pressed_button = 2
- m.append(Player('forward', first_pressed_button, second_pressed_button, glob[i], motion[i], local[i]))
- if i == 2:
- first_pressed_button = 1
- second_pressed_button = 1
- m.append(Player('forward_v2', first_pressed_button, second_pressed_button, glob[i], motion[i], local[i]))
-
- if SIMULATION == 1 :
- t0 = threading.Thread( target = simulation_Trigger_Accumulator, args=(clientID, events, transfer_Datas, lock))
- for i in range(robots_Number):
- t.append(threading.Thread( target = m[i].play_game, args=()))
- if SIMULATION == 1 :
- t0.setDaemon(True)
- t0.start()
- for i in range(robots_Number): t[i].start()
-
-
-except Exception as e:
- if SIMULATION == 2:
- f = open("Error.txt",'w')
- sys.print_exception(e,f)
- f.close()
- sys.print_exception(e,sys.stdout)
- else:
- print(e)
diff --git a/src/azer_robocup_project/mqtt.py b/src/azer_robocup_project/mqtt.py
deleted file mode 100644
index bc62528..0000000
--- a/src/azer_robocup_project/mqtt.py
+++ /dev/null
@@ -1,237 +0,0 @@
-# The MIT License (MIT)
-#
-# Copyright (c) 2013, 2014 micropython-lib contributors
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-#
-# Source: https://github.com/micropython/micropython-lib/tree/master/micropython/umqtt.simple
-
-import usocket as socket
-import ustruct as struct
-
-
-class MQTTException(Exception):
- pass
-
-
-class MQTTClient:
- def __init__(
- self,
- client_id,
- server,
- port=0,
- user=None,
- password=None,
- keepalive=0,
- ssl=False,
- ssl_params={},
- ):
- if port == 0:
- port = 8883 if ssl else 1883
- self.client_id = client_id
- self.sock = None
- self.server = server
- self.port = port
- self.ssl = ssl
- self.ssl_params = ssl_params
- self.pid = 0
- self.cb = None
- self.user = user
- self.pswd = password
- self.keepalive = keepalive
- self.lw_topic = None
- self.lw_msg = None
- self.lw_qos = 0
- self.lw_retain = False
-
- def _send_str(self, s):
- self.sock.write(struct.pack("!H", len(s)))
- self.sock.write(s)
-
- def _recv_len(self):
- n = 0
- sh = 0
- while 1:
- b = self.sock.read(1)[0]
- n |= (b & 0x7F) << sh
- if not b & 0x80:
- return n
- sh += 7
-
- def set_callback(self, f):
- self.cb = f
-
- def set_last_will(self, topic, msg, retain=False, qos=0):
- assert 0 <= qos <= 2
- assert topic
- self.lw_topic = topic
- self.lw_msg = msg
- self.lw_qos = qos
- self.lw_retain = retain
-
- def connect(self, clean_session=True):
- self.sock = socket.socket()
- addr = None
- addr = socket.getaddrinfo(self.server, self.port)[0][-1]
- self.sock.connect(addr)
- if self.ssl:
- import ussl
-
- self.sock = ussl.wrap_socket(self.sock, **self.ssl_params)
- premsg = bytearray(b"\x10\0\0\0\0\0")
- msg = bytearray(b"\x04MQTT\x04\x02\0\0")
-
- sz = 10 + 2 + len(self.client_id)
- msg[6] = clean_session << 1
- if self.user is not None:
- sz += 2 + len(self.user) + 2 + len(self.pswd)
- msg[6] |= 0xC0
- if self.keepalive:
- assert self.keepalive < 65536
- msg[7] |= self.keepalive >> 8
- msg[8] |= self.keepalive & 0x00FF
- if self.lw_topic:
- sz += 2 + len(self.lw_topic) + 2 + len(self.lw_msg)
- msg[6] |= 0x4 | (self.lw_qos & 0x1) << 3 | (self.lw_qos & 0x2) << 3
- msg[6] |= self.lw_retain << 5
-
- i = 1
- while sz > 0x7F:
- premsg[i] = (sz & 0x7F) | 0x80
- sz >>= 7
- i += 1
- premsg[i] = sz
-
- self.sock.write(premsg[0 : i + 2])
- self.sock.write(msg)
- # print(hex(len(msg)), hexlify(msg, ":"))
- self._send_str(self.client_id)
- if self.lw_topic:
- self._send_str(self.lw_topic)
- self._send_str(self.lw_msg)
- if self.user is not None:
- self._send_str(self.user)
- self._send_str(self.pswd)
- resp = self.sock.read(4)
- assert resp[0] == 0x20 and resp[1] == 0x02
- if resp[3] != 0:
- raise MQTTException(resp[3])
- return resp[2] & 1
-
- def disconnect(self):
- self.sock.write(b"\xe0\0")
- self.sock.close()
-
- def ping(self):
- self.sock.write(b"\xc0\0")
-
- def publish(self, topic, msg, retain=False, qos=0):
- pkt = bytearray(b"\x30\0\0\0")
- pkt[0] |= qos << 1 | retain
- sz = 2 + len(topic) + len(msg)
- if qos > 0:
- sz += 2
- assert sz < 2097152
- i = 1
- while sz > 0x7F:
- pkt[i] = (sz & 0x7F) | 0x80
- sz >>= 7
- i += 1
- pkt[i] = sz
- # print(hex(len(pkt)), hexlify(pkt, ":"))
- self.sock.write(pkt[0 : i + 1])
- self._send_str(topic)
- if qos > 0:
- self.pid += 1
- pid = self.pid
- struct.pack_into("!H", pkt, 0, pid)
- self.sock.write(pkt[0:2])
- self.sock.write(msg)
- if qos == 1:
- while 1:
- op = self.wait_msg()
- if op == 0x40:
- sz = self.sock.read(1)
- assert sz == b"\x02"
- rcv_pid = self.sock.read(2)
- rcv_pid = rcv_pid[0] << 8 | rcv_pid[1]
- if pid == rcv_pid:
- return
- elif qos == 2:
- assert 0
-
- def subscribe(self, topic, qos=0):
- assert self.cb is not None, "Subscribe callback is not set"
- pkt = bytearray(b"\x82\0\0\0")
- self.pid += 1
- struct.pack_into("!BH", pkt, 1, 2 + 2 + len(topic) + 1, self.pid)
- # print(hex(len(pkt)), hexlify(pkt, ":"))
- self.sock.write(pkt)
- self._send_str(topic)
- self.sock.write(qos.to_bytes(1, "little"))
- while 1:
- op = self.wait_msg()
- if op == 0x90:
- resp = self.sock.read(4)
- # print(resp)
- assert resp[1] == pkt[2] and resp[2] == pkt[3]
- if resp[3] == 0x80:
- raise MQTTException(resp[3])
- return
-
- # Wait for a single incoming MQTT message and process it.
- # Subscribed messages are delivered to a callback previously
- # set by .set_callback() method. Other (internal) MQTT
- # messages processed internally.
- def wait_msg(self):
- res = self.sock.read(1)
- if res == b"" or res is None:
- return None
- self.sock.setblocking(True)
- if res == b"\xd0": # PINGRESP
- sz = self.sock.read(1)[0]
- assert sz == 0
- return None
- op = res[0]
- if op & 0xF0 != 0x30:
- return op
- sz = self._recv_len()
- topic_len = self.sock.read(2)
- topic_len = (topic_len[0] << 8) | topic_len[1]
- topic = self.sock.read(topic_len)
- sz -= topic_len + 2
- if op & 6:
- pid = self.sock.read(2)
- pid = pid[0] << 8 | pid[1]
- sz -= 2
- msg = self.sock.read(sz)
- self.cb(topic, msg)
- if op & 6 == 2:
- pkt = bytearray(b"\x40\x02\0\0")
- struct.pack_into("!H", pkt, 2, pid)
- self.sock.write(pkt)
- elif op & 6 == 4:
- assert 0
-
- # Checks whether a pending message from server is available.
- # If not, returns immediately with None. Otherwise, does
- # the same processing as wait_msg.
- def check_msg(self):
- self.sock.setblocking(False)
- return self.wait_msg()
diff --git a/src/azer_robocup_project/pose_designer.py b/src/azer_robocup_project/pose_designer.py
deleted file mode 100644
index e37bb50..0000000
--- a/src/azer_robocup_project/pose_designer.py
+++ /dev/null
@@ -1,1208 +0,0 @@
-import wx
-import wx.lib.agw.rulerctrl, wx.lib.intctrl, wx.lib.masked.ipaddrctrl, wx.adv
-import sys
-
-import io
-#import serial, serial.tools.list_ports, socket, time
-#import numpy as np
-import random
-import json
-import threading
-import os
-#import pkg_resources.py2_warn
-import time
-import math
-import datetime
-
-current_work_directory = os.getcwd()
-current_work_directory = current_work_directory.replace('\\', '/') + '/'
-with open("simulator_lib_directory.txt", "r") as f:
- simulator_lib_directory = f.read()
-simulator_lib_directory = simulator_lib_directory.replace('\\', '/')
-sys.path.append(simulator_lib_directory)
-sys.path.append(current_work_directory + '/Soccer/Motion')
-import sim
-from compute_Alpha_v3 import Alpha
-
-SIMULATION = 0
-
-
-
-class RedirectText(object):
- def __init__(self,aWxTextCtrl):
- self.out = aWxTextCtrl
-
- def write(self,string):
- self.out.WriteText(string)
-
-
-class Pose_Designer(wx.Frame):
-
- def __init__(self, *args, **kw):
- super(Pose_Designer, self).__init__(*args, **kw)
-
- self.dof = 23
- a5 = 21.5 # мм раÑÑтоÑние от оÑи Ñимметрии до оÑи Ñервы 5
- b5 = 18.5 # мм раÑÑтоÑние от оÑи Ñервы 5 до оÑи Ñервы 6 по горизонтали
- c5 = 0 # мм раÑÑтоÑние от оÑи Ñервы 6 до Ð½ÑƒÐ»Ñ Z по вертикали
- a6 = 42 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 7
- a7 = 65.5 # мм раÑÑтоÑние от оÑи Ñервы 7 до оÑи Ñервы 8
- a8 = 63.8 # мм раÑÑтоÑние от оÑи Ñервы 8 до оÑи Ñервы 9
- a9 = 35.5 # мм раÑÑтоÑние от оÑи Ñервы 9 до оÑи Ñервы 10
- a10= 25.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до центра Ñтопы по горизонтали
- b10= 26.4 # мм раÑÑтоÑние от оÑи Ñервы 10 до низа Ñтопы
- c10 = 12 # мм раÑÑтоÑние от оÑи Ñервы 6 до оÑи Ñервы 10 по горизонтали
- self.SIZES = [ a5, b5, c5, a6, a7, a8, a9, a10, b10, c10 ]
- self.d10 = 53.4 #53.4 # раÑÑтоÑние по Y от центра Ñтопы до оÑи робота
- limAlpha5 = [-2667, 2667]
- limAlpha6 = [-3000, 740]
- limAlpha7 = [-3555, 3260]
- limAlpha8 = [-4150, 1777]
- limAlpha9 = [-4000, 2960]
- limAlpha10 =[-2815, 600]
- self.LIMALPHA = [limAlpha5, limAlpha6, limAlpha7, limAlpha8, limAlpha9, limAlpha10]
- self.FACTOR = [ 1,1,1,-1,1,1, 1,1,1,1,1,1,1, 1,-1,1,1, 1,1,1,1, 1, 1, 1, 1] # Surrogat 1
-
- self.slot_file_is_loaded = False
- self.TIK2RAD = 0.00058909
- self.syncro = False
- self.physicsOn = False
- self.panel = wx.Panel(self)
- self.panel2 = wx.Panel(self)
- self.robotPanel = wx.Panel(self)
- self.angles = []
- self.side = 0
- self.x = 0
- self.y = -53
- self.z = -233
- self.pitch = 0
- self.roll = 0
- self.yaw = 0
- self.xl = 0
- self.yl = 53
- self.zl = -233
- self.pitchl = 0
- self.rolll = 0
- self.yawl = 0
- self.joint5 = 0
- self.joint6 = 0
- self.joint7 = 0
- self.joint8 = 0
- self.joint9 = 0
- self.joint10 = 0
-
- self.focused_item = 0
- self.dummy_handles_list_incomplete = True
-
- self.filename = ''
- with open(current_work_directory + "pose_designer_config.json", "r") as f:
- self.config = json.loads(f.read())
- #self.config = {'defaultFile':''}
- self.defaultFile = self.config['defaultFile']
- self.jointControls = []
- self.jointLimits = [(-3500,700),(-3800, 3500),(-4600, 1800),(-3400,3400),(-2700,860),(-2400,2800),(-700,3800),(-3000,4000),(-5800,900), (-6500, 2800),(-2500,2500),
- (-700,3500),(-3500, 3800),(-1800, 4600),(-3400,3400),(-860,2700),(-2800,2400),(-3800,700),(-4000,3000),(-900,5800), (-2800, 6500), (-8000, 8000), (-8000, 8000), (-8000, 8000), (-8000, 8000)]
- self.controlBoxPosition = [(10,0), (9,1), (8,1), (7,1),(6,0), (5,1),(4,0),(3,0),(2,0), (2,1), (4,2), (10,4), (9,3),(8,3), (7,3), (6,4), (5,3), (4,4), (3,4), (2,4), (2,3), (1,2),(0,2)]
- self.controlBoxPosition1 = [(55, 500), (120, 450), (80, 400), (120, 350), (140, 300), (140, 250), (5, 245), (5, 195), (5, 145), (140, 145), (210, 200),
- (365, 500), (300, 450), (340, 400), (300, 350), (280, 300), (280, 250), (415, 245), (415, 195), (415, 145), (280, 145), (210, 95), (210,45), (5, 295), (415, 295)]
- self.ACTIVEJOINTS = ['Leg_right_10','Leg_right_9','Leg_right_8','Leg_right_7','Leg_right_6','Leg_right_5','hand_right_4',
- 'hand_right_3','hand_right_2','hand_right_1','Tors1','Leg_left_10','Leg_left_9','Leg_left_8',
- 'Leg_left_7','Leg_left_6','Leg_left_5','hand_left_4','hand_left_3','hand_left_2','hand_left_1','head0','head12', 'hand_right_11', 'hand_left_11']
- self.clientID = sim_Enable('127.0.0.1', -20000)
- self.jointHandle = []
- self.pose0 = [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]
- self.trims = [0,0,0,0,0,0,0,0,-222,0,0,0,0,0,0,0,0,0,0,222,0,0,0,0,0]
- self.activePose = [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]
- self.activePoseOld = self.activePose.copy()
- self.activeFrames = 1
- self.motionPages = [[]]
- self.pageNames = []
- self.activePage = 0
- self.new_file_is_Not_saved = False
- self.slow = 0
- self.dummy_handle_list = []
- self.dummy_handles = []
- self.sim_Start()
- self.InitUI()
-
-
-
-
- def sim_Start(self):
- if SIMULATION == 1 or SIMULATION == 0:
- #self.sim.simxFinish(-1) # just in case, close all opened connections
- #self.clientID=self.sim.simxStart('127.0.0.1',19997,True,True,5000,self.simThreadCycleInMs) # Connect to V-REP
- #if self.clientID!=-1:
- # uprint ('Connected to remote API server')
- #else:
- # uprint ('Failed connecting to remote API server')
- # uprint ('Program ended')
- # exit(0)
- ## Collect Joint Handles and trims from model
- #returnCode, self.Dummy_HHandle = sim.simxGetObjectHandle(self.clientID, 'Dummy_H'+ self.robot_Number, sim.simx_opmode_blocking)
- #returnCode, self.Dummy_1Handle = sim.simxGetObjectHandle(self.clientID, 'Dummy1' + self.robot_Number, sim.simx_opmode_blocking)
- #returnCode, Dummy_Hposition= sim.simxGetObjectPosition(self.clientID, self.Dummy_HHandle , -1, sim.simx_opmode_streaming)
- #returnCode, Dummy_Hquaternion= sim.simxGetObjectQuaternion(self.clientID, self.Dummy_HHandle , -1, sim.simx_opmode_streaming)
- #returnCode, Dummy_1position= sim.simxGetObjectPosition(self.clientID, self.Dummy_1Handle , -1, sim.simx_opmode_streaming)
- #returnCode, Dummy_1quaternion= sim.simxGetObjectQuaternion(self.clientID, self.Dummy_1Handle , -1, sim.simx_opmode_streaming)
- dataType = 0
- objectType = sim.sim_object_joint_type
- returnCode, joint_handles, intdata, floatdata, stringdata = sim.simxGetObjectGroupData(self.clientID, objectType, dataType, sim.simx_opmode_blocking)
- for jointname in stringdata:
- if jointname == 'hand_right_11': self.dof = 25
- print(self.dof)
- for i in range(self.dof):
- returnCode, handle= sim.simxGetObjectHandle(self.clientID, self.ACTIVEJOINTS[i], sim.simx_opmode_blocking)
- self.jointHandle.append(handle)
- returnCode, position= sim.simxGetJointPosition(self.clientID, handle, sim.simx_opmode_blocking)
- #self.trims[i] = position / self.TIK2RAD
- self.activePose[i] = position / self.TIK2RAD
-
- if SIMULATION == 1:
- sim.simxSynchronous(self.clientID,True)
-
-
- def action_To_Simulator(self):
- if self.syncro:
- if self.physicsOn:
- for i in range(self.dof):
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[i] ,
- (self.activePose[i] + self.trims[i]) * self.TIK2RAD * self.FACTOR[i], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- else:
- for i in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[i] ,
- (self.activePose[i] + self.trims[i]) * self.TIK2RAD * self.FACTOR[i], sim.simx_opmode_oneshot)
-
-
-
- def createControlPanel(self, jointNumber):
- pnl = wx.Panel(self.robotPanel, pos=self.controlBoxPosition1[jointNumber], size = (130,50), style = wx.TAB_TRAVERSAL|wx.BORDER_RAISED)
- pnl.SetBackgroundColour(wx.SystemSettings.GetColour(wx.SYS_COLOUR_MENU))
- val1 = 0
- controlName = wx.StaticText(pnl, label=self.ACTIVEJOINTS[jointNumber], pos = (0,0), size=(70,25), style= wx.ALIGN_LEFT)
- controlslider = wx.Slider(pnl, id = jointNumber, value=val1, minValue=-8000, maxValue=8000, pos = (0,25), size = (130,25))
- controlvalue = wx.SpinCtrl(pnl, id = jointNumber, value="0", pos=(70,0), size=wx.DefaultSize,
- style=wx.SP_WRAP, min=self.jointLimits[jointNumber][0], max=self.jointLimits[jointNumber][1], initial=0)
- controlslider.Bind(wx.EVT_SLIDER, self.On_Slider_move)
- controlvalue.Bind(wx.EVT_SPINCTRL, self.On_SPINCTRL_change)
- controlvalue.Bind(wx.EVT_TEXT, self.On_SPINCTRL_change)
- return pnl, controlslider, controlvalue
-
- def refresh_Control_Values(self):
- if len(self.dummy_handles) > 0:
- self.Dummy_Fetch_Data(0)
- for i in range(self.dof):
- self.jointControls[i][1].SetValue(self.activePose[i])
- self.jointControls[i][2].SetValue(self.activePose[i])
- self.frames_input.SetValue(self.activeFrames)
-
- def InitUI(self):
-
- for i in range(self.dof):
- #for i in range(17):
- pnl, controlslider, controlvalue = self.createControlPanel(i)
- self.jointControls.append((pnl, controlslider, controlvalue))
-
- pnl2 = wx.Panel(self.robotPanel, pos=(10,10), size = (80,50), style = wx.TAB_TRAVERSAL)
- self.frames_text = wx.StaticText(pnl2, label='Frames', pos = (0,0), size=(60,25), style= wx.ALIGN_CENTRE_HORIZONTAL)
- self.frames_input = wx.lib.intctrl.IntCtrl(pnl2, value=self.activeFrames, pos =(0,25), size=(60,25), min=1, max=200)
- self.robotPanel.SetBackgroundColour(wx.SystemSettings.GetColour(wx.SYS_COLOUR_APPWORKSPACE))
-
- #self.pages_list_text = wx.StaticText(self.robotPanel, label='Pages list', pos = (100, 5), size=(60,20), style= wx.ALIGN_CENTRE_HORIZONTAL)
- #self.pages_list_control = wx.ListCtrl(self.robotPanel, pos = (75, 25), size = (130, 75), style=wx.LC_LIST | wx.LC_SINGLE_SEL | wx.SIMPLE_BORDER | wx.LC_EDIT_LABELS)
- #self.pages_list_control.SetColumnWidth(0, 40)
- #self.pages_list_control.InsertItem(self.pages_list_control.GetItemCount(), 'trims')
- #self.pages_list_control.Bind(wx.EVT_LEFT_DOWN, self.On_Left_Down_Pages)
-
-
- self.pages_edit_list_control = wx.adv.EditableListBox(self.robotPanel, label = "Pages", pos = (345, 0), size = (200, 140), style=wx.adv.EL_DEFAULT_STYLE)
- self.getNew_button = self.pages_edit_list_control.GetNewButton()
- self.get_Del_button = self.pages_edit_list_control.GetDelButton()
- self.get_Up_button = self.pages_edit_list_control.GetUpButton()
- self.get_Down_button = self.pages_edit_list_control.GetDownButton()
- self.get_Up_button.Bind(wx.EVT_BUTTON, self.On_MoveUp_Page)
- self.get_Down_button.Bind(wx.EVT_BUTTON, self.On_MoveDn_Page)
- self.get_Del_button.Bind(wx.EVT_BUTTON, self.On_Delete_Page)
- self.getNew_button.Bind(wx.EVT_BUTTON, self.On_Clone_Page)
- self.pages_edit_list_control.Bind(wx.EVT_LIST_ITEM_FOCUSED, self.On_Left_Down_edit)
- #self.pages_edit_list_control.Bind(wx.EVT_LIST_ITEM_SELECTED, self.On_Left_Down_edit)
- #self.pages_edit_list_control.Bind(wx.EVT_LIST_ITEM_ACTIVATED, self.On_Left_Down_edit)
- self.pages_edit_list_control.Bind(wx.EVT_LIST_END_LABEL_EDIT, self.On_Label_edit)
-
- #strings =['one', 'two', "three", 'five', 'six', 'seven']
- #self.pages_edit_list_control.SetStrings(strings)
-
- self.console_Panel = wx.Panel(self)
- self.log = wx.TextCtrl(self.console_Panel, -1, style=wx.TE_MULTILINE)
- log_box = wx.BoxSizer(wx.VERTICAL)
- log_box.Add(self.log, proportion = 1, flag=wx.EXPAND|wx.BOTTOM|wx.TOP)
- self.console_Panel.SetSizer(log_box)
- redir = RedirectText(self.log)
- sys.stdout = redir
- #sys.stderr = redir
- self.CreateMenuBar()
-
- #pnl2.SetBackgroundStyle(wx.BG_STYLE_SYSTEM)
-
- self.denied_message = wx.MessageDialog(self, "Denied! No motion slot processing", style=wx.OK|wx.CENTRE)
-
- #self.quit_button = wx.Button(self.panel, wx.ID_ANY, "Quit")
- #self.save_and_exit_button = wx.Button(self.panel, wx.ID_ANY, "Save&Exit")
- self.physics_button = wx.CheckBox(self.panel2, wx.ID_ANY , label = "PhysicsON", style= wx.RB_SINGLE | wx.ALIGN_RIGHT)
- self.physics_button.SetValue(self.physicsOn)
- self.play_page_button = wx.Button(self.panel, wx.ID_ANY, "PlayPage")
- self.play_next_button = wx.Button(self.panel, wx.ID_ANY, "PlayNext")
- self.record_page_button = wx.Button(self.panel, wx.ID_ANY, "RecordPage")
- #self.clone_page_button = wx.Button(self.panel, wx.ID_ANY, "ClonePage")
- #self.moveUp_page_button = wx.Button(self.panel2, wx.ID_ANY, "MoveUp")
- #self.moveDn_page_button = wx.Button(self.panel2, wx.ID_ANY, "MoveDn")
- #self.delete_page_button = wx.Button(self.panel, wx.ID_ANY, "DeletePage")
- self.play_all_button = wx.Button(self.panel, wx.ID_ANY, "PlayAll")
- self.return_button = wx.Button(self.panel, wx.ID_ANY, "Return")
- self.syncro_button = wx.Button(self.panel, id = 1001, label = "NOSYNC")
- self.pose0_button = wx.Button(self.panel2, id = 1002, label = "Pose0")
- self.discard_button = wx.Button(self.panel2, id = 1003, label = "Discard")
- #self.load_file_button = wx.Button(self.panel, wx.ID_ANY, "Load File")
- #self.page_selector = wx.ComboBox(self.panel2, value='start',size=(50,25),
- # choices=['start','','','','','','','','',''], style=wx.CB_DROPDOWN)
- #self.page_text = wx.StaticText(self.panel2, label='Page:',size=(60,25), style= wx.ALIGN_CENTRE_HORIZONTAL)
- self.slow_text = wx.StaticText(self.panel2, label='Slow(ms):',size=(60,25), style= wx.ALIGN_CENTRE_HORIZONTAL)
- self.slow_input = wx.lib.intctrl.IntCtrl(self.panel2, value=self.slow, size=(60,25), min=0, max=5000)
-
- self.button_box = wx.BoxSizer(wx.HORIZONTAL)
- self.button_box2 = wx.BoxSizer(wx.HORIZONTAL)
-
- #self.button_box.Add(self.quit_button, proportion=0)
- #self.button_box.Add(self.save_and_exit_button, proportion=0)
- self.button_box2.Add(self.physics_button, proportion=0)
- self.button_box.Add(self.return_button, proportion=0)
- self.button_box.Add(self.play_page_button, proportion=0)
- self.button_box.Add(self.play_next_button, proportion=0)
- self.button_box.Add(self.play_all_button, proportion=0)
- self.button_box.Add(self.syncro_button, proportion=0)
- self.button_box.Add(self.record_page_button, proportion=0)
- #self.button_box.Add(self.clone_page_button, proportion=0)
- #self.button_box.Add(self.delete_page_button, proportion=0)
- #self.button_box.Add(self.load_file_button, proportion=0)
- #self.button_box2.Add(self.page_text, proportion=0, flag=wx.RIGHT )
- #self.button_box2.Add(self.page_selector, proportion=0, flag=wx.RIGHT )
- self.button_box2.Add(self.slow_text, proportion=0, flag=wx.RIGHT )
- self.button_box2.Add(self.slow_input, proportion=0, flag=wx.RIGHT )
- #self.button_box2.Add(self.moveUp_page_button, proportion=0)
- #self.button_box2.Add(self.moveDn_page_button, proportion=0)
- self.button_box2.Add(self.pose0_button, proportion=0)
- self.button_box2.Add(self.discard_button, proportion=0)
-
- #self.button_box.Add(self.pixel_input, proportion=0, flag=wx.RIGHT )
- #self.button_box.Add(self.area_text, proportion=0, flag=wx.RIGHT )
- #self.button_box.Add(self.area_input, proportion=0, flag=wx.RIGHT )
-
-
-
- self.panel.SetSizer(self.button_box)
- self.panel2.SetSizer(self.button_box2)
-
- self.button_sizer = wx.BoxSizer(wx.HORIZONTAL)
- self.button_sizer2 = wx.BoxSizer(wx.HORIZONTAL)
- self.button_sizer.Add(self.panel, proportion=2, flag=wx.EXPAND )
- self.button_sizer2.Add(self.panel2, proportion=2, flag=wx.EXPAND )
-
-
- sizer = wx.BoxSizer(wx.VERTICAL)
- sizer.Add(self.button_sizer, proportion=0, flag=wx.EXPAND )
- sizer.Add(self.button_sizer2, proportion=0, flag=wx.EXPAND )
- #sizer.Add(robot_sizer, proportion=0, flag = wx.FIXED_MINSIZE)
- #self.robotPanel.SetSizer(robot_sizer)
- sizer.Add(self.robotPanel, proportion=0, flag=wx.EXPAND )
-
- #sizer.Add(self.jointControls[0][0], border=1)
-
- whole_window = wx.BoxSizer(wx.HORIZONTAL)
- whole_window.Add(sizer, proportion = 0, flag=wx.EXPAND )
- whole_window.Add(self.console_Panel, proportion = 1, flag=wx.EXPAND|wx.BOTTOM|wx.TOP )
-
- self.SetMinSize((350, 350))
- self.CreateStatusBar()
- self.SetSizer(whole_window)
-
- self.SetSize((560, 700))
- self.SetTitle('Pose Designer')
- self.Centre()
- #self.Bind(wx.EVT_SLIDER, self.On_Slider_move)
- #self.Bind(wx.EVT_SPINCTRL, self.On_SPINCTRL_change)
- #self.Bind(wx.EVT_TEXT, self.On_SPINCTRL_change)
- self.Bind(wx.EVT_MENU, self.On_New_Slot, id = 100 )
- self.Bind(wx.EVT_MENU, self.On_Load_File, id = 101 )
- self.Bind(wx.EVT_MENU, self.On_Save, id = 102 )
- self.Bind(wx.EVT_MENU, self.On_Save_as, id = 103 )
- self.Bind(wx.EVT_MENU, self.On_Quit_select, id = 105 )
- self.Bind(wx.EVT_MENU, self.OnAbout, id = 121 )
- self.Bind(wx.EVT_MENU, self.Quick_Start, id = 122 )
- self.Bind(wx.EVT_MENU, self.Calculator, id = 123 )
- self.Bind(wx.EVT_MENU, self.Mirror_Transform, id = 124 )
- self.Bind(wx.EVT_MENU, self.ListBox_test, id = 125 )
- self.Bind(wx.EVT_MENU, self.Dummy_report, id = 127 )
- self.Bind(wx.EVT_MENU, self.Rename_All_Pages, id = 126 )
- #self.Bind(wx.EVT_MENU, self.On_Start_Camera, id = 3 )
- self.physics_button.Bind(wx.EVT_CHECKBOX, self.On_Physics)
- #self.quit_button.Bind(wx.EVT_BUTTON, self.On_Quit_select)
- self.return_button.Bind(wx.EVT_BUTTON, self.On_Return )
- self.play_page_button.Bind(wx.EVT_BUTTON, self.On_Play_Page )
- self.play_next_button.Bind(wx.EVT_BUTTON, self.On_Play_Next )
- self.record_page_button.Bind(wx.EVT_BUTTON, self.On_Record_Page )
- #self.clone_page_button.Bind(wx.EVT_BUTTON, self.On_Clone_Page )
- #self.moveUp_page_button.Bind(wx.EVT_BUTTON, self.On_MoveUp_Page )
- #self.moveDn_page_button.Bind(wx.EVT_BUTTON, self.On_MoveDn_Page )
- #self.delete_page_button.Bind(wx.EVT_BUTTON, self.On_Delete_Page )
- self.play_all_button.Bind(wx.EVT_BUTTON, self.On_Play_All )
- self.syncro_button.Bind(wx.EVT_BUTTON, self.On_Syncro_Set )
- self.pose0_button.Bind(wx.EVT_BUTTON, self.On_Pose0_Set )
- self.discard_button.Bind(wx.EVT_BUTTON, self.On_Discard_Set )
- #self.load_file_button.Bind(wx.EVT_BUTTON, self.On_Load_File)
- #self.save_and_exit_button.Bind(wx.EVT_BUTTON, self.On_Save_and_Exit)
- #self.page_selector.Bind(wx.EVT_COMBOBOX, self.On_page_selector)
- self.frames_input.Bind(wx.EVT_TEXT, self.On_number_input)
- self.slow_input.Bind(wx.EVT_TEXT, self.On_number_input)
-
- def Rename_All_Pages(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- new_names = []
- for i in range(len(self.pageNames)):
- name = 'page ' + str(i)
- new_names.append(name)
- self.pageNames = new_names
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_edit_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_edit_list_control.Select(self.activePage)
- pages_edit_list_control.EnsureVisible(self.activePage)
- print(" All pages renamed")
-
- def Dummy_report_Appoint_Focused_Item(self, event):
- self.focused_item = self.dummy_report_table.GetFocusedItem()
- print("focused item chosen")
-
- def Dummy_Fetch_Data(self, event):
- if self.dummy_handles_list_incomplete :
- for i in range(len(self.dummy_handles)):
- if self.dummy_report_table.IsItemChecked(i): self.dummy_handles_checked[i] = True
- else: self.dummy_handles_checked[i] = False
- self.dummy_report_table.EnableCheckBoxes(False)
- self.dummy_handles_list_incomplete = False
- dataType = 3
- objectType = sim.sim_object_dummy_type
- returnCode, self.dummy_handles, intdata, floatdata, stringdata = sim.simxGetObjectGroupData(self.clientID, objectType, dataType, sim.simx_opmode_blocking)
- print("Sim Group Data:", "intdata:", intdata, "floatdata:", floatdata, "stringdata", stringdata)
- self.dummy_report_table.DeleteAllItems()
- for i in range(len(self.dummy_handle_list)):
- if self.dummy_handles_checked[i]:
- content = []
- content.append(self.dummy_handle_list[i])
- content.append(str(int(floatdata[i*3] * 1000)))
- content.append(str(int(floatdata[i*3 +1]* 1000)))
- content.append(str(int(floatdata[i*3 +2]*1000)))
- self.dummy_report_table.Append(content)
- self.dummy_report_table.Select(self.focused_item)
- self.dummy_report_table.EnsureVisible(self.focused_item)
- print("Update button pressed")
-
- def Dummy_report(self, event):
- dialog_panel = wx.Dialog(self, title='Dummy Report', size = (340, 165))
- self.dummy_report_table = wx.ListCtrl(dialog_panel, size = (320, 100), style = wx.LC_REPORT | wx.LC_VRULES | wx.LC_HRULES)
- refresh_button = wx.Button(dialog_panel, label = "Update", pos =(10, 100))
- refresh_button.Bind(wx.EVT_BUTTON, self.Dummy_Fetch_Data)
- self.dummy_report_table.InsertColumn(0,'Dummy #')
- self.dummy_report_table.InsertColumn(1,'X')
- self.dummy_report_table.InsertColumn(2,'Y')
- self.dummy_report_table.InsertColumn(3,'Z')
- self.dummy_report_table.SetColumnWidth(0, 150)
- self.dummy_report_table.SetColumnWidth(1, 50)
- self.dummy_report_table.SetColumnWidth(2, 50)
- self.dummy_report_table.SetColumnWidth(3, 50)
- self.dummy_report_table.EnableCheckBoxes(True)
- objectType = sim.sim_object_dummy_type
- dataType = 0
- returnCode, self.dummy_handles, intdata, floatdata, self.dummy_handle_list = sim.simxGetObjectGroupData(self.clientID, objectType, dataType, sim.simx_opmode_blocking)
- dataType = 3
- returnCode, self.dummy_handles, intdata, floatdata, stringdata = sim.simxGetObjectGroupData(self.clientID, objectType, dataType, sim.simx_opmode_blocking)
- print("Sim Group Data:", "intdata:", intdata, "floatdata:", floatdata, "stringdata", stringdata)
- self.dummy_handles_checked = []
- self.dummy_handles_list_incomplete = True
- for i in range(len(self.dummy_handle_list)):
- content = []
- content.append(self.dummy_handle_list[i])
- content.append(str(int(floatdata[i*3] * 1000)))
- content.append(str(int(floatdata[i*3 +1]* 1000)))
- content.append(str(int(floatdata[i*3 +2]*1000)))
- self.dummy_report_table.Append(content)
- self.dummy_report_table.CheckItem(i, False)
- self.dummy_handles_checked.append(True)
- self.dummy_report_table.Bind(wx.EVT_LIST_ITEM_FOCUSED, self.Dummy_report_Appoint_Focused_Item)
- dialog_panel.Show()
-
- def ListBox_test(self, event):
- def On_List_edit(event):
- #item, flags = pages_list_control.HitTest(event.GetPosition())
- #print(item)
- pages_list_control = pages_edit_list_control.GetListCtrl()
- print(pages_list_control.GetFirstSelected())
- def On_List_inserted(event):
- print('new list item')
- def On_List_change(event):
- pages_list_control = pages_edit_list_control.GetListCtrl()
- print(pages_list_control.GetItem())
- def On_GetUp(event):
- print('getup button pressed')
- dialog_panel = wx.Dialog(self, title='ListBox Test', size = (150, 250))
- pages_edit_list_control = wx.adv.EditableListBox(dialog_panel, pos = (10, 40), size = (150, 250), style=wx.adv.EL_DEFAULT_STYLE)
- strings =['one', 'two', "three"]
- pages_edit_list_control.SetStrings(strings)
- #pages_edit_list_control.Bind(wx.EVT_LEFT_DOWN, On_List_edit)
- #pages_list_control = pages_edit_list_control.GetListCtrl()
- getup_button = pages_edit_list_control.GetUpButton()
- getup_button.Bind(wx.EVT_BUTTON, On_GetUp)
- pages_edit_list_control.Bind(wx.EVT_LIST_ITEM_FOCUSED, On_List_edit)
- pages_edit_list_control.Bind(wx.EVT_LIST_INSERT_ITEM, On_List_inserted)
- pages_edit_list_control.Bind(wx.EVT_LIST_BEGIN_DRAG, On_List_change)
- dialog_panel.ShowModal()
-
- def Mirror_Transform(self, event):
- def On_Select_All(event):
- if select_all_checkbox.IsChecked():
- for i in range(len(self.motionPages)):
- pages_list_control.CheckItem(i, True)
- else:
- for i in range(len(self.motionPages)):
- pages_list_control.CheckItem(i, False)
- def On_Transform(event):
- transform_list = []
- for i in range(len(self.motionPages)):
- if pages_list_control.IsItemChecked(i): transform_list.append(i)
- for item in transform_list:
- itemtext = str(item) + ' Mirrored'
- pages_list_control.SetItemText(item, itemtext)
- for item in transform_list:
- side_copy = self.motionPages[item][1:11].copy()
- for i in range(10):
- self.motionPages[item][i+1] = -self.motionPages[item][i+12]
- self.motionPages[item][i+12] = -side_copy[i]
- self.motionPages[item][11] = -self.motionPages[item][11]
- self.motionPages[item][22] = -self.motionPages[item][22]
-
- mirror_transform_dialog = wx.Dialog(self, title='Mirror Transform', size = (350, 350))
- text2 = wx.StaticText(mirror_transform_dialog, label='Select pages for transformation',size=(300,25), pos =(10, 10), style = wx.ALIGN_CENTRE_HORIZONTAL )
- pages_list_control = wx.ListCtrl(mirror_transform_dialog, pos = (10, 40), size = (200, 200), style=wx.LC_LIST | wx.SIMPLE_BORDER)
- select_all_checkbox = wx.CheckBox(mirror_transform_dialog, pos = (220, 40), label = "Select All")
- transform_button = wx.Button(mirror_transform_dialog, wx.ID_ANY, "Transform", pos = (10, 250))
- pages_list_control.EnableCheckBoxes(True)
- pages_list_control.DeleteAllItems()
- for i in range(len(self.motionPages)):
- pages_list_control.InsertItem(i, self.pageNames[i])
- transform_button.Bind(wx.EVT_BUTTON, On_Transform)
- select_all_checkbox.Bind(wx.EVT_CHECKBOX, On_Select_All)
- mirror_transform_dialog.ShowModal()
-
-
-
-
- def Calculator(self, event):
- def On_Calc_Button(event):
- self.side = side_input.GetSelection()
- self.x = x_input.GetValue()
- self.y = y_input.GetValue()
- self.z = z_input.GetValue()
- self.pitch = pitch_input.GetValue()
- self.roll = roll_input.GetValue()
- self.yaw = yaw_input.GetValue()
- self.xl = xl_input.GetValue()
- self.yl = yl_input.GetValue()
- self.zl = zl_input.GetValue()
- self.pitchl = pitchl_input.GetValue()
- self.rolll = rolll_input.GetValue()
- self.yawl = yawl_input.GetValue()
- al = Alpha()
- xr = math.sin(math.radians(self.pitch))
- yr = math.sin(math.radians(self.roll))
- zr = - math.sqrt(1 - xr**2 - yr**2)
- wr = math.radians(self.yaw)
- xl = math.sin(self.pitchl)
- yl = math.sin(self.rolll)
- zl = - math.sqrt(1 - xl**2 - yl**2)
- wl = math.radians(self.yawl)
- if self.side == 0:
- self.angles = al.compute_Alpha_v3(self.x, self.y, self.z, xr, yr, zr, wr, self.SIZES, self.LIMALPHA)
- else:
- self.angles = al.compute_Alpha_v3(self.xl, -self.yl, self.zl, xl, -yl, zl, wl, self.SIZES, self.LIMALPHA)
- if len(self.angles) == 0:
- result_list_control.DeleteAllItems()
- result_list_control.InsertItem(0, "Result doesn't exist")
- else:
- result_list_control.DeleteAllItems()
- for i in range(len(self.angles)):
- variant_text = 'Variant '+ str(i)
- if self.angles[i][2] < 0 and self.side == 0: variant_text += "-best"
- if self.angles[i][2] < 0 and self.side == 1: variant_text += "-best"
- result_list_control.InsertItem(i, variant_text)
- if self.side == 0:
- sideword = "Right"
- print( "Calc data : ", '\n'," Side = \t", sideword, '\n', " X = \t", self.x,'\n',
- " Y = \t" , self.y, '\n', " Z = \t", self.z, '\n', " Pitch = \t", self.pitch, '\n',
- " Roll = \t", self.roll, '\n', " Yaw = \t", self.yaw)
- else:
- sideword = "Left"
- print( "Calc data : ", '\n'," Side = \t", sideword, '\n', " X = \t", self.xl,'\n',
- " Y = \t" , self.yl, '\n', " Z = \t", self.zl, '\n', " Pitch = \t", self.pitchl, '\n',
- " Roll = \t", self.rolll, '\n', " Yaw = \t", self.yawl)
- def On_Copy_Button(event):
- print('Copy button pressed')
- if side_input.GetSelection() == 0:
- self.activePose[0] = joint10_result.GetValue()
- self.activePose[1] = joint9_result.GetValue()
- self.activePose[2] = joint8_result.GetValue()
- self.activePose[3] = joint7_result.GetValue()
- self.activePose[4] = joint6_result.GetValue()
- self.activePose[5] = joint5_result.GetValue()
- print(" Copy result: ")
- for i in range(6):
- print( self.ACTIVEJOINTS[i], " = \t", self.activePose[i])
- else:
- self.activePose[11] = -joint10_result.GetValue()
- self.activePose[12] = -joint9_result.GetValue()
- self.activePose[13] = -joint8_result.GetValue()
- self.activePose[14] = -joint7_result.GetValue()
- self.activePose[15] = -joint6_result.GetValue()
- self.activePose[16] = -joint5_result.GetValue()
- print(" Copy result: ")
- for i in range(11,17):
- print( self.ACTIVEJOINTS[i], " = \t", self.activePose[i])
- self.refresh_Control_Values()
-
- def On_Result_Selection(event):
- item = event.GetIndex()
- angle10, angle9, angle8, angle7, angle6, angle5 = self.angles[item]
- self.joint5 = int(angle5/self.TIK2RAD)
- self.joint6 = int(angle6/self.TIK2RAD)
- self.joint7 = int(angle7/self.TIK2RAD)
- self.joint8 = int(angle8/self.TIK2RAD)
- self.joint9 = int(angle9/self.TIK2RAD)
- self.joint10 = int(angle10/self.TIK2RAD)
- joint5_result.SetValue(self.joint5)
- joint6_result.SetValue(self.joint6)
- joint7_result.SetValue(self.joint7)
- joint8_result.SetValue(self.joint8)
- joint9_result.SetValue(self.joint9)
- joint10_result.SetValue(self.joint10)
-
- calc_input = wx.Dialog(self, title='IK Calculator', size = (400, 400))
- text1 = wx.StaticText(calc_input, label='Input calc',size=(200,25))
- text2 = wx.StaticText(calc_input, label='X, mm',size=(100,25), pos =(10, 70), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text3 = wx.StaticText(calc_input, label='Y, mm',size=(100,25), pos =(10, 100), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text4 = wx.StaticText(calc_input, label='Z, mm',size=(100,25), pos =(10, 130), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text5 = wx.StaticText(calc_input, label='Pitch, degrees',size=(100,25), pos =(10, 160), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text6 = wx.StaticText(calc_input, label='Roll, degrees',size=(100,25), pos =(10, 190) , style = wx.ALIGN_CENTRE_HORIZONTAL)
- text7 = wx.StaticText(calc_input, label='Yaw, degrees',size=(100,25), pos =(10, 220), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text8 = wx.StaticText(calc_input, label='Joint 5',size=(60,25), pos =(230, 70), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text9 = wx.StaticText(calc_input, label='Joint 6',size=(60,25), pos =(230, 100), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text10 = wx.StaticText(calc_input, label='Joint 7',size=(60,25), pos =(230, 130), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text11 = wx.StaticText(calc_input, label='Joint 8',size=(60,25), pos =(230, 160), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text12 = wx.StaticText(calc_input, label='Joint 9',size=(60,25), pos =(230, 190), style = wx.ALIGN_CENTRE_HORIZONTAL )
- text13 = wx.StaticText(calc_input, label='Joint 10',size=(60,25), pos =(230, 220), style = wx.ALIGN_CENTRE_HORIZONTAL )
- side_input = wx.RadioBox(calc_input, pos = (120, 20), choices = ["Right", "Left"], majorDimension = 2)
- side_input.SetSelection(self.side)
- x_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.x, pos =(120, 70), size=(40,25), min=-300, max=300)
- y_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.y, pos =(120, 100), size=(40,25), min=-300, max=300)
- z_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.z, pos =(120, 130), size=(40,25), min=-300, max=300)
- pitch_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.pitch, pos =(120, 160), size=(40,25), min=-300, max=300)
- roll_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.roll, pos =(120, 190), size=(40,25), min=-300, max=300)
- yaw_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.yaw, pos =(120, 220), size=(40,25), min=-300, max=300)
- xl_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.xl, pos =(170, 70), size=(40,25), min=-300, max=300)
- yl_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.yl, pos =(170, 100), size=(40,25), min=-300, max=300)
- zl_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.zl, pos =(170, 130), size=(40,25), min=-300, max=300)
- pitchl_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.pitchl, pos =(170, 160), size=(40,25), min=-300, max=300)
- rolll_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.rolll, pos =(170, 190), size=(40,25), min=-300, max=300)
- yawl_input = wx.lib.intctrl.IntCtrl(calc_input, value = self.yawl, pos =(170, 220), size=(40,25), min=-300, max=300)
- joint5_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint5, pos =(290, 70), size=(60,25), min=-8000, max=8000)
- joint6_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint6, pos =(290, 100), size=(60,25), min=-8000, max=8000)
- joint7_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint7, pos =(290, 130), size=(60,25), min=-8000, max=8000)
- joint8_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint8, pos =(290, 160), size=(60,25), min=-8000, max=8000)
- joint9_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint9, pos =(290, 190), size=(60,25), min=-8000, max=8000)
- joint10_result = wx.lib.intctrl.IntCtrl(calc_input, value = self.joint10, pos =(290, 220), size=(60,25), min=-8000, max=8000)
- calc_button = wx.Button(calc_input, id = 2000, label = "CALC", pos = (30,250))
- copy_button = wx.Button(calc_input, id = 2001, label = "COPY", pos = (240,250))
- result_list_control = wx.ListCtrl(calc_input, pos = (30, 300), size = (260, 35), style=wx.LC_LIST | wx.LC_SINGLE_SEL | wx.SIMPLE_BORDER)
- if len(self.angles) == 0:
- result_list_control.DeleteAllItems()
- result_list_control.InsertItem(0, "Result doesn't exist")
- else:
- result_list_control.DeleteAllItems()
- for i in range(len(self.angles)):
- variant_text = 'Variant '+ str(i)
- result_list_control.InsertItem(i, variant_text)
- calc_button.Bind(wx.EVT_BUTTON, On_Calc_Button)
- copy_button.Bind(wx.EVT_BUTTON, On_Copy_Button)
- #result_list_control.Bind(wx.EVT_LEFT_DOWN, On_Result_Selection)
- result_list_control.Bind(wx.EVT_LIST_ITEM_SELECTED, On_Result_Selection)
- calc_input.Show()
-
- def On_Discard_Set(self, event):
- print('Discard button pressed')
- self.activePoseOld = self.activePose.copy()
-
- def On_Pose0_Set(self, event):
- print('Pose0 button pressed')
- #self.activePose = self.trims.copy()
- self.activePose = self.pose0.copy()
- self.activePoseOld = self.activePose.copy()
- self.activeFrames = 10
- self.refresh_Control_Values()
- if self.syncro:
- if self.physicsOn:
- for j in range(self.dof):
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- (self.pose0[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- else:
- for j in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[j] ,
- (self.pose0[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
-
-
-
- def On_MoveUp_Page(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- if self.activePage > 0:
- page = self.motionPages.pop(self.activePage)
- name = self.pageNames.pop(self.activePage)
- self.activePage -= 1
- self.motionPages.insert(self.activePage, page)
- self.pageNames.insert(self.activePage, name)
- #self.pages_list_control.DeleteAllItems()
- #for i in range(len(self.motionPages)):
- # self.pages_list_control.InsertItem(i, str(i))
- #self.pages_list_control.Select(self.activePage)
-
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
- def On_MoveDn_Page(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- if self.activePage < len(self.motionPages) - 1:
- page = self.motionPages.pop(self.activePage)
- name = self.pageNames.pop(self.activePage)
- self.activePage += 1
- self.motionPages.insert(self.activePage, page)
- self.pageNames.insert(self.activePage, name)
- #self.pages_list_control.DeleteAllItems()
- #for i in range(len(self.motionPages)):
- # self.pages_list_control.InsertItem(i, str(i))
- #self.pages_list_control.Select(self.activePage)
-
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
- def On_Clone_Page(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- clone_page = self.motionPages[self.activePage].copy()
- page_name = self.pageNames[self.activePage]
- pos1 = page_name.find('clone')
- if pos1 > 0:
- try:
- number = int(page_name[pos1+5:])
- except Exception:
- number = 0
- clone_page_name = page_name[:pos1+5] + ' ' +str(number + 1)
- else:
- clone_page_name = page_name + ' clone 1'
- self.activePage += 1
- self.motionPages.insert(self.activePage, clone_page)
- self.pageNames.insert(self.activePage, clone_page_name)
- #self.pages_list_control.DeleteAllItems()
- #for i in range(len(self.motionPages)):
- # self.pages_list_control.InsertItem(i, str(i))
- #self.pages_list_control.Select(self.activePage)
-
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
-
- def On_Record_Page(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- self.motionPages[self.activePage][0] = self.activeFrames
- for i in range(self.dof):
- try:
- self.motionPages[self.activePage][i+1] = int(self.activePose[i])
- except Exception:
- self.motionPages[self.activePage].append(int(self.activePose[i]))
- print('recorded:', self.motionPages[self.activePage])
-
- #def On_Pages_Motion(self, event):
- # item, flags = self.pages_list_control.HitTest(event.GetPosition())
- # if item >= 0:
- # self.pages_list_control.Select(item)
- # self.curitem = item
-
- def On_Delete_Page(self, event):
- #pages_list_control = self.pages_edit_list_control.GetListCtrl()
- #item = pages_list_control.GetFirstSelected()
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- if len(self.motionPages) > 1:
- self.motionPages.pop(self.activePage)
- self.pageNames.pop(self.activePage)
- self.activePage -= 1
- #self.pages_list_control.DeleteAllItems()
- #for i in range(len(self.motionPages)):
- # self.pages_list_control.InsertItem(i, str(i))
- #self.pages_list_control.Select(self.activePage)
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
- def On_Label_edit(self, event):
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- item = pages_list_control.GetFirstSelected()
- new_name = pages_list_control.GetItemText(item)
- print(new_name)
- self.pageNames[item] = new_name
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
-
-
- def On_Left_Down_edit(self, event):
- if self.slot_file_is_loaded == False:
- self.denied_message.ShowModal()
- return
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- #item = pages_list_control.GetFirstSelected()
- #if event.GetEventType() == wx.EVT_LIST_ITEM_ACTIVATED:
- # item = pages_list_control.GetFocusedItem()
- #elif event.GetEventType() == wx.EVT_LIST_ITEM_FOCUSED:
- item = pages_list_control.GetFocusedItem()
- if 0 <= item < len(self.pageNames):
- print(pages_list_control.GetItemText(item))
- self.activePage = item
- print('self.activePage:', self.activePage)
- if len(self.motionPages) != 0:
- self.activeFrames = self.motionPages[self.activePage][0]
- self.activePoseOld = self.activePose.copy()
- for i in range(1, len(self.motionPages[self.activePage])):
- self.activePose[i-1] = self.motionPages[self.activePage][i]
- self.refresh_Control_Values()
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
- #self.pages_list_control.Select(self.activePage)
- #self.pages_list_control.EnsureVisible(self.activePage)
-
- #def On_Left_Down_Pages(self, event):
- # item, flags = self.pages_list_control.HitTest(event.GetPosition())
- # #item = event.GetIndex()
- # if item >= 0:
- # self.pages_list_control.Select(item)
- # value = item
- # print(value, self.pages_list_control.GetItemText(value))
- # self.activePage = value
- # if self.pages_list_control.GetItemText(value) == 'trims':
- # self.activePose = self.trims.copy()
- # self.activePoseOld = self.activePose.copy()
- # self.activeFrames = 1
- # else:
- # self.activePage = int(value)
- # print('self.activePage:', self.activePage)
- # if len(self.motionPages) != 0:
- # self.activeFrames = self.motionPages[self.activePage][0]
- # self.activePoseOld = self.activePose.copy()
- # for i in range(1, len(self.motionPages[self.activePage])):
- # self.activePose[i-1] = self.motionPages[self.activePage][i]
- # self.refresh_Control_Values()
- # pages_list_control = self.pages_edit_list_control.GetListCtrl()
- # pages_list_control.Select(self.activePage)
- # pages_list_control.EnsureVisible(self.activePage)
- # print('self.activeFrames:', self.activeFrames)
- # print('self.activePose', self.activePose)
-
-
-
- def On_Physics(self, event):
- if event.GetEventObject() == self.physics_button:
- if self.physicsOn:
- self.physics_button.SetValue(False)
- self.physicsOn = False
- sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
- print('self.physicsOn = False')
- else:
- self.physics_button.SetValue(True)
- self.physicsOn = True
- sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
- print('self.physicsOn = True')
-
-
- def Quick_Start(self, event):
- print('1. ЗдеÑÑŒ можно дать поÑледовательноÑÑ‚ÑŒ дейÑтвий оператора',
- '\n2. Ðто должно помочь')
-
-
- def OnAbout(self, event):
- aboutInfo = wx.adv.AboutDialogInfo()
- aboutInfo.SetName("Pose Designer")
- aboutInfo.SetVersion('Version 1.0')
- aboutInfo.SetDescription("With this app you can tune fast and convenient\n Robot pose and motion slots .")
- aboutInfo.SetCopyright("(C) 2020")
- aboutInfo.SetWebSite("www.robokit.su")
- aboutInfo.AddDeveloper("Azer Babaev")
- wx.adv.AboutBox(aboutInfo)
-
-
- def On_number_input(self, event):
- if event.GetEventObject() == self.frames_input:
- self.activeFrames = self.frames_input.GetValue()
- if event.GetEventObject() == self.slow_input:
- self.slow = self.slow_input.GetValue()
-
- def On_New_Slot(self, event):
- #slot_name_input = wx.Dialog(self, title='Slot name input')
- #dialog_sizer = wx.BoxSizer(wx.VERTICAL)
- #button_sizer = slot_name_input.CreateButtonSizer(flags = wx.OK|wx.CANCEL)
- #text1 = wx.StaticText(slot_name_input, label='Input New Slot Name:',size=(200,25))
- radom_slot_number = "Motion_slot_Random_" + str(int( random.random() * 1000))
- slot_name_input = wx.TextEntryDialog(self, 'Slot name input', value=radom_slot_number)
- if slot_name_input.ShowModal() == wx.ID_OK:
- self.slot_name = slot_name_input.GetValue()
- print('self.slot_name = ', self.slot_name)
- title = 'Pose Designer ' + 'Motion Slot: ' + self.slot_name
- self.SetTitle(title)
- self.motionPages = [[ 0 for i in range(self.dof + 1)]]
- self.motionPages[0][0] = 1
- self.pageNames = ['page 0']
- print('self.motionPages:', self.motionPages)
- self.activePage = 0
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
-
- #self.pages_list_control.DeleteAllItems()
- #self.pages_list_control.InsertItem(0, "0")
-
- #self.pages_list_control.Select(self.activePage)
- self.slot_file_is_loaded = True
- self.new_file_is_Not_saved = True
-
-
- def On_Save(self, event):
- print('save button pressed')
- if self.new_file_is_Not_saved :
- self.On_Save_as(event)
- else:
- slot_dict = {self.slot_name: self.motionPages, "pageNames": self.pageNames}
- with open(self.filename, "w") as f:
- json.dump(slot_dict, f)
-
- def On_Save_as(self, event):
- slot_name_input = wx.TextEntryDialog(self, 'Final Slot name?', value=self.slot_name)
- if slot_name_input.ShowModal() == wx.ID_OK:
- self.slot_name = slot_name_input.GetValue()
- print('self.slot_name = ', self.slot_name)
- title = 'Pose Designer ' + 'Motion Slot: ' + self.slot_name
- self.SetTitle(title)
- defaultFile = self.slot_name + '.json'
- save_file_dialog = wx.FileDialog(None, message="Select .json file tobe saved", defaultFile = defaultFile,
- wildcard = '*.json', style =wx.FD_SAVE|wx.FD_OVERWRITE_PROMPT)
- success_code = save_file_dialog.ShowModal()
- if success_code == wx.ID_OK:
- self.filename = save_file_dialog.GetPath()
- self.filename = self.filename.replace('\\', '/')
- slot_dict = {self.slot_name: self.motionPages, "pageNames": self.pageNames}
- with open(self.filename, "w") as f:
- json.dump(slot_dict, f)
- if self.new_file_is_Not_saved : self.new_file_is_Not_saved = False
-
- def On_Save_and_Exit(self, event):
- self.On_Save(event)
- self.On_Quit_select(event)
-
- def CreateMenuBar(self):
-
- menubar = wx.MenuBar()
- self.filem = wx.Menu()
- #reset = wx.Menu()
- #color_devices = wx.Menu()
- help = wx.Menu()
- self.calc = wx.Menu()
- self.utility = wx.Menu()
- #self.blobs = wx.Menu()
-
- #connect = wx.Menu()
- #connect.AppendRadioItem(6, 'Default', 'Connect through USB')
- #connect.AppendRadioItem(7, 'USB', 'Connect through USB')
- #connect.AppendRadioItem(8, 'WiFi', 'Connect through WiFi')
-
- self.filem.Append(100, '&New Slot', 'Create new motion slot from file')
- self.filem.Append(101, '&Load from file', 'Load motion slot from file')
- self.filem.Append(102, '&Save', 'Save motion slot to loaded file')
- self.filem.Append(103, 'Save as', 'Save motion slot to new file')
- self.filem.Append(105, '&Quit', 'Quit application')
-
- menubar.Append(self.filem, '&File')
- menubar.Append(help, '&Help')
- menubar.Append(self.calc, 'Calc')
- menubar.Append(self.utility, 'Utility')
-
- self.calc.Append(123, 'IK Calc', 'Inverse Kinematic')
- self.utility.Append(124, 'Mirror Transform', 'Mirror Transform')
- self.utility.Append(125, "ListBox Test", "ListBox Test")
- self.utility.Append(126, "Rename All Pages", "Rename All Pages to 'page XX'")
- self.utility.Append(127, "Dummy Report", "Monitor Dummy values")
-
- help.Append(121,'About')
- help.Append(122,'Quick Start')
-
-
- self.SetMenuBar(menubar)
-
- #def On_page_selector(self,event):
- # if event.GetEventObject() == self.page_selector:
- # value = self.page_selector.GetValue()
- # if value == 'start':
- # self.activePose = self.trims.copy()
- # self.activePoseOld = self.activePose.copy()
- # self.activeFrames = 1
- # else:
- # self.activePage = int(value)
- # print('self.activePage:', self.activePage)
- # if len(self.motionPages) != 0:
- # self.activeFrames = self.motionPages[self.activePage][0]
- # self.activePoseOld = self.activePose.copy()
- # for i in range(1, len(self.motionPages[self.activePage])):
- # self.activePose[i-1] = self.motionPages[self.activePage][i]
- # self.refresh_Control_Values()
- # print('self.activeFrames:', self.activeFrames)
- # print('self.activePose', self.activePose)
-
- def On_Slider_move(self, event):
- id = event.GetId()
- val = self.jointControls[id][1].GetValue()
- self.jointControls[id][2].SetValue(val)
- self.activePose[id] = val
- self.action_To_Simulator()
-
-
- def On_SPINCTRL_change(self, event):
- id = event.GetId()
- if 0 <= id < self.dof:
- val = self.jointControls[id][2].GetValue()
- self.jointControls[id][1].SetValue(val)
- self.activePose[id] = val
- self.action_To_Simulator()
-
-
-
- def On_Quit_select(self, e):
- self.config = {'defaultFile': self.defaultFile}
- with open(current_work_directory + "pose_designer_config.json", "w") as f:
- json.dump(self.config, f)
-
- t = str(datetime.datetime.now())
- t = t.replace(" ", "_")
- t = t.replace(":", "_")
- t = t[:16]
- logfilename = current_work_directory + 'pose_designer_log/' + t + '.txt'
- self.log.SaveFile(logfilename)
- sys.stdout = sys.__stdout__
- sys.stderr = sys.__stderr__
- sys.exit(0)
-
- def On_Syncro_Set(self, e):
- if self.syncro:
- self.syncro = False
- self.syncro_button.SetLabel("NOSYNC")
- else:
- self.syncro = True
- self.syncro_button.SetLabel ("SYNC")
- self.action_To_Simulator()
-
-
- def On_Play_Page(self, event):
- if event.GetEventObject() == self.play_page_button:
- print('Play button pressed')
- if self.syncro:
- if self.physicsOn:
- for k in range(self.activeFrames):
- for j in range(self.dof):
- tempActivePose = self.activePoseOld[j]+(self.activePose[j]-self.activePoseOld[j])*k/self.activeFrames
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- (tempActivePose + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- time.sleep(self.slow / 1000)
- else:
- for j in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[j] ,
- (self.activePose[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- time.sleep(self.slow / 1000)
-
- def On_Play_Next(self, event):
- print('PlayNext button pressed')
- if self.syncro:
- if self.activePage < len(self.motionPages) - 1:
- self.activePoseOld = self.activePose.copy()
- self.activePage += 1
- self.activeFrames = self.motionPages[self.activePage][0]
- for i in range(1, len(self.motionPages[self.activePage])):
- self.activePose[i-1] = self.motionPages[self.activePage][i]
- if self.physicsOn:
- for k in range(self.activeFrames):
- for j in range(self.dof):
- tempActivePose = self.activePoseOld[j]+(self.activePose[j]-self.activePoseOld[j])*k/self.activeFrames
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- (tempActivePose + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- time.sleep(self.slow / 1000)
- else:
- for j in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[j] ,
- (self.activePose[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- time.sleep(self.slow / 1000)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
- #self.pages_list_control.Select(self.activePage)
- #self.pages_list_control.EnsureVisible(self.activePage)
- self.refresh_Control_Values()
-
- def On_Play_All(self, event):
- if event.GetEventObject() == self.play_all_button:
- print('PlayAll button pressed')
- if self.syncro:
- #value = self.page_selector.GetValue()
- #if value == 'start': self.activePage = 0
- if len(self.motionPages) != 0:
- for page in range(self.activePage, len(self.motionPages)):
- self.activeFrames = self.motionPages[page][0]
- self.activePoseOld = self.activePose.copy()
- for i in range(1, len(self.motionPages[page])):
- self.activePose[i-1] = self.motionPages[page][i]
- if self.physicsOn:
- for k in range(self.activeFrames):
- for j in range(self.dof):
- tempActivePose = self.activePoseOld[j]+(self.activePose[j]-self.activePoseOld[j])*k/self.activeFrames
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- (tempActivePose + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- time.sleep(self.slow / 1000)
- else:
- for j in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[j] ,
- (self.activePose[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- time.sleep(self.slow / 1000)
- self.activePage = page
- #self.page_selector.SetValue(str(self.activePage))
- #self.pages_list_control.Select(self.activePage)
- #self.pages_list_control.EnsureVisible(self.activePage)
-
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
- self.refresh_Control_Values()
-
- def On_Return(self, event):
- if event.GetEventObject() == self.return_button:
- print('Ruturn button pressed')
- if self.syncro:
- if self.physicsOn:
- for k in range(self.activeFrames):
- for j in range(self.dof):
- tempActivePose = self.activePoseOld[j]+(self.activePose[j]-self.activePoseOld[j])*(self.activeFrames - k)/self.activeFrames
- returnCode = sim.simxSetJointTargetPosition(self.clientID, self.jointHandle[j] ,
- (tempActivePose + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
- sim.simxSynchronousTrigger(self.clientID)
- else:
- for j in range(self.dof):
- returnCode = sim.simxSetJointPosition(self.clientID, self.jointHandle[j] ,
- (self.activePoseOld[j] + self.trims[j]) * self.TIK2RAD * self.FACTOR[j], sim.simx_opmode_oneshot)
-
- def On_Load_File(self, event):
- print('On_Load_File')
- load_file_dialog = wx.FileDialog(None, message="Select .json file with motion slot", defaultFile = self.defaultFile, wildcard = '*.json')
- print('FileDialog created')
- success_code = load_file_dialog.ShowModal()
- print('File selected')
- if success_code == wx.ID_OK:
- self.filename = load_file_dialog.GetPath()
- self.defaultFile = self.filename
- self.filename = self.filename.replace('\\', '/')
- with open(self.filename, "r") as f:
- loaded_Dict = json.loads(f.read())
- print(loaded_Dict.keys())
- self.slot_name = str(list(loaded_Dict.keys())[0])
- self.motionPages = loaded_Dict[self.slot_name]
- if loaded_Dict.get("pageNames") != None:
- self.pageNames = loaded_Dict["pageNames"]
- else:
- self.pageNames = []
- for i in range(len(self.motionPages)):
- pageName = 'page '+ str(i)
- self.pageNames.append(pageName)
- title = 'Pose Designer ' + 'Motion Slot: ' + self.slot_name
- self.SetTitle(title)
- print(self.motionPages)
- #for i in range(len(self.motionPages)):
- # self.page_selector.SetString(i+1,str(i))
- #print('GetCount:',self.page_selector.GetCount())
- self.activePage = 0
- self.pages_edit_list_control.SetStrings(self.pageNames)
- pages_list_control = self.pages_edit_list_control.GetListCtrl()
- pages_list_control.Select(self.activePage)
- pages_list_control.EnsureVisible(self.activePage)
- #self.pages_list_control.DeleteAllItems()
- #for i in range(len(self.motionPages)):
- # self.pages_list_control.InsertItem(i, str(i))
-
- #self.pages_list_control.Select(self.activePage)
- self.slot_file_is_loaded = True
- print( 'slot_file_is_loaded =', self.slot_file_is_loaded)
-
-
-
-def sim_Enable(ip_address, port):
- simThreadCycleInMs = 5
- print ('Simulation started')
- #sim.simxFinish(-1) # just in case, close all opened connections
- clientID = sim.simxStart(ip_address, port, True, True, 5000, simThreadCycleInMs)
- if clientID != -1:
- print ('Connected to remote API server')
- else:
- print ('Failed connecting to remote API server')
- print ('Program ended')
- exit(0)
- return clientID
-
-def main():
- app = wx.App()
- pd = Pose_Designer(None)
- pd.Show()
- app.MainLoop()
-
-
-if __name__ == '__main__':
- main()
-
diff --git a/src/azer_robocup_project/pose_designer_config.json b/src/azer_robocup_project/pose_designer_config.json
deleted file mode 100644
index b9b6f36..0000000
--- a/src/azer_robocup_project/pose_designer_config.json
+++ /dev/null
@@ -1 +0,0 @@
-{"defaultFile": "C:\\Users\\ab\\Documents\\GitHub\\Robokit\\Tomsk2021 disk image\\Soccer\\Motion\\motion_slots\\Dance_2.json"}
\ No newline at end of file
diff --git a/src/azer_robocup_project/pose_designer_log.txt b/src/azer_robocup_project/pose_designer_log.txt
deleted file mode 100644
index 139a7a8..0000000
--- a/src/azer_robocup_project/pose_designer_log.txt
+++ /dev/null
@@ -1 +0,0 @@
-log 2
\ No newline at end of file
diff --git a/src/azer_robocup_project/simulator_lib_directory.txt b/src/azer_robocup_project/simulator_lib_directory.txt
deleted file mode 100644
index edbb800..0000000
--- a/src/azer_robocup_project/simulator_lib_directory.txt
+++ /dev/null
@@ -1 +0,0 @@
-C:\Program Files\CoppeliaRobotics\CoppeliaSimEdu\programming\legacyRemoteApi\remoteApiBindings\python\python\
\ No newline at end of file
diff --git a/src/azer_robocup_project/threshold_tuner_server.py b/src/azer_robocup_project/threshold_tuner_server.py
deleted file mode 100644
index b3a677e..0000000
--- a/src/azer_robocup_project/threshold_tuner_server.py
+++ /dev/null
@@ -1,258 +0,0 @@
-# Image Transfer - As The Remote Device
-#
-# This script is meant to talk to the "image_transfer_jpg_streaming_as_the_controller_device.py" on your computer.
-#
-# This script shows off how to transfer the frame buffer to your computer as a jpeg image.
-
-import network, omv, sensor, math, image, sys
-import rpc
-from pyb import Pin
-
-#def threshold_tuner_server():
-sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QVGA)
-sensor.skip_frames(time = 500)
-sensor.set_auto_exposure(False)
-sensor.set_auto_whitebal(False)
-sensor.skip_frames(time = 500)
-#sensor.set_auto_gain(False, gain_db=0 ) # 9.172951)
-#sensor.skip_frames(time = 500)
-#sensor.set_auto_exposure(False, exposure_us=2000) #6576)
-#sensor.skip_frames(time = 500)
-sensor.set_auto_whitebal(False, rgb_gain_db = (-6.0, -3.0, 3.0))
-
-img = sensor.snapshot()
-
-pin2 = Pin('P2', Pin.IN, Pin.PULL_UP)
-pin3 = Pin('P3', Pin.IN, Pin.PULL_UP)
-ala = 0
-while(ala==0):
- if (pin2.value()== 0): # нажатие на кнопку 1 на голове
- ala = 1
- print("нажато 1")
- if (pin3.value()== 0): # нажатие на кнопку 2 на голове
- ala = 1
- print("нажато 2")
- sensor.reset() # Initialize the camera sensor.
- sensor.set_pixformat(sensor.RGB565)
- sensor.set_framesize(sensor.QQVGA)
- sensor.skip_frames(time = 2000) # Let new settings take affect.
- sensor.set_auto_gain(False, gain_db= 15) # must be turned off for color tracking
- sensor.skip_frames(time = 500)
- print('gain: ', sensor.get_gain_db())
- sensor.set_auto_exposure(False, 2500)
- sensor.skip_frames(time = 500)
- print('exposure: ', sensor.get_exposure_us())
- sensor.set_auto_whitebal(False) # must be turned off for color tracking
- sensor.skip_frames(time = 500)
- sensor.set_auto_whitebal(False, rgb_gain_db = (-6.0, -3.0, 3.0))
-
-# Turn off the frame buffer connection to the IDE from the OpenMV Cam side.
-#
-# This needs to be done when manually compressing jpeg images at higher quality
-# so that the OpenMV Cam does not try to stream them to the IDE using a fall back
-# mechanism if the JPEG image is too large to fit in the IDE JPEG frame buffer on the OpenMV Cam.
-
-omv.disable_fb(True)
-
-
-# The RPC library above is installed on your OpenMV Cam and provides mutliple classes for
-# allowing your OpenMV Cam to be controlled over USB or WIFI.
-
-################################################################
-# Choose the interface you wish to control your OpenMV Cam over.
-################################################################
-
-# Uncomment the below line to setup your OpenMV Cam for control over a USB VCP.
-#
-interface = rpc.rpc_usb_vcp_slave()
-
-# Uncomment the below line to setup your OpenMV Cam for control over WiFi.
-#
-# * ssid - WiFi network to connect to.
-# * ssid_key - WiFi network password.
-# * ssid_security - WiFi security.
-# * port - Port to route traffic to.
-# * mode - Regular or access-point mode.
-# * static_ip - If not None then a tuple of the (IP Address, Subnet Mask, Gateway, DNS Address)
-#
-#interface = rpc.rpc_wifi_slave(ssid="FED1",
- #ssid_key="7684067a",
- #ssid_security=network.WINC.WPA_PSK,
- #port=0x1DBA,
- #mode=network.WINC.MODE_STA,
- #static_ip=None)
-
-################################################################
-# Call Backs
-################################################################
-
-# This is called repeatedly by interface.stream_writer().
-def stream_generator_cb():
- return sensor.snapshot().compress(quality=90).bytearray()
-
-# Transmits a stream of bytes()'s generated by stream_generator_cb to the master device.
-def jpeg_image_stream_cb():
- interface.stream_writer(stream_generator_cb)
-
-# When called sets the pixformat and framesize, and then schedules
-# frame streaming to start after the RPC call finishes.
-#
-# data is a pixformat string and framesize string.
-def find_orange_ball_on_green_field(img, data_dict):
- blobs = img.find_blobs([data_dict['orange ball']['th']],
- pixels_threshold=data_dict['orange ball']['pixel'],
- area_threshold=data_dict['orange ball']['area'],
- merge=True, margin=10)
- if (len (blobs) == 1):
- blob = blobs [0]
- x , y , w , h = blob.rect() # ball blob
- x1 = x + w # x1, y1, w1, h1-right rectangle
- y1 = y
- if x1 + w <= 320:
- w1 = w
- else:
- w1 = 320 - x1
- if x1 == 320:
- w1 = 1
- x1 = 319
- if y1 + 2 * h <= 240:
- h1 = 2 * h
- else:
- h1 = 240 - y1
- if y1 + h == 240:
- h1 = h
- y2 = y # x2, y2, w2, h2 - left rectangle
- if x - w > 0:
- x2 = x - w
- w2 = w
- else:
- x2 = 0
- w2 = x1 - x2
- if x1 == 0:
- x2 = 0
- w2 = 1
- y2 = y1
- h2 = h1
- x3 = x # x3, y3, w3, h3 - bottom rectangle
- y3 = y + h - 1
- w3 = w
- h3 = h1 - h + 1
- blob_p = [] # right blobs
- blob_l = [] # left blobs
- blob_n = [] # bottom blobs
- blob_p = img.find_blobs([data_dict['green field']['th']],roi = [x1 , y1 , w1 , h1], pixels_threshold=7, area_threshold=7, merge=True)
- blob_l = img.find_blobs([data_dict['green field']['th']],roi = [x2 , y1 , w2 , h1], pixels_threshold=7, area_threshold=7, merge=True)
- blob_n = img.find_blobs([data_dict['green field']['th']],roi = [x3 , y3 , w3 , h3], pixels_threshold=7, area_threshold=7, merge=True)
- if len(blob_p) > 0 or len( blob_l ) > 0 or len( blob_n ) > 0:
- img.draw_rectangle(blob.rect(), color = (255, 0, 0))
-
-def detect_Post_In_image(img, data_dict, post_color):
- post_thresholds = [data_dict[post_color]['th']]
- for blob in img.find_blobs(post_thresholds, pixels_threshold = data_dict[post_color]['pixel'],
- area_threshold = data_dict[post_color]['area'], merge=True):
- blob_Is_Post = False
- if blob.y() + blob.h() > 235 : continue # blob connected to bottom of picture. No opportunity to recognize data
- else:
- if blob.w() > 314: continue # blob connected to both sides of picture. No opportunity to recognize data
- for y in range (blob.y() + blob.h(), blob.y() + blob.h() + 5, 1 ):
- for x in range (blob.x(), blob.x() + blob.w(), 1 ):
- a= image.rgb_to_lab(img.get_pixel(x , y))
- is_green = (data_dict['green field']['th'][0] < a[0] < data_dict['green field']['th'][1]) and \
- (data_dict['green field']['th'][2] < a[1] < data_dict['green field']['th'][3]) and \
- (data_dict['green field']['th'][4] < a[2] < data_dict['green field']['th'][5])
- is_white = (data_dict['white marking']['th'][0] < a[0] < data_dict['white marking']['th'][1]) and \
- (data_dict['white marking']['th'][2] < a[1] < data_dict['white marking']['th'][3]) and \
- (data_dict['white marking']['th'][4] < a[2] < data_dict['white marking']['th'][5])
- if is_green == True or is_white == True : blob_Is_Post = True
- if blob_Is_Post == True: break
- if blob_Is_Post == True: break
- if blob_Is_Post == True:
- img.draw_rectangle(blob.rect(), color = (255, 0, 0))
-
-def find_all_blobs(img, data_dict):
- current = data_dict['current']
- for blob in img.find_blobs([data_dict[current]["th"]],
- pixels_threshold=data_dict[current]["pixel"],
- area_threshold=data_dict[current]["area"], merge=True):
- # These values depend on the blob not being circular - otherwise they will be shaky.
- if blob.elongation() > 0.5:
- img.draw_edges(blob.min_corners(), color=(255,0,0))
- img.draw_line(blob.major_axis_line(), color=(0,255,0))
- img.draw_line(blob.minor_axis_line(), color=(0,0,255))
- # These values are stable all the time.
- img.draw_rectangle(blob.rect())
- img.draw_cross(blob.cx(), blob.cy())
- # Note - the blob rotation is unique to 0-180 only.
- img.draw_keypoints([(blob.cx(), blob.cy(), int(math.degrees(blob.rotation())))], size=20)
-
-thresholds = None
-even_frame = False
-def jpeg_image_stream(data):
- pixformat, framesize = bytes(data).decode().split(",")
- sensor.set_pixformat(eval(pixformat))
- sensor.set_framesize(eval(framesize))
- interface.schedule_callback(jpeg_image_stream_cb)
- return bytes()
-
-def binary_stream_generator_cb():
- global thresholds
- global even_frame
- data_dict = eval(thresholds)
- current = data_dict['current']
- img = sensor.snapshot()
- if even_frame:
- img.binary([data_dict[current]["th"]])
- even_frame = False
- else:
- even_frame = True
- if data_dict["blob"] == 2:
- find_orange_ball_on_green_field(img, data_dict)
- elif data_dict["blob"] == 3:
- detect_Post_In_image(img, data_dict, "blue posts")
- elif data_dict["blob"] == 4:
- detect_Post_In_image(img, data_dict, "yellow posts")
- elif data_dict["blob"] == 5:
- detect_Post_In_image(img, data_dict, "white posts")
- elif data_dict["blob"] > 0:
- find_all_blobs(img, data_dict)
- return img.compress(quality=90).bytearray()
-
-def jpeg_image_binary_stream_cb():
- interface.stream_writer(binary_stream_generator_cb)
-
-def jpeg_image_binary_stream(data):
- global thresholds
- thresholds = bytes(data).decode()
- print(thresholds)
- interface.schedule_callback(jpeg_image_binary_stream_cb)
- return bytes()
-
-def jpeg_snapshot(data):
- img = sensor.snapshot()
- img1 = img.compress(quality=90)
- return img.bytearray()
-
-def binary_snapshot(data):
- img = sensor.snapshot()
- thresholds = bytes(data).decode() #(0, 100, -65, -37, 10, 64)
- print(thresholds)
- img.binary([eval(thresholds)])
- return img.compress(quality=90).bytearray()
-
-# Register call backs.
-
-interface.register_callback(jpeg_image_stream)
-interface.register_callback(jpeg_image_binary_stream)
-interface.register_callback(jpeg_snapshot)
-interface.register_callback(binary_snapshot)
-
-# Once all call backs have been registered we can start
-# processing remote events. interface.loop() does not return.
-
-interface.loop()
-
-#if __name__=="__main__":
- #threshold_tuner_server()
-
diff --git "a/src/azer_robocup_project/\320\235\320\260\321\201\321\202\321\200\320\276\320\271\320\272\320\260 \321\200\320\276\320\261\320\276\321\202\320\260 2023.docx" "b/src/azer_robocup_project/\320\235\320\260\321\201\321\202\321\200\320\276\320\271\320\272\320\260 \321\200\320\276\320\261\320\276\321\202\320\260 2023.docx"
deleted file mode 100644
index 3e5f637c7665a85755064c3b1fac44b50df4f3cc..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 17452
zcma*P19&CP7B(8&nM`ckwr$(CZQD*JoY=N)+qRua^3NQ8=lu7cduu=YO;_z+)!l1V
zR;}u)x8x*&LB0V%KtKT0N4Tp1{3SvEKGt<GwsNGU`Gb{ny%NCm2w*#&IR@#Tu1(59
zM)jRxGnr!EesL35V3|^4MH<^%a!_q@D!n+2ugAxPg_~F(T^_|r*x;b$ZT=Af`saPQ
zJI>Dy9ih_AX1Zq8KZWj2464RtMxaFEU;NT0#dMA{2!kEw9q%;8=wZ<s-RFUK+sv=%
zJ}&%h=G3h#0~#7BOI&i|KbNC_2x(Gdz>;bVDK#asjs4Ur_80OuzSkOhO)8OO+%cYc
zyT-fnG(Cx*>{X1Dq~U-9m2~-0Xqbh(sikNHr@r!aCsaIvb^4T1Xuh|8-#ePm9Xp*N
z9GRH2>J2C-2@Y}S@FaBd`|)Rh0089vUloM-`wSynLpcXqJ4aeWTL)trH)|_zg>jjF
zI)sjUl=$m#?Bd)p2}OB1StWrPsDVl6rb>K!s&((%M>cU}D5VV-)1i9W;(bAN5KcN3
z6^o#~uv~zo5{J?y!iF^2etf8keTrk@pL#(tdb4cp62T_<3^!&JutVZC{;UcqklX^8
z$4vbz5wJ-cB070otz}Z9*|L)umNYys7<9x3V5vK9Q`p0e`X!#CPIVK)wx!KQW-t!^
zd45Pq^=vT)zbc*eBs_2V`?JU;Ph&7>Bsl9-aVsu@-#>WPNTU~?arc*3`yClA$do%N
z0Ldg}qv`bhI7fhOd)6GV?-MpQr;Eu7f`;(XgKqU?LZTEzMl>UQJF8*WTNqX^Nz?TD
zQn6ebYlbbQ;Z)E{vw2OI;&=gaP_-(XeV)@-Lu4aG<^&&=p=~xNwtb25bFeyxK`Sh)
zxb(kdQ-g>gPam_8L=014ri;#DGRDAM>OMo%r83LBb(*c4$6XPsvP)&XE($4ggWGSW
zw<o%O+Xx(@JGU^!84v*A_<!0A;jcE+x3l|Wvh4WYKPD5p_7XO-x6Tqmm>aVX=F5^R
z6_SWeu5AvNH_&`iSnD1pR!CxO&dz?s^qgEe&Xbkd32Z7NanFbciE5C@Y$9ntGM0}g
z%B7QdSmKkWK^V!{uBmK><FbGdCrXn`gqy7-gRKkX4`Ny);SfCs_m_W5S=*PFbZffv
z1PE*xsY~cX?<JFiW(6D-)5^Z-mNPzl^w}+FZ5i3G@b7&D^0%!beOFS)Ld$Nes3m(7
zf$zDFAizj8ik*7YfQmZb#YAv(dmk?_)+kig*tlwxe<H=2QcmwUlZ7wv4V!)i3QwA4
z_qP^?_TOgY_-<v}3B@&CM_gby`GaFOc;JP&gQ|>;!*XJxbLeUl5}h1gIDLB9L<Qy{
z|FO@Ro>*7MkraEd!SNINkET~llVhlUYZ~%@Y8v^knl^NHbh5SnqwL0cY1=+LH1L~u
z9^w8@fpTTbH4JNJ&WoCXRL+8hghoDq%?#shZ>L5Nrq!hRe#uW>JcLQ!^D%b$pXodF
zH((HKNyzbxK4HB!?JmVVvtajFssZz8OiF?7ci%cG@Jkt@EQU$!umk7N*aPJf6PDEA
zFPNGcgXNf6n#$qizJU@lAs?tI+T_O0pd}Qr<hvXa!#TOvC%#%5(TKHkc5buG9TCs6
zci2a))o_97s>hkA`vEQM2A7`W+n$E>+wJ9(3w+MWF7FyV!eXt`Jo)$ATg!IEIc#qx
zMsuaB6Z1C6hqL0~trNImN9fUh&f`2aaqomA6c`kp+ObG*%zby=7vX-kqLopO>OOB6
z_{Db*=Z&1tQQv*Ixsw~JtKmr6Owb>~gNFTl60#Ff#B$cVa{!d0)@b|G1YW&O_4+sW
z^efp>jqkSs%7Oovd&2r{09RWFBig?^@!y`x+1l8~>5mcq?$8s)Eq*t9;pbN<`gaYN
zx7=^zt4RV)gC79GIJ{wxd>Khc*PKlF_IPPD9UZC}oT7{rth_GYw$bKyvI7Xl`Ewb&
z(6x$p%0J!Qf~6ZNc<mPkR26uGh+2k-LAmAK5*gBQDrM5)PhJWiXDtlSInf(d&*2U^
zrge?7O!%A=$(y1BTCAZE9_e>{>%7<Dz_vTt)tsD)gQc9X=<V}kuoD6jr`W&`KAz5r
z*V_X(^$+-UAZeDc2KEyWDqWS%H^WrJBALN&8%_w~!f?5@2UpVt-E@%rTU;0ddEFTz
z8T-qUSi3vkK>svZa%d2}KMo!1e`@THL;st`{;I1>?sc1Wv6Sm~6i`|^F}fJ%+fDA<
zp4!8Kbr&l&7Y|Q*KR(H%K{(<va*CK|^3+oqR<2AvQxp2mPR?~kG^!{4TEBg@+I>no
zUA)geuhAclWn%J3#*$59iVVO5dq2z6oUfeA&a!lW-MQ-+PaAa>ngzcD*XEr7Btn~0
z7kp#6hOele+|ESPBNn7EB2uOtc?NGfoa>=2{<=cdR79uNo*D-Y<cAKPOLFnetw1f3
zIL*pRm3Y3B&{&qxSZYaKx>M9(DK9j>WyG^Mc?Nd~k<W?{68ec>Nrt+ktulX~0NDIy
z5~P}ryd5!6Fk2f&Q=S0$L4BBHFluqRD!lERqFU*odFkaWInOApEjj)Z34oyb?)r%M
zryeVEWD~uf)o_)()$usG_L_rNT^FI_FXPHV1;s=c){tCGAddy82GmHFhSUjh;nt<K
zqel(T63fH<MGc<KDYN^8&ju}_FciQ7%!o3yb*3AQVgthEb7jJbswJE`Ssw1j8ub|`
zb3srMzYw%18$Ap+eZg_7ecSlYij#*#rY7ZdYiaJpg?Hds`-L+;knNKWKME9<%4Lt&
ztuv0U=&)tPcI?hg3Qfh32E^7I*S5FRv)0%AHqFg)997E2`7`~-zA{ledll}`?aeds
zM>~j2Cb#|$QtX(?b^aOK0gHO!euv#HkGj0wQ)O$x@iF4S!Ghb}bGSKV5E)h927CLF
z9fwq9-kuNoU)#V+>9pmhH^W}a+`vpflw~KeRk%y8GO8!A$v_~o+J;&H8i;Q#J5>%V
z=mA)TVT~r;xFVLzsEm!`wm@efT!fdQYUAU_El<Wws#J^}+$I_&3YR*Un82tw3NY$)
z;$YlWDA95mPIj$ifPa{j-z^&pr|weXafI6kw*-&xB`Jv=Gi6(ku>Xt$jCS+|;7E{{
z7|QT|?CH{Is8%^GD?ErS>j)t9<5ZU9O=kS1u(mWdB%rUNqF+%9NK#tX)CfbNnjG=q
zS6**=M~Dt+?~BL77a;s#3YdYOnVuOw9m|E5>$8?)FntQW{=zCy4G;ijF(m3zaNm~F
z%^SD0;Lt^<m#jWzB3VbzjfMLIEgA?U8@^I=OGdj~7DIztqOGA+a!|{#Ke6C;HZsFo
z;PCJ`3^yf2K1ya_Kd}Iq3f2%E8wVY;8H~uZ-eFDbCV>KWIk%e+{Wg5ozN8+IFAFOZ
z4TZTWS1{J!<OLgzoRUM40puhg3A|VPgLbEI^Nc=HAJ#lVB)2fwC$1{DaFoW5iJ24~
ziSk^6HHk3>s6nN4gmx}AhuD9E@K=>k60J1%^qKpw;P04b0s>^v2*X41^=x6Gq$DTa
zg(|HjsZpVIDW-0KKuXHJZ3*UDbM=D>{K1y0D)wI0-%k)f7T$za$t>LBmD!=g++rT0
z;UP%wD#{p<fD|}j$zpC*+4a0X-I$=hghVef_6%q6Ql==%ESj;~ORTq|$Yg8q&*~^e
zX{)W4b7=#EYAKIV=wMrN%c3!9XIb!~gsr4R&cp`Yr(c5{c}7_@`Gl?LMUbp<21Y`X
zd?bLh#erphNSn{-=RYDP$Uwz)(y5AG!wkdK@d01GzRyOFMo*AwJhzGTjrE*cH&|2g
zWCQs_C-leh0t;Fzl5Ys^@}S^iV^Cu(5eibu2!{^cj;18gJ17zs{Gt)vv$7!4VWRnF
zR!eqE#n=?-Aicp$$VG2WL5K-(+o3`;YOXBK`_t#HZiW?f!V*IcON1yoDtdreCo?ue
zJrElmCstNql^WmB@RmN8@+rCK+d$eq)Cp|XFQYlK)Ox%Vh)k>st!acTLg$&-ma;69
z^TH{ZczyyX=1Y^pL5wYTgHU#kpnP~Y6$!#Ke(te1#vM<R&&5^b$`GcBjIY5!Q+?19
zN~4PpM6`Q=O0Ik9dS)LL?z4q7O%P9`!KJZ<4*~XRNs2SZMGDl$ZEB+mQ~FuX_5fF4
zSJIX<_(ga&R`b|BK3rRIk0RsM@+Y7QOB0{S6@W|(Pr)=cmhIq@7D%O)`NjG(QWfu5
zDSk!#7Fa|FLJ0nDe!khf=|CVF{!%8XeiLHl)tO|#Hs+ZkWRbOkbOG_*N*zGo%?}Yt
zx$V*`;6iwQlw5|AILh2NMAElHA_LdeL!w!G(`+V?dK_`)ruJnw%s0BjNx1}eX<+Ed
z>AZ#=+@4MTY!|@6JIubD&@*&1pZnlf3>sHKoxAwdJ)c{JvpkYgS>x6fzDWFlPT`xH
zV~8yn#B)RgY)NFQA)lCuGgzoc2YllZ;{74S6UH5Fy{KPzrVJhM^0J$3LT_n>`#>Au
zcr{a#%SYp9@uCl%DS<QxOw9dDW5F?pNs)qur1lKjAEh2(ay&;d-!+7QsqKx_^^O+>
z2LNT>U3;+vKWf1}4m90<Rq<CU$)eeow84jYTKd4N=44rxhAyi|oEzh{BY$#-XkhfZ
ziM6RoVWMjXJUxY7(dIzEoBUD@(}6>TYZoJiI|ftG0kwGo!S7=NfZAQ>4~guoI#45I
z4l=ZwxeZn<w3X#y?4!xf(DD4@i=FEX9C$V-0cYs>HN<~?1h)a@L8@=hu3$?}MvI95
zBi-*~=gO?kz*cOdF^|tyMe|K+p8A<u8+-XGHx{;b@o~7cQOJQBC9){NGCcVJ^xW-7
zn!2aJyr1&{WO6fJo7%#*ea1GR@IF{&=T^~{2!l1`?hAq>%>;2+i9$IvJZ>do_N%OR
ziugXM^Pu3_T}oR>_CjM?S~vb4lB5>C`C~=!ssCDM89oz((`(cyVc_=)KOZ_Xd9WAG
z7`KJ;pJEMzKzdn>#r`3E#h!ONBo${Vz}VOpMwPJAo)N<`2_c1;Gc|q9=C;zeo4utA
zS|U11P_l}pwPsw*NRcvdvN5tUQDKN460WH<N=;~z`Ej+Pd)};?aY>?%4wJd{Iv;q$
zKn(PzMjW`g!Tp=tds(B9ZSejJ6fmxY31)0|SKVc~&j?b4Zk|JL5)x6`W%;4pnl3P>
z?<S|~Yg0?gXK;nm8$78}vUq|m)X@8o{6>k-27t$6SaJo8d%zrVPi0pV8Lf;1Wnbo9
zjN7@_-*J5gHf3QnR_JxpyY*X6!kteH_muMa(DD!cI*zu(H)r?6uoiZ6cn`<j+Jn|n
zA1ZTdD3|(%FfAT~s;jT$(M1(a<-oRa@q77jWhSwm#v+;d+Npm@XkK34qfk@qn;oYh
zCvv9BuqdRdK|YHxP*Yb^Q#}uAk-eJfZyk@y`AVH>T$&x-|Ij|a5e_U08x{&*9~o%2
zsL`wnt>*9P4(9y!EV?*my*Wd#{Q+N>;4<lYVN2_C+}7YKfNB0x`oJA>Q_m9DiOj;p
z+XVN9nD@T?1-j`5q^yfK^5rpc<=+)5>NKEjC9k(K(^^Bsvj$$6-~1h#H@r11I@B9r
z%($f`dIk!7y*hBy0ouag`m}(22nYQh<=hN30z1tVq1uZ8y#ajVZgi`xe{caunFJ9S
zKHPB&8FiN{XqxO6C)WIq42u9_n3E>S=4br&m0oo{AsY?P$}nd@xo-{2S3cJ9fQQUC
zHU8O{hvR6-Pq7pyL+Jus-QsV7(e<B#2eogL4Mx$|@yKw;hX+6MvLlSLyYp8+nbTzO
zQ`v_YYx>s?a@W5-QF&l@T~LU#`@6(Wo-62>+|GKL$4%1E)`Bar7f6Oo7}!T7(vN&M
zNtBkgMUP8iaw@Jv53C$ZchH!sA<eK5%XA2;G}Ek`N;ou--!yUk?Yj=-qPX>``aVmu
zrgbf4y{7rg7Qx1iOG@hVtIXOAtf0+O2$pZdp0KPe{Mr1NOpZthFHAFsOzpU_qD9mc
zB#Vu?AH~KEVMu<Cy8CMSA&agI<YTcR0!?<%jAQo^05tK_Mfr6vPzo^sbKP~EO8Ai6
zE4LcyiA^HtF4L`z^mkiM($2)ZnoZ+#F|G%NX!f%uRjn|xDBTj4S(P}mB@LK>xqX6i
zt+(;gAz@+<SKqftq#q;E-YBn?3CMp`NI(<S=Vm}GF3Ukn=+(3nhx=+#-u2lC*Q$TA
ze9yLX@6JOHNwL+=5dd$p!JAu1hjo^p{~^#?k)Il+Y#j;I4=8RSiz5~s)uKpz8`C8&
zY)T&RW54s!?;3j+=l0BL7+p^&V(Cqfa4)aQ=*9urCl7M#dGv$`S9WuL_hrPb60|(O
zSF?_-r)Gp;@=L>2cR|CYSMJTf$keEFAhTrCS~}SQ9fJXkGHEQtB$l+YkE!=8=0(br
z0?u6&A4f4cphIKGOjBk41q1{*7O55Alv(>bUs4@dh%ZbbT$WCR$fnGZkcFGwAecUr
zkm&O4><@908sySjxe;;J7>*sHX0Z-@(oGl+4VfUpBS^d^KX(y<Bgo%*DPO@QNW3aP
zXOZ83VL0|~Tg|N49_3h;B3-{TIP}WZNo2STZNaLSvD!gUF@LX9(whP781PF^2uVqu
zYi*A`tc;8I0CwyV+s7va1;Y-AKl<4K(~VE~2zQi{UmG45-ysyLLS|m3lrNh)7oqS)
z7AnnwbL;<6&%Cix_-NFm^ffWq`rMk}laX=U+o4c?@+?a|mWfaL1VbtllC=_=)k0<}
zC~`qA5*13R+Bp@b7hkj#Nj}k+l95>NfRFu#?HT&j_gJNK+>H{x_X@qFnf!CY#X`p+
z8XJc}pDvwFi}l={Bq>{p4fjrG`bMYa<~x=Xkz<BUVV^Z_XAq-ao?RX_+X;eAypeng
zW9egZT%-rEwdCw>T;$t0gchuOwW%Ae*6tPUmS#uN^G#@u)Y&hyLN@bnI|cDm_PR`c
zSGc+7XLeOyI9H^I3bj{KS*oHtDUlBPr{d3RT^<R@qPbwzZG@;-m89$EF*yk7I-Re8
zmY5nUH0ZZxUMqwW<77XJ0dpB`!HN%lPTur99ojd43ibL6<x(ShBoSqinn42DvIfE4
zyP0Wa)d#l*O^eg{z>0k+aX&d$Y-l2=laYy(L3kz63ac24|K#24ol*U1Ihb@35<C`7
z%kkDBrcB2cTt2xN;=XE@q`+w|z38N=G1EPDIb$;|r4cNAis!wALUqj{H-Nlf06$b^
zlJIqLUT5Of#8cmvqM@#%z2DsR_^V9J3i6p8w|C3@cpWhxP0*)1kW0W{kWV&n+e;ep
zZA*?TMkeVIHecW4Cgjj(TKswER?04YX!bd>pnm(D-8ofDoao3yblcyeKHw=B2A9EW
z<R+W$*RQh)1p?+&wXD}VSGKO?U`$->3#OL*NNAFSy>o;7XEwZa4e^R1lkO~SsRuYr
zyg}EpI{}Ywm2CmEP&zn$tUk6Ndx(96A^a%8zZGnFqq!QZw^2f8SRW#_i%b!fxXCQi
zizkr9h^uC+C!SO$6D)EIc@)2DzCnUF+fRgDaAuXnl(#1h9{<etk33%B#$PL;!>ex=
zlSmU=iAS7?U$t?KJ3jpta(q>oG(}r3%Ef)n{u5{DG7Pt}Lgyt|a6)^6OtJPQ?gPx@
zE;<|G1=)7DX9L|KlILXcsYD;JqHDyNS^I-q99YAHm@6_%7C?t@34c0yf^C8Stb^4R
z;%p)v1<j6q%ItiRo>(W69uKVV`!qD<BV5bFp7}~HS5Zj7IC8w!oR4`I=7HKKDdJ-v
zK~h_mf?CHsQ4S5-^-!A>#tl9%>sUg@LFJ}Qrw^>IIVCTxYYLCrk9UT~rka+N40TCw
z`uZ1kUo0(JJ5Ej<xxIJ@qR}Q>#cp3sxwaVE@7NBLjdmzM(TxHh$}|IkJ8Wjb2H!gD
z?6Z?o5AF;IYOpUYaGo=R?)E13TZ4z;f!CM20G;mc2T=Vl&DIkF8&%w~k0Gynq7~|6
zD{CW3Ck08{tMzwo-R^rTO;K@zP7^M-8sVB`6n#b}eDMu&qpWISmpG_|WdM0-(Tt8T
zsf1g%$v?8K#}f0tdcE|!LfS%;<hCkda&Ueq=D%W}LpQEsc?BO~%jFyRgc|Mo&*K+H
zSY}~ZjhwExGVcQm>v9<a%pItj;THx0N#=PqLS@})4GptFGjAJ694lVoTi)fTcbiXe
z>lC<8(V;3^Q(60d%Nq~Fwjc8flA1Tj*a%iS;{So8^9rC7C+nx>EO(2a1E0+79(0z)
z>v?d)xhJH_|AmnHMzrAi`cZnQ{+)+QW*`X{3!1<DzyK)XSe)&etyBE5tMCQS^24D;
zv0E1z^`T_FiutuZ#w{1~fq_fOmI13V&WolAkXnK5BD7aT5isM(Btlv{)a$l?gTA{|
zd|121zLrz#66Qqir-xprr)-jwgSGiusFPD<s&{}x$MXFmuOwPGzJ0f1z1Y-AWJ>qP
zj2C7VkVktN(_UNIH;8;v#EGBn2>YUi{_wqcJG3zq%LdG`)vIx^?ZcM<1OyeTfcl@L
zkuQ8Oi6f+u&&1UDyFgBlfp~mwV9EY8w}NJRjGn8qQp(i9c2?BzaC2)!_9NjXH$NbL
zDZeiYshY(j3l@3}bB1k20-w;@;oQO2ML;8n_0pSQtC~ie4x(GsyRfajy-6NeStXX;
z?9C(}g@6|07I+UH-?!jJj@PtA#m-bw7p=KqKO7Ag;a@~l&s+T(Y5k?yyag_ZZCkTu
zaB)oKvuvInEfDT)@s%{D%m&6Ymn+X}Xy3KM>7qWn+b*arC$@Yrq?4q5>By`=phM7z
zKkZ{{4N#l%X?YLej|*-K?z(oUR3bTU2|XSQyuSE(tHG<0rVDK)`wli?8E!-?xT;O_
zdVXn!kLhc5XrBgB`6@VBs=xZXixrm<N{S+tdw^#`tz59W2IK4#Uh+VRf{g~#HKo10
zmlNIJafz)aM87j3lD-YJ30m@(_}UQaZa}#2`?Y-bnVqw3jA#>`r{CgxyrpeJkkBcC
zb(4@ty?u_70hAX5CG9`+?L?C7%TUjA6&Y^w`_K}5updD+tTdCp&*{xfu0ts`p>j~5
z-!Dw?KGB1k?3pWr77pe6yo(45(j&{Rj-K;y>64OeGg*!{{0!>W=Oef_9joN97)-5e
z0_GJFW=-3Wt4Pb1-t}jtthHn43u32-XY4TT!-E};L$tb$*aH_1g5y`8O^Ww)Xam~k
zAg5k=AI{s2+RQO$b&eXOwka}=G-%dR5BS5z0SP_MHCDMR`|*0zY&m<4HuFYOk(TT;
zf?up?a+R)oD<lg#*4UIzV!?3axTjr5l0Y6X6-JPY?j=+2h+}_7OOJIt*NAk_EsG0;
zayrsP%c!2{Q`AgGL<^Dl@u42GvShR*ea01X)KB0^?@H8QedhPJ2GU`;L3^{&G8ypl
z^ylA_+42Jb9VV{LCP$2C0W<SAzgN6%2S(uzZWdQAI1z=syT#aDGxM5R`$TF!^-H7Y
z0hsAC1Z6S^STE7aT^&lJ&+l5)%9anpO5!yv@kf$07O*-)zY9ptvuyPssb$MI<4{b=
z<?DhSsx`0zM9V}Q>3Ob=w_j?2DTcv@1kikQ`XD&j%gcoZ&2G=dM0es0Kir_F%4fJm
zV=?`v?w=DFLV4f#y7X(yWnzaWIOh53N%$qM946(MQuVOH-dE2pYLA>VA$31-D`XTD
z897$aJ?vu~#bRVw{P2W2P3aSHXPw*sDCu)txV7ft;3)Vf;RcW^G4!l?*OS$~t~g5o
z-BpH03Fc#APE7iGkDzZ&enjrco|4EhiJRPtqg=o3sOo+G_vZ7!Fn0Gnbd;*Y+(35e
z$rtf(ol8M&N>roGW&a?qIc=O*Mv<;wK{1sy9Mq?te$$VtNV<U;PYK?@P@qoqOOHGL
z5z0&danhn4j)36+uAPT#$TMI%!|bZ7S9T`N$)94PFPuu}XyG})MF8ecCGK$zabIoU
zeqATUz}_-R<y_48ZWt8Q_#QQH=mxMZJ4CZ}g6`YjeLv7ckkFqG@ubxR<Y5E75SG2q
zMrrKWoj}c4|MvI=646n5^)^Cc*xXb29tk1gCGmnHg6*#{;X7sCyBeVdv%xe_)(r}M
zf(FyYs^VAbkriPVn;&7wa2T=4W~tE!H@lW6KSyWp4C}jwhFgyxQ0mztWqLEqZ;B?{
zl^HANV-(!JGj&SHV_DV6G`m43GTw0Ksh0+P5fbL_!6(#z@4e=ZQaMv5s?wq47;36o
z2)`6yXXq5{vbKI~4s5B8;m+ASE4j5McFQi)-hg--vg97UQsjiv*g{EUyTz7_K#GJ%
z!duL}?@jBL@nJykGMxEpuQK0Ew5dCsh7Gr3oJ*(!+dzg&w77Bw4zI^hY#!3h!{@qs
zWLtC8wfFwr<=~n;>6u42w>K;@Pznv~edDHnzbq$m<s+x!Mntr4M~H9`KZCz8NbT<Y
zCbSYhyQC~H-(y0sqhS4h;(I``fejcLxZw{L%8r9|70Y?%bEdH5j_9Mn6m(a!(mO%R
z?*womY!8eREEi(9*;vSsg5DcJ+bx!akP;>EnKruj4<L9OUG*nTCT0dSxo}GWwhp|@
z$I$gT;7tYI_1yL*SGY@wmXhJaot@(JHQBCD4mS9$Ecb3rsj_#v2xcX(exhbz&XX`4
zsIBWG!=s8d%4aYN#k(FtHjts&gi=^Iy@{3~5!TA~$ASF`h$(T0tA3~`HiqmAVkN2q
z>ZoVQem!mQ4-wVv90HT_#q2>lsKS<zJi%AF9l1952T+H>$b;_awPOaNvTn-Iz?&V}
z9Ae)vC?LA?I=Faqq=LqJ!P&r@GQENK!5JeXe}^Bk;DB$u=VCF2`S^8dKhz?R-nm*$
z99weUHgQb3yvb!7l{AYgCu6c^{X1?^eBAF5unj8Sc)5f}Jb$eF%Yw;{@Z=hLsT|GA
zMk9A%&wWTcXZBOAB3Oeu-fKF@cjZHtx`l1;_qgs+Z#*5XHnB3z^5BhG6JCzo0q>l~
z$2Lyfam`+rHhRhzGE^FmRZ@@H;UE?L%whQu;$3~%O$oLe7iclweY*ADwoz|^RV%x1
zSpd%m>9|P1sG5d~uxK}5=f@mxUdf7!L9r18U9O!A;rq3^Y&`iwRYO>kc|twiXxk5t
zu+T}Lt)qEzn%2e*beN7X^rOFJSbOv;BWC<u(}GtEP+U9zg#;Lwir=%cXmp@v{%0E+
zT58okD<)d=-GF$mmP{|5gRhhJMnR&|J5%)wGrMMI`W_8crEVzP`v#`lZmHpVX5Vz3
zft4RNk0aO9#JV@IP9ZeQ@EU0)r5EUK16cTSir6Npn^%RaiX&^;YfAL(;EkeGopgJr
zIZ_N~FQxM}m8lDM+91fs1kd0YvLmiS$U#Ko3eESk=aZ2=n$i8XXO-#F?wEzGMJQLu
znOAxYA{=2ZW4Xwn4mJE;&%WHTAFYAyk90Vw+aS7;r&LNn7O{d(UAuiyy6`W0rST!L
z_c80DCWGMA+xO{WS$1bZ6d|Acf{kl?$IiZHV^yz^DBpjC@D>T<$_&4spa;u(DW-bA
z$(2yL28VdFl@C)t8PueBX<g{_7!=b$j!X}d&k1s+eSv|G(+6uGGz4!8p|+Vt`n^`p
zlWxE8JEiC@C=I4A#_ElfM#|f2(k2CF!DJgA`(N(Xl2r-jX?VcGKdo--{HnUz2n*A(
z>0dpP@fmBP2Zqo+2^ca{hxVS~TJH2#xxduY&XT-fz}X)R@SJ>7^s5hfNDTy&3hH^P
zw>4>WEZ3hy<^{s2CPH%bzck<WbH|4Xxau4b<I<8Q@?ht1jRo)^uEkjKd4i2g3|~DQ
z=B7^PY&hx2On(mN9EOk0o1SNa$(H~Jvdj5_Ys0!d{0#g|67~S{q&C<QqXi=>b1Z47
zRR)>Lr__%YHb#xa(%B3=6(4v2+A5J9UiXGDLEYSd1?`){2t+V7=nK^GYpa$WasSfZ
z70pqBNC2*r1RUg2k@F7deNlj)UV*Etr#eAcn_SKHTgTlKo05;^>!nPVKKTgfOT4XD
zlM8g`u{=#nw@vKzp4Eol-l~aRtDP<P>QdO})82Muo63Td;4D-ER5Rd8Ud(5KUJpru
zQGFc^D$Jnoic}`;>yInLyCmq)wY)~%==W>5*D8A&v}yVTl1mQWBJK|q&XMeGpjzFF
zK%04u=Hc0uQ5LIR5$H9?^%m@KY&yqJijoxthPCfbvop-eI96ongA`<DP{L6H+sc35
zo2dBv6xQXzhLO_6&5C!d#?@!g7;*=REyX#Xv?lwWls7C=A>B-#icy*vlFr8^WU&Z6
zUgpCN2mIPli)S(Z+}txkl5YVOj2RYXbL@LO84z2bs3AIK`5F>&L)TuNV7GCsOX9DY
zjm|@{w5@9>)$CZEfe{>E*0D)}y;bl!vuE!fYR{MyyhmrRTwwzzI#hL<tQm?=UPwR-
z0u;<pbV8#cQ^C}ITLbgWo}Ght1`S5&1I(P1FkY;I&=;nkW9Nia<{&*4hue-nDMaaA
z0x|{xk6$f<HfzcPcV_<h`ZK3(NTJP?i~|6W<O%pc7Ae91b57gQ$=%A>@z3o4V)mrN
z`Uv9iwL8=|+0ou{c=@CQ4R^_eOEHwurHs2a2P_~#dli6?-caM$G{-A<zm6Uig;J$U
zl|%Ff+oWCqJU|vO0eBDP<Um(pgpd9Sr!UVUelM^0&D0)Oo_n(I3|u%--RQ7bIBj#t
zyO*~+`;+Uap}?dvYA5u%A-~*idiHx=t9_m?KgQYIZ`?1)-sybYeYw<#N8b+Z(Ap_+
zYF;o~kMiDZ$P4_4&kUV7J5uADV+zOj)pmh>+0dg+`u)P~<P3%s0ic2~N7ZN&ZMD#0
z4jss~s^rJKxaNF8eIOw=Xka1LoKmZF$+j|x4e-6<xlV>)vU<GT?J>u~sJ#|77k~6p
zE~?75Rlg?u^g%MkEjTZEQS+Mn@}%r1a=Fs2TS3`#vm5fN4IALIR?iNy_l7pLcR+$x
zSw9&X4&;Yyk*`#xh6u8la5u@hj#pyIfvN_xWSSig*@men8P=Vy1M@ZcywzhEZGNTd
zpl-gs860-+Y|WI0cD~sMjMSNsW$4$ZhwI;*RloL`dsq7GeszHuS~Z1)PjtsIJJamy
zNV>v)J_>)^bECsJz~t4@5)}{`3m*<FC@|R10^iee!-1#d458e;m8Hb-iE3@${QS_w
z#u@}uEy{cIyw)r7z8>Dnq@~d%mB2q+k25BGrD43O?^LYiZejV7gv3MzUNh?6qe@I;
z+!DshLrfGA(mM6=iOcSaodsX>tOAzKIp=$ycFtXO#6H<+g>!n*0_S+EgeMY+^&Jgf
z@qhbfDlo80xF;3P_AZo#j(F@B<|sCBijiz0L!ixueKBj-i^V?xstAOLIsqW06?YTf
zg4c~~&(A@+7jPjx06LoCmr2{01x;K}cPgzZksidbE7RBgP+Tilo3<s?H%`Kh6YrD&
zkZ9UuKyW4PnEnYvP<TEu=gcEpH#ZqCX-aN0FP2>HjXBjny`idH*v}<<3*nC_J4aR+
zc^XFRUO46|gTCS<ryH~ZIj_f$kZI{6IkcB2%H>5IPeg77l<{+vLNOW-;>@o#o@nGX
zii%e&qUB4?7~z=d(OYJ~LloH{W3iiMChha5|IkA<3{#P>zT7SPA_59iemsSl5U|Qj
z*bsHBY?(c!nb7{9Ol9sPZZl^5i<c`p!<Em~)kG@2a9?KRdR!?e-`8moo9UTWeb}am
zRi{Ip<7{qZXRISdC-`xQfy{bb$<iZ-(3DcOaM}~@3`Vqs7A8)72MZU0zqQ*3vCGet
z(N&<!tfC+3#gT>b4V&cw?Dz9t{Oh{bRHwN5nt&ZB@=_#$CRF?&-Qy|s$3l{w)|Bt8
zR^w;1<(m~kYaKx1GZMOOu1w}Hzw{>wl~Xn!Njj6eXs=sEMYBlbCk!hoUPlY-54J(h
zQ6hz55H^e7mgl4cCoEO1HroZ4d64m=x3@YV(r&QZPzGKn7k@euE~}@f!@|JcD3OH2
zjHC^w_iX#_Me@!t-3X!d?gGo(%7)3no3Etfr5QOSJcNl^q>~HqGlW`f<-GI@)QuG6
zX^V1o4-xJP4X{Hm@gz4X>2#>RhUzgFy7ShE865|g2ssM^m1nU;%@4+IT;`l@iH9Cc
z%)B$r9^*bLH2ab(^*`#8EZBXdLab$4c#Q0&=GHT7eN8RdDP?ott7Ac@Rx}gGmbDYd
zwsn6*b_bApqf;y9*Z@f4twN;m)!`@kU<A<{$1=y(^k;}}>h{IWas?<lWIPXxxfxB(
z&{4>mp(SH7MU8=Dh@AO-s-7t6ebCOA_PptqC-#S>0HB~LXT{5W33cBo=_%P4>HquN
z^t`q^{!;^FUYZ_-NO?jz&IwsM?M9E9@H;mG{`SkS6vQmNN%Q4+EfZJ#5xVZMOat~z
z8l<s7mzV;FJJK#ZOO2s?E7V?e`J;CQq-@P~8Md`LANqAglQ)sbE$No0^l{a>XE&o0
zMaxbuT}IU&#o|q0s@8AK<pb%}ET-?}X{#sV<8^w~>D=BuMf5q+8WUA5q;t7eWE~zZ
zP0O2TS_GV#Jk}lB;tN#IoEm+GQ$LzEsuC<yz_F=!>ESsw0wTRltv%c(^V^5`*vukx
zq@ztCx&oJQk0kep`kO%KhU&;c;Mej?NKlRkoa=R=b6wlahpe`xxlBBxA|}2?Wlbba
zwJ{K`fjs&9ez)IeKO^<|+S1|%y!&i$Jo?3x4Ed7oG51ka1>I-|Wq8Hp@YMEYr##MZ
zWPg!mGZ8lBJ{)zD<u?%~lRjb=BOz`galBz9;v}PB0>Yu$iw>AtnafS1$?utI1xn7c
zS~g746Lr#oMMpRz5Dd{E=UMS>0wPJyzcpD7ACy`tj1L-x;P)@kbXjtF+%hFITOuWK
z282pt{C_Y1ErAd8x6>9KtZkW^kthmtgP{=3@&rIMN_L)v3;#c)PzdA%K#x=eg1`N%
zB9smIsBaVv!;mOz>kudkRzXnYf3FU~|2u`yKWmpL>DiDdi3|8wQ2^#eyg=|1Tb_U?
zW}ZMq^ncv`zitx}B~23&Md9zyil0x-6PZIMow^!(Ysww1dO>(~KZ^O7=RxUuiDluc
zv`WEP{@w{utgyu%&5Cn%=^PpsOk2n!`~g4*qRQ97TfVhx_=px;rA>YnJky;XpqMsa
zm9mpH)<Atwo{~F~*(B_-h*ZH*Y`xlS0a?^*t~s~ZMnx+jKaWUfBgu%eU)$S3t*JV(
zi_m;{5`mAk+ccp?9|XLS5=$BKE|ZewTKUWwGjHYSO^(+PY#3~(N74{X{E&RR;bU5y
z4yoN~O5)^PknVR^j2P%zfgb!+gKm6mLB?27ei>{*qNfcDsZs@o=BX-})L4!`Hnm7c
zZ}7WJO?92rl(Bbg5y0@;BCO7ZS!lHtL&jK5RS;-tG0xA~c^Yyxtk#uTNWC2+|3V8E
z)%9OLF%zk^V(49H!^$sJg^+Qo22!A3T8Q<uVIb91g@9110!MZJzR=nt!s3yI_xHgr
zjk2`;@A@c$8$nw11H{J>dBTlKwM`W%6d>akkZ)sE8f6@lzrkbrH?+-GX#jmTl&4@|
zTdGnsd#qGxFtlU*K5AXlDREg;NG!KsOqgA4u_#^qCyZWLXf-Yz{2ks8tW>GdwWG^Z
zH2wt>RYS^>(tm*~cKmf^$;v+^e*+WM{j#4V{{h4Wzb~Xv{!d6){d2X^|A5hxKbJiF
zCpi6-0_i2pJpznG3cYf-Kc><luK4PQ<PAZL{rQ#d31_WJE8mH=wXEZtT>FT*yzm*0
zQGWq>lDwY$po*&VIH;uaDYpWXif&46AMxyg!g&gY%Da5lz5JRvaTQE@UGLdKczS-h
z+TJCxPN28{3rDe$+0$k=l*v;QLlXHz?AB`hR<Ewwmr2J+_Hb{oy>^9RH{L?t{c$SR
zCj9Z0fxJz-Vejj?(ZZ&vy3Wu;?%{|NC%C=fWpUO0W6kznf(09gE@&e;0>9adL>*mV
za6X8&xj%Ut$p(_FA2`hUygB^mMtj_t3izuJGm76zMTd4(rQ~GU3<ky4p{n)s{x+_!
z$FJpmUtC}Lug}uYk1ST}UM1wMP-zdydQ<oB>y8&)@?x%8)!WKXviV8ZW5o-@MjDDE
zMhoJwsRq3Tzx*v7zYn1(GkMj+rkCGkM@;3cvTv6hFgOPf-+j35-`C*tz_$mhXUabL
z%QnHM`;4Is&Nzt)gEJaZWU#<Gb@NQ}<vhTSN^h15%UvFZ%^bqwoGniQ&rH?tVN=US
zdkIzypL%3-e=1BmaQP~ET7^B_f}os6`;*^&{TOW5=dgyDH~-4x2rAvjjpZ#rxXl^P
z@%cF4@5c3geYABdc%q_@8$5VQIW7vT+(7(LTGmV0&g_#Bt-WDk+%emY?kF{lb2zW#
z4?-u=WH3@?KhsvRh*`L<z)p6wOY8I^Hukw?g29YGzD_02aqHAJNS;PD(>7q&ob8`w
z=jfaEKXM^i?ei(-U;NHEk5uD!VR|h>0?mu7_Bb5uGUdJ7*5SW}%0lMKwcq7Cc-MKj
z)?s<H^=tk1q-OVB_|s0mz`L+qj!;j{cEWBx9In3uTdk$Ago~$0oXN`Sn-u+;bG)0^
zS&Hg6;P`KCXv!vSr$>Uu1Qmlmi>k#8cS>WcZvwc?p6$Y0@|_8rQ$@F`$}eZUO2{uF
zmchjpR;)S=bu`{q##9R&qx;7n6TUT!xJ#kk<Zol$d#Yc+e}+vOQzv3?FaQ8QJ^$C@
zX{3J+n{1q|4U8SkZA|}+oGvrRV>0Q!g*<ylK$J$(AYG;A;@6joSSTwiGkXJoN%8f$
z7;%n1T%N~`2g&5>A^I=W*Rk<*f2{MoNPg$y>e_Q?5{yDk63S?$Yd|M}l!mU0Y0#aP
zz30ocM@(d*A?J^O!nD7%f85o_O`!JSmpYm}KnMzb7aI>=32K{4#M(`xJpn^pO_dKZ
z#K&FL*j7xjL6b%}RtT5eWPp(!w&!n{r(>FHiV7;GKWY&}J7hw>58C^^ggri2(NU#_
z1{s$)$GmPZENs64Cb>55{K&m__juOD`u^IkwDMqWAC*QeE9_d6tDM_g{E$S2{IDuX
zJJ^a~vV_0^Gd-?WTMH-QVbLBr=Y}{fzp%&N{UTw^^ugow-He9jwtAFZBVP)uKNI(2
z<{MG6cc@+AYYNw-ciOO#Hg@qPdZ^Y%QiZDF$K~S1Nn@esKo>qfLi^?N+2yhm7HGc;
zZa*)1%&2wu6Loim+VNxv^?W%dOCVgxC4O1&X=l3%0@+0sqY`^dC#<9|RLG|PSns(m
zT#Pk&QXs1k`%9<mGP9Bni*Jbo#2e-lH)ww^-(k&#F5C}fCkpURkvQMI!8T+a9zY!c
zT0hht=$%8~p-9-Tf961M7`Sw>eiwt@0J#Bn0O*7aM{=Tcq=k$HGmz*4B|{rab+u5S
zj){5IQvWSOrA(7V|GMYgE?3lxDtc&)8BhD8^v=;;staU_`(+PM*N?UbbqD$e^u+<Z
z>*XJZKwf}d8~(I_zf1od0{lM7b!S{5oi+%`DeB%jrBhg#=5kZZWt*|A+LdojYQ{tL
z>JI=({|1~t!0-n|{(!+BaFOnN^Nfg(&^dUEoWvNju$(KOaAhc0H)$NXFiGDeaLh24
ziJY)bX{BbURzGSS`DK#6O#q!?%pYmBN~NhY{!m_>tm^wW?#!X|n%lx}Hpe~J|F8cE
z6I&Z6C4B>{zY(4~6?$#@=wLdpDf2f(IpF4%<YyYpi^Kc%aCg8`H6->ZrcPeogTKq{
z`;iP$-+KB?Pv2SHUS_Z9f7_Eg4HeHr0UO7JT9@#x+P=zZNC43XjsaHjh|Nd6Z&%l4
z!AdCEz$v_-+$>s19tyLb4U}r>kE;zS2u<p?xAcg~q^T2*uei!i|NeVrHXlE(@g6b5
z29wck@@499oJf2aoPrD<wR8UQ@Trn6*p2%Mv-`Kk0Uhs}P&x!FX!)C-RsV7)c{9cb
zf6``N<$XR}znBDT*+*=lfVEvfhJsbv`Ngf}izyH3>dVJ0^5e>>p@bhopJvXzG?kq$
zU8r_SqHWQuoQr6ojtZ0z`|SRb<1jyrhP*S^jiyZ};SO^8aB&&F{>IpVgr*0;|IOHd
z2KiqK{ulqi(b&o9x6Aq4+nUD2UYm6~n2>94D8c6;nGk1Tx#ANzepRaTUUC-`sy2L*
z@BpD@$DFRH^?YIQ3xxxcaNbv6ywb?=?kyDfGzR;UNiUUjAi?pJK5y%*rt^76MI(Q`
zUi+T4sI<w$vjYP)lW+3MGz|hH<9RHe@hw#28&|V}KkNr`Yhx0@XD!=k!DU!TZ0Jp;
zAFH&K7S&Jv1Jun&MNM>J#=}Yn@gfwJi?~3jTxK{|`>GIQDBJ;5Gi@VN?Z$ZmI<hlN
znHE8Bpgw*{a5@m}hy14OJ%)esKP`|<ZH!7ON?dVR*axsw|G2Ndrsl<{bzz7X26bK;
zIkB7M$M6^^*Ry@2Dnv;R>L-Qx1ozwB@hZ>WjzL_8m?O<zv7ey&GEnD?z?2A|kJWRw
z5M?Z=>MsNX3Bc3_li-r_?h>hK>k+-XWc*aQ&Cl>%B{BQ*GDUn^Hr%kR=4Bl<_Z)vd
zr_A7}oRCdJAVCkAZ$F9)iaVr0UO2K_k5`^gMXJuRe+2o{UwqG?qw@M~y4n9p9YOhL
zf8k_iY;8>YH>Cetm!Ym*J<o#HwOt+NpnI3yAo`HYVy(ZA#g_$)oN9AMa>Qnj(tex;
zg)eQ#Za&Ava%mvezCPFh3t6~XqwC006fOQMm|cc9YYC7gVt`4bH2U(``{Tv?a#+UU
zv(G%D(J)O>sIJK-o6#B81Q9)t%I2IE4m`TZNB=qSUN4lGE*L}9)Cs)dF{pMlMt>$u
z71$$2N^fX=h)5Q2e~^nsgq>|yN5N{P=oFKjFTKhbFWCCy%q@zb1p7P9<7V(Z&k-+r
zfv|AUeW2I_nI^==$q2>gNmK~iatxr&c}ET+J9|*E)G}Re6l;UCqL-85b4M*xU&WA0
z9cI|{hK&s!M_sTs4t#1z)p@Ir>FGAjhwa`Q|MvB}q%ROaepu|nT93!3O7rn#EmBE`
z(#sVn_i)(`@z)es*FMTN)Ry}0ChRXggen6v|1n8f#Ka!BO{gu+-49qVJr@rfxn5|w
z0C|@Ne-`d-NYnytfguoGg+9$6oDdi6Ucn57tIBJp6*g5rcUVTjvG5Y0(Z-lqGJEL5
z*l0k|U>yrD^VoKgZGjjB+#Ag^f#-L-2bWP)2?+GSfkxXZB2Gk<dqoT6V@N70eFr+v
zkPZgtado)7E3!8|9bB%D>))TJuRaQE+Cb{mnmj%?kA1m+3BBLnHc#Vvl@2MQf33&n
ze7=pNTHq!6Pj`7dyeEoIcYR*OUgMJOWMjb?zu-&YFIM#+Uvh}Q_I$(c(f^#Lf<yK*
zij?`T80X(7;(t|N7|_K4j7DastropAnKd2+<!0~EF7SdkS`0((mzvlBv_T5uDqe*(
zf0$3p4mD4u;`&3@c*7YWM%g>`_nUfIQm-z;<Z(KGl#FFMS@tk}uymD%ctQvilV*gn
z0cTuc_B^AB22HVZphB2Yd<=%hS>mL-jtM0Y_b$rX)$USfNR`&>-hGOMYA8Qt8vLrD
zTY@x(I12Q@Z{s7}JxC~{f$BKQKr-mlPB8SdZ-io{V@DP|<ujcVhr-!U-dvL9BUyQb
zC=T!l1caUkWKw(_@(v>E1ctnYqFxa|ml*mINQG?z5e(D7BNp@C$ox;!OHZ&>6=#qN
z4N{Ja34VobIHqnedMw+Z6dphFFOv7VNY1y`lbvY_r@P!=aFHHlVv;B=w1>{^&iAkr
zyn?p8FR%Q~U10AH?{wO>C7-|K+s>EnpFll-&W3m?q1w!?IliOmd-a&0UCHNNCBfS=
z?Ih>pMWr-DCI;zFwU_VgVTD;zlU^7OBm{y;xv{34;1@YA(=U<)-F@&^KT<!XnzzY{
z7*`roUrDtB>XmzRbyJ<a+6;_o@TzSFP;W*mNErkDuzPzWDx4}K!U7HNapy+*-S>W{
z28}FwWcVJEgwplNB*)cvvyp(9V++tPA74D@6Tew$+D4PMsJ|Yu+qh9_+$JE9VWW&R
z$0XGQQKf3jzFe8YvDlO#@_XS=D+@~_py^_QOW$(VzC9~qTEinfFZ$iMQ{Ts|04rcj
zeG^Wtd+t|sos`3vhaU;C6jSoPEMj15i2T@bTNYSWWsACtURV2p#!)*eyCL_e9ajiq
zd8s;zyXL9T>RZT8K3`GR13v@DY=~QiE;yH*jvfji7XDbuELNUY#zr|=)##!Zxyp~U
z$`rDmM34#BZ|W10EiF28@k97&Rk34o!FFiyO||D;Yi80+mMn3(DuY{FB(${-3l&^H
zml`vU#$Cf%w-Jaeq?wsTr7LZ&SN8n*&ZervGqp1(SvO#pOe^uTDTU4V7eUasju^X#
zW6KpM%w9oulC}whB16Z1B89C5w`DO{XB0H~3_oz#n0e~=TX}qhoVS=1oH}QT4Bumw
zU#pu`TW%ln>x)~s8*c;q@CKURv(o1K>|aW@v6<vVidi7o!?6U_y1g5$KHBOs4G=mG
zH-l3bFFs;Y7c-i*blTL<LVeg29EbAT&OmpXSlX#B$AGR}vgMsWXTokn^@p5if_|8k
zBepbc+6T(iU%OmQ8BU6Jz32w1-mHCK{^>95@n!WcK>+~VG5%Lq|A!r`Dfl~%`pwAH
zRdTm8cGUjUTXZIm+XVidlX+kbD$`a{29+R;JI)g6q|I)EXLBpqB%auPjfPN`&qO4>
z28%SY;d$i-Qdl2dk<Y9~+Auakgq{fTZMjv>SVRX^_Le$bTJ+SwvPXEOTuVe{E0a?K
zTi4%+W)|$53Y0bRy_=<^1=^P$@yz+{ONc`y7a8vR4u8=p@VAO8uZNc2VQSPvqBh%i
zAPT~XGzq(-S}DQgt3~ya4ZvNEL(bDZ8ufs&qT$LQdsSZKC<<2qfsoO@`;Q1$;CEta
zKxSIE4V43g$@Ir*DFa|{=){?WzWWQp!;I-l7Pw^te0HxJa5sKzOeUanP%KfQ^LhU^
zG7#{G5@HD?2=Ht{ABxQ7DNA>ju10pG&r<6x>n7Hor>_-<z)v`C%d?#jLPtBeZfG`s
z-FT^zUaTowO1o6q**Ca7W$IKtd52e!&PTm4pi=0j3ZL6C*x-}OP*lLJf+Jd$S$24l
zo0n<7Ck}Q_5yN?WQF3b^g?ZXPhC9zlC3pMX(a(eZF&rS!H-LZdi}2SC0sicZ@So`~
zyCnR#;9ptkf5N+er$6D~|FwI-e~bT>YW;`0{_nK-J6!*l_&<p3|1JGjp75Ur_wU5}
zI}ZJq^uNj3|6RdfX|sRm#Q#n`zgzhKA{YO6O@E!N{7*gH{Z{w?MY{cO@xKn*|IBFq
zI}H*4L;T<KoBu8R*U`<Nas9v32g5&v|6`Q@-|~MA-v3h`k>wxq|1l!^Z~4DQkpKDY
z4y^yY`M(7G|1JJkhw>-P{CA>e|A+X02c7?2#b2BDe>Mbd-v38GBqs^_+bjS8Fuy+l
MzXRXz{C{%)2k{V+DgXcg
diff --git "a/src/azer_robocup_project/\320\274\320\276\320\264\321\203\320\273\321\2141.py" "b/src/azer_robocup_project/\320\274\320\276\320\264\321\203\320\273\321\2141.py"
deleted file mode 100644
index 858b44d..0000000
--- "a/src/azer_robocup_project/\320\274\320\276\320\264\321\203\320\273\321\2141.py"
+++ /dev/null
@@ -1,2 +0,0 @@
-s = [1,2,3,4]
-print(sum(s[-2:]))
\ No newline at end of file
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index 54df0d0..a6785d9 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -8,11 +8,9 @@ from PyQt5.QtWidgets import (
QLineEdit,
QLabel,
)
-from PyQt5.QtCore import QTimer
from .window_interface import WindowInterface
from queue import Queue, Full, Empty
-import time
from typing import Optional
from geometry_msgs.msg import Pose, TransformStamped
import tf_transformations as tf_t
diff --git a/src/calibration_node/launch/multi_camera_calibration.launch.py b/src/calibration_node/launch/multi_camera_calibration.launch.py
index b236865..668cbfc 100644
--- a/src/calibration_node/launch/multi_camera_calibration.launch.py
+++ b/src/calibration_node/launch/multi_camera_calibration.launch.py
@@ -1,6 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
index 0ac50ad..cb8de59 100644
--- a/src/calibration_node/launch/single_camera_calibration.launch.py
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -1,6 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
index 7d65998..db20322 100644
--- a/src/localisation/localisation/dump_aruco_publisher.py
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -1,20 +1,8 @@
-from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped
-import tf2_ros
+from geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
from rclpy.node import Node
-import json
-from tf2_ros import TransformException
-from tf2_ros.buffer import Buffer
-from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
-import tf_transformations as tf_t
-from pyquaternion import Quaternion
-from tf2_ros import TransformBroadcaster
-import numpy as np
-import math
class DumpArucoPubNode(Node):
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 4cac4c5..4a10583 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,22 +1,13 @@
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
+from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
from std_msgs.msg import Bool
from rpy_msg.msg import RPY
from geometry_msgs.msg import Vector3
-import tf2_ros
import rclpy
from rclpy.node import Node
-import json
-from tf2_ros import TransformException
-from tf2_ros.buffer import Buffer
-from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
-import tf_transformations as tf_t
from pyquaternion import Quaternion
-from tf2_ros import TransformBroadcaster
import tf_transformations
-import numpy as np
import math
# from ParticleFilter import ParticleFilter
diff --git a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
index 6fbaea1..3a0de89 100644
--- a/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/multi_camera_aruco_preprocessor.launch.py
@@ -1,6 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
diff --git a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
index 8131ecd..9f2332c 100644
--- a/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
+++ b/src/localization_preprocessor/launch/solo_camera_aruco_preprocessor.launch.py
@@ -1,6 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
diff --git a/src/localization_preprocessor/launch/static_tf_publisher.launch.py b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
index 4442999..2845ca5 100644
--- a/src/localization_preprocessor/launch/static_tf_publisher.launch.py
+++ b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
@@ -1,7 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-import os
-from ament_index_python.packages import get_package_share_directory
import yaml
def load_yaml_file(file_path):
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index 5356212..abe1274 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -1,14 +1,12 @@
import rclpy
from rclpy.node import Node
-from std_msgs.msg import Header
-from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, PoseStamped, TransformStamped, PointStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped, TransformStamped, PointStamped
from tf2_ros import TransformException, TransformListener, Buffer
import tf2_ros
from tf2_geometry_msgs import do_transform_point, do_transform_pose_with_covariance_stamped
from ros2_aruco_interfaces.msg import ArucoMarkers
from tf_transformations import euler_from_quaternion
-import tf2_geometry_msgs
import tf_transformations as tf_t
import json
import math
@@ -106,7 +104,8 @@ class ArucoPreprocessorNode(Node):
r = pose_stamped.pose.orientation
quaternion = [r.x, r.y, r.z, r.w]
roll, pitch, yaw = euler_from_quaternion(quaternion)
- rad2deg = lambda x: x / 3.14 * 180
+ def rad2deg(x):
+ return x / 3.14 * 180
# roll is near 180
# pitch is near 0
# cov is equal for all angles
@@ -198,7 +197,7 @@ class ArucoPreprocessorNode(Node):
#self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
quat = rot.x, rot.y, rot.z, rot.w
- euler = tf_t.euler_from_quaternion(quat)
+ tf_t.euler_from_quaternion(quat)
shifted_pose.pose.position = do_transform_point(shift_point, transform).point
diff --git a/src/pic_to_real/launch/bal_to_real.launch.py b/src/pic_to_real/launch/bal_to_real.launch.py
index ed2273b..ce595bf 100644
--- a/src/pic_to_real/launch/bal_to_real.launch.py
+++ b/src/pic_to_real/launch/bal_to_real.launch.py
@@ -1,10 +1,8 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
import yaml
-import json
from launch.actions import IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -76,7 +74,7 @@ def generate_launch_description():
# transforms_nodes = create_tf_nodes(static_transforms)
# for node in transforms_nodes:
# ld.add_action(node)
- tf_node = create_tf_node()
+ create_tf_node()
yolo_dir = get_package_share_directory("yolo_openvino_ros2")
yolo_config_dir = yolo_dir + "/config/"
diff --git a/src/pic_to_real/pic_to_real/ball_to_real.py b/src/pic_to_real/pic_to_real/ball_to_real.py
index 058433a..7fefa45 100644
--- a/src/pic_to_real/pic_to_real/ball_to_real.py
+++ b/src/pic_to_real/pic_to_real/ball_to_real.py
@@ -1,8 +1,8 @@
-from soccer_vision_2d_msgs.msg import BallArray, Ball
+from soccer_vision_2d_msgs.msg import BallArray
from sensor_msgs.msg import CameraInfo
from image_geometry import PinholeCameraModel
-from geometry_msgs.msg import Pose, PointStamped, Pose2D
+from geometry_msgs.msg import PointStamped, Pose2D
from tf2_geometry_msgs import do_transform_point
from tf2_ros import TransformException
@@ -11,8 +11,7 @@ from tf2_ros.transform_listener import TransformListener
import rclpy
from skspatial.objects import Line, Plane
-from tf2_ros import TransformException, TransformListener, Buffer
-import tf2_ros
+from tf2_ros import TransformListener, Buffer
class Ball2RealNode(rclpy.node.Node):
def __init__(self):
@@ -42,7 +41,7 @@ class Ball2RealNode(rclpy.node.Node):
self.get_camera_to_world_frame()
if (
msg.header.frame_id in self.camera_frames
- and self.camera_models[msg.header.frame_id] == None
+ and self.camera_models[msg.header.frame_id] is None
):
self.get_logger().info(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
return
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index b8d8179..4f08c5a 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -1,7 +1,5 @@
import asyncio
import socket
-import threading
-import time
import rclpy
from rclpy.node import Node
@@ -119,7 +117,7 @@ def main(args=None) -> None:
asyncio.run(launch_proxy())
except KeyboardInterrupt:
- print(f"Exit from program.")
+ print("Exit from program.")
except Exception as exception:
print(exception)
diff --git a/src/server_main/config/field_config.yaml b/src/server_main/config/field_config.yaml
new file mode 100644
index 0000000..c7ae103
--- /dev/null
+++ b/src/server_main/config/field_config.yaml
@@ -0,0 +1,7 @@
+/**:
+ ros__parameters:
+ field_length: 21.
+ field_wide: 10.
+ world_position: [0, 0]
+ goalpost_wide: 1
+ # Some kick areas configs penalty, kick off and etc
\ No newline at end of file
diff --git a/src/server_main/launch/camera_calibration.launch.py b/src/server_main/launch/camera_calibration.launch.py
index 18ac928..962ef7e 100644
--- a/src/server_main/launch/camera_calibration.launch.py
+++ b/src/server_main/launch/camera_calibration.launch.py
@@ -7,7 +7,7 @@ def generate_launch_description():
camera_config = os.path.join(get_package_share_directory('server_main'),
'config',
"camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
+ os.path.join(get_package_share_directory('server_main'),
'config',
"robots_config.json")
return LaunchDescription([
diff --git a/src/server_main/launch/multi_camera_server.launch.py b/src/server_main/launch/multi_camera_server.launch.py
index 2f145ad..7316f1d 100644
--- a/src/server_main/launch/multi_camera_server.launch.py
+++ b/src/server_main/launch/multi_camera_server.launch.py
@@ -16,7 +16,7 @@ def generate_launch_description():
"robots_config.json")
yolo_dir = get_package_share_directory('yolo_openvino_ros2')
config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
+ Node(package='yolo_openvino_ros2',
executable='yolo_openvino_node',
name="yolo_openvino_node",
remappings= [('/ball_on_image', '/camera/ball_on_image')
diff --git a/src/server_main/launch/multi_team_multi_camera.launch.py b/src/server_main/launch/multi_team_multi_camera.launch.py
index 39e7c5b..03c5ddd 100644
--- a/src/server_main/launch/multi_team_multi_camera.launch.py
+++ b/src/server_main/launch/multi_team_multi_camera.launch.py
@@ -19,7 +19,7 @@ def generate_launch_description():
'config',
"robots_config2.json")
config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
+ Node(package='yolo_openvino_ros2',
executable='yolo_openvino_node',
name="yolo_openvino_node",
remappings= [('/ball_on_image', '/camera/ball_on_image')
diff --git a/src/server_main/scripts/camera_calibration.py b/src/server_main/scripts/camera_calibration.py
index 4dfddb9..7be49d6 100644
--- a/src/server_main/scripts/camera_calibration.py
+++ b/src/server_main/scripts/camera_calibration.py
@@ -27,7 +27,7 @@ for fname in images: # Here, 10 can be changed to whatever number you like to c
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (6,8), None)
# If found, add object points, image points (after refining them)
- if ret == True:
+ if ret is True:
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
if (len(objp) == len(corners2) ):
objpoints.append(objp) # Certainly, every loop objp is the same, in 3D.
diff --git a/src/server_main/scripts/camera_pose_calibration.py b/src/server_main/scripts/camera_pose_calibration.py
index 5ad5cbf..9fd75af 100644
--- a/src/server_main/scripts/camera_pose_calibration.py
+++ b/src/server_main/scripts/camera_pose_calibration.py
@@ -1,32 +1,12 @@
from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
-from sensor_msgs.msg import CameraInfo
-from soccer_vision_2d_msgs.msg import BallArray, Ball
-import tf2_ros
+from geometry_msgs.msg import PoseStamped, TransformStamped
-from image_geometry import PinholeCameraModel
-import rclpy
from rclpy.node import Node
-import json
-from tf2_ros import TransformException
-from tf2_ros.buffer import Buffer
-from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
-import tf_transformations as tf_t
-import math
-from skspatial.objects import Line, Plane
-from pyquaternion import Quaternion
-import paho.mqtt.client as mqtt
-import math
-from geometry_msgs.msg import TransformStamped
-import numpy as np
-import rclpy
-from rclpy.node import Node
from tf2_ros import TransformBroadcaster
diff --git a/src/server_main/scripts/create_calib_photo.py b/src/server_main/scripts/create_calib_photo.py
index 75750b0..25bd8af 100644
--- a/src/server_main/scripts/create_calib_photo.py
+++ b/src/server_main/scripts/create_calib_photo.py
@@ -1,7 +1,4 @@
-import numpy as np
import cv2
-import glob
-import yaml
import os
if __name__ == '__main__':
@@ -13,9 +10,9 @@ if __name__ == '__main__':
image_directory = os.path.join(os.getcwd(), 'images')
try:
os.mkdir(image_directory)
- except FileExistsError as error:
+ except FileExistsError:
print(f"Directory allready exists: {image_directory}")
- except OSError as error:
+ except OSError:
print(f"Directory can not be created: {image_directory}")
diff --git a/src/server_main/scripts/is_symlink b/src/server_main/scripts/is_symlink
new file mode 100644
index 0000000..5a5de0c
--- /dev/null
+++ b/src/server_main/scripts/is_symlink
@@ -0,0 +1,5 @@
+#! bin/bash
+
+if [[ -L "/sys/class/video4linux/video2" ]]; then
+ echo "It's a link!"
+fi
diff --git a/src/server_main/server_main/server_node.py b/src/server_main/server_main/server_node.py
index cba30f8..48bd6f3 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -2,7 +2,7 @@ from ros2_aruco_interfaces.msg import ArucoMarkers
from geometry_msgs.msg import Pose, PointStamped, PoseStamped, TransformStamped
from sensor_msgs.msg import CameraInfo
-from soccer_vision_2d_msgs.msg import BallArray, Ball
+from soccer_vision_2d_msgs.msg import BallArray
import tf2_ros
from image_geometry import PinholeCameraModel
@@ -100,7 +100,7 @@ class ServerNode(Node):
self.aruco_size = self.get_parameter("aruco_size").value # m
self.declare_parameter("world_tf_topic", "/tf_static")
- world_topic = self.get_parameter("world_tf_topic").value
+ self.get_parameter("world_tf_topic").value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.tf_from_camera_to_world = dict()
@@ -248,7 +248,7 @@ class ServerNode(Node):
# self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
quat = rot.x, rot.y, rot.z, rot.w
- euler = tf_t.euler_from_quaternion(quat)
+ tf_t.euler_from_quaternion(quat)
# print("TF RPY: ", euler )
# print("Ibverssed TF: ", inverse_transform.transform.translation)
# import time
@@ -347,14 +347,14 @@ class ServerNode(Node):
rotated_to_forward_pose = self.transform_to_robot_centre(pose, id)
world_pos = self.tf_buffer.transform(rotated_to_forward_pose, "world")
- if not (robot_id in robots_poses):
+ if robot_id not in robots_poses:
robots_poses[robot_id] = [world_pos]
else:
robots_poses[robot_id].append(world_pos)
# find mid of x,y
for robot_id in self.robot_ids:
if robot_id in robots_poses:
- if not robot_id in self.world_robots_poses:
+ if robot_id not in self.world_robots_poses:
self.world_robots_poses[robot_id] = AlphaFilter()
self.world_robots_poses[robot_id].update(
self.find_mid_pose(robots_poses[robot_id])
@@ -365,14 +365,14 @@ class ServerNode(Node):
print(msg.header.frame_id)
if (
msg.header.frame_id in self.camera_frames
- and self.camera_models[msg.header.frame_id] == None
+ and self.camera_models[msg.header.frame_id] is None
):
print(
f"No frame from yolo ball coord.self.camera_models[msg.header.frame_id] == None. Frame: {msg.header.frame_id}"
)
return
- if not msg.header.frame_id in self.camera_frames:
+ if msg.header.frame_id not in self.camera_frames:
print(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
print(f"Camera frames: {self.camera_frames} ")
return
@@ -426,7 +426,7 @@ class ServerNode(Node):
pose.orientation.z,
pose.orientation.w,
]
- quat_conj = tf_t.quaternion_conjugate(quat_arr)
+ tf_t.quaternion_conjugate(quat_arr)
# rotation_matrix = tf_t.quaternion_matrix(quat_conj)
rotated_x = Quaternion(quat_arr).rotate(x_ax) # rotation_matrix @ z_ax
return math.atan2(
diff --git a/src/server_main/server_main/strategy.py b/src/server_main/server_main/strategy.py
index 13557ec..5d03de5 100644
--- a/src/server_main/server_main/strategy.py
+++ b/src/server_main/server_main/strategy.py
@@ -9,7 +9,6 @@
# import random
-import rclpy
from rclpy.node import Node
@@ -17,7 +16,6 @@ from visualization_msgs.msg import Marker
import paho.mqtt.client as mqtt
# https://github.com/eclipse/paho.mqtt.python
import tf_transformations
-import math
from ble_communicator import BLECommunicator
from collections import namedtuple
diff --git a/src/server_main/test/logger.py b/src/server_main/test/logger.py
index 58c60f8..fff4ff9 100644
--- a/src/server_main/test/logger.py
+++ b/src/server_main/test/logger.py
@@ -1,6 +1,5 @@
import matplotlib.pyplot as plt
-import numpy as np
# from collections import defaultdict
# from multiprocessing import Process, Value
# import json
diff --git a/src/server_main/test/mqtt_kondo_dummy.py b/src/server_main/test/mqtt_kondo_dummy.py
index f77f672..9094ec5 100644
--- a/src/server_main/test/mqtt_kondo_dummy.py
+++ b/src/server_main/test/mqtt_kondo_dummy.py
@@ -17,7 +17,6 @@ from visualization_msgs.msg import Marker
import paho.mqtt.client as mqtt
# https://github.com/eclipse/paho.mqtt.python
import tf_transformations
-import math
class MqttKondoDummy(Node):
def __init__(self):
super().__init__('MqttKondoDummy')
diff --git a/src/simple_approach/simple_approach/SimpleApproach.py b/src/simple_approach/simple_approach/SimpleApproach.py
index 9ed3f03..66dcf6f 100644
--- a/src/simple_approach/simple_approach/SimpleApproach.py
+++ b/src/simple_approach/simple_approach/SimpleApproach.py
@@ -1,6 +1,6 @@
from dataclasses import dataclass
from typing import List
-from math import atan2, cos, sin, acos, asin
+from math import atan2, cos, sin
import rclpy
@@ -118,7 +118,6 @@ class SimpleApproach(Node):
def rotate(self, init: Pose2D, target: Pose2D):
- yaw_i = init.theta
if abs(init.x - target.x) < 0.02 and abs(init.y - target.y) < 0.02:
yaw_t = target.theta
else:
diff --git a/src/strategy/launch/game.launch.py b/src/strategy/launch/game.launch.py
index d5dbd4f..d08c0be 100644
--- a/src/strategy/launch/game.launch.py
+++ b/src/strategy/launch/game.launch.py
@@ -1,6 +1,5 @@
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
import yaml
@@ -74,7 +73,7 @@ def generate_launch_description():
static_transforms = load_yaml_file(tf_config)
tf_nodes = create_tf_nodes(static_transforms)
localization_nodes = create_localization_nodes(robot_config)
- approach_nodes = create_approach_nodes(robot_config)
+ create_approach_nodes(robot_config)
return LaunchDescription(
[
diff --git a/src/strategy/static_transforms.yaml b/src/strategy/static_transforms.yaml
new file mode 100644
index 0000000..d932cd8
--- /dev/null
+++ b/src/strategy/static_transforms.yaml
@@ -0,0 +1,121 @@
+static_transforms:
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.6016723372662476
+ - 0.5895844827615363
+ - -0.37392631681738836
+ - -0.3880201616498358
+ translation:
+ - 1.7194672464486558
+ - 0.13004539642942464
+ - 0.795737330978838
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.8709112956805877
+ - 0.00026127570606400804
+ - -0.009936401067979887
+ - -0.49133971417419703
+ translation:
+ - 0.37129417947490884
+ - -3.2384551301915
+ - 2.3390352519878563
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.6016723372662476
+ - 0.5895844827615363
+ - -0.37392631681738836
+ - -0.3880201616498358
+ translation:
+ - 1.7194672464486558
+ - 0.13004539642942464
+ - 0.795737330978838
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.8709112956805877
+ - 0.00026127570606400804
+ - -0.009936401067979887
+ - -0.49133971417419703
+ translation:
+ - 0.37129417947490884
+ - -3.2384551301915
+ - 2.3390352519878563
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.6016723372662476
+ - 0.5895844827615363
+ - -0.37392631681738836
+ - -0.3880201616498358
+ translation:
+ - 1.7194672464486558
+ - 0.13004539642942464
+ - 0.795737330978838
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.8709112956805877
+ - 0.00026127570606400804
+ - -0.009936401067979887
+ - -0.49133971417419703
+ translation:
+ - 0.37129417947490884
+ - -3.2384551301915
+ - 2.3390352519878563
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.6016723372662476
+ - 0.5895844827615363
+ - -0.37392631681738836
+ - -0.3880201616498358
+ translation:
+ - 1.7194672464486558
+ - 0.13004539642942464
+ - 0.795737330978838
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.8709112956805877
+ - 0.00026127570606400804
+ - -0.009936401067979887
+ - -0.49133971417419703
+ translation:
+ - 0.37129417947490884
+ - -3.2384551301915
+ - 2.3390352519878563
+- child_frame_id: camera1
+ name: camera1
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.6016723372662476
+ - 0.5895844827615363
+ - -0.37392631681738836
+ - -0.3880201616498358
+ translation:
+ - 1.7194672464486558
+ - 0.13004539642942464
+ - 0.795737330978838
+- child_frame_id: camera2
+ name: camera2
+ parent_frame_id: world
+ rotation: !!python/tuple
+ - 0.8709112956805877
+ - 0.00026127570606400804
+ - -0.009936401067979887
+ - -0.49133971417419703
+ translation:
+ - 0.37129417947490884
+ - -3.2384551301915
+ - 2.3390352519878563
diff --git a/src/strategy/strategy/solo_robot_strategy.py b/src/strategy/strategy/solo_robot_strategy.py
index d0a0721..b7ebb12 100644
--- a/src/strategy/strategy/solo_robot_strategy.py
+++ b/src/strategy/strategy/solo_robot_strategy.py
@@ -1,7 +1,7 @@
from dataclasses import dataclass
from enum import Enum
import typing as tp
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, Pose2D, TransformStamped
+from geometry_msgs.msg import Pose2D
import math
# need struct with motions mapping and json
diff --git a/src/strategy/strategy/strategy_node.py b/src/strategy/strategy/strategy_node.py
index 97cd4da..79a8a21 100644
--- a/src/strategy/strategy/strategy_node.py
+++ b/src/strategy/strategy/strategy_node.py
@@ -1,30 +1,12 @@
-from ros2_aruco_interfaces.msg import ArucoMarkers
-from geometry_msgs.msg import Pose, PointStamped, PoseStamped, PoseWithCovarianceStamped, Pose2D, TransformStamped
-from sensor_msgs.msg import CameraInfo
+from geometry_msgs.msg import PoseWithCovarianceStamped, Pose2D
from std_msgs.msg import Int32
-from soccer_vision_2d_msgs.msg import BallArray, Ball
-import tf2_ros
-from image_geometry import PinholeCameraModel
import rclpy
from rclpy.node import Node
-import json
-from tf2_ros import TransformException
-from tf2_ros.buffer import Buffer
-from tf2_ros.transform_listener import TransformListener
-from tf2_geometry_msgs import do_transform_point
-import tf_transformations as tf_t
-import math
-from skspatial.objects import Line, Plane
-from pyquaternion import Quaternion
-import paho.mqtt.client as mqtt
-
-import numpy as np
-from dataclasses import dataclass
-from solo_robot_strategy import Strategy, Cmd, CmdType, RobotFabrica, FieldPose
-from std_msgs.msg import Int32, Bool
+
+from solo_robot_strategy import Strategy, Cmd, CmdType, RobotFabrica
robots_config = {
"robots":{
diff --git a/src/strategy/strategy/strategy_test.py b/src/strategy/strategy/strategy_test.py
index b575de2..046f0eb 100644
--- a/src/strategy/strategy/strategy_test.py
+++ b/src/strategy/strategy/strategy_test.py
@@ -1,5 +1,4 @@
-import pytest
-from solo_robot_strategy import Robot, RobotState, GameState, RobotFabrica, RobotInfo, Strategy
+from solo_robot_strategy import RobotState, GameState, RobotFabrica, Strategy
from dataclasses import dataclass
@dataclass
--
GitLab
From 5b1d919df40cdb6cb9269b80948980b423dfcf0c Mon Sep 17 00:00:00 2001
From: kuznetsov_n <tatus>
Date: Mon, 29 Apr 2024 13:22:52 +0300
Subject: [PATCH 64/64] Apply ruff format for all packages
---
.../launch/approach_teb.launch.py | 87 +++---
.../calibration_node/__init__.py | 2 +-
.../calibration_node/calibration.py | 8 +-
.../calibration_node/gui_window.py | 8 +-
.../launch/multi_camera_calibration.launch.py | 138 ++++-----
.../single_camera_calibration.launch.py | 84 ++---
src/calibration_node/launch/tf.launch.py | 42 +--
src/calibration_node/setup.py | 38 ++-
src/calibration_node/test/test_copyright.py | 8 +-
src/calibration_node/test/test_flake8.py | 6 +-
src/calibration_node/test/test_pep257.py | 4 +-
.../launch/dummy_launch.launch.py | 5 +-
.../launch/dummy_loc_multilaunch.launch.py | 40 ++-
src/localisation/launch/ekf_filter.launch.py | 5 +-
.../launch/loc_multilaunch.launch.py | 50 ++-
.../launch/renamed_multi.launch.py | 51 ++-
.../localisation/dump_aruco_publisher.py | 56 +++-
.../localisation/localisation_node.py | 133 +++++---
src/localisation/setup.py | 37 ++-
src/localisation/test/test_copyright.py | 8 +-
src/localisation/test/test_flake8.py | 6 +-
src/localisation/test/test_pep257.py | 4 +-
.../launch/static_tf_publisher.launch.py | 22 +-
.../aruco_preprocessor_node.py | 164 ++++++----
src/localization_preprocessor/setup.py | 36 ++-
.../test/dummy_pose_pub.py | 57 +++-
.../test/test_copyright.py | 8 +-
.../test/test_flake8.py | 6 +-
.../test/test_pep257.py | 4 +-
.../odometry_estimator/OdometryEstimator.py | 44 +--
src/odometry_estimator/setup.py | 29 +-
src/odometry_estimator/test/test_copyright.py | 8 +-
src/odometry_estimator/test/test_flake8.py | 6 +-
src/odometry_estimator/test/test_pep257.py | 4 +-
src/pic_to_real/launch/bal_to_real.launch.py | 17 +-
src/pic_to_real/pic_to_real/ball_to_real.py | 13 +-
src/pic_to_real/test/test_copyright.py | 8 +-
src/pic_to_real/test/test_flake8.py | 6 +-
src/pic_to_real/test/test_pep257.py | 4 +-
src/proxy/proxy/CommandSender.py | 24 +-
src/proxy/proxy/Proxy.py | 68 ++--
src/proxy/setup.py | 31 +-
src/proxy/test/test_copyright.py | 8 +-
src/proxy/test/test_flake8.py | 6 +-
src/proxy/test/test_pep257.py | 4 +-
src/rpy_msg/setup.py | 24 +-
src/rpy_msg/test/test_copyright.py | 8 +-
src/rpy_msg/test/test_flake8.py | 6 +-
src/rpy_msg/test/test_pep257.py | 4 +-
.../launch/aruco_detection.launch.py | 49 +--
.../launch/camera_calibration.launch.py | 39 +--
.../launch/multi_camera_server.launch.py | 260 +++++++++-------
.../launch/multi_team_multi_camera.launch.py | 291 ++++++++++--------
src/server_main/launch/server.launch.py | 147 +++++----
src/server_main/launch/server_calib.launch.py | 149 +++++----
.../launch/server_calib_cam2.launch.py | 148 +++++----
.../launch/solo_camera_multi_team.launch.py | 185 +++++------
src/server_main/scripts/camera_calibration.py | 58 ++--
.../scripts/camera_pose_calibration.py | 12 +-
src/server_main/scripts/create_calib_photo.py | 42 +--
.../scripts/xz_angle_from_quaternion.py | 6 +-
src/server_main/server_main/filter.py | 16 +-
src/server_main/server_main/server_node.py | 10 +-
src/server_main/server_main/strategy.py | 255 ++++++++-------
src/server_main/setup.py | 37 +--
src/server_main/test/logger.py | 33 +-
src/server_main/test/mqtt_kondo_dummy.py | 248 +++++++--------
src/server_main/test/test_angle_from_quat.py | 3 +-
src/server_main/test/test_avg_quat.py | 16 +-
src/server_main/test/test_copyright.py | 8 +-
src/server_main/test/test_flake8.py | 6 +-
src/server_main/test/test_pep257.py | 4 +-
src/simple_approach/launch/approach.launch.py | 87 +++---
src/simple_approach/setup.py | 37 ++-
.../simple_approach/SimpleApproach.py | 256 ++++++++-------
src/simple_approach/test/test_copyright.py | 8 +-
src/simple_approach/test/test_flake8.py | 6 +-
src/simple_approach/test/test_pep257.py | 4 +-
src/strategy/launch/game.launch.py | 1 -
src/strategy/setup.py | 25 +-
src/strategy/strategy/solo_robot_strategy.py | 34 +-
src/strategy/strategy/strategy_node.py | 23 +-
src/strategy/strategy/strategy_test.py | 46 ++-
src/strategy/test/test_copyright.py | 8 +-
src/strategy/test/test_flake8.py | 6 +-
src/strategy/test/test_pep257.py | 4 +-
86 files changed, 2223 insertions(+), 1783 deletions(-)
diff --git a/src/androsot_approach_teb/launch/approach_teb.launch.py b/src/androsot_approach_teb/launch/approach_teb.launch.py
index 7143b5b..19caf33 100644
--- a/src/androsot_approach_teb/launch/approach_teb.launch.py
+++ b/src/androsot_approach_teb/launch/approach_teb.launch.py
@@ -3,63 +3,77 @@ import os
from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
class LaunchUnitsCreator:
-
def __init__(self, connections_config_path: str) -> None:
self._config = self._read_connections_config(connections_config_path)
def create_launch(self) -> LaunchDescription:
- return LaunchDescription([
- *self._create_robo_pult_executors(),
- *self._create_proxy_nodes(),
- *self._create_approach_nodes(),
- *self._create_odometry_estimator_node(),
- ])
+ return LaunchDescription(
+ [
+ *self._create_robo_pult_executors(),
+ *self._create_proxy_nodes(),
+ *self._create_approach_nodes(),
+ *self._create_odometry_estimator_node(),
+ ]
+ )
@staticmethod
def _read_connections_config(connections_config_path: str) -> dict:
with open(connections_config_path) as connections_config_json:
- conf = json.load(connections_config_json, parse_float=lambda x: str(x), parse_int=lambda x:str(x))
-
+ conf = json.load(
+ connections_config_json,
+ parse_float=lambda x: str(x),
+ parse_int=lambda x: str(x),
+ )
+
return conf
def _create_robo_pult_executors(self) -> list[ExecuteProcess]:
- return [ExecuteProcess(cmd=[
- 'sudo',
- './roboLinuxPult/roboLinuxPult.sh',
- connection["port"],
- connection["device"],
- connection["robot_name"][5:], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
- # found in Taiwan April 2024
- ]) for connection in self._config["connections"].values()]
+ return [
+ ExecuteProcess(
+ cmd=[
+ "sudo",
+ "./roboLinuxPult/roboLinuxPult.sh",
+ connection["port"],
+ connection["device"],
+ connection["robot_name"][
+ 5:
+ ], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
+ # found in Taiwan April 2024
+ ]
+ )
+ for connection in self._config["connections"].values()
+ ]
def _create_proxy_nodes(self) -> list[Node]:
print(self._config["connections"])
return [
Node(
- package ="proxy",
- executable ="proxy",
- namespace =f"robot_{id}",
- parameters = [
- {"port": connection["port"] },
- {"robot_name": connection["robot_name"] },
+ package="proxy",
+ executable="proxy",
+ namespace=f"robot_{id}",
+ parameters=[
+ {"port": connection["port"]},
+ {"robot_name": connection["robot_name"]},
],
- ) for id, connection in self._config["connections"].items()
+ )
+ for id, connection in self._config["connections"].items()
]
def _create_approach_nodes(self) -> list[Node]:
return [
Node(
- package ="androsot_approach_teb",
- executable ="androsot_approach_teb",
- namespace =f"robot_{id}",
- ) for id in self._config["connections"].keys()
+ package="androsot_approach_teb",
+ executable="androsot_approach_teb",
+ namespace=f"robot_{id}",
+ )
+ for id in self._config["connections"].keys()
]
def _create_odometry_estimator_node(self) -> list[Node]:
@@ -68,13 +82,16 @@ class LaunchUnitsCreator:
package="odometry_estimator",
executable="odometry_estimator",
namespace=f"robot_{id}",
- ) for id in self._config["connections"].keys()
+ )
+ for id in self._config["connections"].keys()
]
-def generate_launch_description():
- connections_config = os.path.join(get_package_share_directory('androsot_approach_teb'),
- 'config',
- "connections_config.json")
+def generate_launch_description():
+ connections_config = os.path.join(
+ get_package_share_directory("androsot_approach_teb"),
+ "config",
+ "connections_config.json",
+ )
- return LaunchUnitsCreator(connections_config).create_launch()
\ No newline at end of file
+ return LaunchUnitsCreator(connections_config).create_launch()
diff --git a/src/calibration_node/calibration_node/__init__.py b/src/calibration_node/calibration_node/__init__.py
index a67dff3..066c77c 100644
--- a/src/calibration_node/calibration_node/__init__.py
+++ b/src/calibration_node/calibration_node/__init__.py
@@ -1 +1 @@
-print("init`")
\ No newline at end of file
+print("init`")
diff --git a/src/calibration_node/calibration_node/calibration.py b/src/calibration_node/calibration_node/calibration.py
index dfe9a2b..9a0934b 100644
--- a/src/calibration_node/calibration_node/calibration.py
+++ b/src/calibration_node/calibration_node/calibration.py
@@ -53,9 +53,7 @@ class CalibrationNode(Node):
)
self.declare_parameter("aruco_size", 0.170) #
self.aruco_size = self.get_parameter("aruco_size").value # m
- self.declare_parameter(
- "camera_frames", ["camera1", "camera2"]
- )
+ self.declare_parameter("camera_frames", ["camera1", "camera2"])
self.camera_frames = self.get_parameter("camera_frames").value
self.declare_parameter("path_to_save", "./static_transforms.yaml")
self.path_to_save = self.get_parameter("path_to_save").value
@@ -72,9 +70,7 @@ class CalibrationNode(Node):
tf_stamped.transform.translation.z = pose.position.z
tf_stamped.transform.rotation = pose.orientation
tf_stamped.header = msg.header
- tf_stamped.child_frame_id = str(
- id
- )
+ tf_stamped.child_frame_id = str(id)
self.current_aruco_tf[msg.header.frame_id][str(id)] = tf_stamped
def calib_cb(self):
diff --git a/src/calibration_node/calibration_node/gui_window.py b/src/calibration_node/calibration_node/gui_window.py
index a6785d9..fc0b530 100644
--- a/src/calibration_node/calibration_node/gui_window.py
+++ b/src/calibration_node/calibration_node/gui_window.py
@@ -52,9 +52,7 @@ class GuiWindow(QMainWindow, WindowInterface):
next_button.clicked.connect(self.save_position)
central_widget.setLayout(layout)
- self.tf_queue = Queue(
- 32
- )
+ self.tf_queue = Queue(32)
self.calibration_ready = False
def _handler_ros_node(self) -> None:
@@ -79,7 +77,7 @@ class GuiWindow(QMainWindow, WindowInterface):
self.tf.transform.rotation.z = quat[2]
self.tf.transform.rotation.w = quat[3]
self.tf.header.frame_id = self.pose_inputs[0].text()
- self.tf.child_frame_id = "world" #
+ self.tf.child_frame_id = "world" #
except Exception as e:
print(e)
try:
@@ -97,7 +95,7 @@ class GuiWindow(QMainWindow, WindowInterface):
try:
return self.tf_queue.get_nowait()
except Empty:
- pass
+ pass
except Exception as e:
print(e)
return None
diff --git a/src/calibration_node/launch/multi_camera_calibration.launch.py b/src/calibration_node/launch/multi_camera_calibration.launch.py
index 668cbfc..f311d2d 100644
--- a/src/calibration_node/launch/multi_camera_calibration.launch.py
+++ b/src/calibration_node/launch/multi_camera_calibration.launch.py
@@ -3,75 +3,73 @@ from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- camera_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params2.yaml")
-
- return LaunchDescription([
-
- Node(
- package='usb_cam_ros2',
- executable='usb_cam_ros2_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera1',
- emulate_tty=True
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera1'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.170} # 0.07
- ],
- remappings=[
- ('/camera/image_raw', '/camera1/image_raw'),
- ('/camera/camera_info', '/camera1/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam_ros2',
- executable='usb_cam_ros2_node_exe',
- parameters=[camera_config2],
- output='screen',
- namespace='camera2',
- emulate_tty=True
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera2'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.170} # 0.07
- ],
- remappings=[
- ('/camera/image_raw', '/camera2/image_raw'),
- ('/camera/camera_info', '/camera2/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
-
- # world frame publisher
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
- Node(
- package='calibration_node',
- executable='calibration_node',
-# parameters=[
-# {'robots_params': robot_config},
-# {'camera_frames': ["camera"]}
-# ],
- output='screen',
- emulate_tty=True
- )
- ])
+ return LaunchDescription(
+ [
+ Node(
+ package="usb_cam_ros2",
+ executable="usb_cam_ros2_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera1",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.170}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera1/image_raw"),
+ ("/camera/camera_info", "/camera1/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam_ros2",
+ executable="usb_cam_ros2_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ namespace="camera2",
+ emulate_tty=True,
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.170}, # 0.07
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ # world frame publisher
+ Node(
+ package="calibration_node",
+ executable="calibration_node",
+ # parameters=[
+ # {'robots_params': robot_config},
+ # {'camera_frames': ["camera"]}
+ # ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/calibration_node/launch/single_camera_calibration.launch.py b/src/calibration_node/launch/single_camera_calibration.launch.py
index cb8de59..740b8e7 100644
--- a/src/calibration_node/launch/single_camera_calibration.launch.py
+++ b/src/calibration_node/launch/single_camera_calibration.launch.py
@@ -3,46 +3,46 @@ from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- return LaunchDescription([
-
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera1'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.170} # 0.07
- ],
- remappings=[
- # ('/camera/image_raw', '/image_raw'),
- # ('/camera/camera_info', '/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam_ros2',
- executable='usb_cam_ros2_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera',
- emulate_tty=True
- ),
- # world frame publisher
-
- Node(
- package='calibration_node',
- executable='calibration_node',
-# parameters=[
-# {'robots_params': robot_config},
-# {'camera_frames': ["camera"]}
-# ],
- output='screen',
- emulate_tty=True
- )
- ])
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ return LaunchDescription(
+ [
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera1"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.170}, # 0.07
+ ],
+ remappings=[
+ # ('/camera/image_raw', '/image_raw'),
+ # ('/camera/camera_info', '/camera_info')
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam_ros2",
+ executable="usb_cam_ros2_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera",
+ emulate_tty=True,
+ ),
+ # world frame publisher
+ Node(
+ package="calibration_node",
+ executable="calibration_node",
+ # parameters=[
+ # {'robots_params': robot_config},
+ # {'camera_frames': ["camera"]}
+ # ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/calibration_node/launch/tf.launch.py b/src/calibration_node/launch/tf.launch.py
index eaa5d95..b451e44 100644
--- a/src/calibration_node/launch/tf.launch.py
+++ b/src/calibration_node/launch/tf.launch.py
@@ -4,38 +4,40 @@ import os
from ament_index_python.packages import get_package_share_directory
import yaml
+
def load_yaml_file(file_path):
- with open(file_path, 'r') as file:
+ with open(file_path, "r") as file:
return yaml.safe_load(file)
+
def generate_launch_description():
ld = LaunchDescription()
# Get the path to the YAML file
config_path = os.path.join(
- get_package_share_directory('calibration_node'),
- 'config',
- 'static_transforms.yaml'
+ get_package_share_directory("calibration_node"),
+ "config",
+ "static_transforms.yaml",
)
-
+
static_transforms = load_yaml_file(config_path)
- for transform in static_transforms['static_transforms']:
+ for transform in static_transforms["static_transforms"]:
node = Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- name=transform['child_frame_id'],
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ name=transform["child_frame_id"],
arguments=[
- str(transform['translation'][0]),
- str(transform['translation'][1]),
- str(transform['translation'][2]),
- str(transform['rotation'][0]),
- str(transform['rotation'][1]),
- str(transform['rotation'][2]),
- str(transform['rotation'][3]),
- transform['parent_frame_id'],
- transform['child_frame_id']
- ]
+ str(transform["translation"][0]),
+ str(transform["translation"][1]),
+ str(transform["translation"][2]),
+ str(transform["rotation"][0]),
+ str(transform["rotation"][1]),
+ str(transform["rotation"][2]),
+ str(transform["rotation"][3]),
+ transform["parent_frame_id"],
+ transform["child_frame_id"],
+ ],
)
ld.add_action(node)
- return ld
\ No newline at end of file
+ return ld
diff --git a/src/calibration_node/setup.py b/src/calibration_node/setup.py
index 7ad9a95..1eb0832 100644
--- a/src/calibration_node/setup.py
+++ b/src/calibration_node/setup.py
@@ -1,29 +1,33 @@
from setuptools import find_packages, setup
import os
from glob import glob
-package_name = 'calibration_node'
+
+package_name = "calibration_node"
setup(
name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
- (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.[yaml]*'))),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*.[yaml]*")),
+ ),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='nakuznetsov',
- maintainer_email='status',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="nakuznetsov",
+ maintainer_email="status",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'calibration_node = calibration_node.calibration:main'
- ],
+ "console_scripts": ["calibration_node = calibration_node.calibration:main"],
},
)
diff --git a/src/calibration_node/test/test_copyright.py b/src/calibration_node/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/calibration_node/test/test_copyright.py
+++ b/src/calibration_node/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/calibration_node/test/test_flake8.py b/src/calibration_node/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/calibration_node/test/test_flake8.py
+++ b/src/calibration_node/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/calibration_node/test/test_pep257.py b/src/calibration_node/test/test_pep257.py
index f78afc8..45d3fbe 100644
--- a/src/calibration_node/test/test_pep257.py
+++ b/src/calibration_node/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test']) # want other codestyle
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"]) # want other codestyle
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/localisation/launch/dummy_launch.launch.py b/src/localisation/launch/dummy_launch.launch.py
index 4581afb..61db7c7 100644
--- a/src/localisation/launch/dummy_launch.launch.py
+++ b/src/localisation/launch/dummy_launch.launch.py
@@ -1,6 +1,7 @@
from launch import LaunchDescription
from launch_ros.actions import Node
+
def generate_launch_description():
ld = LaunchDescription()
@@ -16,5 +17,5 @@ def generate_launch_description():
ld.add_action(localisation_node)
ld.add_action(dump_aruco_pub_node)
-
- return ld
\ No newline at end of file
+
+ return ld
diff --git a/src/localisation/launch/dummy_loc_multilaunch.launch.py b/src/localisation/launch/dummy_loc_multilaunch.launch.py
index e7c5bbd..ebac92e 100644
--- a/src/localisation/launch/dummy_loc_multilaunch.launch.py
+++ b/src/localisation/launch/dummy_loc_multilaunch.launch.py
@@ -1,49 +1,68 @@
from launch import LaunchDescription
from launch_ros.actions import Node
+
def generate_launch_description():
ld = LaunchDescription()
localisation_node_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose')],
+ remappings=[
+ ("/filtered_pose", "/robot_0/filtered_pose"),
+ ("/pose", "/robot_0/pose"),
+ ],
)
-
+
localisation_node_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose')],
+ remappings=[
+ ("/filtered_pose", "/robot_1/filtered_pose"),
+ ("/pose", "/robot_1/pose"),
+ ],
)
localisation_node_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose')],
+ remappings=[
+ ("/filtered_pose", "/robot_2/filtered_pose"),
+ ("/pose", "/robot_2/pose"),
+ ],
)
localisation_node_op_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose')],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_0/filtered_pose"),
+ ("/pose", "/opponent_robot_0/pose"),
+ ],
)
-
+
localisation_node_op_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose')],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_1/filtered_pose"),
+ ("/pose", "/opponent_robot_1/pose"),
+ ],
)
localisation_node_op_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose')],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_2/filtered_pose"),
+ ("/pose", "/opponent_robot_2/pose"),
+ ],
)
dump_aruco_pub_node_1 = Node(
package="localisation",
executable="dump_aruco_pub_node",
- remappings= [("/pose", "robot_1/pose")]
+ remappings=[("/pose", "robot_1/pose")],
)
ld.add_action(localisation_node_0)
@@ -53,6 +72,5 @@ def generate_launch_description():
ld.add_action(localisation_node_op_1)
ld.add_action(localisation_node_op_2)
ld.add_action(dump_aruco_pub_node_1)
-
- return ld
+ return ld
diff --git a/src/localisation/launch/ekf_filter.launch.py b/src/localisation/launch/ekf_filter.launch.py
index 9c8dcef..345d1a1 100644
--- a/src/localisation/launch/ekf_filter.launch.py
+++ b/src/localisation/launch/ekf_filter.launch.py
@@ -1,6 +1,7 @@
from launch import LaunchDescription
from launch_ros.actions import Node
+
def generate_launch_description():
ld = LaunchDescription()
@@ -10,5 +11,5 @@ def generate_launch_description():
)
ld.add_action(ekf_node)
-
- return ld
\ No newline at end of file
+
+ return ld
diff --git a/src/localisation/launch/loc_multilaunch.launch.py b/src/localisation/launch/loc_multilaunch.launch.py
index 5e2ab2b..6e85b2c 100644
--- a/src/localisation/launch/loc_multilaunch.launch.py
+++ b/src/localisation/launch/loc_multilaunch.launch.py
@@ -1,49 +1,68 @@
from launch import LaunchDescription
from launch_ros.actions import Node
+
def generate_launch_description():
ld = LaunchDescription()
localisation_node_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_0/filtered_pose'), ('/pose', '/robot_0/pose'),
- ("/fallen","/robot_0/fallen")],
+ remappings=[
+ ("/filtered_pose", "/robot_0/filtered_pose"),
+ ("/pose", "/robot_0/pose"),
+ ("/fallen", "/robot_0/fallen"),
+ ],
)
-
+
localisation_node_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose'),
- ("/fallen","/robot_1/fallen")],
+ remappings=[
+ ("/filtered_pose", "/robot_1/filtered_pose"),
+ ("/pose", "/robot_1/pose"),
+ ("/fallen", "/robot_1/fallen"),
+ ],
)
localisation_node_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose'),
- ("/fallen","/robot_2/fallen")],
+ remappings=[
+ ("/filtered_pose", "/robot_2/filtered_pose"),
+ ("/pose", "/robot_2/pose"),
+ ("/fallen", "/robot_2/fallen"),
+ ],
)
localisation_node_op_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose'),
- ("/fallen","/opponent_robot_0/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_0/filtered_pose"),
+ ("/pose", "/opponent_robot_0/pose"),
+ ("/fallen", "/opponent_robot_0/fallen"),
+ ],
)
-
+
localisation_node_op_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose'),
- ("/fallen","/opponent_robot_1/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_1/filtered_pose"),
+ ("/pose", "/opponent_robot_1/pose"),
+ ("/fallen", "/opponent_robot_1/fallen"),
+ ],
)
localisation_node_op_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose'),
- ("/fallen","/opponent_robot_2/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_2/filtered_pose"),
+ ("/pose", "/opponent_robot_2/pose"),
+ ("/fallen", "/opponent_robot_2/fallen"),
+ ],
)
ld.add_action(localisation_node_0)
@@ -52,6 +71,5 @@ def generate_launch_description():
ld.add_action(localisation_node_op_0)
ld.add_action(localisation_node_op_1)
ld.add_action(localisation_node_op_2)
-
- return ld
+ return ld
diff --git a/src/localisation/launch/renamed_multi.launch.py b/src/localisation/launch/renamed_multi.launch.py
index 1e59a2d..d91a1c0 100644
--- a/src/localisation/launch/renamed_multi.launch.py
+++ b/src/localisation/launch/renamed_multi.launch.py
@@ -1,50 +1,68 @@
from launch import LaunchDescription
from launch_ros.actions import Node
+
def generate_launch_description():
ld = LaunchDescription()
localisation_node_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_0/filtered_pose'),
- ("/fallen","/robot_0/fallen"),
- ("/filtered_pose_3d_robot_0", 'filt_3d')],
+ remappings=[
+ ("/filtered_pose", "/robot_0/filtered_pose"),
+ ("/fallen", "/robot_0/fallen"),
+ ("/filtered_pose_3d_robot_0", "filt_3d"),
+ ],
)
-
+
localisation_node_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_1/filtered_pose'), ('/pose', '/robot_1/pose'),
- ("/fallen","/robot_1/fallen")],
+ remappings=[
+ ("/filtered_pose", "/robot_1/filtered_pose"),
+ ("/pose", "/robot_1/pose"),
+ ("/fallen", "/robot_1/fallen"),
+ ],
)
localisation_node_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/robot_2/filtered_pose'), ('/pose', '/robot_2/pose'),
- ("/fallen","/robot_2/fallen")],
+ remappings=[
+ ("/filtered_pose", "/robot_2/filtered_pose"),
+ ("/pose", "/robot_2/pose"),
+ ("/fallen", "/robot_2/fallen"),
+ ],
)
localisation_node_op_0 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_0/filtered_pose'), ('/pose', '/opponent_robot_0/pose'),
- ("/fallen","/opponent_robot_0/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_0/filtered_pose"),
+ ("/pose", "/opponent_robot_0/pose"),
+ ("/fallen", "/opponent_robot_0/fallen"),
+ ],
)
-
+
localisation_node_op_1 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_1/filtered_pose'), ('/pose', '/opponent_robot_1/pose'),
- ("/fallen","/opponent_robot_1/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_1/filtered_pose"),
+ ("/pose", "/opponent_robot_1/pose"),
+ ("/fallen", "/opponent_robot_1/fallen"),
+ ],
)
localisation_node_op_2 = Node(
package="localisation",
executable="localisation_node",
- remappings= [("/filtered_pose", '/opponent_robot_2/filtered_pose'), ('/pose', '/opponent_robot_2/pose'),
- ("/fallen","/opponent_robot_2/fallen")],
+ remappings=[
+ ("/filtered_pose", "/opponent_robot_2/filtered_pose"),
+ ("/pose", "/opponent_robot_2/pose"),
+ ("/fallen", "/opponent_robot_2/fallen"),
+ ],
)
ld.add_action(localisation_node_0)
@@ -53,6 +71,5 @@ def generate_launch_description():
ld.add_action(localisation_node_op_0)
ld.add_action(localisation_node_op_1)
ld.add_action(localisation_node_op_2)
-
- return ld
+ return ld
diff --git a/src/localisation/localisation/dump_aruco_publisher.py b/src/localisation/localisation/dump_aruco_publisher.py
index db20322..3ec3b78 100644
--- a/src/localisation/localisation/dump_aruco_publisher.py
+++ b/src/localisation/localisation/dump_aruco_publisher.py
@@ -1,4 +1,3 @@
-
from geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
@@ -9,7 +8,7 @@ class DumpArucoPubNode(Node):
def __init__(self):
super().__init__("dump_aruco_pub_node")
self.robot_id = 0
- self.pose_pub= self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
+ self.pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/pose", 10)
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.counter = 0
@@ -36,9 +35,9 @@ class DumpArucoPubNode(Node):
z = 0.25
q_x = -0.0
q_y = 0.0
- q_z = 0.383
- q_w = 0.924
-
+ q_z = 0.383
+ q_w = 0.924
+
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = z
@@ -47,15 +46,48 @@ class DumpArucoPubNode(Node):
msg.pose.pose.orientation.z = q_z
msg.pose.pose.orientation.w = q_w
- msg.pose.covariance = [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, 0.0, 0.0]
+ msg.pose.covariance = [
+ 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,
+ 0.0,
+ 0.0,
+ ]
self.pose_pub.publish(msg)
-
+
+
def main(args=None):
rclpy.init(args=args)
diff --git a/src/localisation/localisation/localisation_node.py b/src/localisation/localisation/localisation_node.py
index 4a10583..8a34cd4 100644
--- a/src/localisation/localisation/localisation_node.py
+++ b/src/localisation/localisation/localisation_node.py
@@ -1,4 +1,10 @@
-from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped, Pose, PoseWithCovariance
+from geometry_msgs.msg import (
+ PoseStamped,
+ Pose2D,
+ PoseWithCovarianceStamped,
+ Pose,
+ PoseWithCovariance,
+)
from std_msgs.msg import Bool
from rpy_msg.msg import RPY
@@ -37,27 +43,28 @@ from collections import deque
# pose.orientation.w = quaternion[3]
# return pose
-class PoseSinCos():
+
+class PoseSinCos:
def __init__(self, x, y, z, sin, cos):
self.x = x
- self.y = y
+ self.y = y
self.z = z
self.sin = sin
self.cos = cos
-class AlphaFilter():
+
+class AlphaFilter:
def __init__(self):
pass
self.buffer = deque()
- self.buffer_size = 10
- self.alpha = 0.70
+ self.buffer_size = 10
+ self.alpha = 0.70
def update(self, value):
- if (self.buffer_size == len(self.buffer)):
+ if self.buffer_size == len(self.buffer):
self.buffer.popleft()
-
- self.buffer.append(value)
+ self.buffer.append(value)
def get(self):
weight_sum = 0.0
@@ -69,10 +76,12 @@ class AlphaFilter():
mid.z += elem.z * self.alpha ** (len(self.buffer) - i)
mid.sin += elem.sin * self.alpha ** (len(self.buffer) - i)
mid.cos += elem.cos * self.alpha ** (len(self.buffer) - i)
- weight_sum += self.alpha ** (len(self.buffer) - i ) #TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
+ weight_sum += (
+ self.alpha ** (len(self.buffer) - i)
+ ) # TODO: no need to calculate weight_sum every time, also it doesnt seem to be correct
if len(self.buffer) == 0:
- weight_sum = 1.0
+ weight_sum = 1.0
mid.x /= weight_sum
mid.y /= weight_sum
mid.z /= weight_sum
@@ -80,21 +89,30 @@ class AlphaFilter():
mid.cos /= weight_sum
return mid
+
class LocalisationNode(Node):
def __init__(self, debug=False):
super().__init__("LocalisationNode")
self.debug = debug
self.robot_id = 0
- self.create_subscription(PoseWithCovarianceStamped , '/pose', self.aruco_observation_callback, 10)
- self.filtered_pose_pub= self.create_publisher(Pose2D, "/filtered_pose", 10)
+ self.create_subscription(
+ PoseWithCovarianceStamped, "/pose", self.aruco_observation_callback, 10
+ )
+ self.filtered_pose_pub = self.create_publisher(Pose2D, "/filtered_pose", 10)
# self.filtered_pose_cov_pub= self.create_publisher(PoseWithCovariance, "/filtered_pose_with_cov", 10)
# self.filtered_stamped_pose_pub= self.create_publisher(PoseWithCovarianceStamped, "/stamped_filtered_pose_with_cov", 10)
self.on_ground_publisher = self.create_publisher(Bool, "fallen", 10)
if self.debug:
- self.unfiltered_pose_pub= self.create_publisher(Pose2D, "/unfiltered_pose", 10)
+ self.unfiltered_pose_pub = self.create_publisher(
+ Pose2D, "/unfiltered_pose", 10
+ )
self.rpy_pub = self.create_publisher(RPY, "/rpy" + str(self.robot_id), 10)
- self.rpy_vector3_pub = self.create_publisher(Vector3, 'vector3_rpy' + str(self.robot_id), 10)
- self.filtered_pose_3d_pub= self.create_publisher(PoseStamped, "/filtered_pose_3d_robot_" + str(self.robot_id), 10)
+ self.rpy_vector3_pub = self.create_publisher(
+ Vector3, "vector3_rpy" + str(self.robot_id), 10
+ )
+ self.filtered_pose_3d_pub = self.create_publisher(
+ PoseStamped, "/filtered_pose_3d_robot_" + str(self.robot_id), 10
+ )
self.loged_data = []
self.current_aruco_poses = {}
self.current_robot_poses = {}
@@ -106,21 +124,52 @@ class LocalisationNode(Node):
self.timer = self.create_timer(timer_period, self.timer_callback)
xyz_cov = 1e-4
rpy_cov = 1e-6
- self.covariance = [xyz_cov, 0.0, 0.0, 0.0, 0.0, 0.0,# 0 0
- 0.0,xyz_cov, 0.0, 0.0, 0.0, 0.0,# 7
- 0.0, 0.0, xyz_cov, 0.0, 0.0, 0.0,# 14
- 0.0, 0.0, 0.0, rpy_cov, 0.0, 0.0,# 21
- 0.0, 0.0, 0.0, 0.0, rpy_cov, 0.0,# 28
- 0.0, 0.0, 0.0, 0.0, 0.0, rpy_cov]# 35
+ self.covariance = [
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0, # 0 0
+ 0.0,
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0, # 7
+ 0.0,
+ 0.0,
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0, # 14
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ 0.0,
+ 0.0, # 21
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ 0.0, # 28
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ ] # 35
self.lowest_standing_z = 0.2
-
- def aruco_observation_callback(self, msg):
+ def aruco_observation_callback(self, msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
- sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
- # print(sin, cos)
+ sin, cos = self.get_planar_sin_cos_from_quaternion(msg)
+ # print(sin, cos)
pose_sin_cos = PoseSinCos(x, y, z, sin, cos)
self.unfiltered_last_observed_robot_pos = PoseSinCos(x, y, z, sin, cos)
self.robot_pose.update(pose_sin_cos)
@@ -129,7 +178,9 @@ class LocalisationNode(Node):
# Extract quaternion from PoseStamped message
quaternion = msg.pose.pose.orientation
# Convert quaternion to RPY
- rpy = tf_transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w])
+ rpy = tf_transformations.euler_from_quaternion(
+ [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
+ )
roll, pitch, yaw = rpy
rpy_msg = RPY()
@@ -140,7 +191,6 @@ class LocalisationNode(Node):
rpy_vector3_msg.x, rpy_vector3_msg.y, rpy_vector3_msg.z = roll, pitch, yaw
self.rpy_vector3_pub.publish(rpy_vector3_msg)
-
def get_x_y_yaw_pose_filtered(self):
pose_sin_cos = self.robot_pose.get()
pose = Pose()
@@ -149,7 +199,6 @@ class LocalisationNode(Node):
pose.z = pose_sin_cos.z
pose.theta = math.atan2(pose_sin_cos.sin, pose_sin_cos.cos)
return pose
-
def get_x_y_yaw_pose_unfiltered(self):
pose_sin_cos = self.unfiltered_last_observed_robot_pos
@@ -163,9 +212,9 @@ class LocalisationNode(Node):
if self.debug:
unfilt_msg = self.get_x_y_yaw_pose_unfiltered()
self.unfiltered_pose_pub.publish(unfilt_msg)
-
+
pose_sin_cos = self.robot_pose.get()
-
+
x = pose_sin_cos.x
y = pose_sin_cos.y
z = pose_sin_cos.z
@@ -192,7 +241,7 @@ class LocalisationNode(Node):
if self.debug:
msg_3d = self.convert_to_3d(filtered_msg)
self.filtered_pose_3d_pub.publish(msg_3d)
-
+
def is_on_ground(self, z):
if z < self.lowest_standing_z:
return True
@@ -215,11 +264,11 @@ class LocalisationNode(Node):
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
return pose
-
+
def convert_to_3d_with_covariance(self, x, y, z, theta):
# Create a Pose message
p = PoseWithCovariance()
-
+
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
@@ -232,14 +281,14 @@ class LocalisationNode(Node):
p.covariance = self.covariance
return p
-
+
def convert_to_3d_with_covariance_stamped(self, x, y, z, theta):
# Create a Pose message
p = PoseWithCovarianceStamped()
p.header.stamp = self.get_clock().now().to_msg()
p.header.frame_id = "world"
-
+
p.pose.pose.position.x = x
p.pose.pose.position.y = y
p.pose.pose.position.z = z
@@ -252,7 +301,6 @@ class LocalisationNode(Node):
p.pose.covariance = self.covariance
return p
-
def get_planar_sin_cos_from_quaternion(self, pose: Pose) -> float:
x_ax = [1.0, 0.0, 0.0]
@@ -260,9 +308,9 @@ class LocalisationNode(Node):
pose.pose.pose.orientation.w,
pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
- pose.pose.pose.orientation.z
+ pose.pose.pose.orientation.z,
]
-
+
rotated_x = Quaternion(quat_arr).rotate(x_ax)
# print(rotated_x)
# TODO check normalisation
@@ -274,14 +322,15 @@ class LocalisationNode(Node):
pose.pose.pose.orientation.w,
pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
- pose.pose.pose.orientation.z
+ pose.pose.pose.orientation.z,
]
-
+
rotated_x = Quaternion(quat_arr).rotate(x_ax)
# print(rotated_x)
# TODO check normalisation
return rotated_x[1], rotated_x[0]
-
+
+
def main(args=None):
rclpy.init(args=args)
diff --git a/src/localisation/setup.py b/src/localisation/setup.py
index e4404b2..985050d 100644
--- a/src/localisation/setup.py
+++ b/src/localisation/setup.py
@@ -2,30 +2,35 @@ import os
from glob import glob
from setuptools import setup
-package_name = 'localisation'
+package_name = "localisation"
setup(
name=package_name,
- version='0.0.0',
+ version="0.0.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
- (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*")),
+ ),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'localisation_node = localisation.localisation_node:main',
- 'dump_aruco_pub_node = localisation.dump_aruco_publisher:main'
+ "console_scripts": [
+ "localisation_node = localisation.localisation_node:main",
+ "dump_aruco_pub_node = localisation.dump_aruco_publisher:main",
],
},
)
diff --git a/src/localisation/test/test_copyright.py b/src/localisation/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/localisation/test/test_copyright.py
+++ b/src/localisation/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/localisation/test/test_flake8.py b/src/localisation/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/localisation/test/test_flake8.py
+++ b/src/localisation/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/localisation/test/test_pep257.py b/src/localisation/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/localisation/test/test_pep257.py
+++ b/src/localisation/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/localization_preprocessor/launch/static_tf_publisher.launch.py b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
index 2845ca5..09f5bb0 100644
--- a/src/localization_preprocessor/launch/static_tf_publisher.launch.py
+++ b/src/localization_preprocessor/launch/static_tf_publisher.launch.py
@@ -2,16 +2,18 @@ from launch import LaunchDescription
from launch_ros.actions import Node
import yaml
+
def load_yaml_file(file_path):
- with open(file_path, 'r') as file:
+ with open(file_path, "r") as file:
return yaml.safe_load(file)
+
def generate_launch_description():
ld = LaunchDescription()
node_odom = Node(
- package='tf2_ros',
- executable='static_transform_publisher',
+ package="tf2_ros",
+ executable="static_transform_publisher",
name="tf_odom",
arguments=[
str(0.0),
@@ -22,13 +24,13 @@ def generate_launch_description():
str(0),
str(1),
"world",
- "odom"
- ]
+ "odom",
+ ],
)
ld.add_action(node_odom)
node_odom = Node(
- package='tf2_ros',
- executable='static_transform_publisher',
+ package="tf2_ros",
+ executable="static_transform_publisher",
name="tf_base_link",
arguments=[
str(0.0),
@@ -39,9 +41,9 @@ def generate_launch_description():
str(0),
str(1),
"odom",
- "base_link"
- ]
+ "base_link",
+ ],
)
ld.add_action(node_odom)
- return ld
\ No newline at end of file
+ return ld
diff --git a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
index abe1274..90b00ce 100755
--- a/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
+++ b/src/localization_preprocessor/localization_preprocessor/aruco_preprocessor_node.py
@@ -1,9 +1,17 @@
import rclpy
from rclpy.node import Node
-from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped, TransformStamped, PointStamped
+from geometry_msgs.msg import (
+ PoseWithCovarianceStamped,
+ PoseStamped,
+ TransformStamped,
+ PointStamped,
+)
from tf2_ros import TransformException, TransformListener, Buffer
import tf2_ros
-from tf2_geometry_msgs import do_transform_point, do_transform_pose_with_covariance_stamped
+from tf2_geometry_msgs import (
+ do_transform_point,
+ do_transform_pose_with_covariance_stamped,
+)
from ros2_aruco_interfaces.msg import ArucoMarkers
from tf_transformations import euler_from_quaternion
@@ -11,35 +19,64 @@ import tf_transformations as tf_t
import json
import math
-class ArucoPreprocessorNode(Node):
+class ArucoPreprocessorNode(Node):
def __init__(self):
- super().__init__('aruco_preprocessor_node')
+ super().__init__("aruco_preprocessor_node")
self.declare_parameter("aruco_markers_topic", "/aruco_markers")
topic_name = self.get_parameter("aruco_markers_topic").value
self.subscription = self.create_subscription(
- ArucoMarkers,
- topic_name,
- self.aruco_cb,
- 10)
+ ArucoMarkers, topic_name, self.aruco_cb, 10
+ )
self.init_robot_params()
-
+
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.tf_from_camera_to_world = dict()
self.camera_frames = ["camera1", "camera2"]
xyz_cov = 0.03
rpy_cov = 0.17
- self.covariance = [xyz_cov, 0.0, 0.0, 0.0, 0.0, 0.0,# 0 0
- 0.0,xyz_cov, 0.0, 0.0, 0.0, 0.0,# 7
- 0.0, 0.0, xyz_cov, 0.0, 0.0, 0.0,# 14
- 0.0, 0.0, 0.0, rpy_cov, 0.0, 0.0,# 21
- 0.0, 0.0, 0.0, 0.0, rpy_cov, 0.0,# 28
- 0.0, 0.0, 0.0, 0.0, 0.0, rpy_cov]# 35
-
-
- self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
+ self.covariance = [
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0, # 0 0
+ 0.0,
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0, # 7
+ 0.0,
+ 0.0,
+ xyz_cov,
+ 0.0,
+ 0.0,
+ 0.0, # 14
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ 0.0,
+ 0.0, # 21
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ 0.0, # 28
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ rpy_cov,
+ ] # 35
+
+ self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, "/pose", 10)
def init_robot_params(self):
self.declare_parameter("robots_params", rclpy.Parameter.Type.STRING)
@@ -52,7 +89,6 @@ class ArucoPreprocessorNode(Node):
self.create_aruco_rotation_map()
self.init_aruco_ids()
-
def create_aruco_rotation_map(self):
for robot_id, param in self.robots.items():
aruco_ids = param["aruco_ids"]
@@ -79,20 +115,18 @@ class ArucoPreprocessorNode(Node):
def get_camera_to_world_frame(self):
try:
for frame in self.camera_frames:
- self.tf_from_camera_to_world[
- frame
- ] = self.tf_buffer.lookup_transform( # check inversion not allwed
- "world", frame, rclpy.time.Time()
+ self.tf_from_camera_to_world[frame] = (
+ self.tf_buffer.lookup_transform( # check inversion not allwed
+ "world", frame, rclpy.time.Time()
+ )
)
except TransformException as ex:
import time
+
time.sleep(0.1)
self.get_logger().info(f"Could not transform world to camera: {ex}")
# self.get_camera_to_world_frame()
-
-
-
def aruco_cb(self, msg):
self.get_camera_to_world_frame()
for id, pose in zip(msg.marker_ids, msg.poses):
@@ -104,44 +138,49 @@ class ArucoPreprocessorNode(Node):
r = pose_stamped.pose.orientation
quaternion = [r.x, r.y, r.z, r.w]
roll, pitch, yaw = euler_from_quaternion(quaternion)
+
def rad2deg(x):
- return x / 3.14 * 180
+ return x / 3.14 * 180
+
# roll is near 180
# pitch is near 0
- # cov is equal for all angles
+ # cov is equal for all angles
# pi взÑто из как тк Ñто нулевые повороты при проÑмотре на камеру
# cov = max (abs(min(current roll - 3.14, current_roll - 3.14)), abs(current_pitch))
# self.get_logger().info(f"ID:{id}, ROLL: {roll}, PITCH: {pitch}, YAW: {yaw}")
cov = max(min(abs(roll - math.pi), abs(roll + math.pi)), abs(pitch))
- print (f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}")
+ print(
+ f"Roll={rad2deg(roll)}; pitch={rad2deg(pitch)}; yaw={rad2deg(yaw)}"
+ )
field_pose = self.transform_to_robot_centre(pose_stamped, id)
- to_odom_quat = tf_t.quaternion_from_euler(
- -math.pi / 2, 0.0, 0.0
- )
+ to_odom_quat = tf_t.quaternion_from_euler(-math.pi / 2, 0.0, 0.0)
- to_odom_quat_2 = tf_t.quaternion_from_euler(
- 0.0, 0.0, -math.pi / 2
- )
+ to_odom_quat_2 = tf_t.quaternion_from_euler(0.0, 0.0, -math.pi / 2)
- to_odom_quat_3 = tf_t.quaternion_from_euler(
- 0.0, math.pi / 2, 0.0
- )
+ to_odom_quat_3 = tf_t.quaternion_from_euler(0.0, math.pi / 2, 0.0)
a = field_pose.pose.orientation
pose_quat = [a.w, a.x, a.y, a.z]
- itog = tf_t.quaternion_multiply(to_odom_quat, tf_t.quaternion_multiply(to_odom_quat_2, tf_t.quaternion_multiply(
- to_odom_quat_3, pose_quat # !!! ALARM SHIIIIIT
- )))
+ itog = tf_t.quaternion_multiply(
+ to_odom_quat,
+ tf_t.quaternion_multiply(
+ to_odom_quat_2,
+ tf_t.quaternion_multiply(
+ to_odom_quat_3,
+ pose_quat, # !!! ALARM SHIIIIIT
+ ),
+ ),
+ )
print("ITOG: ", itog)
print()
- field_pose.pose.orientation.w = itog[0]
- field_pose.pose.orientation.x = itog[1]
- field_pose.pose.orientation.y = itog[2]
- field_pose.pose.orientation.z = itog[3]
+ field_pose.pose.orientation.w = itog[0]
+ field_pose.pose.orientation.x = itog[1]
+ field_pose.pose.orientation.y = itog[2]
+ field_pose.pose.orientation.z = itog[3]
pose_with_cov = PoseWithCovarianceStamped()
pose_with_cov.header = field_pose.header
@@ -149,18 +188,20 @@ class ArucoPreprocessorNode(Node):
pose_with_cov.header.stamp = self.get_clock().now().to_msg()
pose_with_cov.pose.pose = field_pose.pose
# I think python links fuck me. upd numpy copy sigment fuck me. upd life fuck me
- print (pose_with_cov)
+ print(pose_with_cov)
self.covariance[21] = cov
self.covariance[28] = cov
self.covariance[35] = cov
- tf = self.tf_buffer.lookup_transform_core("world", field_pose.header.frame_id, rclpy.time.Time())
- world_pose = do_transform_pose_with_covariance_stamped(pose_with_cov, tf)
-
-
+ tf = self.tf_buffer.lookup_transform_core(
+ "world", field_pose.header.frame_id, rclpy.time.Time()
+ )
+ world_pose = do_transform_pose_with_covariance_stamped(
+ pose_with_cov, tf
+ )
world_pose.header.frame_id = "world"
- # pose_with_cov.header.frame =
-
+ # pose_with_cov.header.frame =
+
self.publisher_.publish(world_pose)
def transform_to_robot_centre(self, pose: PoseStamped, id) -> PoseStamped:
@@ -169,7 +210,7 @@ class ArucoPreprocessorNode(Node):
return rotated_pose
def shift_pose_to_robot_centre(
- self, pose:PoseStamped, id
+ self, pose: PoseStamped, id
): # Now all arruco has same offset of centre
# https://answers.ros.org/question/289323/simple-tf-transform-without-broadcasterlistener/
shifted_pose = PoseStamped()
@@ -177,8 +218,8 @@ class ArucoPreprocessorNode(Node):
shifted_pose.pose.orientation = pose.pose.orientation
shift_point = PointStamped()
- #shift_point.point.x = 0.0 # 0.035
- shift_point.point.z = -0.035# 0.045
+ # shift_point.point.x = 0.0 # 0.035
+ shift_point.point.z = -0.035 # 0.045
transform = TransformStamped()
transform.header = pose.header
transform.child_frame_id = "aruco" + str(id)
@@ -194,7 +235,7 @@ class ArucoPreprocessorNode(Node):
inverse_transform = buffer_core.lookup_transform_core(
transform.child_frame_id, transform.header.frame_id, rclpy.time.Time()
)
- #self.tf_broadcaster.sendTransform(inverse_transform)
+ # self.tf_broadcaster.sendTransform(inverse_transform)
rot = inverse_transform.transform.rotation
quat = rot.x, rot.y, rot.z, rot.w
tf_t.euler_from_quaternion(quat)
@@ -215,7 +256,8 @@ class ArucoPreprocessorNode(Node):
]
if id in self.aruco_rotation_map.keys():
transformed_quate_iter = tf_t.quaternion_multiply(
- quaternion_iter, self.aruco_rotation_map[id] # I believe it's true
+ quaternion_iter,
+ self.aruco_rotation_map[id], # I believe it's true
)
(
rotated_pose.pose.orientation.x,
@@ -223,11 +265,10 @@ class ArucoPreprocessorNode(Node):
rotated_pose.pose.orientation.z,
rotated_pose.pose.orientation.w,
) = transformed_quate_iter
- rotated_pose.header.frame_id = (
- pose.header.frame_id
- )
+ rotated_pose.header.frame_id = pose.header.frame_id
return rotated_pose
+
def main(args=None):
rclpy.init(args=args)
@@ -239,5 +280,6 @@ def main(args=None):
aruco_subscriber.destroy_node()
rclpy.shutdown()
-if __name__ == '__main__':
+
+if __name__ == "__main__":
main()
diff --git a/src/localization_preprocessor/setup.py b/src/localization_preprocessor/setup.py
index cd1b158..7201880 100644
--- a/src/localization_preprocessor/setup.py
+++ b/src/localization_preprocessor/setup.py
@@ -1,29 +1,35 @@
from setuptools import setup
import os
from glob import glob
-package_name = 'localization_preprocessor'
+
+package_name = "localization_preprocessor"
setup(
name=package_name,
- version='0.0.0',
+ version="0.0.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
- (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.[yaml]*'))),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*.[yaml]*")),
+ ),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'aruco_preprocessor_node = localization_preprocessor.aruco_preprocessor_node:main'
+ "console_scripts": [
+ "aruco_preprocessor_node = localization_preprocessor.aruco_preprocessor_node:main"
],
},
)
diff --git a/src/localization_preprocessor/test/dummy_pose_pub.py b/src/localization_preprocessor/test/dummy_pose_pub.py
index cc41b5c..4365100 100644
--- a/src/localization_preprocessor/test/dummy_pose_pub.py
+++ b/src/localization_preprocessor/test/dummy_pose_pub.py
@@ -3,18 +3,19 @@ from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from std_msgs.msg import Header
+
class PoseWithCovarianceStampedPublisher(Node):
def __init__(self):
- super().__init__('pose_with_covariance_stamped_publisher')
- self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/pose', 10)
- timer_period = 0.02 # seconds
+ super().__init__("pose_with_covariance_stamped_publisher")
+ self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, "/pose", 10)
+ timer_period = 0.02 # seconds
self.timer_ = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
msg = PoseWithCovarianceStamped()
msg.header = Header()
msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = 'camera1'
+ msg.header.frame_id = "camera1"
# Example: Set the pose and covariance
msg.pose.pose.position.x = 1.0
msg.pose.pose.position.y = 2.0
@@ -23,15 +24,48 @@ class PoseWithCovarianceStampedPublisher(Node):
msg.pose.pose.orientation.y = 0.0
msg.pose.pose.orientation.z = 0.7071067811865475
msg.pose.pose.orientation.w = 0.7071067811865476
- msg.pose.covariance = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+ msg.pose.covariance = [
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0,
+ ]
self.publisher_.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg)
+
def main(args=None):
rclpy.init(args=args)
pose_with_covariance_stamped_publisher = PoseWithCovarianceStampedPublisher()
@@ -39,5 +73,6 @@ def main(args=None):
pose_with_covariance_stamped_publisher.destroy_node()
rclpy.shutdown()
-if __name__ == '__main__':
+
+if __name__ == "__main__":
main()
diff --git a/src/localization_preprocessor/test/test_copyright.py b/src/localization_preprocessor/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/localization_preprocessor/test/test_copyright.py
+++ b/src/localization_preprocessor/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/localization_preprocessor/test/test_flake8.py b/src/localization_preprocessor/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/localization_preprocessor/test/test_flake8.py
+++ b/src/localization_preprocessor/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/localization_preprocessor/test/test_pep257.py b/src/localization_preprocessor/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/localization_preprocessor/test/test_pep257.py
+++ b/src/localization_preprocessor/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/odometry_estimator/odometry_estimator/OdometryEstimator.py b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
index 954f8d3..de077f0 100644
--- a/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
+++ b/src/odometry_estimator/odometry_estimator/OdometryEstimator.py
@@ -2,37 +2,27 @@ import rclpy
from rclpy.node import Node
-from geometry_msgs.msg import PoseWithCovariance
-from geometry_msgs.msg import Twist
-from geometry_msgs.msg import TwistWithCovariance
-from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseWithCovariance
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistWithCovariance
+from nav_msgs.msg import Odometry
-class OdometryEstimator(Node):
+class OdometryEstimator(Node):
def __init__(self):
- super().__init__('minimal_publisher')
-
- self._odometry_publisher = self.create_publisher(
- Odometry,
- 'true_odom',
- 10
- )
+ super().__init__("minimal_publisher")
+
+ self._odometry_publisher = self.create_publisher(Odometry, "true_odom", 10)
self._localization_subscriber = self.create_subscription(
- PoseWithCovariance,
- 'filtered_pose',
- self.localization_callback,
- 10
+ PoseWithCovariance, "filtered_pose", self.localization_callback, 10
)
self._twist_subscriber = self.create_subscription(
- Twist,
- 'plan_cmd_vel',
- self.twist_callback,
- 10
+ Twist, "plan_cmd_vel", self.twist_callback, 10
)
- self.last_pose = PoseWithCovariance()
+ self.last_pose = PoseWithCovariance()
self.last_twist = TwistWithCovariance()
def localization_callback(self, msg: PoseWithCovariance):
@@ -60,10 +50,10 @@ class OdometryEstimator(Node):
def _generate_frames_info(self) -> Odometry:
current_odometry = Odometry()
- current_odometry.header.stamp = rclpy.time.Time().to_msg()
- current_odometry.header.frame_id = "world"
-
- current_odometry.child_frame_id = "baselink"
+ current_odometry.header.stamp = rclpy.time.Time().to_msg()
+ current_odometry.header.frame_id = "world"
+
+ current_odometry.child_frame_id = "baselink"
return current_odometry
@@ -82,5 +72,5 @@ def main(args=None):
rclpy.shutdown()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
diff --git a/src/odometry_estimator/setup.py b/src/odometry_estimator/setup.py
index f54416c..4629a41 100644
--- a/src/odometry_estimator/setup.py
+++ b/src/odometry_estimator/setup.py
@@ -1,26 +1,25 @@
from setuptools import find_packages, setup
-package_name = 'odometry_estimator'
+package_name = "odometry_estimator"
setup(
name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='funny-ded',
- maintainer_email='iashin.pa@phystech.edu',
- description='TODO: Package description',
- license='Apache-2.0',
- tests_require=['pytest'],
+ maintainer="funny-ded",
+ maintainer_email="iashin.pa@phystech.edu",
+ description="TODO: Package description",
+ license="Apache-2.0",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'odometry_estimator = odometry_estimator.OdometryEstimator:main',
+ "console_scripts": [
+ "odometry_estimator = odometry_estimator.OdometryEstimator:main",
],
- }
+ },
)
diff --git a/src/odometry_estimator/test/test_copyright.py b/src/odometry_estimator/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/odometry_estimator/test/test_copyright.py
+++ b/src/odometry_estimator/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/odometry_estimator/test/test_flake8.py b/src/odometry_estimator/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/odometry_estimator/test/test_flake8.py
+++ b/src/odometry_estimator/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/odometry_estimator/test/test_pep257.py b/src/odometry_estimator/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/odometry_estimator/test/test_pep257.py
+++ b/src/odometry_estimator/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/pic_to_real/launch/bal_to_real.launch.py b/src/pic_to_real/launch/bal_to_real.launch.py
index ce595bf..a09f7ad 100644
--- a/src/pic_to_real/launch/bal_to_real.launch.py
+++ b/src/pic_to_real/launch/bal_to_real.launch.py
@@ -35,6 +35,7 @@ def create_tf_nodes(static_transforms: dict):
tf_nodes.append(node)
return tf_nodes
+
def create_tf_node():
node = Node(
package="tf2_ros",
@@ -133,14 +134,16 @@ def generate_launch_description():
executable="ball_to_real",
output="screen",
emulate_tty=True,
-
- parameters=[{"camera_frames" : ["camera1", "camera2"]}]
+ parameters=[{"camera_frames": ["camera1", "camera2"]}],
)
)
- #ld.add_action(tf_node)
- ld.add_action(IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [FindPackageShare("calibration_node"), "/launch", "/tf.launch.py"]
- )))
+ # ld.add_action(tf_node)
+ ld.add_action(
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ [FindPackageShare("calibration_node"), "/launch", "/tf.launch.py"]
+ )
+ )
+ )
return ld
diff --git a/src/pic_to_real/pic_to_real/ball_to_real.py b/src/pic_to_real/pic_to_real/ball_to_real.py
index 7fefa45..2dd6a3e 100644
--- a/src/pic_to_real/pic_to_real/ball_to_real.py
+++ b/src/pic_to_real/pic_to_real/ball_to_real.py
@@ -13,6 +13,7 @@ import rclpy
from skspatial.objects import Line, Plane
from tf2_ros import TransformListener, Buffer
+
class Ball2RealNode(rclpy.node.Node):
def __init__(self):
super().__init__("Ball2RealNode")
@@ -32,7 +33,6 @@ class Ball2RealNode(rclpy.node.Node):
self.ball_world_pub = self.create_publisher(Pose2D, "ball_coordinate_world", 10)
self.tf_from_camera_to_world = {}
-
def init_camera_model(self, msg):
self.camera_models[msg.header.frame_id] = PinholeCameraModel()
self.camera_models[msg.header.frame_id].fromCameraInfo(msg)
@@ -43,11 +43,15 @@ class Ball2RealNode(rclpy.node.Node):
msg.header.frame_id in self.camera_frames
and self.camera_models[msg.header.frame_id] is None
):
- self.get_logger().info(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
+ self.get_logger().info(
+ f"No frame from yolo ball coord. Frame: {msg.header.frame_id}"
+ )
return
if msg.header.frame_id not in self.camera_frames:
- self.get_logger().info(f"No frame from yolo ball coord. Frame: {msg.header.frame_id}")
+ self.get_logger().info(
+ f"No frame from yolo ball coord. Frame: {msg.header.frame_id}"
+ )
return
if len(msg.balls) == 0:
@@ -92,7 +96,7 @@ class Ball2RealNode(rclpy.node.Node):
def get_camera_to_world_frame(self):
try:
for frame in self.camera_frames:
- print (f"Camera frame is {frame}")
+ print(f"Camera frame is {frame}")
self.tf_from_camera_to_world[frame] = (
self.tf_buffer.lookup_transform( # check inversion not allwed
"world", frame, rclpy.time.Time()
@@ -110,5 +114,6 @@ def main(args=None):
ball_to_real_node.destroy_node()
rclpy.shutdown()
+
if __name__ == "__main__":
main()
diff --git a/src/pic_to_real/test/test_copyright.py b/src/pic_to_real/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/pic_to_real/test/test_copyright.py
+++ b/src/pic_to_real/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/pic_to_real/test/test_flake8.py b/src/pic_to_real/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/pic_to_real/test/test_flake8.py
+++ b/src/pic_to_real/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/pic_to_real/test/test_pep257.py b/src/pic_to_real/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/pic_to_real/test/test_pep257.py
+++ b/src/pic_to_real/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/proxy/proxy/CommandSender.py b/src/proxy/proxy/CommandSender.py
index 44c2ded..fb09bcc 100644
--- a/src/proxy/proxy/CommandSender.py
+++ b/src/proxy/proxy/CommandSender.py
@@ -3,28 +3,30 @@ from rclpy.node import Node
from geometry_msgs.msg import Twist
-class CommandSender(Node):
+class CommandSender(Node):
def __init__(self):
- super().__init__('androsot_command_sender')
- self.publisher_ = self.create_publisher(Twist, 'plan_cmd_vel', 10)
+ super().__init__("androsot_command_sender")
+ self.publisher_ = self.create_publisher(Twist, "plan_cmd_vel", 10)
timer_period = 0.1 # seconds
-
+
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
msg = Twist()
- # Hardcoded params.
+ # Hardcoded params.
# Actually this can be done by any other node with countings
- msg.linear.x = 0.0 #int(input())
- msg.linear.y = 0.0 #int(input())
- msg.angular.z = float(input())
+ msg.linear.x = 0.0 # int(input())
+ msg.linear.y = 0.0 # int(input())
+ msg.angular.z = float(input())
self.publisher_.publish(msg)
- self.get_logger().info(f'Publishing: {msg.angular.z}|{msg.linear.y}|{msg.linear.x}')
+ self.get_logger().info(
+ f"Publishing: {msg.angular.z}|{msg.linear.y}|{msg.linear.x}"
+ )
def main(args=None):
@@ -41,5 +43,5 @@ def main(args=None):
rclpy.shutdown()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
diff --git a/src/proxy/proxy/Proxy.py b/src/proxy/proxy/Proxy.py
index 4f08c5a..87fe97f 100644
--- a/src/proxy/proxy/Proxy.py
+++ b/src/proxy/proxy/Proxy.py
@@ -8,55 +8,62 @@ from geometry_msgs.msg import Twist
import numpy as np
-class Proxy(Node):
- def __init__(self, host: str="localhost", port: str="1969",
- connection_atempts: int=10, connection_delay: float=2.5,
- robot_name="ROKI-0058"):
- super().__init__('androsot_proxy')
+class Proxy(Node):
+ def __init__(
+ self,
+ host: str = "localhost",
+ port: str = "1969",
+ connection_atempts: int = 10,
+ connection_delay: float = 2.5,
+ robot_name="ROKI-0058",
+ ):
+ super().__init__("androsot_proxy")
self.subscription = self.create_subscription(
msg_type=Twist,
- topic='plan_cmd_vel',
+ topic="plan_cmd_vel",
callback=self.listener_callback,
qos_profile=10,
)
# TODO: wrap parameter declarations
- self.declare_parameter('host', host)
- self.declare_parameter('port', port)
- self.declare_parameter('n_conn_attempts', connection_atempts)
- self.declare_parameter('conn_delay', connection_delay)
+ self.declare_parameter("host", host)
+ self.declare_parameter("port", port)
+ self.declare_parameter("n_conn_attempts", connection_atempts)
+ self.declare_parameter("conn_delay", connection_delay)
- self.declare_parameter('robot_name', robot_name)
+ self.declare_parameter("robot_name", robot_name)
self.connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.writer = None
+ self.writer = None
@property
def host(self):
- return self.get_parameter('host').get_parameter_value().string_value
-
+ return self.get_parameter("host").get_parameter_value().string_value
+
@property
def port(self):
- return self.get_parameter('port').get_parameter_value().string_value
-
+ return self.get_parameter("port").get_parameter_value().string_value
+
@property
def n_conn_attempts(self):
- return self.get_parameter('n_conn_attempts').get_parameter_value().integer_value
-
+ return self.get_parameter("n_conn_attempts").get_parameter_value().integer_value
+
@property
def conn_delay(self):
- return self.get_parameter('conn_delay').get_parameter_value().double_value
-
+ return self.get_parameter("conn_delay").get_parameter_value().double_value
+
@property
def robot_name(self):
- return self.get_parameter('robot_name').get_parameter_value().string_value
+ return self.get_parameter("robot_name").get_parameter_value().string_value
async def connect(self) -> None:
for _ in range(self.n_conn_attempts):
try:
- await asyncio.to_thread(self.connection.connect, (self.host, int(self.port)))
+ await asyncio.to_thread(
+ self.connection.connect, (self.host, int(self.port))
+ )
_, self.writer = await asyncio.open_connection(sock=self.connection)
break
@@ -65,7 +72,9 @@ class Proxy(Node):
await asyncio.sleep(self.conn_delay)
if self.writer is None or self.writer.is_closing():
- raise ConnectionError(f"Connection refused after {self.conn_delay * self.n_conn_attempts} [sec]")
+ raise ConnectionError(
+ f"Connection refused after {self.conn_delay * self.n_conn_attempts} [sec]"
+ )
async def listener_callback(self, msg: Twist) -> None:
error_code = self.connection.getsockopt(socket.SOL_SOCKET, socket.SO_ERROR)
@@ -74,10 +83,14 @@ class Proxy(Node):
self.get_logger().error("Lost connection to host")
raise ConnectionError("Lost connection to host")
- self.writer.write(f"{self.robot_name}|0|{int(msg.angular.z)}|0|{np.clip(int(-msg.linear.y), -128, 128)}|{np.clip(-int(msg.linear.x), -128, 128)}\n".encode())
+ self.writer.write(
+ f"{self.robot_name}|0|{int(msg.angular.z)}|0|{np.clip(int(-msg.linear.y), -128, 128)}|{np.clip(-int(msg.linear.x), -128, 128)}\n".encode()
+ )
await self.writer.drain()
- self.get_logger().info(f'I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}')
+ self.get_logger().info(
+ f"I heard: {self.robot_name}|0|{msg.angular.z}|0|{msg.linear.y}|{msg.linear.x}"
+ )
async def disconnect(self) -> None:
if self.writer is not None:
@@ -86,6 +99,7 @@ class Proxy(Node):
self.connection.close()
+
async def launch_proxy() -> None:
try:
proxy = Proxy()
@@ -111,6 +125,7 @@ async def launch_proxy() -> None:
await proxy.disconnect()
proxy.destroy_node()
+
def main(args=None) -> None:
try:
rclpy.init(args=args)
@@ -125,5 +140,6 @@ def main(args=None) -> None:
finally:
rclpy.shutdown()
+
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
diff --git a/src/proxy/setup.py b/src/proxy/setup.py
index da4aa91..df80b02 100644
--- a/src/proxy/setup.py
+++ b/src/proxy/setup.py
@@ -1,27 +1,26 @@
from setuptools import find_packages, setup
-package_name = 'proxy'
+package_name = "proxy"
setup(
name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='funny-ded',
- maintainer_email='iashin.pa@phystech.edu',
- description='TODO: Package description',
- license='Apache-2.0',
- tests_require=['pytest'],
+ maintainer="funny-ded",
+ maintainer_email="iashin.pa@phystech.edu",
+ description="TODO: Package description",
+ license="Apache-2.0",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'command = proxy.CommandSender:main',
- 'proxy = proxy.Proxy:main',
+ "console_scripts": [
+ "command = proxy.CommandSender:main",
+ "proxy = proxy.Proxy:main",
],
-},
+ },
)
diff --git a/src/proxy/test/test_copyright.py b/src/proxy/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/proxy/test/test_copyright.py
+++ b/src/proxy/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/proxy/test/test_flake8.py b/src/proxy/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/proxy/test/test_flake8.py
+++ b/src/proxy/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/proxy/test/test_pep257.py b/src/proxy/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/proxy/test/test_pep257.py
+++ b/src/proxy/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/rpy_msg/setup.py b/src/rpy_msg/setup.py
index d19dbcb..c864cca 100644
--- a/src/rpy_msg/setup.py
+++ b/src/rpy_msg/setup.py
@@ -1,25 +1,23 @@
from setuptools import setup
-package_name = 'rpy_msg'
+package_name = "rpy_msg"
setup(
name=package_name,
- version='0.0.0',
+ version="0.0.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- ],
+ "console_scripts": [],
},
)
diff --git a/src/rpy_msg/test/test_copyright.py b/src/rpy_msg/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/rpy_msg/test/test_copyright.py
+++ b/src/rpy_msg/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/rpy_msg/test/test_flake8.py b/src/rpy_msg/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/rpy_msg/test/test_flake8.py
+++ b/src/rpy_msg/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/rpy_msg/test/test_pep257.py b/src/rpy_msg/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/rpy_msg/test/test_pep257.py
+++ b/src/rpy_msg/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/server_main/launch/aruco_detection.launch.py b/src/server_main/launch/aruco_detection.launch.py
index 300d48f..fe21d6f 100644
--- a/src/server_main/launch/aruco_detection.launch.py
+++ b/src/server_main/launch/aruco_detection.launch.py
@@ -1,28 +1,31 @@
from launch import LaunchDescription
from launch_ros.actions import Node
import os
+
+
def generate_launch_description():
camera_config = os.path.join("camera_params.yaml")
- return LaunchDescription([
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- ],
- remappings=[
- ('/camera/image_raw', '/image_raw'),
- ('/camera/camera_info', '/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- emulate_tty=True
- )
- ])
\ No newline at end of file
+ return LaunchDescription(
+ [
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/image_raw"),
+ ("/camera/camera_info", "/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/camera_calibration.launch.py b/src/server_main/launch/camera_calibration.launch.py
index 962ef7e..5510962 100644
--- a/src/server_main/launch/camera_calibration.launch.py
+++ b/src/server_main/launch/camera_calibration.launch.py
@@ -3,22 +3,25 @@ from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
+
def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
- return LaunchDescription([
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- emulate_tty=True
- ),
- #Node( package='camera_calibration', executable='cameracalibrator', parameters=[{'size':"6x8"}, {'square':0.056}], remappings=[
- # ('/camera/image_raw', '/image_raw'), ('camera', '/camera_info')])
-# ros2 run camera_calibration cameracalibrator --size 6x8 --square 0.056 --ros-args -r image:=/image_raw -p camera:=/camera_info
- ])
\ No newline at end of file
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+ return LaunchDescription(
+ [
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ emulate_tty=True,
+ ),
+ # Node( package='camera_calibration', executable='cameracalibrator', parameters=[{'size':"6x8"}, {'square':0.056}], remappings=[
+ # ('/camera/image_raw', '/image_raw'), ('camera', '/camera_info')])
+ # ros2 run camera_calibration cameracalibrator --size 6x8 --square 0.056 --ros-args -r image:=/image_raw -p camera:=/camera_info
+ ]
+ )
diff --git a/src/server_main/launch/multi_camera_server.launch.py b/src/server_main/launch/multi_camera_server.launch.py
index 7316f1d..aa397e0 100644
--- a/src/server_main/launch/multi_camera_server.launch.py
+++ b/src/server_main/launch/multi_camera_server.launch.py
@@ -4,123 +4,149 @@ from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config1 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- camera_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params2.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- config_dir = yolo_dir + '/config/'
- Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image')
-],
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')],
-
-)
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.07}
- ],
- remappings=[
- ('/camera/image_raw', '/camera/image_raw'),
- ('/camera/camera_info', '/camera/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config1],
- output='screen',
- emulate_tty=True,
- namespace='camera'
- ), Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
+def generate_launch_description():
+ camera_config1 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ config_dir = yolo_dir + "/config/"
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[("/ball_on_image", "/camera/ball_on_image")],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera/image_raw"),
+ ("/camera/camera_info", "/camera/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config1],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera",
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image'), ('/image_raw', '/camera/image_raw')
-],
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')],
-
-)
-,
-
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera2'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.07}
- ],
- remappings=[
- ('/camera/image_raw', '/camera2/image_raw'),
- ('/camera/camera_info', '/camera2/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config2],
- output='screen',
- emulate_tty=True,
- namespace='camera2'
- ),
- Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
+ remappings=[
+ ("/ball_on_image", "/camera/ball_on_image"),
+ ("/image_raw", "/camera/image_raw"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera2",
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
name="yolo_openvino_node",
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml'),],
- remappings= [('/ball_on_image', '/camera2/ball_on_image'), ('/image_raw', '/camera2/image_raw'),
-],output='screen'
-),
- # world frame publisher
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["-2.250422151907315", "-0.03619429789204953", "2.335802775666039", "-1.5958095381187083", "0.011972815851132468", " -2.581898500326197", "world", "camera"]
- ),
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["2.1788219890330171", "-0.12188665629765472", "1.8975118926888435", "1.5304591267508114", " -0.03915295964351014", "-2.5694155652622093", "world", "camera2"]
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera", "camera2"]}
- ],output='screen',
- emulate_tty=True
- )
- ])
\ No newline at end of file
+ parameters=[
+ os.path.join(config_dir, "yolo_roma.yaml"),
+ ],
+ remappings=[
+ ("/ball_on_image", "/camera2/ball_on_image"),
+ ("/image_raw", "/camera2/image_raw"),
+ ],
+ output="screen",
+ ),
+ # world frame publisher
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "-2.250422151907315",
+ "-0.03619429789204953",
+ "2.335802775666039",
+ "-1.5958095381187083",
+ "0.011972815851132468",
+ " -2.581898500326197",
+ "world",
+ "camera",
+ ],
+ ),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "2.1788219890330171",
+ "-0.12188665629765472",
+ "1.8975118926888435",
+ "1.5304591267508114",
+ " -0.03915295964351014",
+ "-2.5694155652622093",
+ "world",
+ "camera2",
+ ],
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera", "camera2"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/multi_team_multi_camera.launch.py b/src/server_main/launch/multi_team_multi_camera.launch.py
index 03c5ddd..c584f27 100644
--- a/src/server_main/launch/multi_team_multi_camera.launch.py
+++ b/src/server_main/launch/multi_team_multi_camera.launch.py
@@ -4,138 +4,165 @@ from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config1 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- camera_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params2.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- robot_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config2.json")
- config_dir = yolo_dir + '/config/'
- Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image')
-],
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')],
-
-)
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.07}
- ],
- remappings=[
- ('/camera/image_raw', '/camera/image_raw'),
- ('/camera/camera_info', '/camera/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config1],
- output='screen',
- emulate_tty=True,
- namespace='camera'
- ), Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
+def generate_launch_description():
+ camera_config1 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ camera_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ robot_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config2.json"
+ )
+ config_dir = yolo_dir + "/config/"
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[("/ball_on_image", "/camera/ball_on_image")],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera/image_raw"),
+ ("/camera/camera_info", "/camera/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config1],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera",
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image'), ('/image_raw', '/camera/image_raw')
-],
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')],
-
-)
-,
-
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera2'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.07}
- ],
- remappings=[
- ('/camera/image_raw', '/camera2/image_raw'),
- ('/camera/camera_info', '/camera2/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config2],
- output='screen',
- emulate_tty=True,
- namespace='camera2'
- ),
- Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
+ remappings=[
+ ("/ball_on_image", "/camera/ball_on_image"),
+ ("/image_raw", "/camera/image_raw"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07},
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config2],
+ output="screen",
+ emulate_tty=True,
+ namespace="camera2",
+ ),
+ Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
name="yolo_openvino_node",
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml'),],
- remappings= [('/ball_on_image', '/camera2/ball_on_image'), ('/image_raw', '/camera2/image_raw'),
-],output='screen'
-),
- # world frame publisher
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["-2.2984534584977494", "0.03178341624420522", "2.2612959861952744", "-1.5959610015041537", "-0.001000506359060393", " -2.5601661670824227", "world", "camera"]
- ),
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["2.2322242976339948", "-0.10266937938506238", "1.8572548987303894", "1.53711168914945", " -0.028233802143706358", "-2.5508803372385045", "world", "camera2"]
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera", "camera2"]}
- ],output='screen',
- emulate_tty=True
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config2},
- {'camera_frames': ["camera","camera2"]},
- {'team_name': 'starkit2'},
- {'mqtt_port': 1902}
- ],output='screen',
- name='ServerNode2',
- emulate_tty=True
- )
- ])
\ No newline at end of file
+ parameters=[
+ os.path.join(config_dir, "yolo_roma.yaml"),
+ ],
+ remappings=[
+ ("/ball_on_image", "/camera2/ball_on_image"),
+ ("/image_raw", "/camera2/image_raw"),
+ ],
+ output="screen",
+ ),
+ # world frame publisher
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "-2.2984534584977494",
+ "0.03178341624420522",
+ "2.2612959861952744",
+ "-1.5959610015041537",
+ "-0.001000506359060393",
+ " -2.5601661670824227",
+ "world",
+ "camera",
+ ],
+ ),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "2.2322242976339948",
+ "-0.10266937938506238",
+ "1.8572548987303894",
+ "1.53711168914945",
+ " -0.028233802143706358",
+ "-2.5508803372385045",
+ "world",
+ "camera2",
+ ],
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera", "camera2"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config2},
+ {"camera_frames": ["camera", "camera2"]},
+ {"team_name": "starkit2"},
+ {"mqtt_port": 1902},
+ ],
+ output="screen",
+ name="ServerNode2",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/server.launch.py b/src/server_main/launch/server.launch.py
index c25ab2e..9bd6295 100644
--- a/src/server_main/launch/server.launch.py
+++ b/src/server_main/launch/server.launch.py
@@ -4,69 +4,86 @@ from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image'), ('/image_raw', '/camera/image_raw'), ('/camera_info', '/camera/camera_info')
-],
-
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')])
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.07} # 0.172
- ],
- remappings=[
- # ('/camera/image_raw', '/image_raw'),
- # ('/camera/camera_info', '/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera',
- emulate_tty=True
- ),
- yola_node,
- # world frame publisher
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["-2.250422151907315", "-0.03619429789204953", "2.335802775666039", "-1.5958095381187083", "0.011972815851132468", " -2.581898500326197", "world", "camera"]
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera"]}
- ],output='screen',
- emulate_tty=True
- )
- ])
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ config_dir = yolo_dir + "/config/"
+ yola_node = Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera/ball_on_image"),
+ ("/image_raw", "/camera/image_raw"),
+ ("/camera_info", "/camera/camera_info"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07}, # 0.172
+ ],
+ remappings=[
+ # ('/camera/image_raw', '/image_raw'),
+ # ('/camera/camera_info', '/camera_info')
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera",
+ emulate_tty=True,
+ ),
+ yola_node,
+ # world frame publisher
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "-2.250422151907315",
+ "-0.03619429789204953",
+ "2.335802775666039",
+ "-1.5958095381187083",
+ "0.011972815851132468",
+ " -2.581898500326197",
+ "world",
+ "camera",
+ ],
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/server_calib.launch.py b/src/server_main/launch/server_calib.launch.py
index 6a72509..e264b8a 100644
--- a/src/server_main/launch/server_calib.launch.py
+++ b/src/server_main/launch/server_calib.launch.py
@@ -4,72 +4,85 @@ from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "calib_conf.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image'), ('/image_raw', '/camera/image_raw')
-],
-
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')])
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["-2.2984534584977494", "0.03178341624420522", "2.2612959861952744", "-1.5959610015041537", "-0.001000506359060393", " -2.5601661670824227", "world", "camera"]
-
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.172} # 0.07
- ],
- remappings=[
- # ('/camera/image_raw', '/image_raw'),
- # ('/camera/camera_info', '/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera',
- emulate_tty=True
- ),
- yola_node,
- # world frame publisher
-
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera"]}
- ],output='screen',
- emulate_tty=True
- )
- ])
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "calib_conf.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ config_dir = yolo_dir + "/config/"
+ yola_node = Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera/ball_on_image"),
+ ("/image_raw", "/camera/image_raw"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "-2.2984534584977494",
+ "0.03178341624420522",
+ "2.2612959861952744",
+ "-1.5959610015041537",
+ "-0.001000506359060393",
+ " -2.5601661670824227",
+ "world",
+ "camera",
+ ],
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.172}, # 0.07
+ ],
+ remappings=[
+ # ('/camera/image_raw', '/image_raw'),
+ # ('/camera/camera_info', '/camera_info')
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera",
+ emulate_tty=True,
+ ),
+ yola_node,
+ # world frame publisher
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/server_calib_cam2.launch.py b/src/server_main/launch/server_calib_cam2.launch.py
index 246129c..1946963 100644
--- a/src/server_main/launch/server_calib_cam2.launch.py
+++ b/src/server_main/launch/server_calib_cam2.launch.py
@@ -4,70 +4,86 @@ from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params2.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "calib_conf.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera2/ball_on_image'), ('/image_raw', '/camera2/image_raw'), ('/camera_info', '/camera2/camera_info')
-],
-
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')])
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera2'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size': 0.172} # 0.172
- ],
- remappings=[
- ('/camera/image_raw', '/camera2/image_raw'),
- ('/camera/camera_info', '/camera2/camera_info')
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera2',
- emulate_tty=True
- ),
- yola_node,
- # world frame publisher
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
-
- arguments=["2.2322242976339948", "-0.10266937938506238", "1.8572548987303894", "1.53711168914945", " -0.028233802143706358", "-2.5508803372385045", "world", "camera2"]
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera2"]}
- ],output='screen',
- emulate_tty=True
- )
- ])
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params2.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "calib_conf.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ config_dir = yolo_dir + "/config/"
+ yola_node = Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera2/ball_on_image"),
+ ("/image_raw", "/camera2/image_raw"),
+ ("/camera_info", "/camera2/camera_info"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera2"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.172}, # 0.172
+ ],
+ remappings=[
+ ("/camera/image_raw", "/camera2/image_raw"),
+ ("/camera/camera_info", "/camera2/camera_info"),
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera2",
+ emulate_tty=True,
+ ),
+ yola_node,
+ # world frame publisher
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "2.2322242976339948",
+ "-0.10266937938506238",
+ "1.8572548987303894",
+ "1.53711168914945",
+ " -0.028233802143706358",
+ "-2.5508803372385045",
+ "world",
+ "camera2",
+ ],
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera2"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/launch/solo_camera_multi_team.launch.py b/src/server_main/launch/solo_camera_multi_team.launch.py
index 2dd2ab6..462a7bd 100644
--- a/src/server_main/launch/solo_camera_multi_team.launch.py
+++ b/src/server_main/launch/solo_camera_multi_team.launch.py
@@ -1,96 +1,105 @@
-
-
-
-
-
-
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
import os
from ament_index_python.packages import get_package_share_directory
-def generate_launch_description():
- camera_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "camera_params.yaml")
- robot_config = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config.json")
-
- robot_config2 = os.path.join(get_package_share_directory('server_main'),
- 'config',
- "robots_config2.json")
- yolo_dir = get_package_share_directory('yolo_openvino_ros2')
- config_dir = yolo_dir + '/config/'
- yola_node = Node(package='yolo_openvino_ros2',
- executable='yolo_openvino_node',
- name="yolo_openvino_node",
- remappings= [('/ball_on_image', '/camera/ball_on_image'), ('/image_raw', '/camera/image_raw')
-],
-
- parameters=[os.path.join(config_dir, 'yolo_roma.yaml')])
- mosquitto_config = os.path.join(get_package_share_directory('server_main'), 'config','mosquitto.conf')
- return LaunchDescription([
- ExecuteProcess(cmd=['mosquitto', '-c', mosquitto_config],
- output='screen'),
- Node(
- package='tf2_ros',
- executable='static_transform_publisher',
- # args = [1,2,0,0,0,0, 'world', 'camera']
- # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
- # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
- # ]
- # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
- arguments=["-2.2984534584977494", "0.03178341624420522", "2.2612959861952744", "-1.5959610015041537", "-0.001000506359060393", " -2.5601661670824227", "world", "camera"]
- ),
- Node(
- package='ros2_aruco',
- executable='aruco_node',
- parameters=[
- {'camera_frame': 'camera'},
- {'aruco_dictionary_id': 'DICT_4X4_100'},
- {'marker_size':0.07} # 0.07
- ],
- remappings=[
- # ('/camera/image_raw', '/image_raw'),
- # ('/camera/camera_info', '/camera_info')
-
- ],
- output='screen',
- emulate_tty=True
- ),
- Node(
- package='usb_cam',
- executable='usb_cam_node_exe',
- parameters=[camera_config],
- output='screen',
- namespace='camera',
- emulate_tty=True
- ),
- yola_node,
- # world frame publisher
+def generate_launch_description():
+ camera_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "camera_params.yaml"
+ )
+ robot_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config.json"
+ )
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config},
- {'camera_frames': ["camera"]}
- ],output='screen',
- emulate_tty=True
- ),
- Node(
- package='server_main',
- executable='server_node',
- parameters=[
- {'robots_params': robot_config2},
- {'camera_frames': ["camera"]},
- {'team_name': 'starkit2'},
- {'mqtt_port': 1902}
- ],output='screen',
- name='ServerNode2',
- emulate_tty=True
- )
- ])
+ robot_config2 = os.path.join(
+ get_package_share_directory("server_main"), "config", "robots_config2.json"
+ )
+ yolo_dir = get_package_share_directory("yolo_openvino_ros2")
+ config_dir = yolo_dir + "/config/"
+ yola_node = Node(
+ package="yolo_openvino_ros2",
+ executable="yolo_openvino_node",
+ name="yolo_openvino_node",
+ remappings=[
+ ("/ball_on_image", "/camera/ball_on_image"),
+ ("/image_raw", "/camera/image_raw"),
+ ],
+ parameters=[os.path.join(config_dir, "yolo_roma.yaml")],
+ )
+ mosquitto_config = os.path.join(
+ get_package_share_directory("server_main"), "config", "mosquitto.conf"
+ )
+ return LaunchDescription(
+ [
+ ExecuteProcess(cmd=["mosquitto", "-c", mosquitto_config], output="screen"),
+ Node(
+ package="tf2_ros",
+ executable="static_transform_publisher",
+ # args = [1,2,0,0,0,0, 'world', 'camera']
+ # parameters=[{'x' : 1},{'y' : 2},{'z' : 0},{'roll' : 1},{'pitch' : 1},{'yaw' : 1},
+ # {'Frame_id' : 'world'},{'Child_frame_id' : 'camera'},
+ # ]
+ # arguments=["0", "0 ", "1", "0", "0", "-1.57", "world", "camera"]
+ arguments=[
+ "-2.2984534584977494",
+ "0.03178341624420522",
+ "2.2612959861952744",
+ "-1.5959610015041537",
+ "-0.001000506359060393",
+ " -2.5601661670824227",
+ "world",
+ "camera",
+ ],
+ ),
+ Node(
+ package="ros2_aruco",
+ executable="aruco_node",
+ parameters=[
+ {"camera_frame": "camera"},
+ {"aruco_dictionary_id": "DICT_4X4_100"},
+ {"marker_size": 0.07}, # 0.07
+ ],
+ remappings=[
+ # ('/camera/image_raw', '/image_raw'),
+ # ('/camera/camera_info', '/camera_info')
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ parameters=[camera_config],
+ output="screen",
+ namespace="camera",
+ emulate_tty=True,
+ ),
+ yola_node,
+ # world frame publisher
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config},
+ {"camera_frames": ["camera"]},
+ ],
+ output="screen",
+ emulate_tty=True,
+ ),
+ Node(
+ package="server_main",
+ executable="server_node",
+ parameters=[
+ {"robots_params": robot_config2},
+ {"camera_frames": ["camera"]},
+ {"team_name": "starkit2"},
+ {"mqtt_port": 1902},
+ ],
+ output="screen",
+ name="ServerNode2",
+ emulate_tty=True,
+ ),
+ ]
+ )
diff --git a/src/server_main/scripts/camera_calibration.py b/src/server_main/scripts/camera_calibration.py
index 7be49d6..9cfedd7 100644
--- a/src/server_main/scripts/camera_calibration.py
+++ b/src/server_main/scripts/camera_calibration.py
@@ -1,47 +1,47 @@
- # Source: https://nikatsanka.github.io/camera-calibration-using-opencv-and-python.html
+# Source: https://nikatsanka.github.io/camera-calibration-using-opencv-and-python.html
import numpy as np
import cv2
import glob
import yaml
-#import pathlib
+# import pathlib
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
-objp = np.zeros((6*8,3), np.float32)
-objp[:,:2] = np.mgrid[0:6,0:8].T.reshape(-1,2)
+objp = np.zeros((6 * 8, 3), np.float32)
+objp[:, :2] = np.mgrid[0:6, 0:8].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
-objpoints = [] # 3d point in real world space
-imgpoints = [] # 2d points in image plane.
+objpoints = [] # 3d point in real world space
+imgpoints = [] # 2d points in image plane.
-images = glob.glob(r'images/*.jpg')
+images = glob.glob(r"images/*.jpg")
# path = 'results'
-# pathlib.Path(path).mkdir(parents=True, exist_ok=True)
+# pathlib.Path(path).mkdir(parents=True, exist_ok=True)
found = 0
for fname in images: # Here, 10 can be changed to whatever number you like to choose
- img = cv2.imread(fname) # Capture frame-by-frame
- #print(images[im_i])
+ img = cv2.imread(fname) # Capture frame-by-frame
+ # print(images[im_i])
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
- ret, corners = cv2.findChessboardCorners(gray, (6,8), None)
+ ret, corners = cv2.findChessboardCorners(gray, (6, 8), None)
# If found, add object points, image points (after refining them)
if ret is True:
- corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
- if (len(objp) == len(corners2) ):
- objpoints.append(objp) # Certainly, every loop objp is the same, in 3D.
+ corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+ if len(objp) == len(corners2):
+ objpoints.append(objp) # Certainly, every loop objp is the same, in 3D.
imgpoints.append(corners2)
found += 1
# Draw and display the corners
- img = cv2.drawChessboardCorners(img, (6,8), corners2, ret)
- cv2.imshow('img', img)
+ img = cv2.drawChessboardCorners(img, (6, 8), corners2, ret)
+ cv2.imshow("img", img)
cv2.waitKey(500)
- # if you want to save images with detected corners
- # uncomment following 2 lines and lines 5, 18 and 19
- # image_name = path + '/calibresult' + str(found) + '.png'
- # cv2.imwrite(image_name, img)
+ # if you want to save images with detected corners
+ # uncomment following 2 lines and lines 5, 18 and 19
+ # image_name = path + '/calibresult' + str(found) + '.png'
+ # cv2.imwrite(image_name, img)
print("Number of images used for calibration: ", found)
@@ -50,18 +50,22 @@ print("Number of images used for calibration: ", found)
cv2.destroyAllWindows()
# calibration
-ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
+ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
+ objpoints, imgpoints, gray.shape[::-1], None, None
+)
R = cv2.Rodrigues(rvecs[0])[0]
t = tvecs[0]
-Rt = np.concatenate([R,t], axis=-1) # [R|t]
-P = np.matmul(mtx,Rt) # A[R|t]
+Rt = np.concatenate([R, t], axis=-1) # [R|t]
+P = np.matmul(mtx, Rt) # A[R|t]
# transform the matrix and distortion coefficients to writable lists
-data = {'camera_matrix': np.asarray(mtx).tolist(),
- 'dist_coeff': np.asarray(dist).tolist(),
- 'projection_matrix': np.asarray(P).tolist(),}
+data = {
+ "camera_matrix": np.asarray(mtx).tolist(),
+ "dist_coeff": np.asarray(dist).tolist(),
+ "projection_matrix": np.asarray(P).tolist(),
+}
# and save it to a file
with open("calibration_matrix.yaml", "w") as f:
yaml.dump(data, f)
-# done
\ No newline at end of file
+# done
diff --git a/src/server_main/scripts/camera_pose_calibration.py b/src/server_main/scripts/camera_pose_calibration.py
index 9fd75af..c93104e 100644
--- a/src/server_main/scripts/camera_pose_calibration.py
+++ b/src/server_main/scripts/camera_pose_calibration.py
@@ -6,10 +6,9 @@ from geometry_msgs.msg import PoseStamped, TransformStamped
from rclpy.node import Node
-
-
from tf2_ros import TransformBroadcaster
+
class CalibrationNode(Node):
def __init__(self):
super().__init__("CalibrationNode")
@@ -22,6 +21,7 @@ class CalibrationNode(Node):
self.camera_frame = self.get_parameter("camera_frames").value
self.tf_broadcaster = TransformBroadcaster(self)
+
def get_aruco_pose(self, msg):
for id, pose in zip(msg.marker_ids, msg.poses):
pose_stamped = PoseStamped()
@@ -32,8 +32,8 @@ class CalibrationNode(Node):
# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
- t.header.frame_id = 'aruco' + str(id)
- t.child_frame_id = self.camera_frame
+ t.header.frame_id = "aruco" + str(id)
+ t.child_frame_id = self.camera_frame
# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
@@ -45,6 +45,6 @@ class CalibrationNode(Node):
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
t.transform.rotation.x = pose.orientation
-
+
# Send the transformation
- self.tf_broadcaster.sendTransform(t)
\ No newline at end of file
+ self.tf_broadcaster.sendTransform(t)
diff --git a/src/server_main/scripts/create_calib_photo.py b/src/server_main/scripts/create_calib_photo.py
index 25bd8af..e3eb75e 100644
--- a/src/server_main/scripts/create_calib_photo.py
+++ b/src/server_main/scripts/create_calib_photo.py
@@ -1,35 +1,35 @@
import cv2
import os
-if __name__ == '__main__':
- cam = cv2.VideoCapture(2) # Choose your camera
+
+if __name__ == "__main__":
+ cam = cv2.VideoCapture(2) # Choose your camera
cam.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
- cam.set(cv2.CAP_PROP_AUTOFOCUS, 0) # turn the autofocus off
+ cam.set(cv2.CAP_PROP_AUTOFOCUS, 0) # turn the autofocus off
counter = 0
- image_directory = os.path.join(os.getcwd(), 'images')
+ image_directory = os.path.join(os.getcwd(), "images")
try:
- os.mkdir(image_directory)
+ os.mkdir(image_directory)
except FileExistsError:
- print(f"Directory allready exists: {image_directory}")
+ print(f"Directory allready exists: {image_directory}")
except OSError:
- print(f"Directory can not be created: {image_directory}")
+ print(f"Directory can not be created: {image_directory}")
-
while True:
- check, frame = cam.read()
-
- cv2.imshow('video', frame)
+ check, frame = cam.read()
+
+ cv2.imshow("video", frame)
- key = cv2.waitKey(1)
- if key & 0xFF == ord('q'):
- break
- elif key & 0xFF == ord('s'):
- file_name = os.path.join(image_directory, "calib" + str(counter) + ".jpg")
- counter += 1
- cv2.imwrite(file_name, frame)
- elif key & 0xFF == ord('n'):
- pass
+ key = cv2.waitKey(1)
+ if key & 0xFF == ord("q"):
+ break
+ elif key & 0xFF == ord("s"):
+ file_name = os.path.join(image_directory, "calib" + str(counter) + ".jpg")
+ counter += 1
+ cv2.imwrite(file_name, frame)
+ elif key & 0xFF == ord("n"):
+ pass
cam.release()
- cv2.destroyAllWindows
\ No newline at end of file
+ cv2.destroyAllWindows
diff --git a/src/server_main/scripts/xz_angle_from_quaternion.py b/src/server_main/scripts/xz_angle_from_quaternion.py
index 056ab04..a67ff25 100644
--- a/src/server_main/scripts/xz_angle_from_quaternion.py
+++ b/src/server_main/scripts/xz_angle_from_quaternion.py
@@ -1,4 +1,5 @@
-#angle between new z and old x
+# angle between new z and old x
+
def q_mult(q1, q2):
w1, x1, y1, z1 = q1
@@ -9,14 +10,17 @@ def q_mult(q1, q2):
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
+
def q_conjugate(q):
w, x, y, z = q
return (w, -x, -y, -z)
+
def qv_mult(q1, v1):
q2 = (0.0,) + v1
return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]
+
def xz_angle_from_quat(quat):
z_vector = (0, 0, 1)
return qv_mult(quat, z_vector)[0]
diff --git a/src/server_main/server_main/filter.py b/src/server_main/server_main/filter.py
index 34eac97..832002f 100644
--- a/src/server_main/server_main/filter.py
+++ b/src/server_main/server_main/filter.py
@@ -1,7 +1,7 @@
-
from collections import deque
-class AlphaFilter():
+
+class AlphaFilter:
def __init__(self):
pass
self.buffer = deque()
@@ -9,22 +9,22 @@ class AlphaFilter():
self.alpha = 0.1
def update(self, value):
- if (self.buffer_size == len(self.buffer)):
+ if self.buffer_size == len(self.buffer):
self.buffer.popleft()
- mid = value
+ mid = value
weight_sum = 1
## find yaw mid
for i, elem in enumerate(self.buffer):
mid.position.x += elem.position.x * self.alpha ** (len(self.buffer) - i)
mid.position.y += elem.position.y * self.alpha ** (len(self.buffer) - i)
mid.position.z += elem.position.z * self.alpha ** (len(self.buffer) - i)
- weight_sum += self.alpha ** (len(self.buffer) - i )
-
+ weight_sum += self.alpha ** (len(self.buffer) - i)
+
mid.position.x /= weight_sum
mid.position.y /= weight_sum
mid.position.z /= weight_sum
- self.buffer.append(mid)
+ self.buffer.append(mid)
def get(self):
- return self.buffer[-1]
\ No newline at end of file
+ return self.buffer[-1]
diff --git a/src/server_main/server_main/server_node.py b/src/server_main/server_main/server_node.py
index 48bd6f3..e210bc8 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -199,10 +199,10 @@ class ServerNode(Node):
def get_camera_to_world_frame(self):
try:
for frame in self.camera_frames:
- self.tf_from_camera_to_world[
- frame
- ] = self.tf_buffer.lookup_transform( # check inversion not allwed
- "world", frame, rclpy.time.Time()
+ self.tf_from_camera_to_world[frame] = (
+ self.tf_buffer.lookup_transform( # check inversion not allwed
+ "world", frame, rclpy.time.Time()
+ )
)
except TransformException as ex:
import time
@@ -457,7 +457,7 @@ class ServerNode(Node):
)
with open(self.file_to_save, "w") as file:
import csv
-
+
writer = csv.writer(file)
writer.writerows(self.loged_data)
self.mqttc.publish(self.robots_topic[id], robot_msg, 1)
diff --git a/src/server_main/server_main/strategy.py b/src/server_main/server_main/strategy.py
index 5d03de5..84fc618 100644
--- a/src/server_main/server_main/strategy.py
+++ b/src/server_main/server_main/strategy.py
@@ -14,12 +14,15 @@ from rclpy.node import Node
from visualization_msgs.msg import Marker
import paho.mqtt.client as mqtt
+
# https://github.com/eclipse/paho.mqtt.python
import tf_transformations
from ble_communicator import BLECommunicator
from collections import namedtuple
-Pose = namedtuple('Pose', ['x y yaw'], defaults=[0, 0, 0])
+Pose = namedtuple("Pose", ["x y yaw"], defaults=[0, 0, 0])
+
+
class RobotInfo:
def __init__(self):
self.state = None
@@ -29,47 +32,51 @@ class RobotInfo:
self.last_update_timestamp = None
self.connected = False
self.goal = {}
+
+
class StrategyService(Node):
def __init__(self):
- super().__init__('MqttKondoDummy')
- # get from azer project
+ super().__init__("MqttKondoDummy")
+ # get from azer project
self.field_params = None
self.init_publisher()
self.init_mqqt()
self.init_robots()
- #self.timer = self.create_timer(0.0001, self.forward_main_cycle)
+ # self.timer = self.create_timer(0.0001, self.forward_main_cycle)
self.robots_commucator = BLECommunicator()
def init_mqqt(self):
- self.declare_parameter('mqtt_broker', 'localhost')
- self.broker = self.get_parameter('mqtt_broker').value
- self.declare_parameter('mqtt_port', 1900)
- self.port = self.get_parameter('mqtt_port').value
+ self.declare_parameter("mqtt_broker", "localhost")
+ self.broker = self.get_parameter("mqtt_broker").value
+ self.declare_parameter("mqtt_port", 1900)
+ self.port = self.get_parameter("mqtt_port").value
self.team_name = "starkit1"
self.ball_topic = self.team_name + "/ball"
- self.robots_topic = { f"{self.team_name}/{robot_id}/robot" for robot_id in range(0, 3)}
+ self.robots_topic = {
+ f"{self.team_name}/{robot_id}/robot" for robot_id in range(0, 3)
+ }
self.mqttc = mqtt.Client()
self.mqttc.on_message = self.on_message
self.mqttc.on_connect = StrategyService.on_connect
- self.mqttc.on_publish = StrategyService.on_publish
+ self.mqttc.on_publish = StrategyService.on_publish
self.mqttc.on_subscribe = StrategyService.on_subscribe
self.mqttc.connect(self.broker, self.port, 60)
self.mqttc.subscribe(self.ball_topic, 0)
for topic in self.robots_topic:
- self.mqttc.subscribe(topic, 0)
-
+ self.mqttc.subscribe(topic, 0)
+
def on_connect(mqttc, obj, flags, rc):
print("rc: " + str(rc))
def on_message(self, mqttc, obj, msg):
print("get msg")
- if (msg.topic == self.ball_topic):
- self.ball_coord = [float(i) for i in msg.payload.split(b" ")]
- print(self.ball_coord)
- elif (msg.topic in self.robots_topic):
+ if msg.topic == self.ball_topic:
+ self.ball_coord = [float(i) for i in msg.payload.split(b" ")]
+ print(self.ball_coord)
+ elif msg.topic in self.robots_topic:
pf_coord = [float(i) for i in msg.payload.split(b" ")]
self.robots[msg.topic].connected = True
self.robots[msg.topic].pose.x = pf_coord[0]
@@ -77,25 +84,23 @@ class StrategyService(Node):
self.robots[msg.topic].pose.yaw = pf_coord[2]
# [TODO] update timesatmp
else:
- print("Warning: Unrecognized topic")
-
+ print("Warning: Unrecognized topic")
def on_publish(mqttc, obj, mid):
print("mid: " + str(mid))
-
+
def on_subscribe(mqttc, obj, mid, granted_qos):
print("Subscribed: " + str(mid) + " " + str(granted_qos))
def init_robots(self):
self.robots = dict()
for robot_topic in self.robots_topic:
- self.robots[robot_topic] = RobotInfo()
-
+ self.robots[robot_topic] = RobotInfo()
+
pass
def startegy_loop(self):
-
- while(True):
+ while True:
self.mqttc.loop()
self.update_robot_info()
self.update_strategy()
@@ -104,128 +109,118 @@ class StrategyService(Node):
def update_robot_info(self):
for robot in self.robots:
- robot.role = "forward"
+ robot.role = "forward"
self.robots[self.robots_topic[0]] = "goalkeeper"
+
def update_strategy(self):
pass
+
def update_goals(self):
for robot in self.robots:
- if (robot.role == "forward"):
+ if robot.role == "forward":
+ pass
+ elif robot.role == "goalkeeper":
pass
- elif (robot.role == "goalkeeper"):
- pass
def send_commands(self, commands):
pass
+
def init_publisher(self):
- self.robot_pub = self.create_publisher(
- Marker,
- "/robot_dummy",
- 10
- )
- self.ball_pub = self.create_publisher(
- Marker,
- "/ball_dummmy",
- 10
- )
- self.robot_pub2 = self.create_publisher(
- Marker,
- "/robot_dummy2",
- 10
- )
- self.ball_pub2 = self.create_publisher(
- Marker,
- "/ball_dummm2",
- 10
- )
+ self.robot_pub = self.create_publisher(Marker, "/robot_dummy", 10)
+ self.ball_pub = self.create_publisher(Marker, "/ball_dummmy", 10)
+ self.robot_pub2 = self.create_publisher(Marker, "/robot_dummy2", 10)
+ self.ball_pub2 = self.create_publisher(Marker, "/ball_dummm2", 10)
def publish_marker(self):
- if (self.pf_coord is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 100
- marker.type = marker.ARROW
- marker.action = marker.ADD
- marker.pose.position.x = self.pf_coord[0]
- marker.pose.position.y = self.pf_coord[1]
- quaternion = tf_transformations.quaternion_about_axis(self.pf_coord[2], (0,0,1))
- marker.pose.orientation.x = quaternion[0]
- marker.pose.orientation.y = quaternion[1]
- marker.pose.orientation.z = quaternion[2]
- marker.pose.orientation.w = quaternion[3]
- marker.pose.orientation
- marker.scale.x = .02
- marker.scale.y = .02
- marker.scale.z = .02
- marker.color.a = 1.0
- marker.color.r = 0.0
- marker.color.g = 0.9
- marker.color.b = 0.2
- self.robot_pub.publish(marker)
- print("robot:", marker)
- if (self.ball_coord is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 101
- marker.type = marker.SPHERE
- marker.action = marker.ADD
- marker.pose.position.x = self.ball_coord[0]
- marker.pose.position.y = self.ball_coord[1]
- marker.scale.x = .05
- marker.scale.y = .05
- marker.scale.z = .05
- marker.color.a = 1.0
- marker.color.r = 1.0
- marker.color.g = 0.4
- marker.color.b = 0.1
- self.ball_pub.publish(marker)
- print("ball " ,marker)
- if (self.pf_coord2 is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 100
- marker.type = marker.ARROW
- marker.action = marker.ADD
- marker.pose.position.x = self.pf_coord2[0]
- marker.pose.position.y = self.pf_coord2[1]
- quaternion = tf_transformations.quaternion_about_axis(self.pf_coord2[2], (0,0,1))
- marker.pose.orientation.x = quaternion[0]
- marker.pose.orientation.y = quaternion[1]
- marker.pose.orientation.z = quaternion[2]
- marker.pose.orientation.w = quaternion[3]
- marker.pose.orientation
- marker.scale.x = .02
- marker.scale.y = .02
- marker.scale.z = .02
- marker.color.a = 1.0
- marker.color.r = 0.0
- marker.color.g = 0.9
- marker.color.b = 1.0
- self.robot_pub2.publish(marker)
- print("robot:", marker)
- if (self.ball_coord2 is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 101
- marker.type = marker.SPHERE
- marker.action = marker.ADD
- marker.pose.position.x = self.ball_coord2[0]
- marker.pose.position.y = self.ball_coord2[1]
- marker.scale.x = .05
- marker.scale.y = .05
- marker.scale.z = .05
- marker.color.a = 1.0
- marker.color.r = 0.3
- marker.color.g = 0.4
- marker.color.b = 1.0
- self.ball_pub2.publish(marker)
-
+ if self.pf_coord is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord[0]
+ marker.pose.position.y = self.pf_coord[1]
+ quaternion = tf_transformations.quaternion_about_axis(
+ self.pf_coord[2], (0, 0, 1)
+ )
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = 0.02
+ marker.scale.y = 0.02
+ marker.scale.z = 0.02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 0.2
+ self.robot_pub.publish(marker)
+ print("robot:", marker)
+ if self.ball_coord is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord[0]
+ marker.pose.position.y = self.ball_coord[1]
+ marker.scale.x = 0.05
+ marker.scale.y = 0.05
+ marker.scale.z = 0.05
+ marker.color.a = 1.0
+ marker.color.r = 1.0
+ marker.color.g = 0.4
+ marker.color.b = 0.1
+ self.ball_pub.publish(marker)
+ print("ball ", marker)
+ if self.pf_coord2 is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord2[0]
+ marker.pose.position.y = self.pf_coord2[1]
+ quaternion = tf_transformations.quaternion_about_axis(
+ self.pf_coord2[2], (0, 0, 1)
+ )
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = 0.02
+ marker.scale.y = 0.02
+ marker.scale.z = 0.02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 1.0
+ self.robot_pub2.publish(marker)
+ print("robot:", marker)
+ if self.ball_coord2 is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord2[0]
+ marker.pose.position.y = self.ball_coord2[1]
+ marker.scale.x = 0.05
+ marker.scale.y = 0.05
+ marker.scale.z = 0.05
+ marker.color.a = 1.0
+ marker.color.r = 0.3
+ marker.color.g = 0.4
+ marker.color.b = 1.0
+ self.ball_pub2.publish(marker)
def main(args=None):
# rclpy.init(args=args)
- server_node = StrategyService()
+ server_node = StrategyService()
server_node.startegy_loop()
# rclpy.spin(server_node)
@@ -237,5 +232,5 @@ def main(args=None):
# rclpy.shutdown()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
diff --git a/src/server_main/setup.py b/src/server_main/setup.py
index cd0f81c..31470c8 100644
--- a/src/server_main/setup.py
+++ b/src/server_main/setup.py
@@ -2,31 +2,32 @@ import os
from glob import glob
from setuptools import setup
-package_name = 'server_main'
+package_name = "server_main"
setup(
name=package_name,
- version='0.0.0',
+ version="0.0.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
- (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
-
-
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*")),
+ ),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'server_node = server_main.server_node:main'
- ],
+ "console_scripts": ["server_node = server_main.server_node:main"],
},
)
diff --git a/src/server_main/test/logger.py b/src/server_main/test/logger.py
index fff4ff9..0fb8351 100644
--- a/src/server_main/test/logger.py
+++ b/src/server_main/test/logger.py
@@ -1,28 +1,28 @@
-
import matplotlib.pyplot as plt
# from collections import defaultdict
# from multiprocessing import Process, Value
# import json
-plt.style.use('seaborn') # fivethirtyeight is name of style
-plt.rcParams['font.size'] = 10
-plt.rcParams['legend.fontsize'] = 10
-plt.rc('xtick', labelsize=10)
-plt.rc('ytick', labelsize=10)
-plt.rcParams['axes.labelsize'] = 10
-plt.rcParams['axes.titlesize'] = 10
+plt.style.use("seaborn") # fivethirtyeight is name of style
+plt.rcParams["font.size"] = 10
+plt.rcParams["legend.fontsize"] = 10
+plt.rc("xtick", labelsize=10)
+plt.rc("ytick", labelsize=10)
+plt.rcParams["axes.labelsize"] = 10
+plt.rcParams["axes.titlesize"] = 10
-plt.rcParams['xtick.major.size'] = 5.0
-plt.rcParams['xtick.minor.size'] = 3.0
-plt.rcParams['ytick.major.size'] = 5.0
-plt.rcParams['ytick.minor.size'] = 3.0
+plt.rcParams["xtick.major.size"] = 5.0
+plt.rcParams["xtick.minor.size"] = 3.0
+plt.rcParams["ytick.major.size"] = 5.0
+plt.rcParams["ytick.minor.size"] = 3.0
# plt.rcParams["font.family"] = "Times New Roman"
+
class Logger:
def __init__(self):
self.yaw_log = []
- self.plot_name = 'yaw_plot'
+ self.plot_name = "yaw_plot"
def log_yaw(self, value):
self.yaw_log.append(value)
@@ -30,13 +30,12 @@ class Logger:
def init_plot_yaw(self):
self.fig, self.ax = plt.subplots(figsize=(30, 20))
# ax.tick_params(axis='both', which='major', labelsize=30)
- self.ax.set(xlabel='step', ylabel='yaw [rad]',
- title='Robot\'s Yaw')
+ self.ax.set(xlabel="step", ylabel="yaw [rad]", title="Robot's Yaw")
# fig.suptitle('Yaw')
# ax.set_title('RobotYaw', loc='left', fontsize='large')
# self.ax.plot(self.yaw_log)
# plt.savefig(self.plot_name)
-
+
def update_plot_yaw(self):
plt.ion()
plt.show()
@@ -46,5 +45,3 @@ class Logger:
def plot_all_and_save_fig(self):
self.ax.plot(self.yaw_log)
plt.savefig(self.plot_name)
-
-
\ No newline at end of file
diff --git a/src/server_main/test/mqtt_kondo_dummy.py b/src/server_main/test/mqtt_kondo_dummy.py
index 9094ec5..d24e3e8 100644
--- a/src/server_main/test/mqtt_kondo_dummy.py
+++ b/src/server_main/test/mqtt_kondo_dummy.py
@@ -15,16 +15,19 @@ from rclpy.node import Node
from visualization_msgs.msg import Marker
import paho.mqtt.client as mqtt
+
# https://github.com/eclipse/paho.mqtt.python
import tf_transformations
+
+
class MqttKondoDummy(Node):
def __init__(self):
- super().__init__('MqttKondoDummy')
- # get from azer project
- self.pf_coord=None
- self.ball_coord=None
- self.pf_coord2=None
- self.ball_coord2=None
+ super().__init__("MqttKondoDummy")
+ # get from azer project
+ self.pf_coord = None
+ self.ball_coord = None
+ self.pf_coord2 = None
+ self.ball_coord2 = None
self.init_publisher()
self.init_mqqt()
self.timer = self.create_timer(0.0001, self.forward_main_cycle)
@@ -34,32 +37,32 @@ class MqttKondoDummy(Node):
def on_message(self, mqttc, obj, msg):
print("get msg")
- if (msg.topic == self.ball_topic):
- # print(msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
+ if msg.topic == self.ball_topic:
+ # print(msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
self.ball_coord = [float(i) for i in msg.payload.split(b" ")]
print(self.ball_coord)
- elif (msg.topic == self.robot_topic):
- self.pf_coord = [float(i) for i in msg.payload.split(b" ")]
- print(self.pf_coord)
- if (msg.topic == self.ball_topic2):
- # print(msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
+ elif msg.topic == self.robot_topic:
+ self.pf_coord = [float(i) for i in msg.payload.split(b" ")]
+ print(self.pf_coord)
+ if msg.topic == self.ball_topic2:
+ # print(msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
self.ball_coord2 = [float(i) for i in msg.payload.split(b" ")]
print(self.ball_coord2)
- elif (msg.topic == self.robot_topic2):
- self.pf_coord2 = [float(i) for i in msg.payload.split(b" ")]
- print(self.pf_coord2)
+ elif msg.topic == self.robot_topic2:
+ self.pf_coord2 = [float(i) for i in msg.payload.split(b" ")]
+ print(self.pf_coord2)
def on_publish(mqttc, obj, mid):
print("mid: " + str(mid))
-
+
def on_subscribe(mqttc, obj, mid, granted_qos):
print("Subscribed: " + str(mid) + " " + str(granted_qos))
def init_mqqt(self):
- self.declare_parameter('mqtt_broker', 'localhost')
- self.broker = self.get_parameter('mqtt_broker').value
- self.declare_parameter('mqtt_port', 1900)
- self.port = self.get_parameter('mqtt_port').value
+ self.declare_parameter("mqtt_broker", "localhost")
+ self.broker = self.get_parameter("mqtt_broker").value
+ self.declare_parameter("mqtt_port", 1900)
+ self.port = self.get_parameter("mqtt_port").value
self.ball_topic = "starkit1/ball"
self.ball_topic2 = "starkit2/ball"
self.robot_topic = "starkit1/0/robot"
@@ -68,7 +71,7 @@ class MqttKondoDummy(Node):
self.mqttc = mqtt.Client()
self.mqttc.on_message = self.on_message
self.mqttc.on_connect = MqttKondoDummy.on_connect
- self.mqttc.on_publish = MqttKondoDummy.on_publish
+ self.mqttc.on_publish = MqttKondoDummy.on_publish
self.mqttc.on_subscribe = MqttKondoDummy.on_subscribe
self.mqttc.connect(self.broker, self.port, 60)
self.mqttc.subscribe(self.ball_topic, 0)
@@ -76,121 +79,106 @@ class MqttKondoDummy(Node):
self.mqttc.subscribe(self.ball_topic2, 0)
self.mqttc.subscribe(self.robot_topic2, 0)
-
def init_publisher(self):
- self.robot_pub = self.create_publisher(
- Marker,
- "/robot_dummy",
- 10
- )
- self.ball_pub = self.create_publisher(
- Marker,
- "/ball_dummmy",
- 10
- )
- self.robot_pub2 = self.create_publisher(
- Marker,
- "/robot_dummy2",
- 10
- )
- self.ball_pub2 = self.create_publisher(
- Marker,
- "/ball_dummm2",
- 10
- )
-
+ self.robot_pub = self.create_publisher(Marker, "/robot_dummy", 10)
+ self.ball_pub = self.create_publisher(Marker, "/ball_dummmy", 10)
+ self.robot_pub2 = self.create_publisher(Marker, "/robot_dummy2", 10)
+ self.ball_pub2 = self.create_publisher(Marker, "/ball_dummm2", 10)
def publish_marker(self):
- if (self.pf_coord is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 100
- marker.type = marker.ARROW
- marker.action = marker.ADD
- marker.pose.position.x = self.pf_coord[0]
- marker.pose.position.y = self.pf_coord[1]
- quaternion = tf_transformations.quaternion_about_axis(self.pf_coord[2], (0,0,1))
- marker.pose.orientation.x = quaternion[0]
- marker.pose.orientation.y = quaternion[1]
- marker.pose.orientation.z = quaternion[2]
- marker.pose.orientation.w = quaternion[3]
- marker.pose.orientation
- marker.scale.x = .02
- marker.scale.y = .02
- marker.scale.z = .02
- marker.color.a = 1.0
- marker.color.r = 0.0
- marker.color.g = 0.9
- marker.color.b = 0.2
- self.robot_pub.publish(marker)
- print("robot:", marker)
- if (self.ball_coord is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 101
- marker.type = marker.SPHERE
- marker.action = marker.ADD
- marker.pose.position.x = self.ball_coord[0]
- marker.pose.position.y = self.ball_coord[1]
- marker.scale.x = .05
- marker.scale.y = .05
- marker.scale.z = .05
- marker.color.a = 1.0
- marker.color.r = 1.0
- marker.color.g = 0.4
- marker.color.b = 0.1
- self.ball_pub.publish(marker)
- print("ball " ,marker)
- if (self.pf_coord2 is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 100
- marker.type = marker.ARROW
- marker.action = marker.ADD
- marker.pose.position.x = self.pf_coord2[0]
- marker.pose.position.y = self.pf_coord2[1]
- quaternion = tf_transformations.quaternion_about_axis(self.pf_coord2[2], (0,0,1))
- marker.pose.orientation.x = quaternion[0]
- marker.pose.orientation.y = quaternion[1]
- marker.pose.orientation.z = quaternion[2]
- marker.pose.orientation.w = quaternion[3]
- marker.pose.orientation
- marker.scale.x = .02
- marker.scale.y = .02
- marker.scale.z = .02
- marker.color.a = 1.0
- marker.color.r = 0.0
- marker.color.g = 0.9
- marker.color.b = 1.0
- self.robot_pub2.publish(marker)
- print("robot:", marker)
- if (self.ball_coord2 is not None):
- marker = Marker()
- marker.header.frame_id = 'world'
- marker.id = 101
- marker.type = marker.SPHERE
- marker.action = marker.ADD
- marker.pose.position.x = self.ball_coord2[0]
- marker.pose.position.y = self.ball_coord2[1]
- marker.scale.x = .05
- marker.scale.y = .05
- marker.scale.z = .05
- marker.color.a = 1.0
- marker.color.r = 0.3
- marker.color.g = 0.4
- marker.color.b = 1.0
- self.ball_pub2.publish(marker)
+ if self.pf_coord is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord[0]
+ marker.pose.position.y = self.pf_coord[1]
+ quaternion = tf_transformations.quaternion_about_axis(
+ self.pf_coord[2], (0, 0, 1)
+ )
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = 0.02
+ marker.scale.y = 0.02
+ marker.scale.z = 0.02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 0.2
+ self.robot_pub.publish(marker)
+ print("robot:", marker)
+ if self.ball_coord is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord[0]
+ marker.pose.position.y = self.ball_coord[1]
+ marker.scale.x = 0.05
+ marker.scale.y = 0.05
+ marker.scale.z = 0.05
+ marker.color.a = 1.0
+ marker.color.r = 1.0
+ marker.color.g = 0.4
+ marker.color.b = 0.1
+ self.ball_pub.publish(marker)
+ print("ball ", marker)
+ if self.pf_coord2 is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 100
+ marker.type = marker.ARROW
+ marker.action = marker.ADD
+ marker.pose.position.x = self.pf_coord2[0]
+ marker.pose.position.y = self.pf_coord2[1]
+ quaternion = tf_transformations.quaternion_about_axis(
+ self.pf_coord2[2], (0, 0, 1)
+ )
+ marker.pose.orientation.x = quaternion[0]
+ marker.pose.orientation.y = quaternion[1]
+ marker.pose.orientation.z = quaternion[2]
+ marker.pose.orientation.w = quaternion[3]
+ marker.pose.orientation
+ marker.scale.x = 0.02
+ marker.scale.y = 0.02
+ marker.scale.z = 0.02
+ marker.color.a = 1.0
+ marker.color.r = 0.0
+ marker.color.g = 0.9
+ marker.color.b = 1.0
+ self.robot_pub2.publish(marker)
+ print("robot:", marker)
+ if self.ball_coord2 is not None:
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.id = 101
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.pose.position.x = self.ball_coord2[0]
+ marker.pose.position.y = self.ball_coord2[1]
+ marker.scale.x = 0.05
+ marker.scale.y = 0.05
+ marker.scale.z = 0.05
+ marker.color.a = 1.0
+ marker.color.r = 0.3
+ marker.color.g = 0.4
+ marker.color.b = 1.0
+ self.ball_pub2.publish(marker)
+
def forward_main_cycle(self):
- self.mqttc.loop()
- self.publish_marker()
-
+ self.mqttc.loop()
+ self.publish_marker()
def main(args=None):
rclpy.init(args=args)
- server_node = MqttKondoDummy()
-
+ server_node = MqttKondoDummy()
rclpy.spin(server_node)
@@ -201,5 +189,5 @@ def main(args=None):
rclpy.shutdown()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
diff --git a/src/server_main/test/test_angle_from_quat.py b/src/server_main/test/test_angle_from_quat.py
index d31ae20..598b8ef 100644
--- a/src/server_main/test/test_angle_from_quat.py
+++ b/src/server_main/test/test_angle_from_quat.py
@@ -2,9 +2,10 @@ from pyquaternion import Quaternion
import numpy as np
import math
+
def angle_from_quat(x, y, z, w):
quat = Quaternion(w, x, y, z)
- z_vector = np.array([0., 0., 1.])
+ z_vector = np.array([0.0, 0.0, 1.0])
new_z = quat.rotate(z_vector)
print(new_z)
return math.atan2(new_z[1], new_z[0])
diff --git a/src/server_main/test/test_avg_quat.py b/src/server_main/test/test_avg_quat.py
index 564c1e8..4939b04 100644
--- a/src/server_main/test/test_avg_quat.py
+++ b/src/server_main/test/test_avg_quat.py
@@ -1,12 +1,14 @@
import numpy as np
+
+
def quatWAvg(Q):
- '''
+ """
Averaging Quaternions.
Arguments:
Q(ndarray): an Mx4 ndarray of quaternions.
weights(list): an M elements list, a weight for each quaternion.
- '''
+ """
# Form the symmetric accumulator matrix
A = np.zeros((4, 4))
@@ -16,7 +18,7 @@ def quatWAvg(Q):
for i in range(M):
q = Q[:, i]
# w_i = weights[i]
- A += np.outer(q, q)# rank 1 update
+ A += np.outer(q, q) # rank 1 update
# wSum += w_i
# scale
@@ -25,8 +27,6 @@ def quatWAvg(Q):
# Get the eigenvector corresponding to largest eigen value
return np.linalg.eigh(A)[1][:, -1]
-quat_array = np.array(([[0.707, -0.383],
- [0, 0.0],
- [0, 0.0],
- [0.707, 0.924]]))
-print(quatWAvg(quat_array))
\ No newline at end of file
+
+quat_array = np.array(([[0.707, -0.383], [0, 0.0], [0, 0.0], [0.707, 0.924]]))
+print(quatWAvg(quat_array))
diff --git a/src/server_main/test/test_copyright.py b/src/server_main/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/server_main/test/test_copyright.py
+++ b/src/server_main/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/server_main/test/test_flake8.py b/src/server_main/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/server_main/test/test_flake8.py
+++ b/src/server_main/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/server_main/test/test_pep257.py b/src/server_main/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/server_main/test/test_pep257.py
+++ b/src/server_main/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/simple_approach/launch/approach.launch.py b/src/simple_approach/launch/approach.launch.py
index bf7a25d..f27eb99 100644
--- a/src/simple_approach/launch/approach.launch.py
+++ b/src/simple_approach/launch/approach.launch.py
@@ -3,63 +3,77 @@ import os
from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
class LaunchUnitsCreator:
-
def __init__(self, connections_config_path: str) -> None:
self._config = self._read_connections_config(connections_config_path)
def create_launch(self) -> LaunchDescription:
- return LaunchDescription([
- *self._create_robo_pult_executors(),
- *self._create_proxy_nodes(),
- *self._create_approach_nodes(),
- # *self._create_odometry_estimator_node(),
- ])
+ return LaunchDescription(
+ [
+ *self._create_robo_pult_executors(),
+ *self._create_proxy_nodes(),
+ *self._create_approach_nodes(),
+ # *self._create_odometry_estimator_node(),
+ ]
+ )
@staticmethod
def _read_connections_config(connections_config_path: str) -> dict:
with open(connections_config_path) as connections_config_json:
- conf = json.load(connections_config_json, parse_float=lambda x: str(x), parse_int=lambda x:str(x))
-
+ conf = json.load(
+ connections_config_json,
+ parse_float=lambda x: str(x),
+ parse_int=lambda x: str(x),
+ )
+
return conf
def _create_robo_pult_executors(self) -> list[ExecuteProcess]:
- return [ExecuteProcess(cmd=[
- 'sudo',
- './roboLinuxPult/roboLinuxPult.sh',
- connection["port"],
- connection["device"],
- connection["robot_name"][5:], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
- # found in Taiwan April 2024
- ]) for connection in self._config["connections"].values()]
+ return [
+ ExecuteProcess(
+ cmd=[
+ "sudo",
+ "./roboLinuxPult/roboLinuxPult.sh",
+ connection["port"],
+ connection["device"],
+ connection["robot_name"][
+ 5:
+ ], # it's because of https://robotics.stackexchange.com/questions/74377/running-ros-node-with-a-numeric-parameter-passed-as-string-doesnt-work
+ # found in Taiwan April 2024
+ ]
+ )
+ for connection in self._config["connections"].values()
+ ]
def _create_proxy_nodes(self) -> list[Node]:
print(self._config["connections"])
return [
Node(
- package ="proxy",
- executable ="proxy",
- namespace =f"robot_{id}",
- parameters = [
- {"port": connection["port"] },
- {"robot_name": connection["robot_name"] },
+ package="proxy",
+ executable="proxy",
+ namespace=f"robot_{id}",
+ parameters=[
+ {"port": connection["port"]},
+ {"robot_name": connection["robot_name"]},
],
- ) for id, connection in self._config["connections"].items()
+ )
+ for id, connection in self._config["connections"].items()
]
def _create_approach_nodes(self) -> list[Node]:
return [
Node(
- package ="simple_approach",
- executable ="simple_approach",
- namespace =f"robot_{id}",
- ) for id in self._config["connections"].keys()
+ package="simple_approach",
+ executable="simple_approach",
+ namespace=f"robot_{id}",
+ )
+ for id in self._config["connections"].keys()
]
def _create_odometry_estimator_node(self) -> list[Node]:
@@ -68,13 +82,16 @@ class LaunchUnitsCreator:
package="odometry_estimator",
executable="odometry_estimator",
namespace=f"robot_{id}",
- ) for id in self._config["connections"].keys()
+ )
+ for id in self._config["connections"].keys()
]
-def generate_launch_description():
- connections_config = os.path.join(get_package_share_directory('simple_approach'),
- 'config',
- "connections_config.json")
+def generate_launch_description():
+ connections_config = os.path.join(
+ get_package_share_directory("simple_approach"),
+ "config",
+ "connections_config.json",
+ )
- return LaunchUnitsCreator(connections_config).create_launch()
\ No newline at end of file
+ return LaunchUnitsCreator(connections_config).create_launch()
diff --git a/src/simple_approach/setup.py b/src/simple_approach/setup.py
index 2def61e..99b3e8b 100644
--- a/src/simple_approach/setup.py
+++ b/src/simple_approach/setup.py
@@ -2,29 +2,32 @@ import os
from glob import glob
from setuptools import find_packages, setup
-package_name = 'simple_approach'
+package_name = "simple_approach"
setup(
name=package_name,
- version='0.0.0',
- packages=find_packages(exclude=['test']),
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
- (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ (
+ os.path.join("share", package_name, "launch"),
+ glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+ ),
+ (
+ os.path.join("share", package_name, "config"),
+ glob(os.path.join("config", "*")),
+ ),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='Apache-2.0',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="Apache-2.0",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'simple_approach = simple_approach.SimpleApproach:main'
- ],
+ "console_scripts": ["simple_approach = simple_approach.SimpleApproach:main"],
},
)
diff --git a/src/simple_approach/simple_approach/SimpleApproach.py b/src/simple_approach/simple_approach/SimpleApproach.py
index 66dcf6f..1ce6ef1 100644
--- a/src/simple_approach/simple_approach/SimpleApproach.py
+++ b/src/simple_approach/simple_approach/SimpleApproach.py
@@ -10,36 +10,27 @@ from geometry_msgs.msg import Pose2D
from geometry_msgs.msg import Twist
import numpy as np
-
+
+
@dataclass
class Obstacle:
x: float
y: float
r: float
-class SimpleApproach(Node):
+class SimpleApproach(Node):
def __init__(self) -> None:
- super().__init__('simple_approach')
-
- self._goal_cmd_vel_publisher = self.create_publisher(
- Twist,
- 'plan_cmd_vel',
- 10
- )
+ super().__init__("simple_approach")
+
+ self._goal_cmd_vel_publisher = self.create_publisher(Twist, "plan_cmd_vel", 10)
self._filtered_pose_subscriber = self.create_subscription(
- Pose2D,
- 'filtered_pose',
- self._localization_callback,
- 10
+ Pose2D, "filtered_pose", self._localization_callback, 10
)
self._goal_pose_subscriber = self.create_subscription(
- Pose2D,
- 'plan_goal',
- self._approach_callback,
- 10
+ Pose2D, "plan_goal", self._approach_callback, 10
)
self.current_position = Pose2D()
@@ -47,99 +38,125 @@ class SimpleApproach(Node):
def _localization_callback(self, msg: Pose2D):
self.current_position = msg
-
+
def if_bigger(self: Obstacle, other: Obstacle, init: Pose2D):
l_self = (self.x - init.x) ** 2 + (self.y - init.y) ** 2
l_other = (other.x - init.x) ** 2 + (other.y - init.y) ** 2
return l_self < l_other
-
-
+
def partition(self, low, high, path_point: Pose2D):
pivot = self.obstacles[high]
i = low - 1
for j in range(low, high):
-
if self.if_bigger(self.obstacles[j], pivot, path_point):
i += 1
- (self.obstacles[i], self.obstacles[j]) = (self.obstacles[j], self.obstacles[i])
-
- (self.obstacles[i + 1], self.obstacles[high]) = (self.obstacles[high], self.obstacles[i + 1])
-
+ (self.obstacles[i], self.obstacles[j]) = (
+ self.obstacles[j],
+ self.obstacles[i],
+ )
+
+ (self.obstacles[i + 1], self.obstacles[high]) = (
+ self.obstacles[high],
+ self.obstacles[i + 1],
+ )
+
return i + 1
-
-
+
def quicksort(self, low, high, path_point: Pose2D):
if low < high:
pi = self.partition(low, high, path_point)
self.quicksort(low, pi - 1, path_point)
self.quicksort(pi + 1, high, path_point)
-
-
+
def tangent_line(self, robot: Pose2D, obstacle: Obstacle):
# r = r0 + p * t, where r, r0, p -- 2d vectors, t -- parameter
r = [obstacle.x - robot.x, obstacle.y - robot.y]
l_n = obstacle.r / (r[0] ** 2 + r[1] ** 2) ** 0.5
n = [r[1] * l_n, -r[0] * l_n]
r1, r2 = [r[0] + n[0], r[1] + n[1]], [r[0] - n[0], r[1] - n[1]]
- k1, k2 = r1[1]/r1[0], r2[1]/r2[0]
+ k1, k2 = r1[1] / r1[0], r2[1] / r2[0]
c1, c2 = robot.y - k1 * robot.x, robot.y - k2 * robot.x
return [k1, c1], [k2, c2]
-
-
+
def intersection_point(self, line1: List[float], line2: List[float]):
- return [(line2[1] - line1[1]) / (line1[0] - line2[0]),
- line1[0] * (line2[1] - line1[1]) / (line1[0] - line2[0]) + line1[1]]
-
-
+ return [
+ (line2[1] - line1[1]) / (line1[0] - line2[0]),
+ line1[0] * (line2[1] - line1[1]) / (line1[0] - line2[0]) + line1[1],
+ ]
+
@staticmethod
def is_intersect(path_point: Pose2D, target: Pose2D, obstacle: Obstacle):
if path_point.x != target.x and path_point.y != target.y:
-
A, B, C = (
- 1 / (target.x - path_point.x),
- -1/(target.y - path_point.y),
- path_point.y / (target.y - path_point.y) - path_point.x / (target.x - path_point.x)
+ 1 / (target.x - path_point.x),
+ -1 / (target.y - path_point.y),
+ path_point.y / (target.y - path_point.y)
+ - path_point.x / (target.x - path_point.x),
)
elif path_point.x != target.x:
A, B, C = 0, 1, path_point.y
-
+
else:
A, B, C = 1, 0, path_point.x
-
- d = abs(A * obstacle.x + B * obstacle.y + C) / (A ** 2 + B ** 2) ** 0.5
+
+ d = abs(A * obstacle.x + B * obstacle.y + C) / (A**2 + B**2) ** 0.5
phi = atan2(target.y - path_point.y, target.x - path_point.x)
-
+
x = obstacle.x - d * sin(phi)
y = obstacle.y + d * cos(phi)
-
- return abs(obstacle.r - d) > 0.01 and min(path_point.x, target.x) < x < max(path_point.x, target.x) and min(path_point.y, target.y) < y < max(path_point.y, target.y)
-
-
+
+ return (
+ abs(obstacle.r - d) > 0.01
+ and min(path_point.x, target.x) < x < max(path_point.x, target.x)
+ and min(path_point.y, target.y) < y < max(path_point.y, target.y)
+ )
+
def rotate(self, init: Pose2D, target: Pose2D):
if abs(init.x - target.x) < 0.02 and abs(init.y - target.y) < 0.02:
yaw_t = target.theta
else:
- yaw_t = atan2(target.y - init.y, target.x - init.x) # returns value from -pi to pi
+ yaw_t = atan2(
+ target.y - init.y, target.x - init.x
+ ) # returns value from -pi to pi
return yaw_t
-
+
@staticmethod
def is_intersect_circle(obstacle1: Obstacle, obstacle2: Obstacle):
- return ((obstacle1.x - obstacle2.x) ** 2 + (obstacle1.y - obstacle2.y) ** 2) ** 0.5 <= obstacle1.r + obstacle2.r
-
+ return (
+ (obstacle1.x - obstacle2.x) ** 2 + (obstacle1.y - obstacle2.y) ** 2
+ ) ** 0.5 <= obstacle1.r + obstacle2.r
+
# TODO: refactor the function
def obstacle_avoidance(self, robot: Pose2D, target: Pose2D, obstacle: Obstacle):
- if (target.x - obstacle.x) ** 2 + (target.y - obstacle.y) ** 2 > obstacle.r ** 2 and \
- (robot.x - obstacle.x) ** 2 + (robot.y - obstacle.y) ** 2 > obstacle.r ** 2:
+ if (target.x - obstacle.x) ** 2 + (
+ target.y - obstacle.y
+ ) ** 2 > obstacle.r**2 and (robot.x - obstacle.x) ** 2 + (
+ robot.y - obstacle.y
+ ) ** 2 > obstacle.r**2:
line1_r2o, line2_r2o = self.tangent_line(robot, obstacle)
line1_o2t, line2_o2t = self.tangent_line(target, obstacle)
- intersec_points = [self.intersection_point(line1_r2o, line1_o2t), self.intersection_point(line1_r2o, line2_o2t),
- self.intersection_point(line2_r2o, line1_o2t), self.intersection_point(line2_r2o, line2_o2t)]
+ intersec_points = [
+ self.intersection_point(line1_r2o, line1_o2t),
+ self.intersection_point(line1_r2o, line2_o2t),
+ self.intersection_point(line2_r2o, line1_o2t),
+ self.intersection_point(line2_r2o, line2_o2t),
+ ]
path_length = []
for i in range(4):
- path_length.append(((robot.x - intersec_points[i][0]) ** 2 + (robot.y - intersec_points[i][1]) ** 2) ** 0.5 +
- ((intersec_points[i][0] - target.x) ** 2 + (intersec_points[i][1] - target.y) ** 2) ** 0.5)
+ path_length.append(
+ (
+ (robot.x - intersec_points[i][0]) ** 2
+ + (robot.y - intersec_points[i][1]) ** 2
+ )
+ ** 0.5
+ + (
+ (intersec_points[i][0] - target.x) ** 2
+ + (intersec_points[i][1] - target.y) ** 2
+ )
+ ** 0.5
+ )
index_minpath = path_length.index(min(path_length))
x, y = intersec_points[index_minpath]
@@ -149,7 +166,7 @@ class SimpleApproach(Node):
interjacent_point.theta = 0
yaw1 = self.rotate(robot, interjacent_point)
- #yaw2 = rotate(interjacent_point, target)
+ # yaw2 = rotate(interjacent_point, target)
pos1 = Pose2D()
pos1.x = robot.x
@@ -162,14 +179,22 @@ class SimpleApproach(Node):
pos2.theta = yaw1
return pos1, pos2
-
- elif (target.x - obstacle.x) ** 2 + (target.y - obstacle.y) ** 2 <= obstacle.r ** 2 and \
- (robot.x - obstacle.x) ** 2 + (robot.y - obstacle.y) ** 2 > robot.r ** 2:
+
+ elif (target.x - obstacle.x) ** 2 + (
+ target.y - obstacle.y
+ ) ** 2 <= obstacle.r**2 and (robot.x - obstacle.x) ** 2 + (
+ robot.y - obstacle.y
+ ) ** 2 > robot.r**2:
line1, line2 = self.tangent_line(robot, obstacle)
- intersec_points = [[target.x, line1[0] * target.x + line1[1]], [target.x, line2[0] * target.x + line2[1]]]
+ intersec_points = [
+ [target.x, line1[0] * target.x + line1[1]],
+ [target.x, line2[0] * target.x + line2[1]],
+ ]
path_length = []
for i in range(2):
- path_length.append((intersec_points[i][0] ** 2 + intersec_points[i][1] ** 2) ** 0.5)
+ path_length.append(
+ (intersec_points[i][0] ** 2 + intersec_points[i][1] ** 2) ** 0.5
+ )
index_minpath = path_length.index(min(path_length))
x, y = intersec_points[index_minpath]
@@ -177,7 +202,7 @@ class SimpleApproach(Node):
interjacent_point.x = x
interjacent_point.y = y
interjacent_point.theta = 0
-
+
yaw1 = self.rotate(robot, interjacent_point)
pos1 = Pose2D()
@@ -192,7 +217,6 @@ class SimpleApproach(Node):
return pos1, pos2
else:
-
pos1 = Pose2D()
pos1.x = robot.x
pos1.y = robot.y
@@ -204,8 +228,7 @@ class SimpleApproach(Node):
pos2.theta = self.rotate(robot, target)
return pos1, pos2
-
-
+
def all_obstacle_avoidance(self, path_point: Pose2D, target: Pose2D):
self.quicksort(self.obstacles, 0, len(self.obstacles) - 1, path_point)
@@ -213,32 +236,40 @@ class SimpleApproach(Node):
for i in range(len(self.obstacles)):
for j in range(i + 1, len(self.obstacles)):
-
if self.is_intersect_circle(self.obstacles[i], self.obstacles[j]):
- self.obstacles[i] = Obstacle(0.5 * (self.bstacles[i].x + self.obstacles[j].x),
- 0.5 * (self.obstacles[i].y + self.obstacles[j].y),
- 0.5 * (self.obstacles[i].r + self.obstacles[j].r + ((self.obstacles[i].x - self.obstacles[j].x) ** 2 +
- (self.obstacles[i].y - self.obstacles[j].y) ** 2) ** 0.5))
-
+ self.obstacles[i] = Obstacle(
+ 0.5 * (self.bstacles[i].x + self.obstacles[j].x),
+ 0.5 * (self.obstacles[i].y + self.obstacles[j].y),
+ 0.5
+ * (
+ self.obstacles[i].r
+ + self.obstacles[j].r
+ + (
+ (self.obstacles[i].x - self.obstacles[j].x) ** 2
+ + (self.obstacles[i].y - self.obstacles[j].y) ** 2
+ )
+ ** 0.5
+ ),
+ )
+
self.obstacles[j] = Obstacle(0, 0, 0)
for obstacle in self.obstacles:
-
if self.is_intersect(path_point, target, obstacle):
pos1, pos2 = self.obstacle_avoidance(path_point, target, obstacle)
added_poses.append(pos1)
added_poses.append(pos2)
return added_poses
return added_poses
-
-
+
def control_poses(self, target):
positions = [self.current_position]
if len(self.obstacles) != 0:
-
while (positions[-1].x, positions[-1].y) != (target.x, target.y):
- new_poses = self.all_obstacle_avoidance(positions[-1], target, self.obstacles)
+ new_poses = self.all_obstacle_avoidance(
+ positions[-1], target, self.obstacles
+ )
if len(new_poses) != 0:
pos1, pos2 = new_poses[0], new_poses[1]
@@ -266,47 +297,55 @@ class SimpleApproach(Node):
new_position.x = target.x
new_position.y = target.y
new_position.theta = self.rotate(positions[-1], target)
-
+
positions.append(new_position)
return positions
-
-
+
def _approach_callback(self, msg: Pose2D):
positions = self.control_poses(msg)
print("HELLO")
# TODO: fit constants and create params
v_linear = 100.0
omega_z = 100.0
-
if len(positions) > 1 and abs(positions[0].theta - positions[1].theta) < 0.15:
omega_z = 0.0
- if len(positions) > 1 and ((positions[0].x - positions[1].x) ** 2 + (positions[0].y - positions[1].y) ** 2) ** 0.5 < 0.02:
+ if (
+ len(positions) > 1
+ and (
+ (positions[0].x - positions[1].x) ** 2
+ + (positions[0].y - positions[1].y) ** 2
+ )
+ ** 0.5
+ < 0.02
+ ):
v_linear = 0.0
-
- robot_coords = np.array([
- [self.current_position.x],
- [self.current_position.y]
- ])
-
- goal_coords = np.array([
- [positions[1].x],
- [positions[1].y]
- ])
-
- M = np.array([
- [np.cos(self.current_position.theta), -np.sin(self.current_position.theta)],
- [np.sin(self.current_position.theta), np.cos(self.current_position.theta)]
- ])
+
+ robot_coords = np.array([[self.current_position.x], [self.current_position.y]])
+
+ goal_coords = np.array([[positions[1].x], [positions[1].y]])
+
+ M = np.array(
+ [
+ [
+ np.cos(self.current_position.theta),
+ -np.sin(self.current_position.theta),
+ ],
+ [
+ np.sin(self.current_position.theta),
+ np.cos(self.current_position.theta),
+ ],
+ ]
+ )
M_inv = M.transpose()
- coords_ = M_inv@(goal_coords - robot_coords)
+ coords_ = M_inv @ (goal_coords - robot_coords)
phi = atan2(coords_[1][0], coords_[0][0])
self.get_logger().info(f"PHI: {phi}")
-
+
twist = Twist()
twist.linear.x = v_linear * cos(phi)
@@ -315,10 +354,15 @@ class SimpleApproach(Node):
twist.angular.x = 0.0
twist.angular.y = 0.0
- twist.angular.z = omega_z * np.sign(positions[0].theta - positions[1].theta) if len(positions) > 1 else 0.0
-
+ twist.angular.z = (
+ omega_z * np.sign(positions[0].theta - positions[1].theta)
+ if len(positions) > 1
+ else 0.0
+ )
+
self._goal_cmd_vel_publisher.publish(twist)
-
+
+
def main(args=None):
rclpy.init(args=args)
@@ -333,5 +377,5 @@ def main(args=None):
rclpy.shutdown()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
diff --git a/src/simple_approach/test/test_copyright.py b/src/simple_approach/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/simple_approach/test/test_copyright.py
+++ b/src/simple_approach/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/simple_approach/test/test_flake8.py b/src/simple_approach/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/simple_approach/test/test_flake8.py
+++ b/src/simple_approach/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/simple_approach/test/test_pep257.py b/src/simple_approach/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/simple_approach/test/test_pep257.py
+++ b/src/simple_approach/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
diff --git a/src/strategy/launch/game.launch.py b/src/strategy/launch/game.launch.py
index d08c0be..bd4cbbc 100644
--- a/src/strategy/launch/game.launch.py
+++ b/src/strategy/launch/game.launch.py
@@ -144,7 +144,6 @@ def generate_launch_description():
("/ball_on_image", "/camera2/ball_on_image"),
("/image_raw", "/camera2/image_raw"),
],
-
output="screen",
),
*localization_nodes,
diff --git a/src/strategy/setup.py b/src/strategy/setup.py
index e7a11a7..0c464f3 100644
--- a/src/strategy/setup.py
+++ b/src/strategy/setup.py
@@ -1,26 +1,23 @@
from setuptools import setup
-package_name = 'strategy'
+package_name = "strategy"
setup(
name=package_name,
- version='0.0.0',
+ version="0.0.0",
packages=[package_name],
data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
],
- install_requires=['setuptools'],
+ install_requires=["setuptools"],
zip_safe=True,
- maintainer='root',
- maintainer_email='root@todo.todo',
- description='TODO: Package description',
- license='TODO: License declaration',
- tests_require=['pytest'],
+ maintainer="root",
+ maintainer_email="root@todo.todo",
+ description="TODO: Package description",
+ license="TODO: License declaration",
+ tests_require=["pytest"],
entry_points={
- 'console_scripts': [
- 'strategy_node = strategy.strategy_node:main'
- ],
+ "console_scripts": ["strategy_node = strategy.strategy_node:main"],
},
)
diff --git a/src/strategy/strategy/solo_robot_strategy.py b/src/strategy/strategy/solo_robot_strategy.py
index b7ebb12..3249649 100644
--- a/src/strategy/strategy/solo_robot_strategy.py
+++ b/src/strategy/strategy/solo_robot_strategy.py
@@ -95,15 +95,15 @@ def kick_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
return Cmd(2, KICK_LEFT)
-def stand_up_action(robot_info:RobotInfo):
+def stand_up_action(robot_info: RobotInfo):
return Cmd(2, STAND_UP)
-def wait_action(robot_info:RobotInfo) -> tp.Optional[Cmd]:
+def wait_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
return None
-def go_home_action(robot_info:RobotInfo) -> tp.Optional[Cmd]:
+def go_home_action(robot_info: RobotInfo) -> tp.Optional[Cmd]:
# approach to home position
return None
@@ -227,6 +227,7 @@ class Robot(object):
self.state_transition_dict = state_transition_dict
self.robot_info = start_robot_info
self.cmd = None
+
def set_robot_info(self, robot_info):
self.robot_info = robot_info
@@ -268,7 +269,7 @@ class Robot(object):
self.state = state
break
cmd = self.action(self.robot_info)
- if (cmd is not None):
+ if cmd is not None:
self.cmd = cmd
def change_state(self, state: RobotState) -> None:
@@ -298,10 +299,12 @@ behaviour_dict = {}
@dataclass
class RobotTopicConfig:
robot_name: str
- localization_topic: str # "/robot_" + robot_id + "/... "
- low_level_topic: str # "/robot_" + robot_id + "/..."
- approach_topic: str # "/robot_" + robot_id + "/..."
+ localization_topic: str # "/robot_" + robot_id + "/... "
+ low_level_topic: str # "/robot_" + robot_id + "/..."
+ approach_topic: str # "/robot_" + robot_id + "/..."
fallen_topic: str
+
+
class RobotFabrica:
def __init__(self, config):
self.config = config
@@ -316,8 +319,9 @@ class RobotFabrica:
RobotTopicConfig(
robot_name,
robot_name + "/filtered_pose",
- robot_name + "/cmd", robot_name + "/plan_goal",
- robot_name + "/fallen"
+ robot_name + "/cmd",
+ robot_name + "/plan_goal",
+ robot_name + "/fallen",
)
)
role_set = {role.value for role in Role}
@@ -336,7 +340,13 @@ class RobotFabrica:
home_pose.x = -0.5 + 0.5 * int(robot_id)
home_pose.theta = 1.57
default_robot_info = RobotInfo(
- Pose2D(), Pose2D(), GameState.START, Role.ATTACKER, Pose2D(), home_pose, False
+ Pose2D(),
+ Pose2D(),
+ GameState.START,
+ Role.ATTACKER,
+ Pose2D(),
+ home_pose,
+ False,
)
self.robots["robot_" + robot_id] = Robot(
Role.ATTACKER,
@@ -371,9 +381,11 @@ class Strategy(object):
self.game_state = state
for robot in self.robots.values():
robot.set_game_state(state)
- def set_fallen(self, name:str , is_fallen:bool):
+
+ def set_fallen(self, name: str, is_fallen: bool):
self.robots[name].set_fallen(is_fallen)
pass
+
def step(self):
for robot in self.robots.values():
robot.step()
diff --git a/src/strategy/strategy/strategy_node.py b/src/strategy/strategy/strategy_node.py
index 79a8a21..998ce39 100644
--- a/src/strategy/strategy/strategy_node.py
+++ b/src/strategy/strategy/strategy_node.py
@@ -1,4 +1,3 @@
-
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose2D
from std_msgs.msg import Int32
@@ -8,14 +7,8 @@ from rclpy.node import Node
from solo_robot_strategy import Strategy, Cmd, CmdType, RobotFabrica
-robots_config = {
- "robots":{
- "0":
- {
+robots_config = {"robots": {"0": {}}}
- }
- }
-}
class StrategyNode(Node):
def __init__(self):
@@ -44,10 +37,12 @@ class StrategyNode(Node):
),
self.queue_size,
)
- self.create_subscription(Int32,
- config.low_level_topic,
- lambda msg: self.strategy.set_fallen(msg.data),
- self.queue_size)
+ self.create_subscription(
+ Int32,
+ config.low_level_topic,
+ lambda msg: self.strategy.set_fallen(msg.data),
+ self.queue_size,
+ )
self.approach_pubs[config.robot_name] = self.create_publisher(
Pose2D, config.approach_topic, self.queue_size
)
@@ -73,7 +68,7 @@ class StrategyNode(Node):
msg = self._create_low_lvl_msg(cmd)
self.low_lvl_pubs[robot_name].publish(msg)
- def _create_low_lvl_msg(cmd:Cmd)->Int32:
+ def _create_low_lvl_msg(cmd: Cmd) -> Int32:
msg = Int32()
msg.data = cmd.data
return msg
@@ -83,7 +78,7 @@ class StrategyNode(Node):
msg.x = cmd.data.x
msg.y = cmd.data.y
msg.theta = cmd.data.theta
- return msg # what is it
+ return msg # what is it
def _msg_preprocess(self, msg: Pose2D) -> Pose2D:
return msg
diff --git a/src/strategy/strategy/strategy_test.py b/src/strategy/strategy/strategy_test.py
index 046f0eb..863deb5 100644
--- a/src/strategy/strategy/strategy_test.py
+++ b/src/strategy/strategy/strategy_test.py
@@ -1,68 +1,64 @@
from solo_robot_strategy import RobotState, GameState, RobotFabrica, Strategy
from dataclasses import dataclass
+
@dataclass
class Pose2D:
x: float
y: float
theta: float
-solo_config = {
- "robots":
- {
- "0":{None}
- }
-}
+
+solo_config = {"robots": {"0": {None}}}
+
def test_dummy_strategy():
- fabrica = RobotFabrica(solo_config)
+ fabrica = RobotFabrica(solo_config)
strategy = Strategy(fabrica)
strategy.change_game_state(GameState.RUN)
- strategy.set_robot_pose("robot_0", Pose2D(0., 0.0, 0.))
+ strategy.set_robot_pose("robot_0", Pose2D(0.0, 0.0, 0.0))
strategy.step()
strategy.step()
- strategy.set_ball_pose( Pose2D(0.,1., 0.))
+ strategy.set_ball_pose(Pose2D(0.0, 1.0, 0.0))
strategy.step()
- assert (strategy.robots["robot_0"].state == RobotState.APPROACH)
+ assert strategy.robots["robot_0"].state == RobotState.APPROACH
strategy.set_robot_pose("robot_0", Pose2D(0.0, 1.00001, 1.57))
strategy.step()
print(strategy.robots["robot_0"].state)
- assert (strategy.robots["robot_0"].state == RobotState.KICK)
+ assert strategy.robots["robot_0"].state == RobotState.KICK
strategy.step()
- assert (strategy.robots["robot_0"].state == RobotState.WAIT)
+ assert strategy.robots["robot_0"].state == RobotState.WAIT
+
def test_full_strategy():
- fabrica = RobotFabrica(solo_config)
+ fabrica = RobotFabrica(solo_config)
strategy = Strategy(fabrica)
strategy.change_game_state(GameState.START)
- strategy.set_robot_pose("robot_0", Pose2D(0., 0.0, 0.))
+ strategy.set_robot_pose("robot_0", Pose2D(0.0, 0.0, 0.0))
strategy.step()
strategy.step()
# assert(strategy.robots["robot_0"].state == RobotState.GO_HOME)
# strategy.set_robot_pose("robot_0", Pose2D(-0.5, -0.5, 1.57))
# strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ assert strategy.robots["robot_0"].state == RobotState.WAIT
strategy.change_game_state(GameState.RUN)
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ assert strategy.robots["robot_0"].state == RobotState.APPROACH
strategy.set_robot_pose("robot_0", Pose2D(-0.5, 1.0, 1.57))
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ assert strategy.robots["robot_0"].state == RobotState.APPROACH
strategy.set_fallen("robot_0", True)
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.FALL)
+ assert strategy.robots["robot_0"].state == RobotState.FALL
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.FALL)
+ assert strategy.robots["robot_0"].state == RobotState.FALL
strategy.set_fallen("robot_0", False)
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ assert strategy.robots["robot_0"].state == RobotState.WAIT
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.APPROACH)
+ assert strategy.robots["robot_0"].state == RobotState.APPROACH
strategy.change_game_state(GameState.PAUSED)
strategy.step()
- assert(strategy.robots["robot_0"].state == RobotState.WAIT)
+ assert strategy.robots["robot_0"].state == RobotState.WAIT
strategy.change_game_state(GameState.START)
# assert(strategy.robots["robot_0"].state == RobotState.GO_HOME)
-
-
-
\ No newline at end of file
diff --git a/src/strategy/test/test_copyright.py b/src/strategy/test/test_copyright.py
index 97a3919..ceffe89 100644
--- a/src/strategy/test/test_copyright.py
+++ b/src/strategy/test/test_copyright.py
@@ -17,9 +17,11 @@ import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.skip(
+ reason="No copyright header has been placed in the generated source file."
+)
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found errors"
diff --git a/src/strategy/test/test_flake8.py b/src/strategy/test/test_flake8.py
index 27ee107..ee79f31 100644
--- a/src/strategy/test/test_flake8.py
+++ b/src/strategy/test/test_flake8.py
@@ -20,6 +20,6 @@ import pytest
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/strategy/test/test_pep257.py b/src/strategy/test/test_pep257.py
index b234a38..a2c3deb 100644
--- a/src/strategy/test/test_pep257.py
+++ b/src/strategy/test/test_pep257.py
@@ -19,5 +19,5 @@ import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
+ rc = main(argv=[".", "test"])
+ assert rc == 0, "Found code style errors / warnings"
--
GitLab