ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Turtle Can't Go to New Goal Point

asked 2020-12-21 12:35:35 -0500

Ertugrulsngr gravatar image

updated 2020-12-22 08:54:56 -0500

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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-12-22 06:20:50 -0500

tryan gravatar image

If I understand you correctly, you're trying to preempt navigation to your goal pose. In ROS, that's usually handled with the help of actionlib (wiki). Specifically, an action server allows that behavior. Here's the general tutorial for a SimpleActionServer. It's also worth noting that the move_base package implements such a server as part of the ROS navigation stack. I highly recommend moving in that direction.

If you're set on doing this without an action server, be aware that you may run into timing problems. You'll have to break out of your while loops when a new callback starts, and not start the new callback until you have. You can use global variables to help with that. I don't have a good ROS example to share, so I'll leave that to someone else.

edit flag offensive delete link more

Comments

1

If i understand him correctly, this should be the answer. You can cancel all goals in the actionlib and set a new one. Actually i did something similar last week where the robot finds the goals itself and drives towards them but changes the goal during driving, if it finds a better one. Be aware that some local planners have strict states for waiting for goal, planning and driving hence i recommend to cancel all goals before sending a new one.

Tristan9497 gravatar image Tristan9497  ( 2020-12-22 06:56:33 -0500 )edit

I solved. Thanks!

Ertugrulsngr gravatar image Ertugrulsngr  ( 2020-12-22 08:47:59 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-12-21 12:35:35 -0500

Seen: 179 times

Last updated: Dec 22 '20