HamsterPythonExample2

Python Example 2

Detect right/left turn from IMU

• IMU angles are represented in Quaternions [[1]] hence translated to RPY (roll pitch yaw)
• RPY are in radians, translated to degrees

<syntaxhighlight lang="python" line='line'>

1. !/usr/bin/env python

import rospy import math from sensor_msgs.msg import Imu from tf.transformations import euler_from_quaternion, quaternion_from_euler

def calculate_robot_angle(orientation): quaternion = ( orientation.x, orientation.y, orientation.z, orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return math.degrees(yaw)

prev_angle = 0 def callback(msg): global prev_angle curr_angle = calculate_robot_angle(msg.orientation) if curr_angle > prev_angle : print "TURINING LEFT" else: print "TURINIGN RIGHT" prev_angle = curr_angle

rospy.init_node('imu_values')

1. Put your Hamster number instead of agent<number>
2. The agent number is printed on the Hamster cover

sub = rospy.Subscriber('/agent14/imu', Imu, callback)

rospy.spin() </syntaxhighlight>

To execute save this example in example2.py and type in the terminal

<syntaxhighlight lang="bash" line='line'>

$ python example2.py

</syntaxhighlight>