diff --git a/examples/area_goals/app.py b/examples/area_goals/app.py index 444a3aa..0dec91d 100755 --- a/examples/area_goals/app.py +++ b/examples/area_goals/app.py @@ -7,7 +7,6 @@ from commlib.msg import PubSubMessage, MessageHeader from pydantic import Field from commlib.node import Node -from commlib.transports.mqtt import ConnectionParameters """_summary_ @@ -25,7 +24,7 @@ class PoseMessage(PubSubMessage): class Robot(Node): - def __init__(self, name, connection_params, pose_uri, velocity=0.5, + def __init__(self, name, connection_params, pose_uri, velocity=1, *args, **kwargs): self.name = name self.pose = PoseMessage() @@ -70,6 +69,15 @@ def publish_pose(self, x, y): if __name__ == '__main__': + if len(sys.argv) != 2: + from commlib.transports.redis import ConnectionParameters + elif sys.argv[1] == "redis": + from commlib.transports.redis import ConnectionParameters + elif sys.argv[1] == "mqtt": + from commlib.transports.mqtt import ConnectionParameters + elif sys.argv[1] == "amqp": + from commlib.transports.amqp import ConnectionParameters + conn_params = ConnectionParameters(reconnect_attempts=0) robot_1 = Robot(name='robot_1', connection_params=conn_params, diff --git a/examples/do_rectangle/app.py b/examples/do_rectangle/app.py new file mode 100755 index 0000000..e10c8d8 --- /dev/null +++ b/examples/do_rectangle/app.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python + +import sys +import time +from typing import Dict + +from commlib.msg import PubSubMessage, MessageHeader +from pydantic import Field +from commlib.node import Node + + +"""_summary_ +This example demonstrates how to create a simple robot that moves to a specific position. +The robot's position is published to the `robot_1.pose` topic. +""" + + +class PoseMessage(PubSubMessage): + # header: MessageHeader = MessageHeader() + position: Dict[str, float] = Field( + default_factory=lambda: {'x': 0.0, 'y': 0.0, 'z': 0.0}) + orientation: Dict[str, float] = Field( + default_factory=lambda: {'x': 0.0, 'y': 0.0, 'z': 0.0}) + + +class Robot(Node): + def __init__(self, name, connection_params, pose_uri, velocity=2, + *args, **kwargs): + self.name = name + self.pose = PoseMessage() + self.pose_uri = pose_uri + self.velocity = velocity + super().__init__(node_name=name, connection_params=connection_params, + *args, **kwargs) + self.pose_pub = self.create_publisher(msg_type=PoseMessage, + topic=self.pose_uri) + + def move(self, x, y, interval=0.5): + vel = self.velocity + current_x = self.pose.position['x'] + current_y = self.pose.position['y'] + distance = ((x - current_x)**2 + (y - current_y)**2)**0.5 + distance_x = x - current_x + distance_y = y - current_y + steps_x = abs(distance_x) / vel + steps_y = abs(distance_y) / vel + direction_x = 1 if distance_x > 0 else -1 + direction_y = 1 if distance_y > 0 else -1 + + for _ in range(int(steps_x / interval)): + current_x += vel * direction_x * interval + # current_x += vel * direction_x * interval + distance = ((x - current_x)**2 + (y - current_y)**2)**0.5 + self.publish_pose(current_x, current_y) + print(f'Current position: x={current_x:.2f}, y={current_y:.2f}') + print(f'Distance to target: {distance:.2f}') + time.sleep(interval) + for _ in range(int(steps_y / interval)): + current_y += vel * direction_y * interval + distance = ((x - current_x)**2 + (y - current_y)**2)**0.5 + self.publish_pose(current_x, current_y) + print(f'Current position: x={current_x:.2f}, y={current_y:.2f}') + print(f'Distance to target: {distance:.2f}') + time.sleep(interval) + + def publish_pose(self, x, y): + self.pose.position['x'] = x + self.pose.position['y'] = y + self.pose_pub.publish(self.pose) + + +if __name__ == '__main__': + if len(sys.argv) != 2: + from commlib.transports.redis import ConnectionParameters + elif sys.argv[1] == "redis": + from commlib.transports.redis import ConnectionParameters + elif sys.argv[1] == "mqtt": + from commlib.transports.mqtt import ConnectionParameters + elif sys.argv[1] == "amqp": + from commlib.transports.amqp import ConnectionParameters + + conn_params = ConnectionParameters(reconnect_attempts=0) + + robot_1 = Robot(name='robot_1', connection_params=conn_params, + pose_uri='robot_1.pose', heartbeats=False) + + try: + robot_1.run() + robot_1.move(5, 0) + robot_1.move(5, 5) + robot_1.move(0, 5) + robot_1.move(0, 0) + robot_1.stop() + except KeyboardInterrupt: + robot_1.stop() diff --git a/examples/do_rectangle/scenario.goal b/examples/do_rectangle/scenario.goal new file mode 100644 index 0000000..f08edd8 --- /dev/null +++ b/examples/do_rectangle/scenario.goal @@ -0,0 +1,82 @@ +Broker LocsysMQTT + host: 'locsys.issel.ee.auth.gr' + port: 8883 + ssl: true + auth: + username: 'sensors' + password: 'issel.sensors' +end + +Broker HomeMQTT + host: 'localhost' + port: 1883 + auth: + username: '' + password: '' +end + +Broker LocalRedis + host: 'localhost' + port: 6379 + ssl: false + auth: + username: '' + password: '' +end + +Entity Robot1Pose + type: sensor + topic: 'robot_1.pose' + source: LocalRedis + attributes: + - position: dict + - orientation: dict +end + +Entity Robot2Pose + type: sensor + topic: 'robot_2.pose' + source: LocalRedis + attributes: + - position: dict + - orientation: dict +end + +Goal Goal_1 + center: Point3D(5, 0, 0) + radius: 0.2 + tag: ENTER +end + +Goal Goal_2 + center: Point3D(5, 5, 0) + radius: 0.2 + tag: ENTER +end + +Goal Goal_3 + center: Point3D(0, 5, 0) + radius: 0.2 + tag: ENTER +end + +Goal Goal_4 + center: Point3D(0, 0, 0) + radius: 0.2 + tag: ENTER +end + +RTMonitor + broker: LocalRedis + eventTopic: "goaldsl.{U_ID}.event" + logsTopic: "goaldsl.{U_ID}.log" +end + +Scenario DoRectangleScenario + goals: + - Goal_1 + - Goal_2 + - Goal_3 + - Goal_4 + concurrent: False +end diff --git a/goal_dsl/codegen.py b/goal_dsl/codegen.py index e15a700..973f99c 100644 --- a/goal_dsl/codegen.py +++ b/goal_dsl/codegen.py @@ -48,7 +48,6 @@ def generate(model_fpath: str, goals = [goal.goal for goal in wgoals] goals = process_goals(goals) - scenario.scoreWeights = [goal.weight for goal in wgoals] out_file = path.join(out_dir, f"{scenario.name}.py") with open(path.join(out_file), 'w') as f: @@ -80,7 +79,6 @@ def generate_str(model_str: str): set_defaults(scenario, rtmonitor, wgoals) goals = process_goals([goal.goal for goal in wgoals]) - scenario.scoreWeights = [goal.weight for goal in wgoals] code = template.render(rtmonitor=rtmonitor, scenario=scenario,