Best Course of Actions for Following a set of Goals

edit

Hi,

A ROS newbie here and I wanted to discuss the best course of action for the current project I am trying to do. I have a Jackal Robot and I want it to follow a trajectory in discrete time-steps. I have another 2D particle simulator in which a particle is moving from point A to point B, I want my jackal to exactly follow the same trajectory that my particle has ran(ignore the scale and transformations). Right now the trajectory consists of 2D coordinates which I am scaling and sending it to jackal as goals and making it move to the coordinate. My current approach is given below. I do not want any localization, mapping or anything fancy. Since there is tons of stuff like actions, services, navigation stack etc, is there something better that can be done or the logic below is fine?

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Twist
from tf.transformations import euler_from_quaternion
import math

x = 0.0
y = 0.0
theta = 0.0

def newOdom(msg):
    global x
    global y
    global theta
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    #print(x, y)
    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

goals = [Point(0, 0, 0), Point(4, 4, 0), Point(-1, -1, 0)]
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
r = rospy.Rate(10)
speed = Twist()
goal = goals.pop()
while not rospy.is_shutdown():
    delta_x = goal.x -x
    delta_y = goal.y -y
    angle_to_goal = math.atan2(delta_y, delta_x)
    dist = math.sqrt(delta_x**2 + delta_y**2)
    #print(dist)
    if dist > 0.1:
        if abs(angle_to_goal-theta) > 0.1:
            speed.linear.x = 0.0
            speed.angular.z = 0.5
        else:
            speed.linear.x = 1.0
            speed.angular.z = 0.0
    else:
        if goals:
            goal = goals.pop()
            print(goal.x, goal.y)
        else:
            speed.linear.x = 0.0
            speed.angular.z = 0.0
    pub.publish(speed)
    r.sleep()