Messi > CR7

import rospkg
import nav_msgs

from geometry_msgs.msg import PoseStamped

from pathlib import Path # Possible conflict with nav_msgs.msg.Path

def on_init(): # Put
  # Timer
  self.timer_pub_path = rospy.Timer(rospy.Rate(5), callback_timer_publish_path)
  # Publisher
  self.pub_path = rospy.Publisher('/turtlebot_path', nav_msgs.msg.Path, 1)

# Timer to publish the path with a frequency of 5 Hz
def callback_timer_publish_path(self, event):
    self.pub_path.publish(self.path)

def load_trajectory_from_file(self):
  # TODO: MAKE THE LAUNCH LOAD THE TRAJECTORY FILE (https://riptutorial.com/ros/example/24423/launch-ros-nodes-and-load-parameters-from-yaml-file)
  # Get path to the package directory
  rospack = rospkg.RosPack()
  my_package_path = rospack.get_path('your_package_name')
  print(my_package_path)

  # Get path to the trajectory file
  my_filename = "eight_shape_trajectory_r15.txt"
  file_path = Path(my_package_path + '/config/sample_trajectories/') / my_filename
  print(file_path)

  # Read trajectory file
  with open(file_path) as f:
    contents = f.readlines()
    print(contents)

    # Create path message
    self.path = nav_msgs.msg.Path()
    self.path.header.frame_id = 'map'
    self.path.header.stamp = rospy.Time.now

    for line in contents:
      # Convert each line into a geometry_msgs/Pose message
      splitted_line = line.split(" ")
      x = float(splitted_line[0])
      y = float(splitted_line[1])
      heading = float(splitted_line[2])

      pose_stamped = PoseStamped()
      pose_stamped.pose.position.x = x
      pose_stamped.pose.position.y = y
      pose_stamped.pose.position.z = 0

      quat = quaternion_from_euler(0, 0, heading)
      pose_stamped.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])

      # Save trajectory into a nav_msgs/Path message
      self.path.poses.append(pose_stamped)