def _create_object_from_actor(self, carla_actor):
"""
create a object for a given carla actor
Creates also the object for its parent, if not yet existing
"""
parent = None
relative_transform = None
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
parent = self._create_object_from_actor(carla_actor.parent)
# calculate relative transform
actor_transform_matrix = trans.ros_pose_to_transform_matrix(
trans.carla_transform_to_ros_pose(carla_actor.get_transform()))
parent_transform_matrix = trans.ros_pose_to_transform_matrix(
trans.carla_transform_to_ros_pose(
carla_actor.parent.get_transform()))
relative_transform_matrix = np.matrix(
parent_transform_matrix).getI() * np.matrix(
actor_transform_matrix)
relative_transform = trans.transform_matrix_to_ros_pose(
relative_transform_matrix)
parent_id = 0
if parent is not None:
parent_id = parent.uid
name = carla_actor.attributes.get("role_name", "")
if not name:
name = str(carla_actor.id)
obj = self._create_object(carla_actor.id, carla_actor.type_id, name,
parent_id, relative_transform, carla_actor)
return obj
def _callback_sensor_data(self, carla_sensor_data):
"""
Callback function called whenever new sensor data is received
:param carla_sensor_data: carla sensor data object
:type carla_sensor_data: carla.SensorData
"""
if not self._callback_active.acquire(False):
# if acquire fails, sensor is currently getting destroyed
return
if self.synchronous_mode:
if self.sensor_tick_time:
self.next_data_expected_time = carla_sensor_data.timestamp + \
float(self.sensor_tick_time)
self.queue.put(carla_sensor_data)
else:
self.publish_tf(
trans.carla_transform_to_ros_pose(carla_sensor_data.transform),
carla_sensor_data.timestamp)
try:
self.sensor_data_updated(carla_sensor_data)
except ROSException:
if ros_ok():
self.node.logwarn(
"Sensor {}: Error while executing sensor_data_updated()."
.format(self.uid))
self._callback_active.release()
def _update_synchronous_sensor(self, frame, timestamp):
while not self.next_data_expected_time or \
(not self.queue.empty() or
self.next_data_expected_time and
self.next_data_expected_time < timestamp):
while True:
try:
carla_sensor_data = self.queue.get(timeout=1.0)
if carla_sensor_data.frame == frame:
self.node.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_tf(
trans.carla_transform_to_ros_pose(
carla_sensor_data.transform), timestamp)
self.sensor_data_updated(carla_sensor_data)
return
elif carla_sensor_data.frame < frame:
self.node.logwarn(
"{}({}): skipping old frame {}, expected {}".
format(self.__class__.__name__, self.get_id(),
carla_sensor_data.frame, frame))
except queue.Empty:
if ros_ok():
self.node.logwarn(
"{}({}): Expected Frame {} not received".format(
self.__class__.__name__, self.get_id(), frame))
return
def get_marker_pose(self):
"""
Function to return the pose for traffic participants.
:return: the pose of the traffic participant.
:rtype: geometry_msgs.msg.Pose
"""
return trans.carla_transform_to_ros_pose(self.carla_actor.get_transform())
def get_current_ros_pose(self):
"""
Function to provide the current ROS pose
:return: the ROS pose of this actor
:rtype: geometry_msgs.msg.Pose
"""
return trans.carla_transform_to_ros_pose(
self.carla_actor.get_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_marker_pose(self):
"""
Function to return the pose for vehicles.
:return: the pose of the vehicle
:rtype: geometry_msgs.msg.Pose
"""
# Moving pivot point from the bottom (CARLA) to the center (ROS) of the bounding box.
extent = self.carla_actor.bounding_box.extent
marker_transform = self.carla_actor.get_transform()
marker_transform.location -= marker_transform.get_up_vector() * extent.z
return trans.carla_transform_to_ros_pose(marker_transform)
def update_waypoints(self, waypoints, start_time=None):
"""
Execute on tick of the controller's control loop
"""
super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
rospy.loginfo("{}: Waypoints changed.".format(self._role_name))
path = Path()
path.header.stamp = rospy.get_rostime()
path.header.frame_id = "map"
for wpt in waypoints:
path.poses.append(
PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
self._path_publisher.publish(path)
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 _update_synchronous_event_sensor(self, frame):
while True:
try:
carla_sensor_data = self.queue.get(block=False)
if carla_sensor_data.frame != frame:
rospy.logwarn("{}({}): Received event for frame {}"
" (expected {}). Process it anyways.".format(
self.__class__.__name__, self.get_id(),
carla_sensor_data.frame, frame))
rospy.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_tf(
trans.carla_transform_to_ros_pose(
carla_sensor_data.transform))
self.sensor_data_updated(carla_sensor_data)
except queue.Empty:
return
def _callback_sensor_data(self, carla_sensor_data):
"""
Callback function called whenever new sensor data is received
:param carla_sensor_data: carla sensor data object
:type carla_sensor_data: carla.SensorData
"""
if not rospy.is_shutdown():
if self.synchronous_mode:
if self.sensor_tick_time:
self.next_data_expected_time = carla_sensor_data.timestamp + \
float(self.sensor_tick_time)
self.queue.put(carla_sensor_data)
else:
self.publish_tf(
trans.carla_transform_to_ros_pose(
carla_sensor_data.transform))
self.sensor_data_updated(carla_sensor_data)
def get_waypoint(self, req, response=None):
"""
Get the waypoint for a location
"""
carla_position = carla.Location()
carla_position.x = req.location.x
carla_position.y = -req.location.y
carla_position.z = req.location.z
carla_waypoint = self.map.get_waypoint(carla_position)
response = get_service_response(GetWaypoint)
response.waypoint.pose = trans.carla_transform_to_ros_pose(carla_waypoint.transform)
response.waypoint.is_junction = carla_waypoint.is_junction
response.waypoint.road_id = carla_waypoint.road_id
response.waypoint.section_id = carla_waypoint.section_id
response.waypoint.lane_id = carla_waypoint.lane_id
return response
def get_actor_waypoint(self, req, response=None):
"""
Convenience method to get the waypoint for an actor
"""
# self.loginfo("get_actor_waypoint(): Get waypoint of actor {}".format(req.id))
actor = self.world.get_actors().find(req.id)
response = get_service_response(GetActorWaypoint)
if actor:
carla_waypoint = self.map.get_waypoint(actor.get_location())
response.waypoint.pose = trans.carla_transform_to_ros_pose(carla_waypoint.transform)
response.waypoint.is_junction = carla_waypoint.is_junction
response.waypoint.road_id = carla_waypoint.road_id
response.waypoint.section_id = carla_waypoint.section_id
response.waypoint.lane_id = carla_waypoint.lane_id
else:
self.logwarn("get_actor_waypoint(): Actor {} not valid.".format(req.id))
return response
def _update_synchronous_sensor(self, frame, timestamp):
# This is extremely s***e
while not self.next_data_expected_time or\
(not self.queue.empty() or
self.next_data_expected_time and
self.next_data_expected_time < timestamp):
start_time = time.time()
while True:
try:
carla_sensor_data = self.queue.get_nowait() # This takes the time! (~0.16 s)
end_time = time.time()
duration = end_time - start_time
rospy.loginfo("Updating 1 took: {} s".format(duration))
if carla_sensor_data.frame == frame:
rospy.logdebug("{}({}): process {}".format(
self.__class__.__name__, self.get_id(), frame))
self.publish_tf(trans.carla_transform_to_ros_pose(
carla_sensor_data.transform))
self.sensor_data_updated(carla_sensor_data)
return
elif carla_sensor_data.frame < frame:
rospy.logwarn("{}({}): skipping old frame {}, expected {}".format(
self.__class__.__name__,
self.get_id(),
carla_sensor_data.frame,
frame))
except queue.Empty:
if not rospy.is_shutdown():
# rospy.logwarn("{}({}): Expected Frame {} not received".format(
# self.__class__.__name__, self.get_id(), frame))
event = threading.Event()
event.wait(0.005)
