def __init__(self, name="groundtruth_node", log_level=rospy.INFO):
super().__init__(name=name, log_level=log_level)
# Initialize without a road
# The road is loaded in self.update()
road = road_module.Road()
self.groundtruth = Groundtruth(road=road)
self.renderer = Renderer(
road,
remove_model=self._remove_model,
spawn_model=self._spawn_model,
pause_gazebo=self._pause_gazebo,
unpause_gazebo=self._unpause_gazebo,
info_callback=self.update_groundtruth_status,
tile_size=Vector(self.param.tile_size),
tile_resolution=Vector(self.param.tile_resolution),
)
self.object_controller = ObjectController(
road,
remove_model=self._remove_model,
spawn_model=self._spawn_model,
)
self.model_names = []
self.car_state = CarStateMsg()
self._groundtruth_status = GroundtruthStatus()
self.run()
def test_speed_speaker(self):
speed_speaker = SpeedSpeaker(time=0)
car_msg = CarStateMsg()
speeds = []
# speed, (time_before, time_after), expected result
speeds.append((0, (0, 2), SpeakerMsg.SPEED_HALTED))
speeds.append((0, (0, 4), SpeakerMsg.SPEED_STOPPED))
speeds.append((0, (0, 0), SpeakerMsg.SPEED_0))
speeds.append((3, (0, 0), SpeakerMsg.SPEED_1_10))
speeds.append((13, (0, 0), SpeakerMsg.SPEED_11_20))
speeds.append((25, (0, 0), SpeakerMsg.SPEED_21_30))
speeds.append((32, (0, 0), SpeakerMsg.SPEED_31_40))
speeds.append((44, (0, 0), SpeakerMsg.SPEED_41_50))
speeds.append((55, (0, 0), SpeakerMsg.SPEED_51_60))
speeds.append((65, (0, 0), SpeakerMsg.SPEED_61_70))
speeds.append((73, (0, 0), SpeakerMsg.SPEED_71_80))
speeds.append((83, (0, 0), SpeakerMsg.SPEED_81_90))
speeds.append((99, (0, 0), SpeakerMsg.SPEED_91_))
for speed, time, expected in speeds:
speed_speaker.listen(car_msg)
speed_speaker.speak(current_time=time[0])
car_msg.twist.linear = Point(speed / 3.6 / 10, 0,
0).to_geometry_msg() # 1 m/s
speed_speaker.listen(car_msg)
response = speed_speaker.speak(current_time=time[1])
self.assertEqual(response[0].type, expected)
def fake_car_state(self, path: Line, arc_length: float, speed: float):
"""Publish a CarState message depending on situation.
Args:
path: The car drives on this path.
arc_length: Current arc length of the car on the path.
speed: Speed the car drives with.
"""
pose = path.interpolate_pose(arc_length=arc_length)
msg = CarStateMsg()
msg.pose = pose.to_geometry_msg()
msg.frame = (
Transform(pose.to_geometry_msg())
* Polygon([[-0.1, -0.1], [-0.1, 0.1], [0.1, 0.1], [0.1, -0.1]])
).to_geometry_msg()
msg.twist = Twist()
msg.twist.linear = Vector(speed, 0, 0).to_geometry_msg()
self.car_state_publisher.publish(msg)
def state_update(self):
"""Publish new CarState with updated information."""
# Request current pose and twist from model_interface
pose: geometry_msgs.msg.Pose = self.latest_vehicle_pose
twist: geometry_msgs.msg.Twist = self.latest_vehicle_twist
# Transform which is used to calculate frame and view cone
tf = Transform(pose)
rospy.logdebug(f"State update transform: {tf.rotation}")
# Create message
msg = CarStateMsg()
msg.pose = pose
msg.twist = twist
msg.frame = (tf * self.car_frame).to_geometry_msg()
if self.view_cone:
msg.view_cone = (tf * self.view_cone).to_geometry_msg()
if not rospy.is_shutdown():
self.publisher.publish(msg)
def test_speak_function(self):
"""Test speaker msg."""
# Car msg
pose = Pose(Point(1, 3), 0)
# Frames
frames = []
frames.append((
Polygon([
Point(0.1, 0.1),
Point(0.1, 0.3),
Point(0.5, 0.3),
Point(0.5, 0.1)
]),
SpeakerMsg.LEFT_LANE,
)) # On left side
frames.append((
Polygon([
Point(0, -0.3),
Point(0, -0.01),
Point(0.5, -0.01),
Point(0.5, -0.3)
]),
SpeakerMsg.RIGHT_LANE,
)) # On right side
frames.append((
Polygon([
Point(0, -0.3),
Point(0, 0.3),
Point(0.5, 0.3),
Point(0.5, -0.3)
]),
SpeakerMsg.LEFT_LANE,
)) # Between left and right should return left!
frames.append((
Polygon([
Point(-0.5, -0.3),
Point(-0.5, 0.3),
Point(0.5, 0.3),
Point(0.5, -0.3)
]),
SpeakerMsg.OFF_ROAD,
)) # Partly in partly out
frames.append((
Polygon([Point(10, 2),
Point(10, 3),
Point(15, 3),
Point(15, 2)]),
SpeakerMsg.OFF_ROAD,
)) # Off road
# Parking
frames.append((
Polygon(
[Point(1, 0.4),
Point(1, 0.7),
Point(2, 0.7),
Point(2, 0.4)]),
SpeakerMsg.PARKING_LOT,
)) # On left parking
frames.append((
Polygon([
Point(1, -0.4),
Point(1, -0.7),
Point(1, -0.7),
Point(2, -0.4)
]),
SpeakerMsg.PARKING_LOT,
)) # On right parking
frames.append((
Polygon([Point(1, 0),
Point(1, -0.7),
Point(2, -0.7),
Point(2, 0)]),
SpeakerMsg.PARKING_LOT,
)) # On right side and parking lot
car_msg = CarStateMsg()
car_msg.pose = pose.to_geometry_msg()
for frame, expected in frames[-1:]:
print(f"Expecting msg {expected} for frame {frame}.")
self.speaker.listen(car_msg)
self.assertTrue(
utils.assert_msgs_for_pos(self.speaker, frame, expected))