Turtle Can't Go to New Goal Point
Hi everyone, I'm trying to do is that move turtle to new goal point while turtle is already going a different goal point. But when I publish my new goal point, turtle can't go and turn between 2 angle. My code is here:
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point
from turtlesim.msg import Pose
from math import pow, atan2, sqrt
from time import sleep
from threading import Thread
class Turtle():
def __init__(self):
rospy.init_node("turtlecontroller", anonymous=True)
self.goal_pose = Pose()
self.pose = Pose()
self.goal_pose_flag = False
self.velocity_publisher = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
self.goal_pose_subscriber = rospy.Subscriber("/cmd/goal_pose", Pose, self.take_goal_pose)
self.pose_subscriber = rospy.Subscriber("/turtle1/pose", Pose, self.take_pose)
rospy.wait_for_message("/cmd/goal_pose", Pose)
self.rate = rospy.Rate(10)
def take_pose(self, pose_message):
self.pose.x = pose_message.x
self.pose.y = pose_message.y
self.pose.theta = pose_message.theta
def take_goal_pose(self, goal_position_msg):
self.goal_pose_flag = False
sleep(1)
self.goal_pose.x = goal_position_msg.x
self.goal_pose.y = goal_position_msg.y
self.goal_pose.theta = goal_position_msg.theta
self.goal_pose_flag = True
self.start()
def start(self):
if self.turn_turtle_to_goal() == False:
return
if self.move_turtle_to_goal() == False:
return
self.turn_turtle_to_angular_goal()
def find_distance(self):
distance = sqrt(pow((self.goal_pose.x - self.pose.x), 2) + pow((self.goal_pose.y - self.pose.y), 2))
return distance
def find_angle(self):
angle = atan2(self.goal_pose.y - self.pose.y, self.goal_pose.x - self.pose.x)
return angle
def turn_turtle_to_goal(self):
velocity_message = Twist()
coefficient_angular = 2
while True:
if self.goal_pose_flag == False:
return False
angular_speed = (self.find_angle() - self.pose.theta) * coefficient_angular
velocity_message.angular.z = angular_speed
self.velocity_publisher.publish(velocity_message)
if ((self.pose.theta + 0.001) >= self.find_angle() >= (self.pose.theta - 0.001)) == True:
break
def move_turtle_to_goal(self):
velocity_message = Twist()
coefficient_linear = 0.5
while True:
if self.goal_pose_flag == False:
return False
linear_speed = self.find_distance() * coefficient_linear
velocity_message.linear.x = linear_speed
self.velocity_publisher.publish(velocity_message)
if self.find_distance() < 0.1:
break
def turn_turtle_to_angular_goal(self):
velocity_message = Twist()
coefficient_angular = 2
while True:
if self.goal_pose_flag == False:
return False
angular_speed = (self.goal_pose.theta - self.pose.theta) * coefficient_angular
velocity_message.angular.z = angular_speed
self.velocity_publisher.publish(velocity_message)
if ((self.pose.theta + 0.001) >= self.goal_pose.theta >= (self.pose.theta - 0.001)) == True:
break
print(self.pose.x)
print(self.pose.y)
print(self.pose.theta)
print("-------------")
print(self.goal_pose.x)
print(self.goal_pose.y)
print(self.goal_pose.theta)
if __name__ == "__main__":
while True:
turtle = Turtle()
I publish this message on terminal
rostopic pub /cmd/goal_pose turtlesim/Pose "{x: 0.0, y: 0.0, theta: 0.0, linear_velocity: 0.0, angular_velocity: 0.0}"
When receive a message from /cmd/goal_pose topic, take_goal_pose function starts. I think I should set that when receive a message from /cmd/goal_pose topic take_goal_pose function restarts. How can I solve my problem? Thanks!
I solved it with using threading module. I created a function that name is check_new_goal_gived and check is number_of_taken_goal equal itself. When I publish a new goal, number_of_taken_goal is icreased by take_goal_pose function. Then check_new_goal_gived function determine it ...
