In this video we are going to see how can we subscribe to topics so that we can get the position and acceleration of a Sphero robot.

This video is especially good for beginners who have already understood the basics, and want to start writing their own code. We will try to properly structure our Python code, as well as follow some programming good practices.

This is a video based on the following post on The Construct’s Forum: http://forum.theconstructsim.com/questions/1188/print-position-angular-acceleration

// RELATED LINKS

▸ Original question: http://forum.theconstructsim.com/questions/1188/print-position-angular-acceleration
▸ ROS Development Studio (ROSDS)
▸ Robot Ignite Academy

Step 1. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. Let’s create a new project and call it Print_Position_QA.

Step 2. Create a package

To reproduce the question, we’ll use the original code from the forum. Let’s create a package for the code first with the following command.

cd ~/catkin_ws/src
catkin_create package print_pos rospy

Then we create a script file called main.py and put it in the scripts folder under the print_pos package with the following content from the forum.

! /usr/bin/env python
import rospy

from geometry_msgs.msg import Twist

from nav_msgs.msg import Odometry

from sensor_msgs.msg import Imu

rospy.init_node('sphero')

pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) #topic publisher that allows you to move the sphero

rate = rospy.Rate(0.5)

def odom (msg):

go = Odometry()

print "pose x = " + str(go.pose.pose.position.x)

print "pose y = " + str(go.pose.pose.position.y)

print "orientacion x = " + str(go.pose.pose.orientation.x)

print "orientacion y = " + str(go.pose.pose.orientation.y)

rate.sleep()

sub = rospy.Subscriber('odom', Odometry, odom)

def imu (msg):

allez = Imu()

print "veloc angular z = " + str(allez.angular_velocity.z)

print "veloc angular y = " + str(allez.angular_velocity.y)

print "aceleracion linear x = " + str(allez.linear_acceleration.x)

print "aceleracion linear y = " + str(allez.linear_acceleration.y)

rate.sleep()

sub = rospy.Subscriber('sphero/imu/data3', Imu, imu)

def twist (msg):

move = Twist()

print "velocidad linear x = " + str(move.linear.x)

print "velocidad angular z = " + str (move.angular.z)

rate.sleep()

sub=rospy.Subscriber('cmd_vel', Twist, twist)

while not rospy.is_shutdown():

move = Twist()

move.linear.x = 2

move.angular.z= 0.5

pub.publish(move)

rospy.spin()

Some part of the code is not correct(e.g. the subscriber is not subscribing correctly so there is no reading), let’s correct it as follows.

#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu

def odom_callback(msg):
    # go = Odometry() is not needed
    print "------------------------------------------------"
    print "pose x = " + str(msg.pose.pose.position.x)
    print "pose y = " + str(msg.pose.pose.position.y)
    print "orientacion x = " + str(msg.pose.pose.orientation.x)
    print "orientacion y = " + str(msg.pose.pose.orientation.y)
    rate.sleep()

def imu_callback(msg):
    # allez = Imu()
    print "------------------------------------------------"
    print "veloc angular z = " + str(msg.angular_velocity.z)
    print "veloc angular y = " + str(msg.angular_velocity.y)
    print "aceleracion linear x = " + str(msg.linear_acceleration.x)
    print "aceleracion linear y = " + str(msg.linear_acceleration.y)
    rate.sleep()

def twist (msg):
    # move = Twist()
    print "velocidad linear x = " + str(move.linear.x)
    print "velocidad angular z = " + str (move.angular.z)
    rate.sleep()
    #sub=rospy.Subscriber('cmd_vel', Twist, twist)

rospy.init_node('sphero_monitor') # the original name sphero might be the same as other node.
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) #topic publisher that allows you to move the sphero
sub_odom = rospy.Subscriber('/odom', Odometry, odom_callback) # the original name odom might be the same as other function.
sub_imu = rospy.Subscriber('/sphero/imu/data3', Imu, imu_callback)
rate = rospy.Rate(0.5)

while not rospy.is_shutdown():
    move = Twist()
    move.linear.x = 0.1 # m/s. The original value 2 is too large
    move.angular.z= 0.5 # rad/s
    pub.publish(move)
    rate.sleep() # Instead of using rospy.spin(), we should use rate.sleep because we are in a loop

Before executing the code, we have to give it permission first with chmod +x main.py  and we also need the simulation(start it from Simulations->Sphero).

Then we can run the code with rosrun print_pos main.py

You should see the readings from sensors and the robot also starts to move around.

If you are interested in this topic, please check our ROS Basics in 5 Days course. You’ll learn how to control the Sphero robot to exit the maze in this course.

 

Edit by: Tony Huang

---

Feedback

Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know on the comments area and we will do a video about it.