#!/usr/bin/env python
# Copyright (C) 2016 Toyota Motor Corporation
import controller_manager_msgs.srv
import rospy
import math
import time
import datetime

import trajectory_msgs.msg
import actionlib
import control_msgs.msg
import geometry_msgs.msg
import os
import sys
import signal

from sensor_msgs.msg import Image
from nav_msgs.msg import OccupancyGrid
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import PoseStamped
from actionlib_msgs.msg import GoalStatusArray
from std_msgs.msg import Int32
from tmc_msgs.msg import Voice
import numpy as np

import tf2_ros
import tf2_geometry_msgs
from tf.transformations import euler_from_quaternion, quaternion_from_euler, quaternion_matrix
from cv_bridge import CvBridge, CvBridgeError
import cv2

cv_img=None
bridge=CvBridge()

logfile=open("poselog.txt","a")
logfile.write("======="+ unicode(datetime.datetime.now()) +"\n")


def signal_handler(signal,frame):
    print("EXIT!")
    logfile.close()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)

tfbuffer=tf2_ros.Buffer(rospy.Duration(10.0))
tflistener=tf2_ros.TransformListener(tfbuffer)

pub_depth = rospy.Publisher('/pnu/depth_rect_compressed',CompressedImage, queue_size=1)
pub_rgb = rospy.Publisher('/pnu/rgb_rect_compressed',CompressedImage, queue_size=1)
pub_sensor_tr = rospy.Publisher('/pnu/sensor_tr',PoseStamped, queue_size=1)

pub_pcloud=rospy.Publisher('pnu/pointcloud',PointCloud2,queue_size=1)
pub_d_obstacle=rospy.Publisher('pnu/dobstacle',OccupancyGrid,queue_size=1)
pub_lidar = rospy.Publisher('pnu/base_scan',LaserScan,queue_size=1)
pub_basestatus=rospy.Publisher('pnu/basestatus',Int32,queue_size=2)
pub_voice=rospy.Publisher('/talk_request',Voice,queue_size=2)

rospy.init_node('datasender')
cur_pose=None

# wait to establish connection between the controller
# while pub_depth.get_num_connections() == 0: rospy.sleep(0.1)
# print "DEPTH CONNECTED"
# while pub_rgb.get_num_connections() == 0:  rospy.sleep(0.1)
# print "RGB CONNECTED"

depth_count=0
rgb_count=0
pcloud_count=0
lidar_count=0
d_obstacle_count=0

depth_msg=None
rgb_msg=None
sensor_tf=None
pcloud_msg=None
lidar_msg=None
d_obstacle_msg=None


sensor_tf_pose=PoseStamped()
last_time = time.time()
global_stamp=0
tr_stamp=0
pose_requested=0

def pcloud_callback(data):
    global pcloud_msg,pcloud_count
    pcloud_msg=data
    pcloud_count+=1

def lidar_callback(data):
    global lidar_msg,lidar_count
    lidar_msg=data
    lidar_count+=1

def dmap_callback(data):
    global d_obstacle_msg,d_obstacle_count
    d_obstacle_msg=data
    d_obstacle_count+=1

def poselog_callback(data):
    global pose_requested
    pose_requested=1

def movestatus_callback(data):
    num_list=len(data.status_list)
    # print(data.status_list)
    if num_list>0:
    #  print(data.status_list[-1].status,data.status_list[-1].text)
      #  data.status_list[-1].status
      pub_basestatus.publish(data.status_list[-1].status)
    # 1: Moving 3: Complete 4: Plan fail



def pose_callback(data):
  global pose_requested
  global cur_pose
  halfangle = math.atan2(data.pose.orientation.z, data.pose.orientation.w)
  poseturple=(data.pose.position.x,data.pose.position.y,2*halfangle * 180/3.141592)
  cur_pose=poseturple
  if pose_requested>0:
    print poseturple
    logfile.write("%.2f %.2f %.1f \n" % poseturple)
  pose_requested=0

def compress_depth(data):
  global cv_img
  # print("WH",data.width, data.height)
  # print("Encoding",data.encoding)
  # print("raw depth size:", sys.getsizeof(data.data))
  byte_per_pixel=sys.getsizeof(data.data)/data.width/data.height
  msg=CompressedImage()
  msg.header=data.header
  msg.format="jpeg"

  if byte_per_pixel==4:#4 byte per pixel, simulated device
    #distance = float value (in meters)
    #we map 50cm-305cm to 0-255
    # print "Step:", data.step
    np_arr = np.fromstring(data.data, np.float32)*100-50+0.5 #change to cm
    np2_arr=np.clip(np_arr,0,255).astype(np.uint8)
    image_np = np2_arr.reshape(640,480,1)
    msg.data=np.array(cv2.imencode('.jpg',image_np,[1,100])[1]).tostring()
    return msg
    # print("jpg size:", sys.getsizeof(depth_msg.data))
  else: #2 byte per pixel, actual device
    #distance = (UB + LB*256) *0.001 (in meters)
    np_arr = (np.fromstring(data.data, np.uint16)+5)/10 -50 #change to cm
    np2_arr=np.clip(np_arr,0,255).astype(np.uint8)
    image_np = np2_arr.reshape(640,480,1)
    # msg.format="jpeg"
    # msg.data=np.array(cv2.imencode('.jpg',image_np)[1]).tostring()
    msg.format="png"
    msg.data=np.array(cv2.imencode('.png',image_np)[1]).tostring()

#    print("org size:", sys.getsizeof(data.data))
#    print("compressed size:", sys.getsizeof(msg.data))
  return msg

def compress_rgb(data):
  global cv_img
  # print("WH",data.width, data.height)
  # print("Encoding",data.encoding)
  # print("RGB size:", sys.getsizeof(data.data))
  cv_img=bridge.imgmsg_to_cv2(data,"bgr8")
  msg=CompressedImage()
  msg.header=data.header
  msg.format="jpeg"
  msg.data=np.array(cv2.imencode('.jpg',cv_img,[1,50])[1]).tostring()
  return msg


def depth_callback(data):
    global depth_msg,depth_count
    depth_msg=data
    depth_count+=1

def rgb_callback(data):
    global rgb_msg,rgb_count
    rgb_msg=data
    rgb_count+=1

    try:
        # sensor_tf =tfbuffer.lookup_transform("map","head_rgbd_sensor_rgb_frame",rospy.Time(0))
        sensor_tf =tfbuffer.lookup_transform("map","head_rgbd_sensor_rgb_frame",rgb_msg.header.stamp,rospy.Duration(0.2))
        tr_stamp=sensor_tf.header.stamp.to_sec()
        sensor_tf_pose.header.seq=rgb_count
        sensor_tf_pose.header.stamp=data.header.stamp
        sensor_tf_pose.pose.position.x=sensor_tf.transform.translation.x
        sensor_tf_pose.pose.position.y=sensor_tf.transform.translation.y
        sensor_tf_pose.pose.position.z=sensor_tf.transform.translation.z
        sensor_tf_pose.pose.orientation.x=sensor_tf.transform.rotation.x
        sensor_tf_pose.pose.orientation.y=sensor_tf.transform.rotation.y
        sensor_tf_pose.pose.orientation.z=sensor_tf.transform.rotation.z
        sensor_tf_pose.pose.orientation.w=sensor_tf.transform.rotation.w
        rgb_count+=1
    except  (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        print "err"

rospy.Subscriber("/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", Image, depth_callback)
rospy.Subscriber("/hsrb/head_rgbd_sensor/rgb/image_rect_color", Image,rgb_callback)
rospy.Subscriber("/hsrb/head_rgbd_sensor/depth_registered/rectified_points", PointCloud2,pcloud_callback)
rospy.Subscriber("/hsrb/base_scan", LaserScan,lidar_callback)
rospy.Subscriber("/dynamic_obstacle_map_ref", OccupancyGrid,dmap_callback)

rospy.Subscriber("/move_base/move/status", GoalStatusArray,movestatus_callback)
rospy.Subscriber("/global_pose", PoseStamped,pose_callback)
rospy.Subscriber("/pnu/poselog_request", Int32,poselog_callback)


last_time = time.time()
t_last_send=last_time
t_last_send_pcl=last_time+0.5
send_interval = 1.0
send_interval_pcl = 3.0

while True:
    t=time.time()
    if t-t_last_send >= send_interval:
        if depth_count>0 and rgb_count>0:
            print "SEND" ,depth_count/(t-t_last_send),rgb_count/(t-t_last_send)
            print "Pose ",cur_pose
            global_stamp = rospy.Time.now()
            depth_msg.header.seq=global_stamp
            rgb_msg.header.seq=global_stamp
            sensor_tf_pose.header.seq=global_stamp
            pub_depth.publish(compress_depth(depth_msg))
            pub_rgb.publish(compress_rgb(rgb_msg))
            pub_sensor_tr.publish(sensor_tf_pose)
            pub_lidar.publish(lidar_msg)
######TODOTODOTODOTODOTDO!!!!
#            pub_d_obstacle.publish(d_obstacle_msg)

            depth_count=0
            rgb_count=0
            t_last_send=t

    if t-t_last_send_pcl >= send_interval_pcl:
        if pcloud_count>0:
            pub_pcloud.publish(pcloud_msg)
            pcloud_count=0
            t_last_send_pcl=t

    time.sleep(0.1)

#!/usr/bin/env python
# Copyright (C) 2016 Toyota Motor Corporation
import controller_manager_msgs.srv
import rospy
import math
import time
import datetime

import trajectory_msgs.msg
import actionlib
import control_msgs.msg
import geometry_msgs.msg
import os
import sys
import signal

from sensor_msgs.msg import Image
from nav_msgs.msg import OccupancyGrid
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import PoseStamped
from actionlib_msgs.msg import GoalStatusArray
from std_msgs.msg import Int32
from tmc_msgs.msg import Voice
import numpy as np

import tf2_ros
import tf2_geometry_msgs
from tf.transformations import euler_from_quaternion, quaternion_from_euler, quaternion_matrix
from cv_bridge import CvBridge, CvBridgeError
import cv2

cv_img=None
bridge=CvBridge()

logfile=open("poselog.txt","a")
logfile.write("======="+ unicode(datetime.datetime.now()) +"\n")


def signal_handler(signal,frame):
    print("EXIT!")
    logfile.close()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)

tfbuffer=tf2_ros.Buffer(rospy.Duration(10.0))
tflistener=tf2_ros.TransformListener(tfbuffer)

pub_depth = rospy.Publisher('/pnu/depth_rect_compressed',CompressedImage, queue_size=1)
pub_rgb = rospy.Publisher('/pnu/rgb_rect_compressed',CompressedImage, queue_size=1)
pub_sensor_tr = rospy.Publisher('/pnu/sensor_tr',PoseStamped, queue_size=1)

pub_pcloud=rospy.Publisher('pnu/pointcloud',PointCloud2,queue_size=1)
pub_d_obstacle=rospy.Publisher('pnu/dobstacle',OccupancyGrid,queue_size=1)
pub_lidar = rospy.Publisher('pnu/base_scan',LaserScan,queue_size=1)
pub_basestatus=rospy.Publisher('pnu/basestatus',Int32,queue_size=2)
pub_voice=rospy.Publisher('/talk_request',Voice,queue_size=2)

rospy.init_node('datasender')
cur_pose=None

# wait to establish connection between the controller
# while pub_depth.get_num_connections() == 0: rospy.sleep(0.1)
# print "DEPTH CONNECTED"
# while pub_rgb.get_num_connections() == 0:  rospy.sleep(0.1)
# print "RGB CONNECTED"

depth_count=0
rgb_count=0
pcloud_count=0
lidar_count=0
d_obstacle_count=0

depth_msg=None
rgb_msg=None
sensor_tf=None
pcloud_msg=None
lidar_msg=None
d_obstacle_msg=None


sensor_tf_pose=PoseStamped()
last_time = time.time()
global_stamp=0
tr_stamp=0
pose_requested=0

def pcloud_callback(data):
    global pcloud_msg,pcloud_count
    pcloud_msg=data
    pcloud_count+=1

def lidar_callback(data):
    global lidar_msg,lidar_count
    lidar_msg=data
    lidar_count+=1

def dmap_callback(data):
    global d_obstacle_msg,d_obstacle_count
    d_obstacle_msg=data
    d_obstacle_count+=1

def poselog_callback(data):
    global pose_requested
    pose_requested=1

def movestatus_callback(data):
    num_list=len(data.status_list)
    # print(data.status_list)
    if num_list>0:
    #  print(data.status_list[-1].status,data.status_list[-1].text)
      #  data.status_list[-1].status
      pub_basestatus.publish(data.status_list[-1].status)
    # 1: Moving 3: Complete 4: Plan fail



def pose_callback(data):
  global pose_requested
  global cur_pose
  halfangle = math.atan2(data.pose.orientation.z, data.pose.orientation.w)
  poseturple=(data.pose.position.x,data.pose.position.y,2*halfangle * 180/3.141592)
  cur_pose=poseturple
  if pose_requested>0:
    print poseturple
    logfile.write("%.2f %.2f %.1f \n" % poseturple)
  pose_requested=0

def compress_depth(data):
  global cv_img
  # print("WH",data.width, data.height)
  # print("Encoding",data.encoding)
  # print("raw depth size:", sys.getsizeof(data.data))
  byte_per_pixel=sys.getsizeof(data.data)/data.width/data.height
  msg=CompressedImage()
  msg.header=data.header
  msg.format="jpeg"

  if byte_per_pixel==4:#4 byte per pixel, simulated device
    #distance = float value (in meters)
    #we map 50cm-305cm to 0-255
    # print "Step:", data.step
    np_arr = np.fromstring(data.data, np.float32)*100-50+0.5 #change to cm
    np2_arr=np.clip(np_arr,0,255).astype(np.uint8)
    image_np = np2_arr.reshape(640,480,1)
    msg.data=np.array(cv2.imencode('.jpg',image_np,[1,100])[1]).tostring()
    return msg
    # print("jpg size:", sys.getsizeof(depth_msg.data))
  else: #2 byte per pixel, actual device
    #distance = (UB + LB*256) *0.001 (in meters)
    np_arr = (np.fromstring(data.data, np.uint16)+5)/10 -50 #change to cm
    np2_arr=np.clip(np_arr,0,255).astype(np.uint8)
    image_np = np2_arr.reshape(640,480,1)
    # msg.format="jpeg"
    # msg.data=np.array(cv2.imencode('.jpg',image_np)[1]).tostring()
    msg.format="png"
    msg.data=np.array(cv2.imencode('.png',image_np)[1]).tostring()

#    print("org size:", sys.getsizeof(data.data))
#    print("compressed size:", sys.getsizeof(msg.data))
  return msg

def compress_rgb(data):
  global cv_img
  # print("WH",data.width, data.height)
  # print("Encoding",data.encoding)
  # print("RGB size:", sys.getsizeof(data.data))
  cv_img=bridge.imgmsg_to_cv2(data,"bgr8")
  msg=CompressedImage()
  msg.header=data.header
  msg.format="jpeg"
  msg.data=np.array(cv2.imencode('.jpg',cv_img,[1,50])[1]).tostring()
  return msg


def depth_callback(data):
    global depth_msg,depth_count
    depth_msg=data
    depth_count+=1

def rgb_callback(data):
    global rgb_msg,rgb_count
    rgb_msg=data
    rgb_count+=1

    try:
        # sensor_tf =tfbuffer.lookup_transform("map","head_rgbd_sensor_rgb_frame",rospy.Time(0))
        sensor_tf =tfbuffer.lookup_transform("map","head_rgbd_sensor_rgb_frame",rgb_msg.header.stamp,rospy.Duration(0.2))
        tr_stamp=sensor_tf.header.stamp.to_sec()
        sensor_tf_pose.header.seq=rgb_count
        sensor_tf_pose.header.stamp=data.header.stamp
        sensor_tf_pose.pose.position.x=sensor_tf.transform.translation.x
        sensor_tf_pose.pose.position.y=sensor_tf.transform.translation.y
        sensor_tf_pose.pose.position.z=sensor_tf.transform.translation.z
        sensor_tf_pose.pose.orientation.x=sensor_tf.transform.rotation.x
        sensor_tf_pose.pose.orientation.y=sensor_tf.transform.rotation.y
        sensor_tf_pose.pose.orientation.z=sensor_tf.transform.rotation.z
        sensor_tf_pose.pose.orientation.w=sensor_tf.transform.rotation.w
        rgb_count+=1
    except  (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        print "err"

rospy.Subscriber("/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", Image, depth_callback)
rospy.Subscriber("/hsrb/head_rgbd_sensor/rgb/image_rect_color", Image,rgb_callback)
rospy.Subscriber("/hsrb/head_rgbd_sensor/depth_registered/rectified_points", PointCloud2,pcloud_callback)
rospy.Subscriber("/hsrb/base_scan", LaserScan,lidar_callback)
rospy.Subscriber("/dynamic_obstacle_map_ref", OccupancyGrid,dmap_callback)

rospy.Subscriber("/move_base/move/status", GoalStatusArray,movestatus_callback)
rospy.Subscriber("/global_pose", PoseStamped,pose_callback)
rospy.Subscriber("/pnu/poselog_request", Int32,poselog_callback)


last_time = time.time()
t_last_send=last_time
t_last_send_pcl=last_time+0.5
send_interval = 1.0
send_interval_pcl = 3.0

while True:
    t=time.time()
    if t-t_last_send >= send_interval:
        if depth_count>0 and rgb_count>0:
            print "SEND" ,depth_count/(t-t_last_send),rgb_count/(t-t_last_send)
            print "Pose ",cur_pose
            global_stamp = rospy.Time.now()
            depth_msg.header.seq=global_stamp
            rgb_msg.header.seq=global_stamp
            sensor_tf_pose.header.seq=global_stamp
            pub_depth.publish(compress_depth(depth_msg))
            pub_rgb.publish(compress_rgb(rgb_msg))
            pub_sensor_tr.publish(sensor_tf_pose)
            pub_lidar.publish(lidar_msg)
######TODOTODOTODOTODOTDO!!!!
#            pub_d_obstacle.publish(d_obstacle_msg)

            depth_count=0
            rgb_count=0
            t_last_send=t

    if t-t_last_send_pcl >= send_interval_pcl:
        if pcloud_count>0:
            pub_pcloud.publish(pcloud_msg)
            pcloud_count=0
            t_last_send_pcl=t

    time.sleep(0.1)

#!/usr/bin/env python # Copyright (C) 2016 Toyota Motor Corporation # wait to establish connection between the controller # while pub_depth.get_num_connections() == 0: rospy.sleep(0.1) # print "DEPTH CONNECTED" # while pub_rgb.get_num_connections() == 0:  rospy.sleep(0.1) # print "RGB CONNECTED" # print(data.status_list) #  print(data.status_list[-1].status,data.status_list[-1].text) #  data.status_list[-1].status # 1: Moving 3: Complete 4: Plan fail # print("WH",data.width, data.height) # print("Encoding",data.encoding) # print("raw depth size:", sys.getsizeof(data.data)) #4 byte per pixel, simulated device #distance = float value (in meters) #we map 50cm-305cm to 0-255 # print "Step:", data.step #change to cm # print("jpg size:", sys.getsizeof(depth_msg.data)) #2 byte per pixel, actual device #distance = (UB + LB*256) *0.001 (in meters) #change to cm # msg.format="jpeg" # msg.data=np.array(cv2.imencode('.jpg',image_np)[1]).tostring() #    print("org size:", sys.getsizeof(data.data)) #    print("compressed size:", sys.getsizeof(msg.data)) # print("WH",data.width, data.height) # print("Encoding",data.encoding) # print("RGB size:", sys.getsizeof(data.data)) # sensor_tf =tfbuffer.lookup_transform("map","head_rgbd_sensor_rgb_frame",rospy.Time(0)) ######TODOTODOTODOTODOTDO!!!! #            pub_d_obstacle.publish(d_obstacle_msg)