diff --git a/src/azer_robocup_project/Init_params/Real/wifi_params.json b/src/azer_robocup_project/Init_params/Real/wifi_params.json
index b9d65e3c221220750d3737cab177ca30938c7eec..40e13090b9b3cdba5a1bd47c7bf4e57d8f1843b7 100644
--- a/src/azer_robocup_project/Init_params/Real/wifi_params.json
+++ b/src/azer_robocup_project/Init_params/Real/wifi_params.json
@@ -8,5 +8,6 @@
"MQTT_IS_ON": true,
"MQTT_PORT": 1901,
"MQTT_BALL_TOPIC": "starkit1/ball",
- "MQTT_ROBOT_TOPIC": "starkit1/0/robot"
+ "MQTT_ROBOT_TOPIC": "starkit1/0/robot",
+ "MQTT_CAPTAIN_ID_TOPIC": "starkit1/captain_id"
}
diff --git a/src/azer_robocup_project/Soccer/Localisation/class_Glob.py b/src/azer_robocup_project/Soccer/Localisation/class_Glob.py
index e0f04b767e3b31a5d68caf575fd02195983b26b1..43ab3b0c1250662453e82ef15c58b0b567f9bf8a 100644
--- a/src/azer_robocup_project/Soccer/Localisation/class_Glob.py
+++ b/src/azer_robocup_project/Soccer/Localisation/class_Glob.py
@@ -18,6 +18,7 @@ class Glob:
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.captain_id = 1
self.obstacles = []
self.mqttc = None
self.mqtt_ball_coord = None
@@ -63,10 +64,12 @@ class Glob:
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.captain_id_topic = self.wifi_params['MQTT_CAPTAIN_ID_TOPIC']
self.mqttc.set_callback(self.mqtt_callback)
self.mqttc.connect()
self.mqttc.subscribe(self.ball_topic)
self.mqttc.subscribe(self.robot_topic)
+ self.mqttc.subscribe(self.captain_id_topic)
import time
time.sleep(3000)
print ("Successful connection to mqtt: " , self.wifi_params['HOST'] )
@@ -101,6 +104,10 @@ class Glob:
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]
+ elif (topic == bytes(self.captain_id_topic, 'utf-8')):
+ self.captain_id = int(msg)
+ print("Captain's id:", self.captain_id)
+
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
@@ -111,17 +118,19 @@ class Glob:
self.mqttc.connect()
self.mqttc.subscribe(self.ball_topic)
self.mqttc.subscribe(self.robot_topic)
+ self.mqttc.subscribe(self.captain_id_topic)
tmp_coord = self.pf_coord
tmp_ball = self.ball_coord
- while (self.pf_coord == tmp_coord and self.ball_coord == tmp_ball):
+ tmp_captain_id = self.captain_id
+ while (self.pf_coord == tmp_coord and self.ball_coord == tmp_ball and self.captain_id == tmp_captain_id):
self.mqttc.check_msg()
print("Try get mqtt msg")
- print('coord =', self.pf_coord, 'ball =', self.ball_coord)
+ print('coord =', self.pf_coord, 'ball =', self.ball_coord, 'captain_id =', self.captain_id)
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
+ return success_Code, napravl, dist, self.captain_id
def import_strategy_data(self, current_work_directory):
with open(current_work_directory + "Init_params/strategy_data.json", "r") as f:
diff --git a/src/azer_robocup_project/Soccer/strategy.py b/src/azer_robocup_project/Soccer/strategy.py
index 40755d5fa2533a9cc5e290ff89850a018cd99855..d435cd3fa0761be79f32750519d61cd53d77e394 100644
--- a/src/azer_robocup_project/Soccer/strategy.py
+++ b/src/azer_robocup_project/Soccer/strategy.py
@@ -358,6 +358,13 @@ class Player():
return yaw
def forward_main_cycle(self, pressed_button):
+ id = 1
+ if self.first_pressed_button == 6:
+ id = 0
+ elif self.first_pressed_button == 2:
+ id = 1
+ elif self.first_pressed_button == 3:
+ id = 2
self.glob.with_pf = False
second_player_timer = self.motion.utime.time()
self.f = Forward_Vector_Matrix(self.motion, self.local, self.glob)
@@ -386,8 +393,9 @@ class Player():
success_Code = None
napravl = None
dist = None
+ captain_id = None
if (self.glob.mqttc):
- success_Code, napravl, dist = self.glob.update_coord_by_mqtt()
+ success_Code, napravl, dist, captain_id = 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)
@@ -395,35 +403,37 @@ class Player():
# 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)
+
+ if id == captain_id:
+ 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):
diff --git a/src/server_main/scripts/xz_angle_from_quaternion.py b/src/server_main/scripts/xz_angle_from_quaternion.py
new file mode 100644
index 0000000000000000000000000000000000000000..056ab040cfc30c0122f21235040892dc3f5abbbd
--- /dev/null
+++ b/src/server_main/scripts/xz_angle_from_quaternion.py
@@ -0,0 +1,22 @@
+#angle between new z and old x
+
+def q_mult(q1, q2):
+ w1, x1, y1, z1 = q1
+ w2, x2, y2, z2 = q2
+ w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
+ x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
+ y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
+ 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/server_node.py b/src/server_main/server_main/server_node.py
index 444e2114e8240ca6c67cdb2761e96f1088a60984..6b7517540e81e29a8ece1eccc9ebf31890fa0fb3 100644
--- a/src/server_main/server_main/server_node.py
+++ b/src/server_main/server_main/server_node.py
@@ -2,6 +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 std_msgs.msg import Int32
from soccer_vision_2d_msgs.msg import BallArray, Ball
import tf2_ros
@@ -20,6 +21,7 @@ from skspatial.objects import Line, Plane
from pyquaternion import Quaternion
import paho.mqtt.client as mqtt
+import numpy as np
class ServerNode(Node):
def __init__(self):
@@ -58,6 +60,7 @@ class ServerNode(Node):
self.file_to_save = "/home/root/workspace/a.out"
self.create_publisher(PoseStamped, "/robot0_pose", 10)
self.ball_pub = self.create_publisher(PointStamped, "/ball_point", 10)
+ self.captain_id_pub = self.create_publisher(Int32, "/captain_id", 10)
self.declare_parameter("camera_frames", rclpy.Parameter.Type.STRING_ARRAY)
self.camera_frames = self.get_parameter("camera_frames").value
self.camera_models = {} # {"frame_id": PinholeCameraModel}
@@ -75,6 +78,8 @@ class ServerNode(Node):
self.current_aruco_poses = {} # {if: x, y}
self.current_robot_poses = {}
self.world_robots_poses = {}
+ self.dists = {}
+ self.captain_id = 1
self.init_mqqt()
def on_connect(mqttc, obj, flags, rc):
@@ -94,6 +99,7 @@ class ServerNode(Node):
self.broker = self.get_parameter("mqtt_broker").value
self.declare_parameter("mqtt_port", 1900)
self.port = self.get_parameter("mqtt_port").value
+ self.captain_id_topic = self.team_name + "/" + "captain_id"
self.ball_topic = self.team_name + "/" + "ball"
self.robots_topic = {}
for robot_id in self.robot_ids:
@@ -213,22 +219,54 @@ 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))
+ 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()
- # find mid of orientation euler angles
- for pose in poses:
+ 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
- mid_pose.orientation.x = pose.pose.orientation.x # [TODO] Find medium
- mid_pose.orientation.y = pose.pose.orientation.y
- mid_pose.orientation.z = pose.pose.orientation.z
- mid_pose.orientation.w = pose.pose.orientation.w
+
+ 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)
+ print('MID_quat')
+ print(mid_quat)
+ 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):
@@ -255,6 +293,32 @@ class ServerNode(Node):
) # transform aruco to forward
pass
+ self.current_robot_poses = robots_poses
+
+ def compute_distances(self):
+ self.dists = {}
+ for robot_id in self.current_robot_poses.keys:
+ robot_x = self.world_robots_poses[robot_id].position.x
+ robot_y = self.world_robots_poses[robot_id].position.y
+
+ ball_x = self.ball_coordinate_world[0]
+ ball_y = self.ball_coordinate_world[1]
+
+ self.dists[robot_id] = math.sqrt((robot_x - ball_x)**2 + (robot_y - ball_y)**2)
+
+ def find_captain(self):
+ self.compute_distances()
+ if bool(self.dists):
+ self.captain_id = min(self.dists, key=self.dists.get)
+ else:
+ self.captain_id = 0 #goalkeeper
+
+ def team_play(self):
+ if self.captain_id == 0: #goalkeeper
+ pass
+ else:
+ pass
+
def update_ball_pose(self, msg):
self.get_camera_to_world_frame()
print(msg.header.frame_id)
@@ -300,24 +364,22 @@ class ServerNode(Node):
],
)
self.ball_coordinate_world = plane.intersect_line(line)
+
+ #self.ball_coordinate_world = (ray_to_ball_world.point.x, ray_to_ball_world.point.y, ray_to_ball_world.point.z)
+
+ @staticmethod
+ def angle_from_quat(x, y, z, w):
+ quat = Quaternion(w, x, y, z)
+ z_vector = np.array([0., 0., 1.])
+ new_z = quat.rotate(z_vector)
+ return math.atan2(new_z[1], new_z[0])
+
+ def get_yaw_world_rotation(self, pose:Pose) -> float:
+ return self.angle_from_quat(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w)
+
# self.ball_coordinate_world = (ray_to_ball_world.point.x, ray_to_ball_world.point.y, ray_to_ball_world.point.z)
- 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 send_coord_to_robot(self, id):
if id in self.world_robots_poses:
msg = PoseStamped()
@@ -357,6 +419,17 @@ class ServerNode(Node):
self.mqttc.publish(self.ball_topic, ball_msg, 1)
self.count_robots_poses()
+
+ self.find_captain()
+ captain_id_msg = Int32()
+ captain_id_msg.data = self.captain_id
+ self.captain_id_pub.publish(captain_id_msg)
+
+ captain_id_msg = (
+ f"{self.captain_id}"
+ )
+ self.mqttc.publish(self.captain_id_topic, captain_id_msg, 1)
+
for id in self.robot_ids:
self.send_coord_to_robot(id)
self.mqttc.loop()
diff --git a/src/server_main/test/logger.py b/src/server_main/test/logger.py
new file mode 100644
index 0000000000000000000000000000000000000000..58c60f8e58877c92da39e8a3b9e71e711144af53
--- /dev/null
+++ b/src/server_main/test/logger.py
@@ -0,0 +1,51 @@
+
+import matplotlib.pyplot as plt
+import numpy as np
+# 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.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'
+
+ def log_yaw(self, value):
+ self.yaw_log.append(value)
+
+ 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')
+ # 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()
+ self.ax.plot(self.yaw_log)
+ plt.pause(0.5)
+
+ 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/test_angle_from_quat.py b/src/server_main/test/test_angle_from_quat.py
new file mode 100644
index 0000000000000000000000000000000000000000..d31ae207946fd96f24c1adb22952b15a2b81f2c2
--- /dev/null
+++ b/src/server_main/test/test_angle_from_quat.py
@@ -0,0 +1,13 @@
+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.])
+ new_z = quat.rotate(z_vector)
+ print(new_z)
+ return math.atan2(new_z[1], new_z[0])
+
+
+print(angle_from_quat(-0.707, 0, 0, 0.707))
diff --git a/src/server_main/test/test_avg_quat.py b/src/server_main/test/test_avg_quat.py
new file mode 100644
index 0000000000000000000000000000000000000000..564c1e8b4e382bdc9021ebc5ba780bb915d207d8
--- /dev/null
+++ b/src/server_main/test/test_avg_quat.py
@@ -0,0 +1,32 @@
+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))
+ M = Q.shape[1]
+ # wSum = 0
+
+ for i in range(M):
+ q = Q[:, i]
+ # w_i = weights[i]
+ A += np.outer(q, q)# rank 1 update
+ # wSum += w_i
+
+ # scale
+ # A /= wSum
+
+ # 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
diff --git a/src/strategy/launch/game.launch.py b/src/strategy/launch/game.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..d5dbd4fdac9088455a9314209395ef922b688a27
--- /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 0000000000000000000000000000000000000000..d23693e91f15436262ca6f39dc3f32d1296b60df
--- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/strategy/setup.cfg b/src/strategy/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6731b2b86a5b64120d6fa2a86646bb26b5b1f02a
--- /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 0000000000000000000000000000000000000000..e7a11a712592a75790def0c31adbe1d11a9e158b
--- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/strategy/strategy/__pycache__/__init__.cpython-310.pyc b/src/strategy/strategy/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..30d5f0c0e098f222149efe2b318b02d4110a1ffe
Binary files /dev/null and b/src/strategy/strategy/__pycache__/__init__.cpython-310.pyc differ
diff --git a/src/strategy/strategy/solo_robot_strategy.py b/src/strategy/strategy/solo_robot_strategy.py
new file mode 100644
index 0000000000000000000000000000000000000000..d9d79c23f62097aa0779d4c4bc93edd20673225e
--- /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 0000000000000000000000000000000000000000..f7b6dfd2cba7cb81173d31fe77898dc1dbaf4f3e
--- /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 0000000000000000000000000000000000000000..5b5004c5b95ad754b817ef657fc4d92d8cdd826d
--- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/strategy/test/__pycache__/test_copyright.cpython-310-pytest-6.2.5.pyc b/src/strategy/test/__pycache__/test_copyright.cpython-310-pytest-6.2.5.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ffa8b3545fe30ce7220ae46fcad3a9de27f56513
Binary files /dev/null and b/src/strategy/test/__pycache__/test_copyright.cpython-310-pytest-6.2.5.pyc differ
diff --git a/src/strategy/test/__pycache__/test_flake8.cpython-310-pytest-6.2.5.pyc b/src/strategy/test/__pycache__/test_flake8.cpython-310-pytest-6.2.5.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..814f78f72a579a58925b9155b0bdef3832c5a9cd
Binary files /dev/null and b/src/strategy/test/__pycache__/test_flake8.cpython-310-pytest-6.2.5.pyc differ
diff --git a/src/strategy/test/__pycache__/test_pep257.cpython-310-pytest-6.2.5.pyc b/src/strategy/test/__pycache__/test_pep257.cpython-310-pytest-6.2.5.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d69972022a7afb18379182291ce901d9a7bf2523
Binary files /dev/null and b/src/strategy/test/__pycache__/test_pep257.cpython-310-pytest-6.2.5.pyc differ
diff --git a/src/strategy/test/test_copyright.py b/src/strategy/test/test_copyright.py
new file mode 100644
index 0000000000000000000000000000000000000000..97a39196e84db97954341162a6d2e7f771d938c0
--- /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 0000000000000000000000000000000000000000..27ee1078ff077cc3a0fec75b7d023101a68164d1
--- /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 0000000000000000000000000000000000000000..b234a3840f4c5bd38f043638c8622b8f240e1185
--- /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'