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))