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)