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(