diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py b/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py new file mode 100755 index 0000000000000000000000000000000000000000..ee5ecfa979e10f2389ef5d4565010fa44ca72cb5 --- /dev/null +++ b/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import rospy + +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +import matplotlib.pyplot as plt + +# Global variables to store position data +x_data = [] +y_data = [] + +# Obstacle coordinates +ox = [] +oy = [] + +# Define obstacles +for i in range(-4, 6): + ox.append(-2) + oy.append(i) +for i in range(-4, 6): + ox.append(8) + oy.append(i) +for i in range(-2, 9): + ox.append(i) + oy.append(5) +for i in range(-2, 9): + ox.append(i) + oy.append(-4) +# Table obstacles +for i in range(25, 36, 5): # Range from 2.5 to 3.4 (exclusive), multiplied by 10 for integer range + ox.append(i / 10) # Divide by 10 to get the actual floating-point value + oy.append(-3.25) +for i in range(25, 36, 5): + ox.append(i / 10) + oy.append(-2.75) +for i in range(-325, -275, 50): # Range from -3.25 to 2.74 (exclusive), multiplied by 100 for integer range + ox.append(2.5) + oy.append(i / 100) # Divide by 100 to get the actual floating-point value +for i in range(-325, -275, 50): + ox.append(3.5) + oy.append(i / 100) + +# Additional obstacles +ox1 =[1.973, 2.033, 3.982, 3.982, 5.953, 6.013, 7.956, 7.956, 7.986, 7.986, 0.0, 0.0, -1.99, -1.99, 0.0, 2.315, 3.0, 3.687, 5.045, 7.712, 5.983, 3.982, 2.003, 0.0, -1.99, 7.712] + +oy1 =[0.0, 0.0, 0.955, 1.015, 0.0, 0.0, 0.955, 1.015, -0.106, 2.0, 0.955, 1.015, 0.0, -3.0, -4.523, -3.1, -2.696, -3.1, -4.523, 4.0, 5.474, 5.474, 5.474, 5.474, 3.0, -2.0] +ox.extend(ox1) +oy.extend(oy1) + +def position_callback(msg): + global x_data, y_data + x_data.append(msg.pose.pose.position.x) + y_data.append(msg.pose.pose.position.y) + +def main(): + rospy.init_node('ppf') + rospy.Subscriber('/odom', Odometry, position_callback) + + plt.ion() # Enable interactive mode + fig, ax = plt.subplots() + rate = rospy.Rate(5) # Adjust the rate as needed + + while not rospy.is_shutdown(): + ax.clear() + ax.plot(ox, oy, 'ro', label='Obstacles') # Plot obstacles + ax.plot(x_data, y_data, 'b-', label='Path') + + ax.set_xlabel('X Position') + ax.set_ylabel('Y Position') + ax.set_title('Robot Position Plot') + ax.grid(True) + ax.legend() + plt.pause(0.1) + rate.sleep() + + plt.ioff() + plt.show() + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass