Agfdhyk

I'm new to ROS and I was given some code to "play with".
I want my turtle to go straight one meter and then turn in a 45 degrees angle.
I'm getting the right result (Or at least it looks that way...) but I also want
to print the starting and final location of my turtle. I added some code that
prints the log in a non stop fashion, meaning every iteration I get the x,y position of my turtle, but I only want it in the beginning and the end, plus I want to add an angle theta that will represent the angle that my turtle is at.

This is my code:

import sys, rospy

from geometry_msgs.msg import Twist

from turtlesim.msg import Pose

PI = 3.1415926535897



theta = 0



def pose_callback(pose_msg):

    rospy.loginfo("x: %.2f, y: %.2f" % (pose_msg.x, pose_msg.y))



def move():

    msg.linear.x = FORWARD_SPEED_IN_MPS

    t0 = rospy.Time.now().to_sec()

    current_distance = 0

    # Move turtle as wanted distance

    while current_distance < DISTANCE_IN_METERS:

        pub.publish(msg)

        # Get current time.

        t1 = rospy.Time.now().to_sec()

        # Calc how much distance our turtle moved.

        current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)

    msg.linear.x = 0



def turn():

    current_angle = 0

    angular_speed = ROUND_SPEED * 2 * PI / 360

    relative_angle = 45 * 2 * PI / 360

    t0 = rospy.Time.now().to_sec()

    msg.angular.z = abs(angular_speed)

    while current_angle < relative_angle:

        pub.publish(msg)

        t1 = rospy.Time.now().to_sec()

        current_angle = angular_speed * (t1 - t0)



if __name__ == "__main__":

    robot_name = sys.argv[1]

    FORWARD_SPEED_IN_MPS = 0.5

    DISTANCE_IN_METERS = 1

    ROUND_SPEED = 5



    # Initialize the node

    rospy.init_node("move_turtle")

    # A publisher for the movement data

    pub = rospy.Publisher(robot_name+"/cmd_vel", Twist, queue_size=10)

    # A listener for pose

    sub = rospy.Subscriber(robot_name+"/pose", Pose, pose_callback, None, 10)



    # The default constructor will set all commands to 0

    msg = Twist()

    pose = Pose()

    # Loop at 10Hz, publishing movement commands until we shut down

    rate = rospy.Rate(10)

    # Drive forward at a given speed.  The robot points up the x-axis.

    move()

    # Turn counter-clockwise at a given speed.

    turn()

Thanks for your help.

You can get the position, orientation, and velocity from Turtlesim Pose Message and I added a condition which checking the robot velocities:

import rospy

import time

from geometry_msgs.msg import Twist

from turtlesim.msg import Pose



PI = 3.1415926535897

theta = 0



def pose_callback(msg):

    if msg.linear_velocity == 0 and msg.angular_velocity == 0:

        rospy.loginfo("x: %.2f, y: %.2f" % (msg.x, msg.y))

        rospy.loginfo('Orientation in euler - theta:{}'.format(msg.theta))



def move():

    msg.linear.x = FORWARD_SPEED_IN_MPS

    t0 = rospy.Time.now().to_sec()

    current_distance = 0



    while current_distance < DISTANCE_IN_METERS:

        pub.publish(msg)

        t1 = rospy.Time.now().to_sec()

        current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)

    msg.linear.x = 0



def turn():

    current_angle = 0

    angular_speed = ROUND_SPEED * 2 * PI / 360

    relative_angle = 45 * 2 * PI / 360

    t0 = rospy.Time.now().to_sec()

    msg.angular.z = abs(angular_speed)



    while current_angle < relative_angle:

        pub.publish(msg)

        t1 = rospy.Time.now().to_sec()

        current_angle = angular_speed * (t1 - t0)



if __name__ == "__main__":

    FORWARD_SPEED_IN_MPS = 0.5

    DISTANCE_IN_METERS = 1

    ROUND_SPEED = 5

    rospy.init_node("move_turtle")

    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10)

    sub = rospy.Subscriber("turtle1/pose", Pose, pose_callback)

    msg = Twist()

    rate = rospy.Rate(100)

    move()

    turn()

    time.sleep(2)

[NOTE]:

Orientation default message in turtlesim is euler type, but in most of ROS nodes type is quaternion, so if you want to get the quaternion orientation type, you must convert it:

from tf.transformations import quaternion_from_euler



euler = (0, 0, pose.z)



quaternion = quaternion_from_euler(euler)

x = quaternion[0]

y = quaternion[1]

z = quaternion[2]

w = quaternion[3]