Printing only the starting position and final position in ROS

Question

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.

Answer 1

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]