def get_ros_transform(self, pose, timestamp):
if self.synchronous_mode:
if not self.relative_spawn_pose:
self.node.logwarn("{}: No relative spawn pose defined".format(
self.get_prefix()))
return
pose = self.relative_spawn_pose
child_frame_id = self.get_prefix()
if self.parent is not None:
frame_id = self.parent.get_prefix()
else:
frame_id = "map"
else:
child_frame_id = self.get_prefix()
frame_id = "map"
transform = tf2_ros.TransformStamped()
transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True)
transform.header.frame_id = frame_id
transform.child_frame_id = child_frame_id
transform.transform.translation.x = pose.position.x
transform.transform.translation.y = pose.position.y
transform.transform.translation.z = pose.position.z
transform.transform.rotation.x = pose.orientation.x
transform.transform.rotation.y = pose.orientation.y
transform.transform.rotation.z = pose.orientation.z
transform.transform.rotation.w = pose.orientation.w
return transform
def update_waypoints(self, waypoints, start_time=None):
super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
self.node.loginfo("{}: Waypoints changed.".format(self._role_name))
path = Path()
path.header.stamp = ros_timestamp(sec=self.node.get_time(), from_sec=True)
path.header.frame_id = "map"
for wpt in waypoints:
print(wpt)
path.poses.append(PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
self._path_publisher.publish(path)
def get_msg_header(self):
"""
Get a filled ROS message header
:return: ROS message header
:rtype: std_msgs.msg.Header
"""
header = Header()
header.frame_id = "map"
header.stamp = roscomp.ros_timestamp(sec=self.get_time(),
from_sec=True)
return header
def update_clock(self, carla_timestamp):
"""
perform the update of the clock
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if roscomp.ok():
self.ros_timestamp = roscomp.ros_timestamp(
carla_timestamp.elapsed_seconds, from_sec=True)
self.clock_publisher.publish(Clock(clock=self.ros_timestamp))
def publish_waypoints(self):
"""
Publish the ROS message containing the waypoints
"""
msg = Path()
msg.header.frame_id = "map"
msg.header.stamp = ros_timestamp(self.get_time(), from_sec=True)
if self.current_route is not None:
for wp in self.current_route:
pose = PoseStamped()
pose.pose = trans.carla_transform_to_ros_pose(wp[0].transform)
msg.poses.append(pose)
self.waypoint_publisher.publish(msg)
self.loginfo("Published {} waypoints.".format(len(msg.poses)))
def get_msg_header(self, frame_id=None, timestamp=None):
"""
Get a filled ROS message header
:return: ROS message header
:rtype: std_msgs.msg.Header
"""
header = Header()
if frame_id:
header.frame_id = frame_id
else:
header.frame_id = self.get_prefix()
if not timestamp:
timestamp = self.node.get_time()
header.stamp = roscomp.ros_timestamp(sec=timestamp, from_sec=True)
return header
def publish_tf(self, pose, timestamp):
if self.synchronous_mode:
if not self.relative_spawn_pose:
self.node.logwarn("{}: No relative spawn pose defined".format(
self.get_prefix()))
return
pose = self.relative_spawn_pose
child_frame_id = self.get_prefix()
if self.parent is not None:
frame_id = self.parent.get_prefix()
else:
frame_id = "map"
else:
child_frame_id = self.get_prefix()
frame_id = "map"
transform = tf2_ros.TransformStamped()
transform.header.stamp = ros_timestamp(sec=timestamp, from_sec=True)
transform.header.frame_id = frame_id
transform.child_frame_id = child_frame_id
transform.transform.translation.x = pose.position.x
transform.transform.translation.y = pose.position.y
transform.transform.translation.z = pose.position.z
transform.transform.rotation.x = pose.orientation.x
transform.transform.rotation.y = pose.orientation.y
transform.transform.rotation.z = pose.orientation.z
transform.transform.rotation.w = pose.orientation.w
try:
self._tf_broadcaster.sendTransform(transform)
except ROSException:
if ros_ok():
self.node.logwarn("Sensor {} failed to send transform.".format(
self.uid))
def initialize_bridge(self, carla_world, params):
"""
Initialize the bridge
"""
self.parameters = params
self.carla_world = carla_world
self.ros_timestamp = roscomp.ros_timestamp()
self.callback_group = roscomp.callback_groups.ReentrantCallbackGroup()
self.synchronous_mode_update_thread = None
self.shutdown = Event()
self.carla_settings = carla_world.get_settings()
if not self.parameters["passive"]:
# workaround: settings can only applied within non-sync mode
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
self.loginfo("synchronous_mode: {}".format(
self.parameters["synchronous_mode"]))
self.carla_settings.synchronous_mode = self.parameters[
"synchronous_mode"]
self.loginfo("fixed_delta_seconds: {}".format(
self.parameters["fixed_delta_seconds"]))
self.carla_settings.fixed_delta_seconds = self.parameters[
"fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self.loginfo("Parameters:")
for key in self.parameters:
self.loginfo(" {}: {}".format(key, self.parameters[key]))
# active sync mode in the ros bridge only if CARLA world is configured in sync mode and
# passive mode is not enabled.
self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters[
"passive"]
if self.carla_settings.synchronous_mode and self.parameters["passive"]:
self.loginfo(
"Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world."
)
self.carla_control_queue = queue.Queue()
# actor factory
self.actor_factory = ActorFactory(self, carla_world, self.sync_mode)
# add world info
self.world_info = WorldInfo(carla_world=self.carla_world, node=self)
# add debug helper
self.debug_helper = DebugHelper(carla_world.debug, self)
# Communication topics
self.clock_publisher = self.new_publisher(Clock, 'clock', 10)
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds, self)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.sync_mode:
self.carla_run_state = CarlaControl.PLAY
self.carla_control_subscriber = \
self.new_subscription(CarlaControl, "/carla/control",
lambda control: self.carla_control_queue.put(control.command),
qos_profile=10, callback_group=self.callback_group)
self.synchronous_mode_update_thread = Thread(
target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.actor_factory.start()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# services configuration.
self._registered_actors = []
self.spawn_object_service = self.new_service(SpawnObject,
"/carla/spawn_object",
self.spawn_object)
self.destroy_object_service = self.new_service(
DestroyObject, "/carla/destroy_object", self.destroy_object)
self.get_blueprints_service = self.new_service(
GetBlueprints,
"/carla/get_blueprints",
self.get_blueprints,
callback_group=self.callback_group)
self.carla_weather_subscriber = \
self.new_subscription(CarlaWeatherParameters, "/carla/weather_control",
self.on_weather_changed, qos_profile=10, callback_group=self.callback_group)
