navScript.py

GNU nano 6.2                                                                                                                                                                                                 nav_and_arm.py                                                                                                                                                                                                           
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import time
import math

class NavAndArm(Node):
    def __init__(self):
        super().__init__('nav_and_arm')
        self.navigator = BasicNavigator()
        self.arm_pub = self.create_publisher(Float64, '/arm_joint_position_controller/commands', 10)

        self.current_pose = None
        self.last_movement_time = time.time()
        self.position_tolerance = 0.05  # meters
        self.timeout_seconds = 100.0     # seconds

        # Subscribe to current estimated pose from AMCL
        self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, 10)
        self.get_logger().warn("Assuming 'arm_joint_position_controller' is already active.")

    def pose_callback(self, msg):
        new_pose = msg.pose.pose
        if self.current_pose is not None:
            dx = new_pose.position.x - self.current_pose.position.x
            dy = new_pose.position.y - self.current_pose.position.y
            dist = math.sqrt(dx**2 + dy**2)
            if dist > self.position_tolerance:
                self.last_movement_time = time.time()
        self.current_pose = new_pose

    def send_nav_goal(self, x, y, yaw=0.0):
        goal = PoseStamped()
        goal.header.frame_id = 'map'
        goal.header.stamp = self.get_clock().now().to_msg()
        goal.pose.position.x = x
        goal.pose.position.y = y
        goal.pose.orientation.w = 1.0

        self.navigator.goToPose(goal)
        self.last_movement_time = time.time()

        while not self.navigator.isTaskComplete():
            if time.time() - self.last_movement_time > self.timeout_seconds:
                self.get_logger().error("Robot appears stuck. Cancelling navigation.")
                self.navigator.cancelTask()
                return False
            time.sleep(1.0)

        result = self.navigator.getResult()
        return result == 4  # SUCCEEDED

    def move_arm(self, angle):
        msg = Float64()
        msg.data = angle
        self.arm_pub.publish(msg)
        self.get_logger().info(f"Moved arm to {angle:.2f} rad")
        time.sleep(2)

def main():
    rclpy.init()
    node = NavAndArm()

    try:
        x1 = float(input("Enter X coordinate of pickup: "))
        y1 = float(input("Enter Y coordinate of pickup: "))
        x2 = float(input("Enter X coordinate of dropoff/home: "))
        y2 = float(input("Enter Y coordinate of dropoff/home: "))

        node.get_logger().info("Sending robot to pickup location...")
        pickup_success = node.send_nav_goal(x1, y1)

        if pickup_success:
            node.move_arm(-1.0)  # Simulate pickup
            time.sleep(2)
            node.move_arm(0.0)   # Return arm to default
        else:
            node.get_logger().warn("Pickup failed or timeout reached, skipping pickup.")

        node.get_logger().info("Returning to dropoff/home position...")
        node.send_nav_goal(x2, y2)

    except KeyboardInterrupt:
        node.get_logger().info("Interrupted by user.")
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()