Unpredictable Behavior with Goal Oriented turtlesim script
Hi,
I wrote a short script in python to move the turtle to 4 different goal positions, similar to the "go to goal" code found here: http://wiki.ros.org/turtlesim/Tutoria...
When the turtle has only 1 goal (comment out the last 3 goals), the code works fine. However, with 2 or more consecutive goals, the turtle moves sporadically and never makes it to any of the goal poses. Instead, the turtle will endlessly try to move off the turtlesim window. Here is the code I wrote:
class TurtleBot(object):
def __init__(self):
rospy.init_node('turtlebot', anonymous=True)
self.k_linear = 2
self.k_angular = 3.0
self.dist_thresh = 0.5
self.pose = Pose()
self.rate = rospy.Rate(10)
self.turtle_sub = rospy.Subscriber("/turtle1/pose", Pose, self.callback)
self.vel_pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
def callback(self, data):
self.pose = data
def moveToGoal(self, goal_pose):
while self.dist(goal_pose) >= self.dist_thresh:
vel_msg = Twist()
#linear velocity
vel_msg.linear.x = self.k_linear * self.dist(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.k_angular * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta)
self.vel_pub.publish(vel_msg)
print(self.dist(goal_pose))
self.rate.sleep()
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.vel_pub.publish(vel_msg)
def dist(self, goal_pose):
#distance = np.sqrt( (goal_pose.x - self.pose.x)**2 + (goal_pose.y - self.pose.y)**2 )
distance = sqrt( pow(goal_pose.x - self.pose.x, 2) + pow(goal_pose.y - self.pose.y, 2) )
return distance
def newGoal(self, x_pose, y_pose):
goal = Pose()
goal.x = x_pose
goal.y = y_pose
return goal
def main(args):
turtle1 = TurtleBot()
#random goal x,y positions
goal1 = turtle1.newGoal(7, 7)
goal2 = turtle1.newGoal(3, 7)
goal3 = turtle1.newGoal(2, 3)
goal4 = turtle1.newGoal(7, 2)
turtle1.moveToGoal(goal1)
turtle1.moveToGoal(goal2)
turtle1.moveToGoal(goal3)
turtle1.moveToGoal(goal4)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
if __name__=='__main__':
main(sys.argv)
The underlying math is the same as the 'go to goal' code referenced above (or at least, it should be). I'm not sure what is causing the poor behavior. Any help is appreciated!
Misc. Info: Running on Ubuntu 16.04 VM, ROS Kinetic, using ROS turtlesim package
Looks to me like this is not a ROS problem, but instead a control problem. Your controller doesn't actually work to stabilize the way you think it should. Ideas to fix: try implementing a provably stable controller for posture stabilization of a unicycle model (sometimes aka kinematic car)....
Here's one possible reference Stabilized Feedback Control of Unicycle Mobile Robots . Or you could try implementing a simpler controller that does something like turn towards goal, drive to goal, turn towards goal orientation.
Ah I see. I didn't think I would have to model the controller the same way, I just assumed the turtle could pivot. I'll try to implement this. Thanks!
Not sure what you mean by "pivot", but the turtle's kinematics are the same as a differential drive car aka unicycle model aka kinematic car. (xdot, ydot, thetadot) = f(x,u) = (v cos(theta), v sin(theta), omega) This can rotate in place if given an omega, but no v.
So after thinking about this some more, I don't think I fully understand your previous comments. The current feedback controller works fine for just 1 goal and is a slightly more complex version of the 'turn towards goal, drive to goal' controller.
Since the controller works for just 1 goal, and the subsequent 3 are just loops of the same logic with just different initial conditions and a different goal position, I don't understand why this works for 1 goal but not 4.
Does your controller always work for one goal? My guess is that it doesn't. There are likely goal positions where your controller fails (as mentioned in my first comment). You are asserting that the issue is that you have more than one goal, and I'm asserting that it is actually that you have....
... goals that never work when you start increasing the number. Try sticking with 1 goal, but move it's position around all over the window. Does it always work?