Assignment 1 (Kaushik Kumar)

submissions/kaushik/assignment0.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from turtlesim.msg import Pose
+from geometry_msgs.msg import Twist
+
+class TurtleControlNode(Node):
+
+    def __init__(self):
+        super().__init__("node1")
+        self.cmd_vel_publisher_ = self.create_publisher(Twist , "/turtle1/cmd_vel",10)
+        self.pose_subscriber_ = self.create_subscription(Pose , "/turtle1/pose",self.pose_callback ,10)
+        self.get_logger().info ("Turtle controller has been started.")
+        self.a=-0.5
+
+        self.prev_y=5.4964447021
+
+
+
+
+    def pose_callback(self, pose: Pose):
+        cmd = Twist()
+
+    # Define boundaries
+        x_min, x_max = 5.4964447021, 8.0
+        y_min, y_max = 5.564447021, 8.0
+
+    # Move forward if inside safe area
+        if   pose.y<=5.564447021<=self.prev_y or pose.y>=5.564447021>=self.prev_y :
+                self.a+=0.5
+                print(self.a)
+
+        if y_min  < pose.y < y_max and x_min  < pose.x < x_max :
+            cmd.linear.x = -3.0
+            cmd.angular.z = 0.0
+        else:
+        # Stop or reverse direction near edges
+            # if pose.x==8.008444786071777 and pose.y==5.4964447021:
+            #     self.a+=1
+            #     print(a)
+
+
+            if pose.x > x_max:
+                cmd.linear.x = 0.0
+                cmd.linear.y = -3.0
+
+            if pose.y < y_min:
+                cmd.linear.x = -3.0
+                cmd.linear.y = 0.0
+
+            if pose.x < x_min:
+                cmd.linear.x = 0.0
+                cmd.linear.y = 3.0
+            if pose.y > y_max:
+                cmd.linear.x = 3.0
+                cmd.linear.y = 0.0
+            # if pose.x > x_max:
+            #     cmd.linear.x = 0.0
+            #     cmd.linear.y = -1.0
+            if pose.y> y_max and pose.x>x_max:
+                cmd.linear.x= 0.0
+                cmd.linear.y = -1.0
+    # 8.008444786071777,5.5444445610
+        self.prev_x=pose.x
+        self.prev_y=pose.y
+
+        self.cmd_vel_publisher_.publish(cmd)
+        # self.get_logger().info (a)
+        self.get_logger().info(f"a = {(int)(self.a)}")
+
+
+    # def pose_callback(self ,pose:Pose):
+    #     cmd=Twist()
+    #     if pose.x>9 or pose.x<2 or pose.y>9 or pose.y<2 :
+    #         cmd.linear.x=1.0
+    #         cmd.angular.z=0.9
+    #     else:
+    #         cmd.linear.x=5.0
+    #         cmd.angular.z=0.0
+    #     self.cmd_vel_publisher_.publish(cmd)
+
+
+
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TurtleControlNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__== '__main__':
+    main()