def get_rotation1(msg, pub):
# establish a threshold for the wrap counter, in this case the threshold is 15 degrees
# but the value must be converted to radians for use on the angles directly
threshold = 15 * math.pi / 180
# create a blank object of type Position()
pos_msg = Position()
# in python use of the "global" keyword inside a function indicates that use of the
# specified variable names within that function should refer to the global instance
# of variables with that same name, in this case within the function get_rotation
# anytime we use "roll","pitch","yaw","wrapped_yaw","old_yaw","num_wraps" we are
# referring to the global variables with these names (these variables are defined
# at the global scope, outside of the calling function)
global roll1, pitch1, yaw1, wrapped_yaw1, old_yaw1, num_wraps1
# the following code is copied from an example on converting quaternions to euler angles
# source:
# http://www.theconstructsim.com/ros-qa-how-to-convert-quaternions-to-euler-angles/
orientation_q = msg.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
# the following block catches the yaw discontinuity and increments the wrap counter
# valid yaw range is from -pi to pi
if old_yaw < -threshold and yaw > threshold: # from -pi to pi (increasing negative)
num_wraps = num_wraps - 1
elif old_yaw > threshold and yaw < -threshold:
num_wraps = num_wraps + 1
# establish the new value of wrapped_yaw using the updated num_wraps
wrapped_yaw = yaw + 2 * math.pi * num_wraps
# update old_yaw, this variable is used in the wrap catch block
old_yaw = yaw
# write the converted angles to the angular properties of the position message
# also convert the radian positions to degrees
pos_msg.angular.roll = roll * 180 / math.pi
pos_msg.angular.pitch = pitch * 180 / math.pi
pos_msg.angular.yaw = wrapped_yaw * 180 / math.pi
# make sure we publish the message
pub1.publish(pos_msg)
def odom(data,pub):
global x
global y
global old_yaw,num_wraps,yaw
threshold = 15*math.pi/180
pos_msg = Position()
x = data.pose.pose.position.x
y = data.pose.pose.position.y
rot_quat = data.pose.pose.orientation
(roll,pitch,yaw) = euler_from_quaternion ([rot_quat.x,rot_quat.y,rot_quat.z,rot_quat.w])
if old_yaw < -threshold and yaw > threshold: # from -pi to pi (increasing negative)
num_wraps = num_wraps - 1
elif old_yaw > threshold and yaw < -threshold:
num_wraps = num_wraps + 1
wrapped_yaw = yaw + 2*math.pi*num_wraps
old_yaw = yaw
pos_msg.angular.roll = roll * 180/math.pi
pos_msg.angular.pitch = pitch * 180/math.pi
pos_msg.angular.yaw = wrapped_yaw * 180/math.pi
pub1.publish(pos_msg)
def callback_ori(data, data_out):
data_out.angular.roll = data.angular.roll
data_out.angular.pitch = data.angular.pitch
data_out.angular.yaw = data.angular.yaw
def callback_cmd(data, data_out):
data_out.linear.x = data.linear.x
data_out.angular.yaw = data.angular.yaw
if __name__ == "__main__":
try:
position = Position()
twist = Twist()
rospy.Subscriber('/eul', Position, callback_ori, (position))
rospy.Subscriber('/odom', Odometry, callback_pos, (position))
rospy.Subscriber('cmd_vel', Twist, callback_cmd, (twist))
rospy.init_node('LogFileNode', anonymous=True)
# create a rate object for timing, the argument is in Hz
r = rospy.Rate(100) #HZ
# this will constitute the header for the columns in the csv file, this is simply because
# it is the first line which will be written
myData = ["x", "y", "z", "roll", "pitch", "yaw", "CMD", "CMD2"]
# the following code creates a base filename containing the data and time
fileNameBase = "/home/reven/me_401/homework_ws/src/beginner_tutorials/scripts" + datetime.datetime.now(
