robot_info.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('teo_apps')
import os
import sys
import numpy as np
import rospy
import rosbag
import tf
import pprint
import math
import subprocess, yaml

from geometry_msgs.msg import Twist
from iri_segway_rmp_msgs.msg import SegwayRMP400Status
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from sensor_msgs.msg import NavSatFix
from iri_common_drivers_msgs.msg import lifebatt_status

from tf.transformations import euler_from_quaternion

# Terminal colors for Ubuntu/Linux
SB_='\033[1m' #style bold
CW_='\033[1;37m' #White
CLG_='\033[37m' #Light Gray
CG_='\033[1;30m' #Gray    
CN_='\033[30m' #Black   
CR_='\033[31m' #Red        
CLR_='\033[1;31m' #Light Red  
CV_='\033[32m' #Green      
CLV_='\033[1;32m' #Light Green
CM_='\033[33m' #Brown      
CY_='\033[1;33m' #Yellow     
CB_='\033[34m' #Blue       
CLB_='\033[1;34m' #Light Blue 
CP_='\033[35m' #Purple     
CK_='\033[1;35m' #Pink       
CC_='\033[36m' #Cyan      
CLC_='\033[1;36m' #Light Cyan
_EC='\033[0m' #end color

u_vel_x           = 0
u_vel_yaw         = 0
seg_vel_x         = 0
seg_x             = 0
imu_roll          = 0        
imu_pitch         = 0        
imu_yaw           = 0      
imu_vel_x         = 0        
imu_vel_y         = 0        
imu_vel_z         = 0        
imu_acc_x         = 0        
imu_acc_y         = 0        
imu_acc_z         = 0        
odom_x            = 0
odom_y            = 0
odom_o            = 0
gps_lat           = 0
gps_lon           = 0
gps_alt           = 0
seg_ui_bat        = [0,0]
powerbase_battery = [0,0]
bat_vol           = 0
bat_cur           = 0
bat_cap           = 0
bat_time          = 0

def cmd_vel_cb(data):
  global u_vel_x,u_vel_yaw
  u_vel_x   = data.linear.x
  u_vel_yaw = data.angular.z
    
def seg_sta_cb(data):
  global seg_vel_x,seg_x,seg_ui_bat,powerbase_battery
  seg_vel_x = (data.rmp200[0].left_wheel_velocity  + data.rmp200[0].right_wheel_velocity  + \
               data.rmp200[1].left_wheel_velocity  + data.rmp200[1].right_wheel_velocity) /4
  seg_x     = (data.rmp200[0].forward_displacement + data.rmp200[1].forward_displacement) /2
  seg_ui_bat        = [data.rmp200[0].ui_battery, data.rmp200[1].ui_battery]
  powerbase_battery = [data.rmp200[0].powerbase_battery, data.rmp200[1].powerbase_battery]

def imu_cb(data):
  global imu_roll,imu_pitch,imu_yaw,imu_vel_x,imu_vel_y,imu_vel_z,imu_acc_x,imu_acc_y,imu_acc_z
  q = [data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w]
  imu_roll, imu_pitch, imu_yaw = euler_from_quaternion(q)
  if imu_roll <0:
    imu_roll = math.pi+imu_roll # trick
  else:
    imu_roll = -math.pi+imu_roll
  imu_roll, imu_pitch, imu_yaw = imu_roll*180/math.pi, imu_pitch*180/math.pi, imu_yaw*180/math.pi
  imu_vel_x = data.angular_velocity.x
  imu_vel_y = data.angular_velocity.y
  imu_vel_z = data.angular_velocity.z
  imu_acc_x = data.linear_acceleration.x
  imu_acc_y = data.linear_acceleration.y
  imu_acc_z = data.linear_acceleration.z

def odom_cb(data):
  global odom_x,odom_y,odom_o,odom_roll,odom_pitch,odom_yaw
  odom_x = data.pose.pose.position.x
  odom_y = data.pose.pose.position.y
  q = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
  odom_roll, odom_pitch, odom_yaw = euler_from_quaternion(q)

def gps_cb(data):
  global gps_lat,gps_lon,gps_alt
  gps_lat = data.latitude
  gps_lon = data.longitude
  gps_alt = data.altitude

def bat_cb(data):
  global bat_vol,bat_cur,bat_cap,bat_time
  bat_vol  = data.voltage
  bat_cur  = data.output_current
  bat_cap  = data.remaining_capacity
  bat_time = data.time_to_discharged
    
if __name__ == '__main__':

  print CY_ +"- TEO ROBOT info -"+"\n"+_EC

  rospy.init_node('robot_info')

  rospy.Subscriber("cmd_vel", Twist, cmd_vel_cb)
  rospy.Subscriber("segway_status", SegwayRMP400Status, seg_sta_cb)
  rospy.Subscriber("imu", Imu, imu_cb)
  rospy.Subscriber("odom", Odometry, odom_cb)
  rospy.Subscriber("gps", NavSatFix, gps_cb)
  rospy.Subscriber("battery", lifebatt_status, bat_cb)

  r = rospy.Rate(0.5) # [Hz]
  while not rospy.is_shutdown():
    print CY_ +"-- TEO ROBOT STATUS --------------------"+"\n\n"\
         +CLB_+"Velocitat: "+"U=","%.3f"%u_vel_x,",","%.3f"%u_vel_yaw,"\t\t[m/s,rad/s]\n"\
              +"           "+"S=","%.3f"%seg_vel_x,"\t\t\t[m/s]\n"\
              +"           "+"I=","%.3f"%imu_vel_x,",","%.3f"%imu_vel_y,",","%.3f"%imu_vel_z,"\t[rad/s]\n"\
         +CLC_+"Posicio:   "+"S=","%.3f"%seg_x,"\t\t\t[m]\n"\
              +"           "+"I=","%.3f"%imu_roll,",","%.3f"%imu_pitch,",","%.3f"%imu_yaw,"\t[⁰]\n"\
              +"           "+"O=","%.3f"%odom_x,",","%.3f"%odom_y,",","%.3f"%odom_o,"\t[m,m,⁰]\n"\
              +"Acceler.:  "+"I=","%.3f"%imu_acc_x,",","%.3f"%imu_acc_y,",","%.3f"%imu_acc_z,"\t[m/s²]\n"\
          +CV_+"GPS:       "+"Lat=",gps_lat,"\tLon=",gps_lon,"\tAlt=",gps_alt,"\n"\
         +CLR_+"Bateries:  "+"UI=","%.3f"%seg_ui_bat[0],",","%.3f"%seg_ui_bat[1],"\t\t[V]\n"\
              +"           "+"PB=","%.3f"%powerbase_battery[0],",","%.3f"%powerbase_battery[1],"\t\t[V]\n"\
              +"           "+"LifeBat:","%.3f"%bat_vol, "\t\t[V]\n"\
              +"           "+"        ","%.3f"%bat_cur, "\t\t[A]\n"\
              +"           "+"        ","%.3f"%bat_cap, "\t\t[Ah]\n"\
              +"           "+"        ","%.3f"%bat_time,"\t\t[s]\n"+_EC
    r.sleep()