Skip to content

Instantly share code, notes, and snippets.

@WenchaoDing
Last active December 21, 2018 15:03
Show Gist options
  • Save WenchaoDing/8bdf2b6753f0cc6cb1c4019529a79c9f to your computer and use it in GitHub Desktop.
Save WenchaoDing/8bdf2b6753f0cc6cb1c4019529a79c9f to your computer and use it in GitHub Desktop.
Carla ros
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment