diff --git a/webots_ros2_mavic/webots_ros2_mavic/Gpt.py b/webots_ros2_mavic/webots_ros2_mavic/Gpt.py new file mode 100644 index 000000000..110c3a49d --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/Gpt.py @@ -0,0 +1,111 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +import openai +import gradio as gr +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("--prompt", type=str, default="prompts/mavic_basic.txt") +args = parser.parse_args() + +class LLMIntegrationNode(Node): + def __init__(self): + super().__init__('llm_integration_node') + + # Publisher untuk mengirim respons LLM ke parser + self.command_publisher = self.create_publisher(String, '/llm_to_drone_command', 10) + + # Subscriber untuk menerima perintah dari pengguna + self.user_command_subscription = self.create_subscription( + String, + '/user_command', + self.process_user_command, + 10 + ) + + # Set API key OpenAI + openai.api_key = "API_Key" + + def process_user_command(self, msg): + user_query = msg.data + self.get_logger().info(f"Received user command: {user_query}") + + # Kirim perintah ke LLM dan dapatkan respons + self.llm_response = self.query_llm(user_query) + if self.llm_response: + self.get_logger().info(f"LLM Response: {self.llm_response}") + + # Publikasikan respons ke topic /llm_to_drone_command + self.command_publisher.publish(String(data=self.llm_response)) + else: + self.get_logger().error("Failed to get a response from LLM.") + + + + def query_llm(self, user_query): + try: + response = openai.ChatCompletion.create( + model="gpt-4o", + messages=[ + { + "role": "system", + "content": prompt, + }, + {"role": "user", "content": user_query}, + ] + ) + return response['choices'][0]['message']['content'] + except Exception as e: + self.get_logger().error(f"Error querying LLM: {e}") + return None + + def send_prompt(self, prompt): + # Publikasikan prompt ke topik /user_command + + msg = String() + msg.data = prompt + self.user_command_subscription.callback(msg) + self.get_logger().info(f"Sent Prompt: {prompt}") + + def process_prompt(self, prompt: str): + # Fungsi untuk mengolah input prompt (misalnya memanggil send_prompt) + self.send_prompt(prompt) + return self.llm_response + +with open(args.prompt, "r") as f: + prompt = f.read() + +def main(args=None): + rclpy.init(args=args) + node = LLMIntegrationNode() + # Fungsi untuk menerima input dari Gradio dan memprosesnya + def gradio_interface(prompt): + response=node.process_prompt(prompt) + return response + + # Membuat antarmuka Gradio + interface = gr.Interface( + fn=gradio_interface, # Fungsi yang akan dipanggil + inputs="text", # Input berupa teks + outputs="text", # Output berupa teks + ) + + # Menjalankan UI Gradio di thread terpisah + interface.launch(share=True) + + try: + # node.send_prompt(prompt) + # prompt = "Create waypoints in a good circular shape with the center at x:10, y:10, z:5, a radius of 5, altitude 10, customize the number of waypoints with the shape." + + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + diff --git a/webots_ros2_mavic/webots_ros2_mavic/Gpt_parser.py b/webots_ros2_mavic/webots_ros2_mavic/Gpt_parser.py new file mode 100644 index 000000000..cbb36080e --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/Gpt_parser.py @@ -0,0 +1,280 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from geometry_msgs.msg import Twist, PointStamped, PoseStamped +from visualization_msgs.msg import Marker, MarkerArray +from sensor_msgs.msg import Imu +from nav_msgs.msg import Path +import math +import re +import csv +import json +class LLMResponseParser(Node): + def __init__(self): + super().__init__('llm_response_parser') + + # Subscriber untuk data GPS + self.gps_subscription = self.create_subscription( + PointStamped, + '/Mavic_2_PRO/gps', + self.gps_callback, + 10 + ) + self.imu_subscription = self.create_subscription( + Imu, + '/imu', + self.imu_callback, + 10 + ) + + # Subscriber untuk menerima respons dari LLM + self.llm_response_subscription = self.create_subscription( + String, + '/llm_to_drone_command', + self.process_llm_response, + 10 + ) + # Publisher untuk marker waypoint + self.marker_publisher = self.create_publisher(MarkerArray, '/waypoints_marker', 10) + + + # Publisher untuk mengirim perintah navigasi ke drone + self.command_publisher = self.create_publisher(Twist, '/cmd_vel', 10) + + # Publisher untuk path + self.path_publisher = self.create_publisher(Path, '/drone_path', 10) + + # Inisialisasi variabel + self.current_position = None + self.target_waypoint = None # Waypoint dari LLM + self.reached_target = False + self.orientation = None + self.waypoints = [] # Daftar waypoint + self.current_waypoint_index = 0 + self.markers = MarkerArray() + self.csv_file = 'gps_point_data.csv' + self.initialize_csv() + + # Path untuk RViz + self.path = Path() + self.path.header.frame_id = "map" # Ubah sesuai dengan frame RViz Anda + + def initialize_csv(self): + with open(self.csv_file, mode='w', newline='') as file: + writer = csv.writer(file) + writer.writerow(['Timestamp', 'X', 'Y', 'Z']) # Header file CSV + + def process_llm_response(self, msg): + response = msg.data.strip() + self.get_logger().info(f"Parsing LLM Response: {response}") + #memisahkan json waypoints dengan text lainnya + match = re.search(r'\[\s*{.*?}\s*\]', response, re.S) + if match: + response = match.group(0) # Ambil string JSON yang cocok + response = response.strip() # Hapus spasi atau karakter kosong di awal/akhir + # Parsing respons dari LLM + try: + self.waypoints = self.parse_response(response) + if self.waypoints: + self.get_logger().info(f"Parsed Waypoints: {self.waypoints}") + self.current_waypoint_index = 0 # Reset ke waypoint pertama + # Buat marker untuk setiap waypoint + self.markers.markers = [] # Reset marker sebelumnya + for i, waypoint in enumerate(self.waypoints): + marker = self.create_marker(waypoint, i) + self.markers.markers.append(marker) + + # Publikasikan marker ke RViz + self.marker_publisher.publish(self.markers) + else: + self.get_logger().error("Parsed response is empty or invalid.") + except Exception as e: + self.get_logger().error(f"Error during parsing: {e}") + + def gps_callback(self, msg): + with open(self.csv_file, mode='a', newline='') as file: + writer = csv.writer(file) + timestamp = msg.header.stamp + writer.writerow([ + f"{timestamp.sec}.{timestamp.nanosec}", # Timestamp dari header + msg.point.x, # Koordinat X + msg.point.y, # Koordinat Y + msg.point.z # Koordinat Z + ]) + # Update posisi GPS saat ini + self.current_position = msg.point + # Buat PoseStamped untuk posisi saat ini + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = "map" # Ubah sesuai frame RViz + pose.pose.position.x = self.current_position.x + pose.pose.position.y = self.current_position.y + pose.pose.position.z = self.current_position.z + + # Tambahkan pose ke path + self.path.header.stamp = pose.header.stamp + self.path.poses.append(pose) + + # Batasi jumlah pose dalam path (opsional, untuk efisiensi) + max_path_length = 100 + if len(self.path.poses) > max_path_length: + self.path.poses.pop(0) # Hapus pose tertua + + # Publikasikan path ke RViz + self.path_publisher.publish(self.path) + + self.navigate_to_waypoint() + + def imu_callback(self, imu_msg): + # Update orientasi dari data IMU + orientation_q = imu_msg.orientation + roll, pitch, yaw = self.quaternion_to_euler(orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w) + + #self.get_logger().info(f'Orientation (roll, pitch, yaw): ({roll:.2f}, {pitch:.2f}, {yaw:.2f})') + + # Gunakan data IMU untuk menjaga kestabilan + self.orientation = (roll, pitch, yaw) + + def quaternion_to_euler(self, x, y, z, w): + # Mengonversi quaternion ke Euler angles (roll, pitch, yaw) + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = math.asin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(t3, t4) + + return roll, pitch, yaw + + def create_marker(self, waypoint, marker_id): + marker = Marker() + marker.header.frame_id = "map" # Sesuaikan frame RViz + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = "waypoints" + marker.id = marker_id + marker.type = Marker.SPHERE # Marker berbentuk bola + marker.action = Marker.ADD + + # Koordinat waypoint + marker.pose.position.x = waypoint['x'] + marker.pose.position.y = waypoint['y'] + marker.pose.position.z = waypoint['z'] + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + + # Ukuran marker + marker.scale.x = 0.2 # Diameter bola + marker.scale.y = 0.2 + marker.scale.z = 0.2 + + # Warna marker + marker.color.r = 1.0 # Merah + marker.color.g = 0.0 # Hijau + marker.color.b = 0.0 # Biru + marker.color.a = 1.0 # Transparansi (1 = solid) + + return marker + + def parse_response(self, response): + try: + # Parsing respons sebagai JSON jika berbentuk list + waypoints = json.loads(response) + # Validasi bahwa semua waypoint memiliki x, y, z + if not isinstance(waypoints, list): + self.get_logger().error("Parsed response is not a list of waypoints.") + return None + # Validasi bahwa semua waypoint memiliki x, y, z + for waypoint in waypoints: + if not all(k in waypoint for k in ('x', 'y', 'z')): + self.get_logger().error(f"Invalid waypoint format: {waypoint}") + return None + + return waypoints + except Exception as e: + self.get_logger().error(f"Error parsing response: {e}") + return None + + def navigate_to_waypoint(self): + # Pastikan ada daftar waypoint + if not self.waypoints or self.current_waypoint_index >= len(self.waypoints): + self.get_logger().info("No more waypoints to navigate.") + # Hentikan drone sepenuhnya setelah waypoint terakhir + stop_cmd = Twist() + stop_cmd.linear.x = 0.0 + stop_cmd.linear.y = 0.0 + stop_cmd.linear.z = 0.0 + self.command_publisher.publish(stop_cmd) + return + + # Ambil waypoint saat ini + target_waypoint = self.waypoints[self.current_waypoint_index] + + # Hitung perbedaan posisi dari target + delta_x = -(target_waypoint['x'] - self.current_position.x) + delta_y = -(target_waypoint['y'] - self.current_position.y) + delta_z = target_waypoint['z'] - self.current_position.z + distance = math.sqrt(delta_x**2 + delta_y**2 + delta_z**2) + + # Jarak ke target + distance = math.sqrt(delta_x**2 + delta_y**2 + delta_z**2) + + # Buat perintah kecepatan berdasarkan jarak ke target + move_cmd = Twist() + max_speed = 0.5 # Kecepatan maksimum dalam m/s + speed_factor = min(max_speed, max(0.05, distance / 5)) + if distance > 0.05: # Jika masih jauh dari target + # Normalisasi delta agar drone bergerak ke arah target + move_cmd.linear.x = speed_factor * (delta_x / distance) + move_cmd.linear.y = speed_factor * (delta_y / distance) + move_cmd.linear.z = speed_factor * (delta_z / distance) + else: + move_cmd.linear.x = 0.0 + move_cmd.linear.y = 0.0 + move_cmd.linear.z = 0.0 + + # Koreksi stabilitas dengan data IMU + imu_correction_factor = 0.005 # Faktor koreksi IMU + if self.orientation is not None: + roll, pitch, _ = self.orientation + move_cmd.linear.x -= imu_correction_factor * pitch + move_cmd.linear.y -= imu_correction_factor * roll + + # Publikasikan perintah gerak jika belum mencapai target + if distance > 0.05 or abs(delta_z) > 0.05: + self.command_publisher.publish(move_cmd) + self.get_logger().info(f'Published command to move towards target, horizontal distance: {distance:.5f}, vertical delta: {delta_z:.5f}') + else: + self.get_logger().info(f"Waypoint {self.current_waypoint_index + 1} Reached!") + self.current_waypoint_index += 1 # Pindah ke waypoint berikutnya + # Jika ini adalah waypoint terakhir, hentikan drone sepenuhnya + if self.current_waypoint_index >= len(self.waypoints): + self.get_logger().info("All waypoints reached. Drone is now holding position.") + stop_cmd = Twist() + stop_cmd.linear.x = 0.0 + stop_cmd.linear.y = 0.0 + stop_cmd.linear.z = 0.0 + self.command_publisher.publish(stop_cmd) + +def main(args=None): + rclpy.init(args=args) + node = LLMResponseParser() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/webots_ros2_mavic/webots_ros2_mavic/README.txt b/webots_ros2_mavic/webots_ros2_mavic/README.txt new file mode 100644 index 000000000..136623c1c --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/README.txt @@ -0,0 +1,8 @@ +These are some additional files to run the program. +1. open your terminal in this folder and run : + ros2 launch webots_ros2_mavic robot_launch.py +2. open a second terminal and run : + python3 Gpt.py +3. on this terminal there is an IP address to open the UI to enter prompts via the webserver +4. open the third terminal and run : + python3 Gpt_parser.py diff --git a/webots_ros2_mavic/webots_ros2_mavic/flagged/log.csv b/webots_ros2_mavic/webots_ros2_mavic/flagged/log.csv new file mode 100644 index 000000000..7e9e27467 --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/flagged/log.csv @@ -0,0 +1,305 @@ +Drone ROS 2 Command,Response,flag,username,timestamp +Stop this drone,"Response: + +To stop the drone, we can publish a zero velocity command to the `/cmd_vel` topic. The following Python code snippet provides a simple way to send a stop command to the drone. + +Here’s the complete code to stop the drone: + +```python +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist + +class DroneStop(Node): + def __init__(self): + super().__init__('drone_stop') + self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) + self.stop_drone() + + def stop_drone(self): + vel_msg = Twist() + vel_msg.linear.x = 0.0 # Set linear velocity in x direction to zero + vel_msg.linear.y = 0.0 # Set linear velocity in y direction to zero + vel_msg.linear.z = 0.0 # Set linear velocity in z direction to zero + vel_msg.angular.x = 0.0 # Set angular velocity around x axis to zero + vel_msg.angular.y = 0.0 # Set angular velocity around y axis to zero + vel_msg.angular.z = 0.0 # Set angular velocity around z axis to zero + + self.publisher_.publish(vel_msg) # Publish the stop command + self.get_logger().info('Drone stopped.') + +def main(args=None): + rclpy.init(args=args) + drone_stop = DroneStop() + try: + rclpy.spin_once(drone_stop) # Spin once to ensure the command is sent + finally: + drone_stop.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +### Explanation: + +1. **DroneStop Class**: This is a ROS 2 Node named `drone_stop`, which is responsible for stopping the drone. + +2. **Publisher**: The node creates a publisher to the `/cmd_vel` topic to send velocity commands. + +3. **stop_drone Method**: This method constructs a `Twist` message (`vel_msg`) with all linear and angular velocities set to zero. + - This effectively commands the drone to stop moving. + +4. **Publishing the Command**: The stop command is published to the `/cmd_vel` topic. + +5. **Logging**: After publishing the stop command, a log message confirms that the drone has stopped. + +6. **Main Function**: Initializes ROS 2, creates the `DroneStop` instance, and calls `rclpy.spin_once` to allow the command to be sent before shutting down. + +This script will effectively stop the drone by publishing a zero velocity command. Ensure that your ROS 2 environment is correctly configured to run this script.",,,2024-10-05 03:45:41.554436 +Move forward 1 meter and rotate left,"Response: + +To move the DJI Mavic forward by 1 meter and then rotate left (without specifying an angle), we will need to calculate the duration for which the drone should move forward at a certain speed to cover 1 meter. We will then rotate the drone left indefinitely. Here’s how you can write the Python code to achieve this: + +```python +import rclpy +from geometry_msgs.msg import Twist + +def move_drone(): + rclpy.init() + node = rclpy.create_node('drone_controller') + cmd_vel_pub = node.create_publisher(Twist, '/cmd_vel', 10) + + # Move forward 1 meter (considering a speed of 1 m/s, it will take 1 second) + forward_cmd = Twist() + forward_cmd.linear.x = 1.0 # Move forward at 1 m/s + forward_cmd.angular.z = 0.0 # No rotation + cmd_vel_pub.publish(forward_cmd) + + # Sleep for 1 second to move forward 1 meter + rclpy.sleep(1.0) # Move forward for 1 second + + # Rotate left (to rotate continuously, set a non-zero angular velocity) + rotate_cmd = Twist() + rotate_cmd.linear.x = 0.0 # No forward movement + rotate_cmd.angular.z = 0.5 # Rotate left at a speed of 0.5 radians/s + cmd_vel_pub.publish(rotate_cmd) + + # Keep rotating left for a specified duration (e.g., 5 seconds) + rclpy.sleep(5.0) # Rotate left for 5 seconds + + # Stop the drone + stop_cmd = Twist() + stop_cmd.linear.x = 0.0 # Stop forward movement + stop_cmd.angular.z = 0.0 # Stop rotation + cmd_vel_pub.publish(stop_cmd) + + # Clean up + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + move_drone() +``` + +### Explanation of the Code: + +1. **Initialization**: + - Similar to the previous example, the code initializes the ROS 2 environment and creates a node called `drone_controller`. + +2. **Node and Publisher Creation**: + - A publisher is created for sending `Twist` messages to the `/cmd_vel` topic to control the drone's movement. + +3. **Moving Forward 1 Meter**: + - A `Twist` message (`forward_cmd`) is created specifying: + - `linear.x` is set to `1.0`, which means the drone will move forward at 1 meter per second. + - `angular.z` is set to `0.0`, which means there’s no rotation during this time. + - The forward command is published, and the code sleeps for 1 second to allow the drone to travel 1 meter (since it's moving at 1 m/s). + +4. **Rotating Left**: + - A new `Twist` message (`rotate_cmd`) is created for rotation: + - `linear.x` is set to `0.0`, meaning there’s no forward movement. + - `angular.z` is set to `0.5`, allowing the drone to rotate left at a speed of 0.5 radians per second. + - The rotating command is published, and the code sleeps for 5 seconds to continue the left rotation. + +5. **Stopping the Drone**: + - After rotation, a `stop_cmd` message is created and published to stop the drone by setting both `linear.x` and `angular.z` to `0.0`. + +6. **Clean-Up**: + - Finally, the node is destroyed, and the ROS 2 environment is cleaned up. + +This code effectively allows the drone to move forward by 1 meter and then rotate left for a specified duration before coming to a stop. + +An error occurred during command execution: +Traceback (most recent call last): + File ""/home/hadi/ros2_webots/src/webots_ros2/webots_ros2_mavic/webots_ros2_mavic/mavic_ros2_controller.py"", line 118, in chatbot_interface + time.sleep(1) # Delay untuk memastikan setiap perintah dieksekusi satu per satu +NameError: name 'time' is not defined +",,,2024-10-06 02:59:19.780454 +"Create waypoints in a good straight line with start point at x:10, y:10, z:5, and start point at x:20, y:15, z:10. the number of waypoints 20.","[{""x"": 10.0, ""y"": 10.0, ""z"": 5.0}, {""x"": 10.526, ""y"": 10.263, ""z"": 5.263}, {""x"": 11.053, ""y"": 10.526, ""z"": 5.526}, {""x"": 11.579, ""y"": 10.789, ""z"": 5.789}, {""x"": 12.105, ""y"": 11.053, ""z"": 6.053}, {""x"": 12.632, ""y"": 11.316, ""z"": 6.316}, {""x"": 13.158, ""y"": 11.579, ""z"": 6.579}, {""x"": 13.684, ""y"": 11.842, ""z"": 6.842}, {""x"": 14.211, ""y"": 12.105, ""z"": 7.105}, {""x"": 14.737, ""y"": 12.368, ""z"": 7.368}, {""x"": 15.263, ""y"": 12.632, ""z"": 7.632}, {""x"": 15.789, ""y"": 12.895, ""z"": 7.895}, {""x"": 16.316, ""y"": 13.158, ""z"": 8.158}, {""x"": 16.842, ""y"": 13.421, ""z"": 8.421}, {""x"": 17.368, ""y"": 13.684, ""z"": 8.684}, {""x"": 17.895, ""y"": 13.947, ""z"": 8.947}, {""x"": 18.421, ""y"": 14.211, ""z"": 9.211}, {""x"": 18.947, ""y"": 14.474, ""z"": 9.474}, {""x"": 19.474, ""y"": 14.737, ""z"": 9.737}, {""x"": 20.0, ""y"": 15.0, ""z"": 10.0}]",,,2024-12-26 17:37:14.231331 +"Create waypoints in a good circular shape with center at x:10, y:10, z:10, a radius of 5, rotation axis at x with angle 45 degrees. the number of waypoints 30 units.","'To generate the waypoints, I will create a circle in the yz-plane at the specified center and rotate it around the x-axis by 45 degrees. Here are the waypoints: + +```json +[ + {""x"": 10.0, ""y"": 10.0, ""z"": 15.0}, + {""x"": 10.0, ""y"": 11.047, ""z"": 14.938}, + {""x"": 10.0, ""y"": 12.038, ""z"": 14.753}, + {""x"": 10.0, ""y"": 12.918, ""z"": 14.455}, + {""x"": 10.0, ""y"": 13.632, ""z"": 14.055}, + {""x"": 10.0, ""y"": 14.138, ""z"": 13.568}, + {""x"": 10.0, ""y"": 14.408, ""z"": 13.010}, + {""x"": 10.0, ""y"": 14.425, ""z"": 12.405}, + {""x"": 10.0, ""y"": 14.184, ""z"": 11.77}, + {""x"": 10.0, ""y"": 13.69, ""z"": 11.125}, + {""x"": 10.0, ""y"": 12.964, ""z"": 10.489}, + {""x"": 10.0, ""y"": 12.042, ""z"": 9.882}, + {""x"": 10.0, ""y"": 10.971, ""z"": 9.327}, + {""x"": 10.0, ""y"": 9.804, ""z"": 8.848}, + {""x"": 10.0, ""y"": 8.602, ""z"": 8.465}, + {""x"": 10.0, ""y"": 7.437, ""z"": 8.192}, + {""x"": 10.0, ""y"": 6.365, ""z"": 8.035}, + {""x"": 10.0, ""y"": 5.438, ""z"": 7.996}, + {""x"": 10.0, ""y"": 4.702, ""z"": 8.072}, + {""x"": 10.0, ""y"": 4.19, ""z"": 8.257}, + {""x"": 10.0, ""y"": 3.91, ""z"": 8.541}, + {""x"": 10.0, ""y"": 3.858, ""z"": 8.915}, + {""x"": 10.0, ""y"": 4.017, ""z"": 9.361}, + {""x"": 10.0, ""y"": 4.358, ""z"": 9.86}, + {""x"": 10.0, ""y"": 4.849, ""z"": 10.391}, + {""x"": 10.0, ""y"": 5.462, ""z"": 10.933}, + {""x"": 10.0, ""y"": 6.154, ""z"": 11.464}, + {""x"": 10.0, ""y"": 6.876, ""z"": 11.961}, + {""x"": 10.0, ""y"": 7.583, ""z"": 12.404}, + {""x"": 10.0, ""y"": 8.236, ""z"": 12.777} +] +``` + +Note: The above coordinates assume a specific convention for the x, y, and z axes, and the generated path revolves around the x-axis by 45 degrees. Each waypoint is calculated assuming a perfect circle initially, then applying the rotation. It approximates 30 evenly spaced points around this path.",,,2024-12-26 19:45:01.202159 +"Create waypoints in a good rectangle shape with center at x:10, y:10, z:10, rotation axis at y with rotation angle 45 degrees. the number of waypoints 30 units.","'To create waypoints in a rectangular shape and rotate them around the y-axis by 45 degrees, you'll first need to define the rectangle. For simplicity, let's create a rectangle that spans -5 to +5 in the x and z directions. Then, rotate these points around the y-axis by 45 degrees. + +Here's the Python list of dictionaries representing the waypoints: + +```json +[ + {""x"": 13.54, ""y"": 10, ""z"": 13.54}, + {""x"": 12.6, ""y"": 10, ""z"": 12.98}, + {""x"": 11.66, ""y"": 10, ""z"": 12.43}, + {""x"": 10.71, ""y"": 10, ""z"": 11.88}, + {""x"": 9.77, ""y"": 10, ""z"": 11.33}, + {""x"": 8.83, ""y"": 10, ""z"": 10.78}, + {""x"": 7.88, ""y"": 10, ""z"": 10.23}, + {""x"": 6.94, ""y"": 10, ""z"": 9.68}, + {""x"": 6.0, ""y"": 10, ""z"": 9.12}, + {""x"": 5.05, ""y"": 10, ""z"": 8.57}, + {""x"": 4.11, ""y"": 10, ""z"": 8.02}, + {""x"": 3.17, ""y"": 10, ""z"": 7.47}, + {""x"": 2.22, ""y"": 10, ""z"": 6.92}, + {""x"": 1.28, ""y"": 10, ""z"": 6.37}, + {""x"": 0.34, ""y"": 10, ""z"": 5.82}, + {""x"": -0.6, ""y"": 10, ""z"": 5.27}, + {""x"": -1.54, ""y"": 10, ""z"": 4.71}, + {""x"": -2.49, ""y"": 10, ""z"": 4.16}, + {""x"": -3.43, ""y"": 10, ""z"": 3.61}, + {""x"": -4.37, ""y"": 10, ""z"": 3.06}, + {""x"": -5.32, ""y"": 10, ""z"": 2.51}, + {""x"": -6.26, ""y"": 10, ""z"": 1.96}, + {""x"": -7.2, ""y"": 10, ""z"": 1.41}, + {""x"": -8.15, ""y"": 10, ""z"": 0.86}, + {""x"": -9.09, ""y"": 10, ""z"": 0.3}, + {""x"": -10.03, ""y"": 10, ""z"": -0.25}, + {""x"": -10.98, ""y"": 10, ""z"": -0.8}, + {""x"": -11.92, ""y"": 10, ""z"": -1.35}, + {""x"": -12.86, ""y"": 10, ""z"": -1.9}, + {""x"": -13.81, ""y"": 10, ""z"": -2.45} +] +``` + +",,,2024-12-26 20:25:13.834477 +"Create waypoints in a perfect circle shape with center at x:10, y:10, z:10, a radius of 3, rotation axis at x with rotation angle 45 degrees. the number of waypoints 30 units.","'To create waypoints in a circular path in 3D space centered at (10, 10, 10) with a radius of 3 and rotated around the x-axis by 45 degrees, we need to calculate the coordinates of the waypoints. The number of waypoints is 30, so the angles will be spaced evenly at intervals of 12 degrees (360/30). + +Here's a Python list of dictionaries with the calculated waypoints: + +```python +[ + {""x"": 10.0, ""y"": 13.0, ""z"": 10.0}, + {""x"": 10.0, ""y"": 12.919, ""z"": 10.187}, + {""x"": 10.0, ""y"": 12.678, ""z"": 10.368}, + {""x"": 10.0, ""y"": 12.293, ""z"": 10.537}, + {""x"": 10.0, ""y"": 11.793, ""z"": 10.693}, + {""x"": 10.0, ""y"": 11.207, ""z"": 10.825}, + {""x"": 10.0, ""y"": 10.563, ""z"": 10.931}, + {""x"": 10.0, ""y"": 9.903, ""z"": 11.0}, + {""x"": 10.0, ""y"": 9.272, ""z"": 11.026}, + {""x"": 10.0, ""y"": 8.707, ""z"": 11.0}, + {""x"": 10.0, ""y"": 8.167, ""z"": 10.931}, + {""x"": 10.0, ""y"": 7.592, ""z"": 10.825}, + {""x"": 10.0, ""y"": 7.007, ""z"": 10.693}, + {""x"": 10.0, ""y"": 6.507, ""z"": 10.537}, + {""x"": 10.0, ""y"": 6.123, ""z"": 10.368}, + {""x"": 10.0, ""y"": 5.882, ""z"": 10.187}, + {""x"": 10.0, ""y"": 5.801, ""z"": 10.0}, + {""x"": 10.0, ""y"": 5.882, ""z"": 9.813}, + {""x"": 10.0, ""y"": 6.123, ""z"": 9.632}, + {""x"": 10.0, ""y"": 6.507, ""z"": 9.463}, + {""x"": 10.0, ""y"": 7.007, ""z"": 9.307}, + {""x"": 10.0, ""y"": 7.592, ""z"": 9.175}, + {""x"": 10.0, ""y"": 8.167, ""z"": 9.069}, + {""x"": 10.0, ""y"": 8.707, ""z"": 9.0}, + {""x"": 10.0, ""y"": 9.272, ""z"": 8.974}, + {""x"": 10.0, ""y"": 9.903, ""z"": 9.0}, + {""x"": 10.0, ""y"": 10.563, ""z"": 9.069}, + {""x"": 10.0, ""y"": 11.207, ""z"": 9.175}, + {""x"": 10.0, ""y"": 11.793, ""z"": 9.307}, + {""x"": 10.0, ""y"": 12.293, ""z"": 9.463} +] +``` + +This sequence considers rotation along the x-axis by 45 degrees, affecting the y and z coordinates in each waypoint. The coordinates have been calculated at each interval using trigonometric transformations.",,,2024-12-26 20:37:18.730755 +"Create waypoints in a perfect circle shape with center at x:10, y:10, z:10, a radius of 2, rotation axis at z with rotation angle 90 degrees. the number of waypoints 40 units.","[{""x"": 10.0, ""y"": 12.0, ""z"": 10.0}, {""x"": 9.69, ""y"": 11.98, ""z"": 10.0}, {""x"": 9.38, ""y"": 11.92, ""z"": 10.0}, {""x"": 9.08, ""y"": 11.84, ""z"": 10.0}, {""x"": 8.79, ""y"": 11.71, ""z"": 10.0}, {""x"": 8.51, ""y"": 11.56, ""z"": 10.0}, {""x"": 8.25, ""y"": 11.36, ""z"": 10.0}, {""x"": 8.01, ""y"": 11.14, ""z"": 10.0}, {""x"": 7.79, ""y"": 10.89, ""z"": 10.0}, {""x"": 7.61, ""y"": 10.61, ""z"": 10.0}, {""x"": 7.45, ""y"": 10.31, ""z"": 10.0}, {""x"": 7.33, ""y"": 10.0, ""z"": 10.0}, {""x"": 7.25, ""y"": 9.69, ""z"": 10.0}, {""x"": 7.21, ""y"": 9.38, ""z"": 10.0}, {""x"": 7.21, ""y"": 9.08, ""z"": 10.0}, {""x"": 7.25, ""y"": 8.77, ""z"": 10.0}, {""x"": 7.33, ""y"": 8.46, ""z"": 10.0}, {""x"": 7.45, ""y"": 8.16, ""z"": 10.0}, {""x"": 7.61, ""y"": 7.89, ""z"": 10.0}, {""x"": 7.79, ""y"": 7.61, ""z"": 10.0}, {""x"": 8.01, ""y"": 7.36, ""z"": 10.0}, {""x"": 8.25, ""y"": 7.14, ""z"": 10.0}, {""x"": 8.51, ""y"": 6.94, ""z"": 10.0}, {""x"": 8.79, ""y"": 6.79, ""z"": 10.0}, {""x"": 9.08, ""y"": 6.66, ""z"": 10.0}, {""x"": 9.38, ""y"": 6.58, ""z"": 10.0}, {""x"": 9.69, ""y"": 6.52, ""z"": 10.0}, {""x"": 10.0, ""y"": 6.5, ""z"": 10.0}, {""x"": 10.31, ""y"": 6.52, ""z"": 10.0}, {""x"": 10.61, ""y"": 6.58, ""z"": 10.0}, {""x"": 10.92, ""y"": 6.66, ""z"": 10.0}, {""x"": 11.21, ""y"": 6.79, ""z"": 10.0}, {""x"": 11.49, ""y"": 6.94, ""z"": 10.0}, {""x"": 11.75, ""y"": 7.14, ""z"": 10.0}, {""x"": 11.99, ""y"": 7.36, ""z"": 10.0}, {""x"": 12.21, ""y"": 7.61, ""z"": 10.0}, {""x"": 12.39, ""y"": 7.89, ""z"": 10.0}, {""x"": 12.55, ""y"": 8.16, ""z"": 10.0}, {""x"": 12.67, ""y"": 8.46, ""z"": 10.0}, {""x"": 12.75, ""y"": 8.77, ""z"": 10.0}, {""x"": 12.79, ""y"": 9.08, ""z"": 10.0}, {""x"": 12.79, ""y"": 9.38, ""z"": 10.0}, {""x"": 12.75, ""y"": 9.69, ""z"": 10.0}, {""x"": 12.67, ""y"": 10.0, ""z"": 10.0}, {""x"": 12.55, ""y"": 10.31, ""z"": 10.0}, {""x"": 12.39, ""y"": 10.61, ""z"": 10.0}, {""x"": 12.21, ""y"": 10.89, ""z"": 10.0}, {""x"": 12.01, ""y"": 11.14, ""z"": 10.0}, {""x"": 11.75, ""y"": 11.36, ""z"": 10.0}, {""x"": 11.49, ""y"": 11.56, ""z"": 10.0}, {""x"": 11.21, ""y"": 11.71, ""z"": 10.0}, {""x"": 10.92, ""y"": 11.84, ""z"": 10.0}, {""x"": 10.61, ""y"": 11.92, ""z"": 10.0}, {""x"": 10.31, ""y"": 11.98, ""z"": 10.0}]",,,2024-12-26 21:08:16.628245 +"Create waypoints in a perfect circle shape with center at x:10, y:10, z:10, a radius of 2, rotation axis at y with rotation angle 90 degrees. the number of waypoints 40 units.","'To generate waypoints forming a circle with a center at (10, 10, 10), radius of 2, and with rotation around the y-axis by 90 degrees, the z coordinates will vary. The waypoints are distributed evenly across the circle's circumference. Below is the JSON format list of waypoints: + +```json +[ + {""x"": 10.0, ""y"": 10.0, ""z"": 12.0}, + {""x"": 10.31, ""y"": 10.0, ""z"": 11.98}, + {""x"": 10.62, ""y"": 10.0, ""z"": 11.92}, + {""x"": 10.92, ""y"": 10.0, ""z"": 11.81}, + {""x"": 11.19, ""y"": 10.0, ""z"": 11.66}, + {""x"": 11.43, ""y"": 10.0, ""z"": 11.48}, + {""x"": 11.62, ""y"": 10.0, ""z"": 11.26}, + {""x"": 11.76, ""y"": 10.0, ""z"": 11.02}, + {""x"": 11.86, ""y"": 10.0, ""z"": 10.75}, + {""x"": 11.90, ""y"": 10.0, ""z"": 10.47}, + {""x"": 11.90, ""y"": 10.0, ""z"": 10.17}, + {""x"": 11.86, ""y"": 10.0, ""z"": 9.89}, + {""x"": 11.76, ""y"": 10.0, ""z"": 9.62}, + {""x"": 11.62, ""y"": 10.0, ""z"": 9.38}, + {""x"": 11.43, ""y"": 10.0, ""z"": 9.16}, + {""x"": 11.19, ""y"": 10.0, ""z"": 8.98}, + {""x"": 10.92, ""y"": 10.0, ""z"": 8.83}, + {""x"": 10.62, ""y"": 10.0, ""z"": 8.73}, + {""x"": 10.31, ""y"": 10.0, ""z"": 8.68}, + {""x"": 10.0, ""y"": 10.0, ""z"": 8.68}, + {""x"": 9.69, ""y"": 10.0, ""z"": 8.73}, + {""x"": 9.38, ""y"": 10.0, ""z"": 8.83}, + {""x"": 9.11, ""y"": 10.0, ""z"": 8.98}, + {""x"": 8.87, ""y"": 10.0, ""z"": 9.16}, + {""x"": 8.68, ""y"": 10.0, ""z"": 9.38}, + {""x"": 8.54, ""y"": 10.0, ""z"": 9.62}, + {""x"": 8.44, ""y"": 10.0, ""z"": 9.89}, + {""x"": 8.40, ""y"": 10.0, ""z"": 10.17}, + {""x"": 8.40, ""y"": 10.0, ""z"": 10.47}, + {""x"": 8.44, ""y"": 10.0, ""z"": 10.75}, + {""x"": 8.54, ""y"": 10.0, ""z"": 11.02}, + {""x"": 8.68, ""y"": 10.0, ""z"": 11.26}, + {""x"": 8.87, ""y"": 10.0, ""z"": 11.48}, + {""x"": 9.11, ""y"": 10.0, ""z"": 11.66}, + {""x"": 9.38, ""y"": 10.0, ""z"": 11.81}, + {""x"": 9.69, ""y"": 10.0, ""z"": 11.92}, + {""x"": 10.0, ""y"": 10.0, ""z"": 11.98}, + {""x"": 10.31, ""y"": 10.0, ""z"": 12.0} +] +``` + +These coordinates assume that all scaling and rotations from the circle's original definition around the y-axis remain circular. The coordinates are calculated based on trigonometric functions to match a circle in a rotated plane.",,,2024-12-26 21:09:37.170081 +"Create waypoints in a perfect circle shape with center at x:10, y:10, z:10, a radius of 2, rotation axis at x with rotation angle 90 degrees. the number of waypoints 40 units.","[{""x"": 10.0, ""y"": 10.0, ""z"": 12.0}, {""x"": 10.0, ""y"": 10.31, ""z"": 11.98}, {""x"": 10.0, ""y"": 10.62, ""z"": 11.92}, {""x"": 10.0, ""y"": 10.92, ""z"": 11.81}, {""x"": 10.0, ""y"": 11.21, ""z"": 11.67}, {""x"": 10.0, ""y"": 11.48, ""z"": 11.48}, {""x"": 10.0, ""y"": 11.67, ""z"": 11.21}, {""x"": 10.0, ""y"": 11.81, ""z"": 10.92}, {""x"": 10.0, ""y"": 11.92, ""z"": 10.62}, {""x"": 10.0, ""y"": 11.98, ""z"": 10.31}, {""x"": 10.0, ""y"": 12.0, ""z"": 10.0}, {""x"": 10.0, ""y"": 11.98, ""z"": 9.69}, {""x"": 10.0, ""y"": 11.92, ""z"": 9.38}, {""x"": 10.0, ""y"": 11.81, ""z"": 9.08}, {""x"": 10.0, ""y"": 11.67, ""z"": 8.79}, {""x"": 10.0, ""y"": 11.48, ""z"": 8.52}, {""x"": 10.0, ""y"": 11.21, ""z"": 8.33}, {""x"": 10.0, ""y"": 10.92, ""z"": 8.19}, {""x"": 10.0, ""y"": 10.62, ""z"": 8.08}, {""x"": 10.0, ""y"": 10.31, ""z"": 8.02}, {""x"": 10.0, ""y"": 10.0, ""z"": 8.0}, {""x"": 10.0, ""y"": 9.69, ""z"": 8.02}, {""x"": 10.0, ""y"": 9.38, ""z"": 8.08}, {""x"": 10.0, ""y"": 9.08, ""z"": 8.19}, {""x"": 10.0, ""y"": 8.79, ""z"": 8.33}, {""x"": 10.0, ""y"": 8.52, ""z"": 8.52}, {""x"": 10.0, ""y"": 8.33, ""z"": 8.79}, {""x"": 10.0, ""y"": 8.19, ""z"": 9.08}, {""x"": 10.0, ""y"": 8.08, ""z"": 9.38}, {""x"": 10.0, ""y"": 8.02, ""z"": 9.69}, {""x"": 10.0, ""y"": 8.0, ""z"": 10.0}, {""x"": 10.0, ""y"": 8.02, ""z"": 10.31}, {""x"": 10.0, ""y"": 8.08, ""z"": 10.62}, {""x"": 10.0, ""y"": 8.19, ""z"": 10.92}, {""x"": 10.0, ""y"": 8.33, ""z"": 11.21}, {""x"": 10.0, ""y"": 8.52, ""z"": 11.48}, {""x"": 10.0, ""y"": 8.79, ""z"": 11.67}, {""x"": 10.0, ""y"": 9.08, ""z"": 11.81}, {""x"": 10.0, ""y"": 9.38, ""z"": 11.92}, {""x"": 10.0, ""y"": 9.69, ""z"": 11.98}]",,,2024-12-26 21:14:54.547342 diff --git a/webots_ros2_mavic/webots_ros2_mavic/gps_point_data.csv b/webots_ros2_mavic/webots_ros2_mavic/gps_point_data.csv new file mode 100644 index 000000000..8db0e046d --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/gps_point_data.csv @@ -0,0 +1,371 @@ +Timestamp,X,Y,Z +1735791939.812491884,0.06342538555805356,0.02104492352611366,0.7840791729475076 +1735791939.859413043,0.06342558111371221,0.021044969555147165,0.784079198882194 +1735791939.870437857,0.06342562021547322,0.021044978715915416,0.7840792040629884 +1735791939.880319856,0.06342565931433845,0.02104498786078211,0.784079209242201 +1735791939.889603730,0.06342569841079265,0.021044996989245966,0.7840792144200944 +1735791939.898920356,0.06342573750819476,0.02104500611038717,0.7840792195975076 +1735791939.908210640,0.06342577660224359,0.02104501523619971,0.7840792247614697 +1735791939.917856974,0.06342581569434441,0.0210450243846329,0.7840792299267549 +1735791939.925784560,0.06342585478471151,0.021045033553748916,0.7840792350925698 +1735791939.934869788,0.06342589387291955,0.021045042742571646,0.784079240258123 +1735791939.943759809,0.06342593295925152,0.02104505195082336,0.7840792454232427 +1735791939.951732658,0.06342597204387429,0.021045061178470218,0.784079250588017 +1735791939.959124778,0.06342601112682011,0.021045070425840112,0.7840792557520881 +1735791939.967474298,0.06342605020808459,0.021045079693298344,0.784079260915458 +1735791939.983726561,0.06342608928798432,0.02104508898145199,0.7840792660781254 +1735791939.991265879,0.06342612836281314,0.021045098276673238,0.7840792712275798 +1735791940.14722604,0.06342616743474845,0.02104510756319961,0.7840792763698469 +1735791940.21976312,0.06342620650470546,0.021045116842243312,0.7840792815080369 +1735791940.30572865,0.06342624557354266,0.021045126114600958,0.7840792866437923 +1735791940.38222312,0.06342628464104717,0.02104513538067789,0.7840792917774615 +1735791940.51053427,0.06342632370753247,0.021045144640803145,0.7840792969094185 +1735791940.60098046,0.06342636277292082,0.021045153895083435,0.7840793020398231 +1735791940.71829109,0.06342640183765416,0.021045163143656835,0.7840793071689652 +1735791940.82249420,0.06342644089813523,0.021045172374693964,0.7840793122860424 +1735791940.91898460,0.06342647995648393,0.021045181589261774,0.7840793173967344 +1735791940.101324226,0.06342651901358581,0.021045190787644004,0.784079322503837 +1735791940.116583518,0.06342655806970068,0.021045199969870053,0.7840793276082606 +1735791940.121619092,0.06342659712484318,0.021045209135607625,0.7840793327106134 +1735791940.130473772,0.06342663617926564,0.02104521828448297,0.7840793378111963 +1735791940.140263964,0.0634266752330512,0.021045227415892397,0.7840793429103401 +1735791940.150670392,0.06342671428274363,0.021045236537683122,0.7840793480071478 +1735791940.155368088,0.06342675332527986,0.021045245661617373,0.7840793530926727 +1735791940.165235189,0.06342679236660835,0.021045254797327988,0.7840793581843375 +1735791940.173876598,0.06342683140553924,0.021045263942670603,0.7840793632796994 +1735791940.180906311,0.06342687044191282,0.02104527309655242,0.7840793683778331 +1735791940.188001023,0.06342690947862345,0.021045282266810992,0.7840793734792698 +1735791940.199171096,0.06342694851521682,0.021045291453201286,0.7840793785822322 +1735791940.206682520,0.06342698755086536,0.021045300655658735,0.7840793836854906 +1735791940.217783207,0.06342702658558777,0.021045309874401896,0.7840793887886834 +1735791940.223676299,0.06342706561936337,0.02104531910966934,0.7840793938918209 +1735791940.237041925,0.06342710465208203,0.02104532836193765,0.7840793989945172 +1735791940.242631796,0.06342714368362605,0.02104533763164963,0.7840794040967676 +1735791940.251618965,0.06342718270778853,0.02104534689920734,0.7840794091871344 +1735791940.260160153,0.0634272217272325,0.021045356166215726,0.7840794142731095 +1735791940.275005259,0.06342726074299176,0.021045365433767223,0.7840794193577318 +1735791940.281943273,0.0634272997553859,0.02104537470248966,0.7840794244425655 +1735791940.299599736,0.0634273387649532,0.02104538397294133,0.7840794295285315 +1735791940.303178826,0.06342737777127708,0.021045393245487574,0.7840794346156339 +1735791940.327304428,0.06342741677451211,0.021045402520538062,0.7840794397040765 +1735791940.337711338,0.06342745577101237,0.02104541178581496,0.7840794447827457 +1735791940.343641310,0.06342749476305033,0.021045421042774293,0.7840794498578166 +1735791940.347364434,0.06342753375092132,0.021045430292098884,0.7840794549317069 +1735791940.359691631,0.06342757273503997,0.02104543953419373,0.7840794600057324 +1735791940.368339850,0.06342761171873333,0.021045448760992323,0.7840794650813308 +1735791940.375680213,0.06342765070106242,0.02104545797252503,0.7840794701566315 +1735791940.396630026,0.06342768968144734,0.021045467168519,0.784079475230972 +1735791940.406588787,0.06342772865983239,0.021045476348687576,0.7840794803040712 +1735791940.412962545,0.06342776763613271,0.021045485512526014,0.7840794853759919 +1735791940.419865091,0.06342780661018403,0.021045494659572037,0.7840794904463875 +1735791940.427110333,0.06342784558182292,0.021045503789158836,0.7840794955152847 +1735791940.431552497,0.0634278845475491,0.02104551291030881,0.7840795005821364 +1735791940.436353605,0.06342792350420154,0.021045522034448337,0.7840795056381665 +1735791940.440497323,0.06342796245765352,0.021045531171671395,0.7840795107009749 +1735791940.451592275,0.06342800140692063,0.021045540319792617,0.7840795157681958 +1735791940.460749889,0.06342804035199084,0.0210455494777115,0.7840795208389346 +1735791940.465911896,0.0634280792922293,0.02104555864476975,0.7840795259123488 +1735791940.476931169,0.06342811822771427,0.021045567820716975,0.7840795309882381 +1735791940.487411103,0.06342815715840931,0.021045577005376404,0.7840795360666727 +1735791940.493983313,0.06342819608414253,0.021045586198826252,0.7840795411472808 +1735791940.497123545,0.06342823500470436,0.02104559540110923,0.7840795462300514 +1735791940.501660954,0.06342827392020575,0.021045604612455273,0.7840795513149701 +1735791940.508585468,0.06342831283061695,0.0210456138330401,0.7840795564021931 +1735791940.519801390,0.06342835173576462,0.02104562306322423,0.7840795614913892 +1735791940.528179579,0.06342839063543664,0.021045632303292643,0.7840795665825662 +1735791940.536904398,0.06342842952994646,0.021045641553688668,0.7840795716759182 +1735791940.548426323,0.06342846841879089,0.021045650814805843,0.7840795767711032 +1735791940.560109122,0.06342850730210009,0.02104566008718027,0.7840795818681521 +1735791940.563685435,0.06342854617624272,0.021045669358775092,0.7840795869558956 +1735791940.573475788,0.06342858504350084,0.02104567863130387,0.7840795920404339 +1735791940.579673830,0.06342862390769208,0.021045687897066553,0.7840795971248358 +1735791940.586420501,0.06342866276845086,0.021045697156793962,0.7840796022084103 +1735791940.591610416,0.06342870162550675,0.02104570641083818,0.7840796072910294 +1735791940.599342575,0.06342874047858059,0.021045715659523703,0.7840796123722542 +1735791940.617617042,0.0634287793274375,0.02104572490294708,0.7840796174520633 +1735791940.625114701,0.0634288181722189,0.021045734141248152,0.7840796225304565 +1735791940.634636634,0.06342885701296577,0.02104574337439538,0.784079627607613 +1735791940.648722449,0.06342889584960475,0.021045752602440262,0.784079632683228 +1735791940.660687727,0.0634289346820457,0.021045761825266383,0.7840796377573364 +1735791940.678317232,0.06342897351074038,0.021045771042829665,0.7840796428301592 +1735791940.686783535,0.06342901233533577,0.021045780254954845,0.7840796479013807 +1735791940.698189089,0.06342905115612116,0.021045789461531662,0.7840796529710582 +1735791940.706569795,0.0634290899730429,0.021045798662306853,0.7840796580391942 +1735791940.714634885,0.0634291287865861,0.021045807857105425,0.7840796631059948 +1735791940.717992242,0.06342916759642613,0.02104581704562645,0.7840796681711371 +1735791940.724165567,0.06342920640287535,0.021045826227638842,0.7840796732346748 +1735791940.727605730,0.06342924520610266,0.021045835402768585,0.7840796782968091 +1735791940.737912903,0.06342928400613299,0.02104584457074486,0.7840796833572441 +1735791940.744473127,0.06342932280295316,0.021045853731144735,0.7840796884160179 +1735791940.755719815,0.0634293615968719,0.021045862883635615,0.7840796934731544 +1735791940.759286082,0.06342940038805936,0.021045872027745466,0.7840796985288426 +1735791940.773558982,0.06342943917654315,0.02104588116310924,0.7840797035827818 +1735791940.781094445,0.06342947796219496,0.021045890306449065,0.7840797086353379 +1735791940.795889970,0.06342951674190787,0.021045899469666784,0.7840797136761184 +1735791940.807891072,0.06342955551745127,0.021045908651385854,0.7840797187109384 +1735791940.813995307,0.06342959429001013,0.021045917851087136,0.7840797237426574 +1735791940.817395736,0.06342963305987946,0.021045927068580945,0.784079728772557 +1735791940.830592181,0.06342967182765476,0.021045936304058738,0.7840797338014244 +1735791940.836910400,0.06342971059302266,0.021045945557798162,0.7840797388291979 +1735791940.844733796,0.06342974935626933,0.021045954830309375,0.7840797438560458 +1735791940.850170103,0.06342978811752709,0.021045964122094474,0.7840797488822167 +1735791940.865711036,0.06342982687328558,0.021045973424652652,0.7840797539063609 +1735791940.871287401,0.06342986562456925,0.021045982716297,0.7840797589196934 +1735791940.880115584,0.06342990437315328,0.021045991998859384,0.7840797639270614 +1735791940.885320141,0.0634299431197671,0.021046001273277534,0.7840797689308768 +1735791940.892565313,0.06342998186460595,0.021046010540188304,0.7840797739318709 +1735791940.899008160,0.06343002060766606,0.021046019799853467,0.7840797789305656 +1735791940.909649431,0.06343005934940997,0.021046029052492164,0.7840797839274174 +1735791940.929457009,0.06343009808946684,0.021046038298125666,0.7840797889222302 +1735791940.942007674,0.06343013682809936,0.021046047536798836,0.7840797939151268 +1735791940.950310330,0.0634301755652268,0.021046056768385533,0.7840797989061506 +1735791940.963658510,0.06343025303600168,0.02104607520988155,0.7840798088829842 +1735791940.970737064,0.06343029176959673,0.021046084419422535,0.7840798138685677 +1735791940.974370727,0.06343033050224721,0.021046093621126867,0.7840798188525064 +1735791940.980913459,0.06343036923396342,0.021046102814772007,0.7840798238345214 +1735791940.996575526,0.06343044669501272,0.021046121176412115,0.7840798337931851 +1735791941.10937053,0.06343048542452773,0.02104613034365194,0.7840798387697755 +1735791941.19618976,0.06343052415357851,0.021046139501313694,0.7840798437445144 +1735791941.29171673,0.06343056287846371,0.021046148657974054,0.7840798487166937 +1735791941.32023908,0.06343060160051964,0.0210461578132299,0.7840798536886017 +1735791941.41138920,0.06343064031606828,0.02104616697907983,0.7840798586499745 +1735791941.49554917,0.06343067902763036,0.02104617615412691,0.7840798636074571 +1735791941.64589360,0.06343071773623736,0.021046185337540387,0.7840798685642635 +1735791941.71237466,0.06343075644216839,0.021046194529000337,0.7840798735214738 +1735791941.76712016,0.06343079514539166,0.021046203728283152,0.7840798784797529 +1735791941.87733775,0.06343083384607753,0.021046212935419376,0.7840798834394075 +1735791941.94851710,0.06343087254420708,0.021046222150418365,0.7840798884007509 +1735791941.98148478,0.06343091123959942,0.021046231373489654,0.7840798933635345 +1735791941.115899742,0.06343094993203023,0.021046240604773454,0.7840798983278133 +1735791941.123096246,0.06343098862180085,0.021046249844569917,0.7840799032938132 +1735791941.137699310,0.06343102730839735,0.02104625909312857,0.7840799082612153 +1735791941.144867272,0.06343106599194177,0.021046268350839597,0.7840799132300709 +1735791941.152769483,0.06343110467220918,0.021046277618022007,0.7840799182003748 +1735791941.161147717,0.06343114334610055,0.021046286882647727,0.7840799231611872 +1735791941.172585865,0.06343118201508034,0.021046296146218293,0.784079928118091 +1735791941.181328111,0.0634312206800945,0.021046305409708,0.7840799330738473 +1735791941.195628520,0.06343125934143419,0.02104631467364492,0.7840799380299 +1735791941.202610479,0.06343129799902691,0.02104632393850951,0.7840799429865228 +1735791941.215581048,0.06343133665267323,0.02104633320455822,0.7840799479440161 +1735791941.222385940,0.06343137530246407,0.021046342472106707,0.784079952902522 +1735791941.248402923,0.06343145259368703,0.021046361003626314,0.7840799628237218 +1735791941.270718629,0.06343149123744778,0.021046370259116584,0.7840799677848805 +1735791941.295840551,0.06343156851637158,0.021046388738507633,0.7840799776934606 +1735791941.323335087,0.06343164578023205,0.021046407148360712,0.7840799875729981 +1735791941.331776103,0.06343168440858059,0.021046416327730376,0.7840799925078803 +1735791941.347119024,0.06343176165817775,0.021046434633650656,0.7840800023701154 +1735791941.351868280,0.06343180027646893,0.021046443767335003,0.7840800072974442 +1735791941.367390538,0.0634318774995206,0.021046462025782784,0.7840800171521324 +1735791941.377596158,0.06343195470647371,0.02104648032600099,0.7840800270138588 +1735791941.382980509,0.06343199330392758,0.02104648948809551,0.7840800319468547 +1735791941.390784534,0.06343203189720163,0.021046498657373902,0.7840800368811202 +1735791941.398636163,0.06343207048668179,0.021046507833635396,0.7840800418168389 +1735791941.405203582,0.06343210907193082,0.02104651701672711,0.7840800467536669 +1735791941.410587534,0.06343214765313908,0.021046526206693488,0.7840800516916377 +1735791941.418300090,0.06343218623014293,0.0210465354035426,0.7840800566307335 +1735791941.426334390,0.06343222480330851,0.02104654460745274,0.7840800615711407 +1735791941.436599443,0.06343226337218615,0.02104655381855921,0.7840800665125179 +1735791941.440064258,0.06343230193695877,0.02104656303714417,0.7840800714549011 +1735791941.446458556,0.0634323404976629,0.021046572263419543,0.7840800763984734 +1735791941.459744151,0.0634323790541893,0.021046581497769605,0.7840800813429206 +1735791941.471044600,0.06343241760638996,0.021046590740492647,0.7840800862882633 +1735791941.477630413,0.06343245615443908,0.021046599992040615,0.7840800912345068 +1735791941.492772078,0.06343249469837263,0.02104660925278669,0.7840800961818212 +1735791941.501394988,0.06343253323478108,0.021046618511160728,0.7840801011190929 +1735791941.512918687,0.06343257176542653,0.021046627768670445,0.7840801060520728 +1735791941.531646947,0.06343261029147867,0.021046637026326774,0.7840801109835986 +1735791941.541363414,0.06343264881279584,0.021046646284723858,0.7840801159145518 +1735791941.548640559,0.06343268732965666,0.021046655544382342,0.7840801208455377 +1735791941.566714149,0.06343272584190356,0.02104666480561747,0.7840801257768077 +1735791941.573145870,0.063432764349881,0.021046674068799544,0.7840801307086797 +1735791941.595309845,0.06343280285658989,0.021046683325540452,0.7840801356415709 +1735791941.601457626,0.06343284136139282,0.021046692576122114,0.7840801405735953 +1735791941.611576348,0.06343287986072449,0.021046701808718492,0.7840801454934831 +1735791941.624306234,0.06343291835621649,0.021046711024487085,0.7840801504061757 +1735791941.633636463,0.063432956848482,0.0210467202237705,0.7840801553141272 +1735791941.639227137,0.06343299533803505,0.021046729406628942,0.7840801602184827 +1735791941.646505427,0.06343303382507984,0.02104673857276228,0.7840801651199583 +1735791941.656135451,0.06343307230961673,0.021046747721836394,0.7840801700185067 +1735791941.662888898,0.06343311079159333,0.02104675685327047,0.7840801749142924 +1735791941.667354690,0.06343314927148604,0.021046765966497932,0.7840801798076067 +1735791941.676728415,0.06343318774504643,0.021046775070494246,0.7840801846973966 +1735791941.685619780,0.06343322621712233,0.021046784177913922,0.7840801895978402 +1735791941.690499676,0.06343326468597163,0.021046793286829783,0.7840801945036409 +1735791941.695364862,0.06343330315093232,0.02104680241776662,0.7840801994024301 +1735791941.701202753,0.06343334161277432,0.021046811568804356,0.7840802042972269 +1735791941.709766084,0.06343338007225037,0.02104682073910489,0.7840802091896109 +1735791941.723719077,0.0634334185296849,0.02104682992827731,0.7840802140804767 +1735791941.732004010,0.06343345698514016,0.021046839136401266,0.7840802189698405 +1735791941.743361527,0.06343349543859642,0.021046848363653132,0.7840802238578779 +1735791941.752958349,0.06343353389034405,0.021046857610484433,0.7840802287446694 +1735791941.762427822,0.06343357234053605,0.021046866877356553,0.784080233630423 +1735791941.766384743,0.06343361078602827,0.021046876156634496,0.784080238513875 +1735791941.774044758,0.06343364922421862,0.021046885436881282,0.7840802433860193 +1735791941.782504372,0.06343368765782859,0.021046894719900008,0.7840802482535703 +1735791941.790691903,0.06343372608745286,0.02104690400677744,0.784080253119182 +1735791941.794707127,0.06343376451372695,0.021046913298376227,0.7840802579842705 +1735791941.802271865,0.06343380294408478,0.021046922573640086,0.7840802628612003 +1735791941.810373474,0.06343384137601774,0.021046931834249268,0.7840802677421699 +1735791941.821676261,0.06343387980152793,0.021046941061010755,0.784080272611886 +1735791941.830370884,0.06343391822288891,0.021046950255338426,0.7840802774759794 +1735791941.843201887,0.06343395664109855,0.021046959417547428,0.7840802823372186 +1735791941.852857120,0.0634339950564928,0.021046968547500416,0.7840802871964975 +1735791941.863639232,0.0634340334691326,0.021046977644562696,0.7840802920544142 +1735791941.870323857,0.06343407187495703,0.021046986742124043,0.784080296899702 +1735791941.882933906,0.06343411027974119,0.021046995850156593,0.7840803017505191 +1735791941.886405393,0.06343414868246337,0.021047004966275675,0.7840803066040452 +1735791941.895105079,0.0634341870825647,0.021047014089023874,0.7840803114591578 +1735791941.904619861,0.06343422548023947,0.02104702321762641,0.7840803163155424 +1735791941.914893176,0.06343426387496795,0.02104703235153344,0.7840803211726323 +1735791941.918650278,0.06343430226690949,0.021047041490482466,0.7840803260303645 +1735791941.926814781,0.06343434065247949,0.021047050645637992,0.7840803308786837 +1735791941.935398239,0.06343437903396364,0.021047059815927735,0.7840803357234237 +1735791941.942059433,0.06343441741171635,0.02104706900084876,0.7840803405668341 +1735791941.946634756,0.0634344557862309,0.021047078200382097,0.7840803454101409 +1735791941.952553286,0.06343449415764152,0.02104708741463083,0.7840803502540767 +1735791941.957870801,0.06343453252585012,0.021047096643986414,0.7840803550985818 +1735791941.966716304,0.06343457089068893,0.021047105888829822,0.7840803599437955 +1735791941.974938173,0.06343460925230486,0.02104711514974949,0.7840803647897798 +1735791941.980014885,0.06343464761070892,0.021047124427297007,0.7840803696367319 +1735791941.984998528,0.06343468596231822,0.02104713370955128,0.7840803744730485 +1735791941.996289795,0.06343472431280107,0.021047142985504648,0.7840803793159709 +1735791942.5187586,0.06343476266120991,0.0210471522575938,0.7840803841621128 +1735791942.15275451,0.0634348010031512,0.021047161518275365,0.7840803890084765 +1735791942.18592470,0.0634348393428392,0.021047170759744216,0.7840803938569064 +1735791942.29910817,0.06343487767955125,0.02104717998235648,0.7840803987059202 +1735791942.42750812,0.06343491601341351,0.02104718918625244,0.7840804035550543 +1735791942.46456639,0.06343495434388388,0.021047198371259727,0.7840804084036881 +1735791942.51860833,0.06343499267112115,0.021047207537146426,0.784080413251748 +1735791942.58356467,0.06343503099516934,0.021047216683439302,0.7840804180993846 +1735791942.69386598,0.06343506931594113,0.021047225809687174,0.7840804229462849 +1735791942.76363568,0.06343510763331629,0.02104723491521563,0.7840804277924872 +1735791942.86717909,0.0634351459472311,0.021047244017862368,0.7840804326383375 +1735791942.100666909,0.06343518425415594,0.021047253129137144,0.7840804374734773 +1735791942.106082893,0.06343522255610698,0.021047262247418128,0.7840804423037945 +1735791942.120361066,0.06343526085383303,0.021047271371668435,0.784080447132156 +1735791942.127503097,0.06343529914806316,0.021047280501354123,0.784080451960076 +1735791942.144246305,0.06343533743847371,0.021047289636070728,0.7840804567878281 +1735791942.153599363,0.0634353757252771,0.021047298775649405,0.7840804616157351 +1735791942.164494076,0.06343541400830072,0.02104730791990067,0.7840804664439175 +1735791942.170326412,0.06343545228789617,0.021047317068809395,0.7840804712726301 +1735791942.181793019,0.06343549056360338,0.021047326222315622,0.7840804761015673 +1735791942.192364233,0.06343552883560223,0.0210473353805016,0.7840804809307851 +1735791942.199538189,0.06343556710393351,0.02104734454337326,0.7840804857604794 +1735791942.212533208,0.06343560536849917,0.021047353711102335,0.7840804905903451 +1735791942.239586056,0.06343564362916783,0.021047362883767913,0.7840804954204097 +1735791942.250731459,0.06343568188613471,0.021047372061596206,0.7840805002506849 +1735791942.275726879,0.06343572013946058,0.021047381244728373,0.7840805050813461 +1735791942.284554918,0.06343575838906751,0.02104739043346467,0.7840805099120789 +1735791942.298846471,0.063435796634843,0.021047399628007113,0.7840805147429064 +1735791942.304710556,0.06343583487720289,0.021047408828695335,0.7840805195740379 +1735791942.320124904,0.06343587311574841,0.021047418035799657,0.7840805244051459 +1735791942.327672762,0.06343591135071527,0.021047427249714534,0.7840805292362765 +1735791942.338738747,0.06343594958199077,0.02104743647074806,0.7840805340674216 +1735791942.341804781,0.06343598780999668,0.02104744569934096,0.7840805388987769 +1735791942.348547257,0.06343602603108248,0.021047454923913505,0.7840805437193603 +1735791942.353293147,0.06343606424737104,0.021047464146000307,0.7840805485348714 +1735791942.362222513,0.06343610245973601,0.021047473366409743,0.7840805533480999 +1735791942.367886659,0.06343614066841774,0.021047482585751443,0.7840805581599375 +1735791942.377775838,0.06343617887340759,0.021047491804327183,0.7840805629709688 +1735791942.380938525,0.06343621707823284,0.021047501014260735,0.7840805677821263 +1735791942.388635950,0.06343625528219408,0.02104751021565053,0.7840805725918562 +1735791942.399617967,0.06343629348487492,0.02104751940859717,0.7840805773990761 +1735791942.413333532,0.06343633168603445,0.021047528592983228,0.7840805822034792 +1735791942.421250918,0.06343636988606989,0.021047537768729956,0.7840805870051435 +1735791942.436662158,0.0634364080846242,0.021047546935602617,0.7840805918036997 +1735791942.452687086,0.06343644628201044,0.021047556093411652,0.7840805965991929 +1735791942.462410936,0.0634364844782156,0.021047565241807453,0.7840806013916315 +1735791942.469164782,0.06343652267377502,0.021047574380503458,0.7840806061812354 +1735791942.478594793,0.06343656086842121,0.021047583509072976,0.7840806109676995 +1735791942.489152958,0.06343663725274022,0.02104760174309915,0.7840806205309345 +1735791942.498575792,0.06343671361538435,0.02104762000386814,0.7840806300591285 +1735791942.502074651,0.06343675179171991,0.021047629146317942,0.7840806348180487 +1735791942.507473608,0.06343678996575788,0.021047638296047225,0.7840806395763171 +1735791942.525806109,0.06343690447511378,0.02104766578761982,0.7840806538509257 +1735791942.546456668,0.06343698080434852,0.021047684150754465,0.784080663368631 +1735791942.555386927,0.0634370189657915,0.021047693343300235,0.7840806681279093 +1735791942.560623881,0.06343705712545236,0.021047702543462943,0.7840806728876855 +1735791942.580489260,0.06343713343834927,0.021047720967577975,0.7840806824078153 +1735791942.587359203,0.06343717159185505,0.021047730192116955,0.7840806871684199 +1735791942.590387188,0.06343720974003242,0.02104773941335678,0.7840806919183504 +1735791942.594169652,0.06343724788466777,0.021047748632676774,0.7840806966633784 +1735791942.601233085,0.06343728602674983,0.021047757850959494,0.7840807014061595 +1735791942.607329369,0.06343732416663343,0.021047767068646136,0.7840807061480934 +1735791942.615701388,0.06343736230114086,0.021047776277753536,0.7840807108884685 +1735791942.624672708,0.06343740043085787,0.021047785478425667,0.7840807156293195 +1735791942.629445131,0.0634374385564596,0.021047794670765424,0.7840807203717781 +1735791942.640283127,0.06343751479444759,0.02104781303009852,0.7840807298620565 +1735791942.651082231,0.06343755290679426,0.02104782219678492,0.7840807346102063 +1735791942.657594878,0.06343759101492065,0.021047831354558746,0.7840807393606338 +1735791942.670876403,0.0634376291182823,0.021047840503098558,0.7840807441130299 +1735791942.684113807,0.06343766721695906,0.021047849642139217,0.7840807488674516 +1735791942.690760707,0.06343770531087913,0.02104785877126044,0.7840807536240982 +1735791942.698487530,0.06343774339982136,0.021047867890133777,0.7840807583826687 +1735791942.705248125,0.06343778148332233,0.021047877018151154,0.7840807631341831 +1735791942.711927443,0.06343781956263222,0.021047886153917835,0.7840807678825585 +1735791942.718136107,0.06343785763815012,0.02104789529658728,0.7840807726297697 +1735791942.726205468,0.0634378957098377,0.02104790444580959,0.7840807773763339 +1735791942.744369666,0.0634379337775087,0.021047913601319298,0.7840807821226613 +1735791942.750993927,0.06343797184147229,0.021047922763090126,0.7840807868691427 +1735791942.760185945,0.06343800990122293,0.021047931931085513,0.7840807916155363 +1735791942.767801531,0.06343804795690165,0.021047941105429714,0.7840807963619291 +1735791942.776772227,0.06343808600831657,0.0210479502861855,0.7840808011083319 +1735791942.783260789,0.06343812405582494,0.02104795947356689,0.7840808058549485 +1735791942.803588348,0.0634381620989846,0.021047968667729264,0.7840808106014487 +1735791942.815682277,0.06343820013799964,0.02104797786896122,0.7840808153478759 +1735791942.825632966,0.06343823817293857,0.021047987077468574,0.784080820094419 +1735791942.830944167,0.06343827620373316,0.021047996293617748,0.7840808248407685 +1735791942.836923024,0.063438314230283,0.021048005517678006,0.7840808295869482 +1735791942.842578694,0.06343835225281522,0.021048014750061576,0.7840808343329674 +1735791942.849411716,0.06343839027142296,0.0210480239910916,0.7840808390789988 +1735791942.857126317,0.06343842828606115,0.02104803324124637,0.7840808438247256 +1735791942.865301884,0.06343846629311178,0.02104804248793221,0.7840808485586079 +1735791942.871766538,0.06343850429535156,0.021048051715559297,0.7840808532867076 +1735791942.877379838,0.06343854229317122,0.021048060924955054,0.7840808580113011 +1735791942.885181934,0.06343858028711448,0.021048070116504572,0.7840808627336443 +1735791942.899329878,0.06343861827717004,0.021048079290156885,0.7840808674542993 +1735791942.905486980,0.06343865626377689,0.021048088445758915,0.7840808721737379 +1735791942.917325413,0.06343869424653105,0.021048097582910794,0.7840808768917678 +1735791942.929023999,0.06343873222565564,0.021048106701193715,0.7840808816085102 +1735791942.951454417,0.06343877020123033,0.021048115799974714,0.784080886324204 +1735791942.961619006,0.06343880817349905,0.02104812489669775,0.7840808910390273 +1735791942.977385176,0.06343884614231747,0.02104813399062832,0.7840808957529234 +1735791942.981848008,0.06343888410789429,0.02104814308126556,0.78408090046587 +1735791942.996427280,0.06343892207362355,0.021048152180171028,0.7840809051888312 +1735791943.2291453,0.06343896003409327,0.021048161297480935,0.7840809099056115 +1735791943.12401227,0.06343899799038899,0.021048170431254017,0.7840809146193481 +1735791943.21589283,0.06343903594348502,0.021048179580636896,0.7840809193316719 +1735791943.29027384,0.0634390738932523,0.021048188745230928,0.7840809240429031 +1735791943.33669011,0.06343911184006754,0.021048197925062052,0.7840809287533813 +1735791943.46798640,0.06343914978390282,0.021048207120252585,0.7840809334632303 +1735791943.59623334,0.06343918772524126,0.021048216331177948,0.7840809381727033 +1735791943.73750534,0.06343922566374459,0.02104822555822678,0.7840809428814915 +1735791943.81656446,0.06343926359970706,0.02104823480197647,0.7840809475896477 +1735791943.93938632,0.063439301529804,0.021048244050233603,0.784080952286014 +1735791943.102554345,0.06343933945604378,0.02104825330485753,0.7840809569763125 +1735791943.108341130,0.06343937737922084,0.021048262566974868,0.7840809616633395 +1735791943.120646448,0.06343941529997933,0.021048271817456735,0.7840809663572497 +1735791943.126833592,0.06343945321740828,0.021048281057939412,0.784080971055036 +1735791943.137072247,0.06343949113442676,0.02104829028103788,0.7840809757555989 +1735791943.142665319,0.06343952905001189,0.021048299487160388,0.7840809804563923 +1735791943.151234255,0.06343956696420583,0.021048308676487333,0.7840809851564683 +1735791943.159310748,0.06343960487648025,0.021048317848884356,0.7840809898549879 +1735791943.166901591,0.06343964278705119,0.021048327004160587,0.7840809945517803 +1735791943.172340759,0.06343968069583514,0.02104833614189396,0.7840809992467557 +1735791943.184532545,0.06343971860330344,0.021048345261670865,0.7840810039400896 +1735791943.193208457,0.06343975650912258,0.02104835436289229,0.7840810086314568 +1735791943.202559825,0.06343979440609668,0.021048363466785992,0.7840810133092906 +1735791943.214371591,0.06343983229733195,0.02104837257151811,0.7840810179820966 +1735791943.226847729,0.06343987018729931,0.021048381684575804,0.7840810226543585 +1735791943.244480482,0.06343990807569468,0.021048390805197204,0.7840810273260819 +1735791943.258641932,0.0634399459625881,0.0210483999330265,0.7840810319972787 +1735791943.265598436,0.0634399838479505,0.021048409067766127,0.7840810366681346 +1735791943.278690883,0.06344002173164093,0.021048418209360424,0.7840810413383478 +1735791943.284681330,0.06344005961350464,0.02104842735770651,0.7840810460079551 +1735791943.300607469,0.06344009749393131,0.021048436512873404,0.7840810506771789 +1735791943.310595179,0.06344013537250849,0.021048445674886174,0.7840810553457042 +1735791943.321377367,0.06344017324946814,0.021048454843914575,0.7840810600135891 +1735791943.329608132,0.06344021112470155,0.021048464020059866,0.7840810646808373 +1735791943.337628548,0.06344024899864027,0.02104847320357228,0.7840810693476551 +1735791943.348658707,0.0634402868709079,0.02104848239464352,0.7840810740137208 +1735791943.374713067,0.06344032474176661,0.021048491593599845,0.7840810786790888 diff --git a/webots_ros2_mavic/webots_ros2_mavic/prompts/mavic_basic.txt b/webots_ros2_mavic/webots_ros2_mavic/prompts/mavic_basic.txt new file mode 100644 index 000000000..53520eb04 --- /dev/null +++ b/webots_ros2_mavic/webots_ros2_mavic/prompts/mavic_basic.txt @@ -0,0 +1,226 @@ +Generate a list of waypoints for a drone in a Webots simulator in JSON format. +Each waypoint should include x, y, and z coordinates. +The waypoints should form a specific path (e.g., a straight line, a circular path, or any user-defined shape). +Please ensure the output is only formatted as a Python list of dictionaries. + + +Example Input: +- Path type: Circle +- Center of circle: x:10, y:10, z:10 +- Radius: 2 units +- Number of waypoints: 40 +- Altitude: z:10 +- Rotation axis : y +- Rotation angle : 90 degrees + +Example Output: +[ + {"x": 10.0, "y": 10.0, "z": 12.0}, + {"x": 10.31, "y": 10.0, "z": 11.98}, + {"x": 10.62, "y": 10.0, "z": 11.92}, + {"x": 10.92, "y": 10.0, "z": 11.81}, + {"x": 11.19, "y": 10.0, "z": 11.66}, + {"x": 11.43, "y": 10.0, "z": 11.48}, + {"x": 11.62, "y": 10.0, "z": 11.26}, + {"x": 11.76, "y": 10.0, "z": 11.02}, + {"x": 11.86, "y": 10.0, "z": 10.75}, + {"x": 11.90, "y": 10.0, "z": 10.47}, + {"x": 11.90, "y": 10.0, "z": 10.17}, + {"x": 11.86, "y": 10.0, "z": 9.89}, + {"x": 11.76, "y": 10.0, "z": 9.62}, + {"x": 11.62, "y": 10.0, "z": 9.38}, + {"x": 11.43, "y": 10.0, "z": 9.16}, + {"x": 11.19, "y": 10.0, "z": 8.98}, + {"x": 10.92, "y": 10.0, "z": 8.83}, + {"x": 10.62, "y": 10.0, "z": 8.73}, + {"x": 10.31, "y": 10.0, "z": 8.68}, + {"x": 10.0, "y": 10.0, "z": 8.68}, + {"x": 9.69, "y": 10.0, "z": 8.73}, + {"x": 9.38, "y": 10.0, "z": 8.83}, + {"x": 9.11, "y": 10.0, "z": 8.98}, + {"x": 8.87, "y": 10.0, "z": 9.16}, + {"x": 8.68, "y": 10.0, "z": 9.38}, + {"x": 8.54, "y": 10.0, "z": 9.62}, + {"x": 8.44, "y": 10.0, "z": 9.89}, + {"x": 8.40, "y": 10.0, "z": 10.17}, + {"x": 8.40, "y": 10.0, "z": 10.47}, + {"x": 8.44, "y": 10.0, "z": 10.75}, + {"x": 8.54, "y": 10.0, "z": 11.02}, + {"x": 8.68, "y": 10.0, "z": 11.26}, + {"x": 8.87, "y": 10.0, "z": 11.48}, + {"x": 9.11, "y": 10.0, "z": 11.66}, + {"x": 9.38, "y": 10.0, "z": 11.81}, + {"x": 9.69, "y": 10.0, "z": 11.92}, + {"x": 10.0, "y": 10.0, "z": 11.98}, + {"x": 10.31, "y": 10.0, "z": 12.0} +] + +Example Input: +- Path type: Circle +- Center of circle: x:10, y:10, z:10 +- Radius: 2 units +- Number of waypoints: 40 +- Altitude: z:10 +- Rotation axis : y +- Rotation angle : 45 degrees + +Example Output: +[ + {"x": 11.414213562373096, "y": 10.0, "z": 8.585786437626904}, + {"x": 11.39680224666742, "y": 10.312868930080462, "z": 8.60319775333258}, + {"x": 11.344997023927915, "y": 10.618033988749895, "z": 8.655002976072085}, + {"x": 11.2600735106701, "y": 10.907980999479093, "z": 8.7399264893299}, + {"x": 11.14412280563537, "y": 11.175570504584947, "z": 8.85587719436463}, + {"x": 11.0, "y": 11.414213562373096, "z":9.0}, + {"x": 10.831253875554907, "y": 11.618033988749895, "z": 9.168746124445093}, + {"x": 10.642039521920207, "y": 11.782013048376736, "z": 9.357960478079795}, + {"x": 10.437016024448821, "y": 11.902113032590307, "z": 9.562983975551179}, + {"x": 10.221231742082475, "y": 11.975376681190276, "z": 9.778768257917525}, + {"x": 10.0, "y": 12.0, "z": 10.0}, + {"x": 9.778768257917525, "y": 11.975376681190276, "z": 10.221231742082475}, + {"x": 9.562983975551179, "y": 11.902113032590307, "z": 10.437016024448821}, + {"x": 9.357960478079795, "y": 11.782013048376736, "z": 10.642039521920205}, + {"x": 9.168746124445093, "y": 11.618033988749895, "z": 10.831253875554907}, + {"x": 9.0, "y": 11.414213562373096, "z": 11.0}, + {"x": 8.85587719436463, "y": 11.175570504584947, "z": 11.144122805635368}, + {"x": 8.7399264893299, "y": 10.907980999479093, "z": 11.2600735106701}, + {"x": 8.655002976072085, "y": 10.618033988749895, "y": 11.344997023927915}, + {"x": 8.60319775333258, "y": 10.312868930080462, "z": 11.39680224666742}, + {"x": 8.585786437626904, "y": 10.0, "z": 11.414213562373096}, + {"x": 8.60319775333258, "y": 9.687131069919538, "z": 11.39680224666742}, + {"x": 8.655002976072085, "y": 9.381966011250105, "z": 11.344997023927915}, + {"x": 8.7399264893299, "y": 9.092019000520907, "z": 11.2600735106701}, + {"x": 8.85587719436463, "y": 8.824429495415053, "z": 11.14412280563537}, + {"x": 9.0, "y": 8.585786437626904, "z": 11.0}, + {"x": 9.168746124445093, "y": 8.381966011250105, "z": 10.831253875554907}, + {"x": 9.357960478079793, "y": 8.217986951623264, "z": 10.642039521920207}, + {"x": 9.562983975551179, "y": 8.097886967409693, "z": 10.437016024448821}, + {"x": 9.778768257917525, "y": 8.024623318809725, "z": 10.221231742082475}, + {"x": 10.0, "y": 8.0, "z": 10.0}, + {"x": 10.221231742082473, "y": 8.024623318809724, "z": 9.778768257917527}, + {"x": 10.437016024448821, "y": 8.097886967409693, "z": 9.562983975551179}, + {"x": 10.642039521920205, "y": 8.217986951623264, "z": 9.357960478079795}, + {"x": 10.831253875554907, "y": 8.381966011250105, "z": 9.168746124445093"}, + {"x": 11.0, "y": 8.585786437626904, "z": 9.0}, + {"x": 11.14412280563537, "y": 8.824429495415053, "z": 8.855877194364632}, + {"x": 11.2600735106701, "y": 9.092019000520907, "z": 8.7399264893299}, + {"x": 11.344997023927915, "y": 9.381966011250105, "z": 8.655002976072085}, + {"x": 11.39680224666742, "y": 9.687131069919538, "z": 8.60319775333258} +] + +Example Input: +- Path type: Circle +- Center of circle: x:10, y:10, z:10 +- Radius: 2 units +- Number of waypoints: 40 +- Altitude: z:10 +- Rotation axis : x +- Rotation angle : 60 degrees + +Example Output: +[ + {"x": 12.0, "y": 10.0, "z": 10.0}, + {"x": 11.975376681190276, "y": 10.156434465040231, "z": 10.270952441504537}, + {"x": 11.902113032590307, "y": 10.309016994374947, "z": 10.535233134659634}, + {"x": 11.782013048376736, "y": 10.453990499739547, "z": 10.78633461170248}, + {"x": 11.618033988749895, "y": 10.587785252292473, "z": 11.018073920910254}, + {"x": 11.414213562373096, "y": 10.707106781186548, "z": 11.22474487139159}, + {"x": 11.175570504584947, "y": 10.809016994374948, "z": 11.401258538444074}, + {"x": 10.907980999479093, "y": 10.891006524188368, "z": 11.543268569769602}, + {"x": 10.618033988749895, "y": 10.951056516295154, "z": 11.647278207092663}, + {"x": 10.312868930080462, "y": 10.987688340595138, "z": 11.710726387954173}, + {"x": 10.0, "y": 11.0, "z": 11.732050807568877}, + {"x": 9.687131069919538, "y": 10.987688340595138, "z": 11.710726387954173}, + {"x": 9.381966011250105, "y": 10.951056516295154, "z": 11.647278207092665}, + {"x": 9.092019000520907, "y": 10.891006524188368, "z": 11.543268569769602}, + {"x": 8.824429495415053, "y": 10.809016994374948, "z": 11.401258538444074}, + {"x": 8.585786437626904, "y": 10.707106781186548, "z": 11.22474487139159}, + {"x": 8.381966011250105, "y": 10.587785252292473, "z": 11.018073920910254}, + {"x": 8.217986951623264, "y": 10.453990499739547, "z": 10.78633461170248}, + {"x": 8.097886967409693, "y": 10.309016994374948, "z": 10.535233134659634}, + {"x": 8.024623318809725, "y": 10.156434465040231, "z": 10.270952441504537}, + {"x": 8.0, "y": 10.0, "z": 10.0}, + {"x": 8.024623318809724, "y": 9.843565534959769, "z": 9.729047558495463}, + {"x": 8.097886967409693, "y": 9.690983005625053, "z": 9.464766865340366}, + {"x": 8.217986951623264, "y": 9.546009500260453, "z": 9.21366538829752}, + {"x": 8.381966011250105, "y": 9.412214747707527, "z": 8.981926079089746}, + {"x": 8.585786437626904, "y": 9.292893218813452, "z": 8.77525512860841}, + {"x": 8.824429495415053, "y": 9.190983005625052, "z": 8.598741461555926}, + {"x": 9.092019000520907, "y": 9.108993475811632, "z": 8.456731430230398}, + {"x": 9.381966011250105, "y": 9.048943483704846, "z": 8.352721792907337}, + {"x": 9.687131069919538, "y": 9.012311659404862, "z": 8.289273612045827}, + {"x": 10.0, "y": 9.0, "z": 8.267949192431123}, + {"x": 10.31286893008046, "y": 9.012311659404862, "z": 8.289273612045827}, + {"x": 10.618033988749895, "y": 9.048943483704846, "z": 8.352721792907335}, + {"x": 10.907980999479093, "y": 9.108993475811632, "z": 8.456731430230398}, + {"x": 11.175570504584947, "y": 9.190983005625052, "z": 8.598741461555926}, + {"x": 11.414213562373094, "y": 9.292893218813452, "z": 8.77525512860841}, + {"x": 11.618033988749895, "y": 9.412214747707527, "z": 8.981926079089746}, + {"x": 11.782013048376736, "y": 9.546009500260453, "z": 9.21366538829752}, + {"x": 11.902113032590307, "y": 9.690983005625052, "z": 9.464766865340366}, + {"x": 11.975376681190275, "y": 9.843565534959769, "z": 9.729047558495463} +] + + +Example Input: +- Path type: Straight line +- Start point: x:10, y:5, z:5 +- End point: x:20, y:15, z:10 +- Number of waypoints: 3 + +Example Output: + +[ + {"x": 10.0, "y": 5.0, "z": 5.0}, + {"x": 15.0, "y": 10.0, "z": 7.5}, + {"x": 20.0, "y": 15.0, "z": 10.0} +] + +You can use the following python code to generate more accurate waypoints. + +import numpy as np +import json + +def rotation_matrix(axis, angle): + + axis = axis / np.linalg.norm(axis) # Normalisasi sumbu + ux, uy, uz = axis + cos_a = np.cos(angle) + sin_a = np.sin(angle) + return np.array([ + [cos_a + ux**2 * (1 - cos_a), ux*uy*(1 - cos_a) - uz*sin_a, ux*uz*(1 - cos_a) + uy*sin_a], + [uy*ux*(1 - cos_a) + uz*sin_a, cos_a + uy**2 * (1 - cos_a), uy*uz*(1 - cos_a) - ux*sin_a], + [uz*ux*(1 - cos_a) - uy*sin_a, uz*uy*(1 - cos_a) + ux*sin_a, cos_a + uz**2 * (1 - cos_a)] + ]) + +def generate_waypoints(center, radius, num_points, axis, angle): + + # Buat lingkaran dalam bidang XY + t = np.linspace(0, 2 * np.pi, num_points, endpoint=False) + x = radius * np.cos(t) + y = radius * np.sin(t) + z = np.zeros_like(x) + waypoints = np.array([x, y, z]) + + # Terapkan rotasi untuk kemiringan + rot_matrix = rotation_matrix(axis, angle) + rotated_waypoints = rot_matrix @ waypoints + + # Pindahkan lingkaran ke pusat yang ditentukan + final_waypoints = rotated_waypoints + np.array(center).reshape(-1, 1) + + return final_waypoints.T # Kembalikan dalam bentuk array waypoint + +# Parameter +center = [10, 10, 10] # centre of circle +radius = 2 # radius of circle +num_points = 40 # number of wayponts +axis = [0, 1, 0] # axis rotation (exc, axis of y) +angle = np.radians(45) # angle tilt (on radian) + +# Generate waypoints +waypoints = generate_waypoints(center, radius, num_points, axis, angle) + +waypoint_list = [{"x": waypoint[0], "y": waypoint[1], "z": waypoint[2]} for waypoint in waypoints] + +json_data = json.dumps(waypoint_list)