ROS 2 - Basic Programming & Practice

1import rclpy
2from rclpy.node import Node
3import math
4from rclpy.qos import QoSProfile
5
6from sfr_coursework1_interface_package.msg import WheelAngularVelocities, TaskSpacePose
7from sfr_coursework1_interface_package.srv import TurnRobotOn, TurnRobotOff
8
9
10class ControllerNode(Node):
11    def __init__(self):
12        super().__init__("controller_node")
13
14        # Assigned from coursework spreadsheet
15        self.desired_angle_deg = 124.0
16        self.desired_angle_rad = math.radians(self.desired_angle_deg)
17
18        # Robot physical parameters
19        self.r = 0.09
20        self.l = 0.28
21
22        # Robot state
23        self.current_phi = 0.0
24        self.current_x = 0.0
25        self.current_y = 0.0
26
27        self.rotation_done = False
28        self.translation_done = False
29
30        self.start_x = None
31        self.start_y = None
32
33        qos = QoSProfile(depth=10)
34
35        # Publisher for wheel angular velocities
36        self.pub = self.create_publisher(
37            WheelAngularVelocities,
38            "robot/wheel_angular_velocities",
39            qos
40        )
41
42        # Subscriber for pose feedback
43        self.create_subscription(
44            TaskSpacePose,
45            "robot/task_space_pose",
46            self.pose_callback,
47            qos
48        )
49
50        # Service clients
51        self.on_client = self.create_client(TurnRobotOn, "robot/turn_robot_on")
52        self.off_client = self.create_client(TurnRobotOff, "robot/turn_robot_off")
53
54        self.wait_for_services()
55        self.turn_robot_on()
56
57        # Control loop timer
58        self.timer = self.create_timer(0.1, self.control_loop)
59
60        self.get_logger().info(f"Controller node started. Target angle = {self.desired_angle_deg}°")
61
62
63    # -------------------------------------------------------
64    def wait_for_services(self):
65        while not self.on_client.wait_for_service(timeout_sec=1.0):
66            self.get_logger().info("Waiting for turn_robot_on service...")
67
68        while not self.off_client.wait_for_service(timeout_sec=1.0):
69            self.get_logger().info("Waiting for turn_robot_off service...")
70
71
72    # -------------------------------------------------------
73    def turn_robot_on(self):
74        req = TurnRobotOn.Request()
75        future = self.on_client.call_async(req)
76        rclpy.spin_until_future_complete(self, future)
77        self.get_logger().info("Robot turned ON by controller.")
78
79
80    def turn_robot_off(self):
81        req = TurnRobotOff.Request()
82        future = self.off_client.call_async(req)
83        rclpy.spin_until_future_complete(self, future)
84        self.get_logger().info("Robot turned OFF by controller.")
85
86
87    # -------------------------------------------------------
88    def pose_callback(self, msg):
89        self.current_x = msg.x
90        self.current_y = msg.y
91        self.current_phi = msg.phi_z
92
93
94    # -------------------------------------------------------
95    def control_loop(self):
96
97        # ============================
98        # PHASE 1 — ROTATE ROBOT
99        # ============================
100        if not self.rotation_done:
101
102            self.get_logger().info(
103                f"Rotating robot toward {self.desired_angle_deg} degrees... (current={math.degrees(self.current_phi):.2f})"
104            )
105
106            if self.current_phi >= self.desired_angle_rad:
107                # STOP rotation
108                msg = WheelAngularVelocities()
109                msg.left_wheel_angular_velocity = 0.0
110                msg.right_wheel_angular_velocity = 0.0
111                self.pub.publish(msg)
112
113                self.rotation_done = True
114                self.start_x = self.current_x
115                self.start_y = self.current_y
116
117                self.get_logger().info("Rotation complete. Starting translation forward 1 meter.")
118                return
119
120            # Continue rotating
121            omega = 0.5  # rad/s rotation speed
122            v_r = (self.l * omega) / 2
123            v_l = -v_r
124
125            msg = WheelAngularVelocities()
126            msg.left_wheel_angular_velocity = v_l / self.r
127            msg.right_wheel_angular_velocity = v_r / self.r
128            self.pub.publish(msg)
129            return
130
131        # ============================
132        # PHASE 2 — MOVE FORWARD 1 METER
133        # ============================
134        if not self.translation_done:
135
136            dx = self.current_x - self.start_x
137            dy = self.current_y - self.start_y
138            distance = math.sqrt(dx*dx + dy*dy)
139
140            self.get_logger().info(f"Moving forward... Distance = {distance:.3f} m")
141
142            if distance >= 1.0:
143                # STOP
144                msg = WheelAngularVelocities()
145                msg.left_wheel_angular_velocity = 0.0
146                msg.right_wheel_angular_velocity = 0.0
147                self.pub.publish(msg)
148
149                self.get_logger().info("Target distance achieved (1.000 m). Stopping robot.")
150
151                self.translation_done = True
152                self.turn_robot_off()
153                self.get_logger().info("Translation complete. Controller stopping.")
154                self.destroy_timer(self.timer)
155                return
156
157            # Continue straight motion
158            v = 0.1  # m/s
159
160            msg = WheelAngularVelocities()
161            msg.left_wheel_angular_velocity = v / self.r
162            msg.right_wheel_angular_velocity = v / self.r
163            self.pub.publish(msg)
164
165
166# -------------------------------------------------------
167def main(args=None):
168    rclpy.init(args=args)
169    node = ControllerNode()
170
171    try:
172        rclpy.spin(node)
173    except KeyboardInterrupt:
174        node.get_logger().info("Keyboard interrupt received. Shutting down cleanly.")
175
176
177
178if __name__ == "__main__":
179    main()
180