Carla ros

Carla ros helper

#!/usr/bin/env python
from __future__ import print_function

#import ros packages
import roslib; roslib.load_manifest('carlaros_client')
import rospy

from sensor_msgs.msg import Joy
#self define ros msgs
from carla_msgs.msg import CarOdom
from carla_msgs.msg import Agent
from carla_msgs.msg import AgentArray

from sensor_msgs.msg import Image as RosImage

#quaternion in ros
from pyquaternion import Quaternion

import numpy as np
from math import pow, sqrt, cos, sin, atan2 
import argparse

import cv2
from cv_bridge import CvBridge, CvBridgeError

#import carla
from carla.client import VehicleControl

toMeter = 0.01
toMpers = 0.2777

def orientation_to_quaternion(orientation):
	#convert orientation to quaternion
	theta = atan2(-orientation.y, orientation.x)

	rotation = np.array([ [cos(theta), -sin(theta), 0.0 ], [sin(theta), cos(theta), 0.0 ], [0.0, 0.0, 1.0]  ]  )

	q = Quaternion(matrix=rotation)

	return theta, q.elements


class CarlaROSHelper:
	def __init__(self, len_track = 1.7, len_wheelbase = 2.85 ):
		#interaction with ros
		self.frame_id = 'world'
		self.odom_pub 	= rospy.Publisher('~car_odom', CarOdom,queue_size=100)
		self.agent_pub 	= rospy.Publisher('~agent', AgentArray,queue_size=100)
		self.depth_pub 	= rospy.Publisher('~depth', RosImage,queue_size=100)
		self.rgb_pub 	= rospy.Publisher('~rgb', RosImage,queue_size=100)
		self.len_track     = len_track
		self.len_wheelbase = len_wheelbase
		self.bridge = CvBridge()

	def update(self, measurements):
		self.measurements = measurements


	def publish_odom(self):
		player_measurement = self.measurements.player_measurements

		odom_msg = CarOdom()
		odom_msg.header.stamp = rospy.Time.now()
		odom_msg.header.frame_id = self.frame_id

		#in game it is a left hand axis??? 

		odom_msg.pose.position.x =  toMeter * player_measurement.transform.location.x
		odom_msg.pose.position.y = -toMeter * player_measurement.transform.location.y
		odom_msg.pose.position.z =  toMeter * player_measurement.transform.location.z

		theta, quat = orientation_to_quaternion(player_measurement.transform.orientation)

		odom_msg.pose.orientation.w =  quat[0]
		odom_msg.pose.orientation.x =  quat[1]
		odom_msg.pose.orientation.y =  quat[2]
		odom_msg.pose.orientation.z =  quat[3]

		odom_msg.theta  = theta
		odom_msg.vel    = toMpers * player_measurement.forward_speed
		odom_msg.len_track = self.len_track
		odom_msg.len_wheelbase = self.len_wheelbase

		self.odom_pub.publish(odom_msg)


	def get_agent_array_msg(self):
		non_player_agents = self.measurements.non_player_agents
		player_measurement = self.measurements.player_measurements

		agent_array_msg = AgentArray()

		for agent in non_player_agents:
			agent_msg = Agent()
			agent_msg.header.stamp    = rospy.Time.now()
			agent_msg.header.frame_id = self.frame_id
			agent_msg.box_extent.x    =  0.0  
			agent_msg.box_extent.y    =  0.0
			agent_msg.box_extent.z    =  0.0 				
			agent_msg.state	    	  =  4  				
			agent_msg.vel	    	  =  0.0  				

			#refill the fileds
			if agent.HasField('traffic_light'):
				agent_info = agent.traffic_light
				agent_type = Agent.TRAFFIC_LIGHT
				agent_msg.state = int(agent_info.state)

			elif agent.HasField('speed_limit_sign'):
				agent_info = agent.speed_limit_sign
				agent_type = Agent.SPEED_LIMIT

			elif agent.HasField('vehicle'):
				agent_info = agent.vehicle
				agent_type = Agent.VEHICLE
				agent_msg.box_extent.x    =  toMeter * agent_info.box_extent.x  
				agent_msg.box_extent.y    =  toMeter * agent_info.box_extent.y
				agent_msg.box_extent.z    =  toMeter * agent_info.box_extent.z 					
				agent_msg.vel    		  =  toMpers * agent_info.forward_speed

			elif agent.HasField('pedestrian'):	
				agent_info = agent.pedestrian
				agent_type = Agent.PEDESTRIAN
				agent_msg.box_extent.x    =  toMeter * agent_info.box_extent.x  
				agent_msg.box_extent.y    =  toMeter * agent_info.box_extent.y
				agent_msg.box_extent.z    =  toMeter * agent_info.box_extent.z 					
				agent_msg.vel    		  =  toMpers * agent_info.forward_speed

			"""
			if  sqrt( pow(agent_info.transform.location.x - player_measurement.transform.location.x, 2) +\
					  pow(agent_info.transform.location.y - player_measurement.transform.location.y, 2)) > 5000. :
				continue					 
			"""
			agent_msg.agent_type 	  =   agent_type
			agent_msg.pose.position.x =   toMeter * agent_info.transform.location.x
			agent_msg.pose.position.y = - toMeter * agent_info.transform.location.y
			agent_msg.pose.position.z =   toMeter * agent_info.transform.location.z

			theta, quat = orientation_to_quaternion(agent_info.transform.orientation)

			agent_msg.theta				 = theta
			agent_msg.pose.orientation.w = quat[0];
			agent_msg.pose.orientation.x = quat[1];
			agent_msg.pose.orientation.y = quat[2];
			agent_msg.pose.orientation.z = quat[3];

			agent_array_msg.agents.append(agent_msg)

		return agent_array_msg
			

	def publish_agents(self):
		msg = self.get_agent_array_msg()
		#print ('publish msg with', len(msg.agents) )
		self.agent_pub.publish(msg)


	def publish_cam(self, sensor_data, camera_name, encoding = "rgb8"):
		img_array = sensor_data[camera_name].data
		img_msg = self.bridge.cv2_to_imgmsg(img_array, encoding)
		self.rgb_pub.publish(img_msg)