Review follow-up changes:
- control inputs removed - will be a separate request if necessary - style adjustments - compliant with PEP8
This commit is contained in:
parent
e15586d4e7
commit
a864cc27c3
|
@ -29,4 +29,3 @@ _images*
|
||||||
_out
|
_out
|
||||||
_site
|
_site
|
||||||
core
|
core
|
||||||
/.project
|
|
||||||
|
|
|
@ -7,7 +7,7 @@ carla:
|
||||||
NumberOfVehicles: 30
|
NumberOfVehicles: 30
|
||||||
NumberOfPedestrians: 10
|
NumberOfPedestrians: 10
|
||||||
WeatherId: 1
|
WeatherId: 1
|
||||||
Framesperepisode: -1
|
Framesperepisode: -1 # -1: run episode indefinitely until interrupted
|
||||||
Episodes: 2
|
Episodes: 2
|
||||||
|
|
||||||
sensors:
|
sensors:
|
||||||
|
|
|
@ -4,12 +4,11 @@ Rosbridge class:
|
||||||
Class that handle communication between CARLA and ROS
|
Class that handle communication between CARLA and ROS
|
||||||
"""
|
"""
|
||||||
import random
|
import random
|
||||||
|
from itertools import count
|
||||||
|
|
||||||
from rosgraph_msgs.msg import Clock
|
from rosgraph_msgs.msg import Clock
|
||||||
from tf2_msgs.msg import TFMessage
|
from tf2_msgs.msg import TFMessage
|
||||||
import rospy
|
import rospy
|
||||||
from std_msgs.msg import Float32, Bool
|
|
||||||
from itertools import count
|
|
||||||
|
|
||||||
from carla.settings import CarlaSettings
|
from carla.settings import CarlaSettings
|
||||||
from carla_ros_bridge.control import InputController
|
from carla_ros_bridge.control import InputController
|
||||||
|
@ -55,14 +54,6 @@ class CarlaRosBridge(object):
|
||||||
# creating input controller listener
|
# creating input controller listener
|
||||||
self.input_controller = InputController()
|
self.input_controller = InputController()
|
||||||
|
|
||||||
# control inputs - float std msgs
|
|
||||||
self.steer = Float32(0.0)
|
|
||||||
self.brake = Float32(0.0)
|
|
||||||
self.throttle = Float32(0.0)
|
|
||||||
# control inputs - Bool msgs
|
|
||||||
self.reverse = Bool(False)
|
|
||||||
self.hand_brake = Bool(False)
|
|
||||||
|
|
||||||
def setup_carla_client(self, client, params):
|
def setup_carla_client(self, client, params):
|
||||||
self.client = client
|
self.client = client
|
||||||
self.param_sensors = params.get('sensors', {})
|
self.param_sensors = params.get('sensors', {})
|
||||||
|
@ -150,7 +141,7 @@ class CarlaRosBridge(object):
|
||||||
self.client.start_episode(player_start)
|
self.client.start_episode(player_start)
|
||||||
|
|
||||||
for frame in count():
|
for frame in count():
|
||||||
if frame == self.frames_per_episode:
|
if (frame == self.frames_per_episode) or rospy.is_shutdown():
|
||||||
break
|
break
|
||||||
measurements, sensor_data = self.client.read_data()
|
measurements, sensor_data = self.client.read_data()
|
||||||
|
|
||||||
|
@ -169,6 +160,9 @@ class CarlaRosBridge(object):
|
||||||
for name, data in sensor_data.items():
|
for name, data in sensor_data.items():
|
||||||
self.sensors[name].process_sensor_data(data, self.cur_time)
|
self.sensors[name].process_sensor_data(data, self.cur_time)
|
||||||
|
|
||||||
|
# publish all messages
|
||||||
|
self.send_msgs()
|
||||||
|
|
||||||
# handle control
|
# handle control
|
||||||
if rospy.get_param('carla_autopilot', True):
|
if rospy.get_param('carla_autopilot', True):
|
||||||
control = measurements.player_measurements.autopilot_control
|
control = measurements.player_measurements.autopilot_control
|
||||||
|
@ -177,13 +171,6 @@ class CarlaRosBridge(object):
|
||||||
control = self.input_controller.cur_control
|
control = self.input_controller.cur_control
|
||||||
self.client.send_control(**control)
|
self.client.send_control(**control)
|
||||||
|
|
||||||
self.steer.data = control.steer
|
|
||||||
self.throttle.data = control.throttle
|
|
||||||
self.brake.data = control.brake
|
|
||||||
|
|
||||||
# publish all messages
|
|
||||||
self.send_msgs()
|
|
||||||
|
|
||||||
def __enter__(self):
|
def __enter__(self):
|
||||||
return self
|
return self
|
||||||
|
|
||||||
|
|
|
@ -3,12 +3,12 @@ RosBridge class with rosbag support
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import time
|
import time
|
||||||
|
from datetime import datetime
|
||||||
|
|
||||||
from tf2_msgs.msg import TFMessage
|
from tf2_msgs.msg import TFMessage
|
||||||
import rosbag
|
import rosbag
|
||||||
import rospy
|
import rospy
|
||||||
import os
|
import os
|
||||||
from datetime import datetime
|
|
||||||
|
|
||||||
from carla_ros_bridge.bridge import CarlaRosBridge
|
from carla_ros_bridge.bridge import CarlaRosBridge
|
||||||
|
|
||||||
|
@ -29,10 +29,6 @@ class CarlaRosBridgeWithBag(CarlaRosBridge):
|
||||||
tf_msg = TFMessage(self.tf_to_publish)
|
tf_msg = TFMessage(self.tf_to_publish)
|
||||||
self.bag.write('tf', tf_msg, self.cur_time)
|
self.bag.write('tf', tf_msg, self.cur_time)
|
||||||
|
|
||||||
self.bag.write('steer', self.steer, self.cur_time)
|
|
||||||
self.bag.write('brake', self.brake, self.cur_time)
|
|
||||||
self.bag.write('gas', self.throttle, self.cur_time)
|
|
||||||
|
|
||||||
super(CarlaRosBridgeWithBag, self).send_msgs()
|
super(CarlaRosBridgeWithBag, self).send_msgs()
|
||||||
|
|
||||||
def __enter__(self):
|
def __enter__(self):
|
||||||
|
|
Loading…
Reference in New Issue