imu2pose.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('iri_msg_to_odom')
import os
import sys
import rospy
import tf
from sensor_msgs.msg import Imu
from iri_segway_rmp_msgs.msg import SegwayRMP400Status
from geometry_msgs.msg import PoseStamped,Quaternion,Vector3
from tf.transformations import euler_from_quaternion
import math


#  -------------------------------------------
#  node: imu2pose.py
#  author: Marti Morta (mmorta@iri.upc.edu)
#  date: 25 Jun 2013
#  -------------------------------------------
#
#   This node converts imu and segway data to ready to be plot data
#   
#   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
#   Subscribers
#     imu: Imu topic
#     segway rmp400 status: SegwayRMP400Status topic
#     
#   >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#   Publishers
#     imu_pose: PoseStamped as 0 and imu orientation. Rviz axes
#     imu_angles: Vector of roll,pitch,yaw angles from Imu orientation quaternion. Rxplot
#     imu_angles_from_vel: RPY from the integration of Imu angular velocities. Rxplot
#     segway_angles_from_vel: RPY from the integratino of Segway's angular velocities. Rxplot
#     
#   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#   Parameters:
#     use_degrees: Publishes the angles in degrees
#     use_first_only: In segway's angles calculation use only the first base
#     use_second_only: In segway's angles calculation use only the second base
#   

# Set Publishers
p_p = rospy.Publisher('imu_pose', PoseStamped)
p_a = rospy.Publisher('imu_angles', Vector3)
p_A = rospy.Publisher('imu_angles_from_vel', Vector3)
p_s = rospy.Publisher('segway_angles_from_vel', Vector3)

# IMU Callback function
#   pre: imu message
#   post: 1) Imu orientation as PoseStamped
#         2) RPY from Imu orientation quaternion
#         3) RPY from Imu angular velocities integration
def callback_imu(data):
  # 
  # 1)
  # 
  # Set transform position between /teo/imu2 (=/teo/imu), frame where the axes will be attached, and base_link
  br = tf.TransformBroadcaster()
  br.sendTransform((0.0, 0.0, 0.0),(0.0, 0.0, -0.7071, 0.7071), rospy.Time.now(), "/teo/imu2", "/teo/base_link")
  # cook the message
  imu_pose.header = data.header
  imu_pose.header.frame_id = "/teo/imu2"  
  imu_pose.pose.orientation  = data.orientation
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  p_p.publish(imu_pose)
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  #  
  # 2)
  # 
  # Transform the data
  q = [data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w]
  imu_angles.x, imu_angles.y,imu_angles.z = euler_from_quaternion(q)
  # Adapt message for degrees/radians and x (roll) for plot reading convenience
  if (use_degrees):
    if imu_angles.x <0:
      imu_angles.x = 360+math.degrees(imu_angles.x) # trick
    else:
      imu_angles.x = math.degrees(imu_angles.x)
    imu_angles.y = math.degrees(imu_angles.y)
    imu_angles.z = math.degrees(imu_angles.z)
  else:
    if imu_angles.x <0:
      imu_angles.x = 2*math.pi+imu_angles.x # trick
    else:
      imu_angles.x = imu_angles.x
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  p_a.publish(imu_angles)
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  #
  # 3)
  # 
  global imu_last_time, imu_angles_from_vel
  # Calculate time delta
  current_time = data.header.stamp
  dt = (current_time - imu_last_time).to_sec()
  imu_last_time = current_time
  # Avoid huge first value
  if math.fabs(dt)>1: dt = 0 # trick
  # Cook the message
  imu_angles_from_vel.x += data.angular_velocity.x*dt
  imu_angles_from_vel.y -= data.angular_velocity.y*dt
  imu_angles_from_vel.z -= data.angular_velocity.z*dt
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  p_A.publish(imu_angles_from_vel)
  # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  

# Segway Callback function
#   pre: segway status message
#   post: 1) RPY from segway angular velocities integration
def callback_seg(data):
  # 
  # 1)
  # 
  global seg_last_time, segway_angles_from_vel
  # Calculate time delta
  current_time = data.rmp200[0].uptime
  dt = current_time - seg_last_time
  seg_last_time = current_time
  # Avoid huge first value
  if math.fabs(dt)>1: dt = 0 # trick
  # Cook the message
  if (use_first_only):
    segway_angles_from_vel.x += data.rmp200[0].roll_rate*dt
    segway_angles_from_vel.y += data.rmp200[0].pitch_rate*dt
    segway_angles_from_vel.z -= data.rmp200[0].yaw_rate*dt
  elif (use_second_only):
    segway_angles_from_vel.x += data.rmp200[1].roll_rate*dt
    segway_angles_from_vel.y += data.rmp200[1].pitch_rate*dt
    segway_angles_from_vel.z -= data.rmp200[1].yaw_rate*dt
  else:
    segway_angles_from_vel.x += (data.rmp200[1].roll_rate+data.rmp200[0].roll_rate)/2*dt
    segway_angles_from_vel.y += (data.rmp200[1].pitch_rate+data.rmp200[0].pitch_rate)/2*dt
    segway_angles_from_vel.z -= (data.rmp200[1].yaw_rate+data.rmp200[0].yaw_rate)/2*dt
  #>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  p_s.publish(segway_angles_from_vel)
  #>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  

# Main function
#   pre: -
#   post: -
def main(args):
  #
  # Init Node
  # 
  rospy.init_node('imu2pose')
  rospy.loginfo("IMU 2 POSE Node started")
  r = rospy.Rate(10) # 10hz
  #
  # Init variables
  #
  global imu_pose, imu_angles, imu_angles_from_vel, segway_angles_from_vel
  imu_pose = PoseStamped()
  imu_angles = Vector3()
  imu_angles_from_vel = Vector3()
  imu_angles_from_vel.x = imu_angles_from_vel.y = imu_angles_from_vel.z = 0
  segway_angles_from_vel = Vector3()
  segway_angles_from_vel.x = segway_angles_from_vel.y = segway_angles_from_vel.z = 0
  global imu_last_time, seg_last_time
  imu_last_time = rospy.Time()
  imu_last_time = rospy.Time.now()
  seg_last_time = 0
  global use_degrees, use_first_only, use_second_only
  use_degrees = rospy.get_param('use_degrees', 'False')
  use_first_only = rospy.get_param('use_first_only', 'False')
  use_second_only = rospy.get_param('use_second_only', 'False')
  #
  # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
  rospy.Subscriber('imu', Imu, callback_imu)
  rospy.Subscriber('status', SegwayRMP400Status, callback_seg)
  # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
  #
  # Spin!
  # 
  rospy.spin()


if __name__ == '__main__':
    main(sys.argv)