def test_marker(self):
"""
Tests marker
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/markers",
MarkerArray,
timeout=TIMEOUT)
self.assertEqual(len(msg.markers), 1) # only ego vehicle exists
ego_marker = msg.markers[0]
self.assertEqual(ego_marker.header.frame_id, "map")
self.assertNotEqual(ego_marker.id, 0)
self.assertEqual(ego_marker.type, 1)
self.assertNotEqual(ego_marker.pose, Pose())
self.assertNotEqual(ego_marker.scale, Vector3())
self.assertEqual(ego_marker.color.r, 0.0)
self.assertEqual(ego_marker.color.g, 255.0)
self.assertEqual(ego_marker.color.b, 0.0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("ad_agent", args=args)
controller = None
try:
executor = roscomp.executors.MultiThreadedExecutor()
controller = CarlaAdAgent()
executor.add_node(controller)
roscomp.on_shutdown(controller.emergency_stop)
update_timer = controller.new_timer(
0.05, lambda timer_event=None: controller.run_step())
controller.spin()
except (ROSInterruptException, ROSException) as e:
if roscomp.ok():
roscomp.logwarn("ROS Error during exection: {}".format(e))
except KeyboardInterrupt:
roscomp.loginfo("User requested shut down.")
finally:
roscomp.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("carla_ros_scenario_runner", args=args)
scenario_runner = CarlaRosScenarioRunner()
if ROS_VERSION == 2:
spin_thread = threading.Thread(target=scenario_runner.spin, daemon=True)
spin_thread.start()
try:
scenario_runner.run()
except KeyboardInterrupt:
loginfo("User requested shut down.")
finally:
if scenario_runner._scenario_runner.is_running():
scenario_runner.loginfo("Scenario Runner still running. Shutting down.")
scenario_runner._scenario_runner.shutdown()
del scenario_runner
if ROS_VERSION == 2:
spin_thread.join()
roscomp.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("local_planner", args=args)
local_planner = None
update_timer = None
try:
local_planner = LocalPlanner()
roscomp.on_shutdown(local_planner.emergency_stop)
update_timer = local_planner.new_timer(
local_planner.control_time_step,
lambda timer_event=None: local_planner.run_step())
local_planner.spin()
except KeyboardInterrupt:
pass
finally:
roscomp.loginfo('Local planner shutting down.')
roscomp.shutdown()
def test_vehicle_info(self):
"""
Tests vehicle_info
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/ego_vehicle/vehicle_info",
CarlaEgoVehicleInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(msg.id, 0)
self.assertEqual(msg.type, "vehicle.tesla.model3")
self.assertEqual(msg.rolename, "ego_vehicle")
self.assertEqual(len(msg.wheels), 4)
self.assertNotEqual(msg.max_rpm, 0.0)
self.assertNotEqual(msg.moi, 0.0)
self.assertNotEqual(msg.damping_rate_full_throttle, 0.0)
self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged,
0.0)
self.assertNotEqual(
msg.damping_rate_zero_throttle_clutch_disengaged, 0.0)
self.assertTrue(msg.use_gear_autobox)
self.assertNotEqual(msg.gear_switch_time, 0.0)
self.assertNotEqual(msg.mass, 0.0)
self.assertNotEqual(msg.clutch_strength, 0.0)
self.assertNotEqual(msg.drag_coefficient, 0.0)
self.assertNotEqual(msg.center_of_mass, Vector3())
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
"""
roscomp.init("spawn_objects", args=args)
spawn_objects_node = None
try:
spawn_objects_node = CarlaSpawnObjects()
roscomp.on_shutdown(spawn_objects_node.destroy)
except KeyboardInterrupt:
roscomp.logerr(
"Could not initialize CarlaSpawnObjects. Shutting down.")
if spawn_objects_node:
try:
spawn_objects_node.spawn_objects()
try:
spawn_objects_node.spin()
except (ROSInterruptException, ServiceException,
KeyboardInterrupt):
pass
except (ROSInterruptException, ServiceException, KeyboardInterrupt):
spawn_objects_node.logwarn(
"Spawning process has been interrupted. There might be actors that have not been destroyed properly"
)
except RuntimeError as e:
roscomp.logfatal("Exception caught: {}".format(e))
finally:
roscomp.shutdown()
def main():
"""
main function
"""
roscomp.init("set_initial_pose")
try:
set_initial_pose_node = SetInitialPose()
set_initial_pose_node.spin()
except KeyboardInterrupt:
roscomp.loginfo("Cancelled by user.")
finally:
roscomp.shutdown()
def test_clock(self):
"""
Tests clock
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/clock", Clock, timeout=TIMEOUT)
self.assertNotEqual(Clock(), msg)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_dvs_camera_events(self):
"""
Tests dvs camera events
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/dvs_front/events",
PointCloud2,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front")
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("carla_ackermann_control", args=args)
try:
controller = CarlaAckermannControl()
controller.run()
except KeyboardInterrupt:
pass
finally:
roscomp.shutdown()
def test_actor_list(self):
"""
Tests actor_list
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/actor_list",
CarlaActorList,
timeout=TIMEOUT)
self.assertNotEqual(len(msg.actors), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_semantic_lidar(self):
"""
Tests semantic_lidar sensor node
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/semantic_lidar",
PointCloud2,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/semantic_lidar")
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_vehicle_status(self, proc_output):
"""
Tests vehicle_status
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/vehicle_status",
CarlaEgoVehicleStatus)
self.assertNotEqual(msg.header, Header())
self.assertEqual(msg.header.frame_id, 'map')
self.assertNotEqual(msg.orientation, Quaternion())
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_objects(self):
"""
Tests carla objects
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/objects",
ObjectArray,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(len(msg.objects), 1) # only ego vehicle exists
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_ego_vehicle_objects(self):
"""
Tests objects node for ego_vehicle
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/objects",
ObjectArray,
timeout=15)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(len(msg.objects), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_odometry(self):
"""
Tests Odometry
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/odometry",
Odometry,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "map")
self.assertEqual(msg.child_frame_id, "ego_vehicle")
self.assertNotEqual(msg.pose, Pose())
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_camera_info(self):
"""
Tests camera_info
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/ego_vehicle/rgb_front/camera_info",
CameraInfo,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front")
self.assertEqual(msg.height, 600)
self.assertEqual(msg.width, 800)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
"""
roscomp.init('carla_waypoint_publisher', args)
waypoint_converter = None
try:
waypoint_converter = CarlaToRosWaypointConverter()
waypoint_converter.spin()
except (RuntimeError, ROSException):
pass
except KeyboardInterrupt:
roscomp.loginfo("User requested shut down.")
finally:
roscomp.loginfo("Shutting down.")
if waypoint_converter:
waypoint_converter.destroy()
roscomp.shutdown()
def test_traffic_lights_info(self):
"""
Tests traffic_lights
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/traffic_lights/info",
CarlaTrafficLightInfoList,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.traffic_lights), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_map(self):
"""
Tests map
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/map",
String,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.data), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
:return:
"""
roscomp.init("twist_to_control", args)
twist_to_vehicle_control = None
try:
twist_to_vehicle_control = TwistToVehicleControl()
twist_to_vehicle_control.spin()
except KeyboardInterrupt:
pass
finally:
if twist_to_vehicle_control is not None:
twist_to_vehicle_control.loginfo("Done, deleting twist to control")
del twist_to_vehicle_control
roscomp.shutdown()
def test_dvs_camera_image(self):
"""
Tests dvs camera images
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/dvs_front/image",
Image,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front")
self.assertEqual(msg.height, 70)
self.assertEqual(msg.width, 400)
self.assertEqual(msg.encoding, "bgr8")
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_imu(self):
"""
Tests IMU sensor node
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/imu",
Imu,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/imu")
self.assertNotEqual(msg.linear_acceleration, 0.0)
self.assertNotEqual(msg.angular_velocity, 0.0)
self.assertNotEqual(msg.orientation, 0.0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_gnss(self):
"""
Tests Gnss
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message("/carla/ego_vehicle/gnss",
NavSatFix,
timeout=TIMEOUT)
self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss")
self.assertNotEqual(msg.latitude, 0.0)
self.assertNotEqual(msg.longitude, 0.0)
self.assertNotEqual(msg.altitude, 0.0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def test_world_info(self):
"""
Tests world_info
"""
try:
node = None
roscomp.init("test_node")
node = CompatibleNode('test_node')
msg = node.wait_for_message(
"/carla/world_info",
CarlaWorldInfo,
timeout=TIMEOUT,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.assertNotEqual(len(msg.map_name), 0)
self.assertNotEqual(len(msg.opendrive), 0)
finally:
if node is not None:
node.destroy_node()
roscomp.shutdown()
def main(args=None):
"""
main function
"""
roscomp.init("manual_control", args=args)
# resolution should be similar to spawned camera with role-name 'view'
resolution = {"width": 800, "height": 600}
pygame.init()
pygame.font.init()
pygame.display.set_caption("CARLA ROS manual control")
try:
display = pygame.display.set_mode(
(resolution['width'], resolution['height']),
pygame.HWSURFACE | pygame.DOUBLEBUF)
manual_control_node = ManualControl(resolution)
clock = pygame.time.Clock()
executor = roscomp.executors.MultiThreadedExecutor()
executor.add_node(manual_control_node)
spin_thread = Thread(target=manual_control_node.spin)
spin_thread.start()
while roscomp.ok():
clock.tick_busy_loop(60)
if manual_control_node.render(clock, display):
return
pygame.display.flip()
except KeyboardInterrupt:
roscomp.loginfo("User requested shut down.")
finally:
roscomp.shutdown()
spin_thread.join()
pygame.quit()
def __init__(self, actor, args=None):
super(RosVehicleControl, self).__init__(actor)
self._carla_actor = actor
self._role_name = actor.attributes["role_name"]
if not self._role_name:
roscomp.logerr("Invalid role_name")
self._path_topic_name = "waypoints"
if "path_topic_name" in args:
self._path_topic_name = args["path_topic_name"]
roscomp.init("ros_agent_{}".format(self._role_name), args=None)
self.node = CompatibleNode('ros_agent_{}'.format(self._role_name))
self._current_target_speed = None
self._current_path = None
self.controller_launch = None
self._target_speed_publisher = self.node.new_publisher(
Float64, "/carla/{}/target_speed".format(self._role_name),
QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.node.loginfo(
"Publishing target_speed on /carla/{}/target_speed".format(
self._role_name))
self._path_publisher = self.node.new_publisher(
Path, "/carla/{}/{}".format(self._role_name,
self._path_topic_name),
QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.node.loginfo("Publishing path on /carla/{}/{}".format(
self._role_name, self._path_topic_name))
if "launch" in args and "launch-package" in args:
launch_file = args["launch"]
launch_package = args["launch-package"]
if ROS_VERSION == 1:
executable = "roslaunch"
cli_args = [launch_package, launch_file]
elif ROS_VERSION == 2:
executable = "ros2 launch"
cli_args = [launch_package, launch_file + '.py']
cli_args.append('role_name:={}'.format(self._role_name))
# add additional launch parameters
launch_parameters = []
for key, value in args.items():
if not key == "launch" and not key == "launch-package" and not key == "path_topic_name":
launch_parameters.append('{}:={}'.format(key, value))
cli_args.append('{}:={}'.format(key, value))
self.node.loginfo(
"{}: Launching {} from package {} (parameters: {})...".format(
self._role_name, launch_file, launch_package,
launch_parameters))
cmdline = [executable] + cli_args
self.controller_launch = ApplicationRunner(
self.controller_runner_status_updated,
self.controller_runner_log,
'RosVehicleControl: launching controller node')
self.controller_launch.execute(
cmdline,
env=os.environ,
)
self.node.loginfo(
"{}: Successfully started ros vehicle control".format(
self._role_name))
else:
self.node.logwarn(
"{}: Missing value for 'launch' and/or 'launch-package'.".
format(self._role_name))
def main(args=None):
"""
main function for carla simulator ROS bridge
maintaining the communication client and the CarlaBridge object
"""
roscomp.init("bridge", args=args)
carla_bridge = None
carla_world = None
carla_client = None
executor = None
parameters = {}
executor = roscomp.executors.MultiThreadedExecutor()
carla_bridge = CarlaRosBridge()
executor.add_node(carla_bridge)
roscomp.on_shutdown(carla_bridge.destroy)
parameters['host'] = carla_bridge.get_param('host', 'localhost')
parameters['port'] = carla_bridge.get_param('port', 2000)
parameters['timeout'] = carla_bridge.get_param('timeout', 2)
parameters['passive'] = carla_bridge.get_param('passive', False)
parameters['synchronous_mode'] = carla_bridge.get_param(
'synchronous_mode', True)
parameters[
'synchronous_mode_wait_for_vehicle_control_command'] = carla_bridge.get_param(
'synchronous_mode_wait_for_vehicle_control_command', False)
parameters['fixed_delta_seconds'] = carla_bridge.get_param(
'fixed_delta_seconds', 0.05)
parameters['register_all_sensors'] = carla_bridge.get_param(
'register_all_sensors', True)
parameters['town'] = carla_bridge.get_param('town', 'Town01')
role_name = carla_bridge.get_param(
'ego_vehicle_role_name',
["hero", "ego_vehicle", "hero1", "hero2", "hero3"])
parameters["ego_vehicle"] = {"role_name": role_name}
carla_bridge.loginfo("Trying to connect to {host}:{port}".format(
host=parameters['host'], port=parameters['port']))
try:
carla_client = carla.Client(host=parameters['host'],
port=parameters['port'])
carla_client.set_timeout(parameters['timeout'])
# check carla version
dist = pkg_resources.get_distribution("carla")
if LooseVersion(dist.version) != LooseVersion(
CarlaRosBridge.CARLA_VERSION):
carla_bridge.logfatal(
"CARLA python module version {} required. Found: {}".format(
CarlaRosBridge.CARLA_VERSION, dist.version))
sys.exit(1)
if LooseVersion(carla_client.get_server_version()) != \
LooseVersion(carla_client.get_client_version()):
carla_bridge.logwarn(
"Version mismatch detected: You are trying to connect to a simulator that might be incompatible with this API. Client API version: {}. Simulator API version: {}"
.format(carla_client.get_client_version(),
carla_client.get_server_version()))
carla_world = carla_client.get_world()
if "town" in parameters and not parameters['passive']:
if parameters["town"].endswith(".xodr"):
carla_bridge.loginfo(
"Loading opendrive world from file '{}'".format(
parameters["town"]))
with open(parameters["town"]) as od_file:
data = od_file.read()
carla_world = carla_client.generate_opendrive_world(str(data))
else:
if carla_world.get_map().name != parameters["town"]:
carla_bridge.loginfo(
"Loading town '{}' (previous: '{}').".format(
parameters["town"],
carla_world.get_map().name))
carla_world = carla_client.load_world(parameters["town"])
carla_world.tick()
carla_bridge.initialize_bridge(carla_client.get_world(), parameters)
carla_bridge.spin()
except (IOError, RuntimeError) as e:
carla_bridge.logerr("Error: {}".format(e))
except KeyboardInterrupt:
pass
finally:
roscomp.shutdown()
del carla_world
del carla_client
