Removed UWB and added the CALLMELIDAR command.

ros2/src/robobin/launch/robobin_launch.py

@@ -14,14 +14,6 @@ def generate_launch_description():
 
         ),
     
-        Node(
-            package='robobin',
-            executable='uwb_navigation_node',    
-            name='uwb_navigation_node',
-            output='screen',
-            emulate_tty=True
-        ),
-    
         Node(
             package='robobin',
             executable='imu_node',    
@@ -43,13 +35,7 @@ def generate_launch_description():
             output='screen',
             emulate_tty=True
         ),
-        Node(
-            package='robobin',
-            executable='uwb_pathing_node',    
-            name='uwb_pathing_node',
-            output='screen',
-            emulate_tty=True
-        ),
+
 
         # Add additional nodes
         # Example:

ros2/src/robobin/robobin/api_node.py

@@ -107,6 +107,7 @@ class ApiNode(Node):
                 if result is not None:
                     topic, message = result  # Safely unpack after checking
                     if topic is not None:
+                        self.get_logger().info(f"Received message: {message}")
                         self.get_logger().info(f"Publishing to topic: {topic}")
                         self.publish_to_topic(topic, message)
         except Exception as e:

ros2/src/robobin/robobin/helpers/message_handler.py

@@ -16,8 +16,19 @@ class MessageHandler:
             "CALLME": self.handle_call_me,
             "BPM": self.handle_call_bpm,
             "REQMAP": self.handle_request_map,
-            "LOCATION": self.handle_request_location
+            "LOCATION": self.handle_request_location,
+            "CALLMELIDAR": self.handle_call_me_lidar
         }
+    def handle_call_me_lidar(self, client_socket, message):
+        #CALLMELIDAR 1 (for example)
+        response = b"Requested Number is " + message.encode()
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
+        return "nav_command", "CALLMELIDAR " + message
+
+
     def handle_request_location(self, client_socket, message):
         """Handles the LOC command."""
         response = f"location is {self.api_node.connection_manager.location}".encode()

ros2/src/robobin/robobin/uwb_pathing_node.py

-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist, Point
-from math import sqrt, pow, atan2, radians, sin, cos
-import time
-
-
-class UWBPathingNode(Node):
-    def __init__(self):
-        super().__init__('uwb_pathing_node')
-
-        # Publisher for cmd_vel
-        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
-
-        # Subscriptions
-        self.create_subscription(Point, '/start_location', self.current_location_callback, 10)
-        self.create_subscription(Point, '/target_location', self.target_location_callback, 10)
-
-        # Current and target positions
-        self.current_x = None
-        self.current_y = None
-        self.target_x = None
-        self.target_y = None
-
-        # Navigation thresholds
-        self.distance_threshold = 0.25  # meters
-        self.angle_threshold = radians(0.5)  # radians
-
-        self.kp_linear = 0.5  # Proportional gain for linear movement
-        self.kp_angular = 1.0  # Proportional gain for angular movement
-
-        self.get_logger().info('UWB Pathing Node started and waiting for positions.')
-
-    def current_location_callback(self, msg: Point):
-        """Callback to update the robot's current position."""
-        self.current_x = msg.x
-        self.current_y = msg.y
-        self.get_logger().info(f"Current Location Updated: x={self.current_x:.2f}, y={self.current_y:.2f}")
-        self.check_and_navigate()
-
-    def target_location_callback(self, msg: Point):
-        """Callback to update the target position."""
-        self.target_x = msg.x
-        self.target_y = msg.y
-        self.get_logger().info(f"Target Location Updated: x={self.target_x:.2f}, y={self.target_y:.2f}")
-        self.check_and_navigate()
-
-    def check_and_navigate(self):
-        """Check if both positions are available and navigate to the target."""
-        if self.current_x is not None and self.current_y is not None and self.target_x is not None and self.target_y is not None:
-            self.get_logger().info("Navigating to target...")
-            self.navigate_to_target()
-        else:
-            self.get_logger().warning("Waiting for both current and target positions to be available...")
-
-    def navigate_to_target(self):
-        # Ensure positions are known
-        if self.current_x is None or self.current_y is None or self.target_x is None or self.target_y is None:
-            self.get_logger().warning("Positions not fully known yet.")
-            return
-
-        # Calculate distance and angle to the target
-        displacement_x = self.target_x - self.current_x
-        displacement_y = self.target_y - self.current_y
-        distance_to_target = sqrt(pow(displacement_x, 2) + pow(displacement_y, 2))
-        angle_to_target = atan2(displacement_y, displacement_x)
-
-
-        # Check if we are close enough to the target
-        if distance_to_target <= self.distance_threshold:
-            self.stop_robot()
-            self.get_logger().info("Target reached successfully.")
-            return
-
-        # Calculate yaw error (assuming robot orientation 0 = facing x-axis)
-
-        yaw_error = angle_to_target
-        yaw_error = atan2(sin(yaw_error), cos(yaw_error))  # Normalize angle
-        self.get_logger().info(f"Current Position: x={self.current_x:.2f}, y={self.current_y:.2f}")
-        self.get_logger().info(f"Target Position: x={self.target_x:.2f}, y={self.target_y:.2f}")
-        self.get_logger().info(f"Distance to Target: distance_to_target={distance_to_target:.2f} meters")
-        self.get_logger().info(f"Angle to Target: angle_to_target={angle_to_target:.2f} radians")
-        twist = Twist()
-
-        # Decide on angular velocity first
-        if abs(yaw_error) > self.angle_threshold:
-            angular_speed = self.kp_angular * abs(yaw_error)
-            twist.angular.z = angular_speed if yaw_error > 0 else -angular_speed
-            self.get_logger().info(f"Correcting Heading: yaw_error={yaw_error:.2f} radians")
-        else:
-            # Move forward when aligned with the target
-            linear_speed = self.kp_linear * distance_to_target
-            twist.linear.x = min(0.2, linear_speed)  # Limit max speed
-            self.get_logger().info(f"Moving to Target: distance={distance_to_target:.2f} meters")
-
-        # # Publish movement command
-        # self.cmd_vel_pub.publish(twist)
-
-
-    def stop_robot(self):
-        """Stops the robot by publishing zero velocities."""
-        twist = Twist()
-        self.cmd_vel_pub.publish(twist)
-        time.sleep(0.5)
-        self.get_logger().info("Robot stopped.")
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = UWBPathingNode()
-
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

ros2/src/robobin/setup.py

@@ -26,12 +26,10 @@ setup(
         'console_scripts': [
             'api_node = robobin.api_node:main',
             'motor_control_node = robobin.motor_control_node:main',
-            'uwb_navigation_node = robobin.uwb_navigation_node:main',
             'imu_node = robobin.imu_node:main',
             'motor_node = robobin.motor_control_node:main',
             'encoder_node = robobin.encoder:main',
             'control_feedback = robobin.control_feedback:main',
-            'uwb_pathing_node = robobin.uwb_pathing_node:main',
         ],
     },
 )