webpage_sample

webpage_sample.py

# Perform sensory fusion and generate 3d obstacle map using depth, RGB cameras, lidars
# and SLAM outputs
obstacle_signals = self._costmap_generator.generate_costmap(
    planar_scans=layer_observations.planar_scans,
    voxel_scans=layer_observations.voxel_scans,
    depth_observations=layer_observations.depth_observations,
    point_cloud_observations=layer_observations.point_cloud_observations,
    camera_observations=layer_observations.camera_observations,
    robot_state_observations=layer_observations.robot_state_observations,
    current_time=self._last_added_observation_timestamp,
    current_pose=current_pose,
    static_map=static_map
)
# Unpack obstacle map signals for detection and tracking
odom_costmap = obstacle_signals[ObstacleGeneratorSignal.OBSTACLE_MAP]
planar_obstacle_costmap = obstacle_signals[ObstacleGeneratorSignal.PLANAR_OBSTACLE_MAP]
# Detect moving object subtracting robot movement
moving_objects_costmap, valid_rectangles = self._movement_detector.detect_movement(
    moving_obstacle_costmap_with_unknown=planar_obstacle_costmap,
    full_obstacle_map_with_unknown=odom_costmap,
    static_map_in_odom=static_map_in_odom,
    time=self._last_added_observation_timestamp
)
# Detect people from camera and movement observations
people = self._people_tracker.update_tracker(
    camera_observations=layer_observations.camera_observations,
    costmap = planar_obstacle_costmap,
    hypothesis_rectangles = valid_rectangles,
)
# Convert people detection results to be used by motion planning
people_costmap = CostmapTracker.people_to_people_costmap(
    obstacles_costmap=planar_obstacle_costmap, people=people)