Python OccupancyGrid.data[col][0]方法代码示例

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import data[col][0] [as 别名]
  def __init__(self, pt=None):
    self.n_particles = 10
    self.lin_spread = .05
    self.ang_spread = 1
    self.pt = pt if pt else (5,5,5)

    self.variance = 10
    self.gauss_constant = math.sqrt(2*math.pi)
    self.expected_value = 0 

    map = OccupancyGrid()
    map.header.frame_id = '/odom'
    # map.info.map_load_time = rospy.Time.now()
    map.info.resolution = .1 # The map resolution [m/cell]
    map.info.width = 1000
    map.info.height = 1000

    map.data = [[0 for _ in range(map.info.height)] for _ in range(map.info.width)]

    # for row in range(map.info.height):
    #     for col in range(map.info.width):
    #         print str(col) + str(row)

    #         map.data[col] = 0
    map.data[40][50]=1
    map.data[120][100] = 1
    map.data[12][110] = 1
    # map.data[13][10] = 1
    # map.data[13][11] = 1
    # map.data[11][10] = 1

    for row in range(map.info.height):
        map.data[0][row] = 1
        map.data[map.info.width-1][row] = 1
    for col in range(map.info.width):
        map.data[col][0] = 1
        map.data[col][map.info.height -1] = 1
    #https://github.com/z2daj/rbe3002/blob/master/scripts/lab3.py

    print "start"
    self.occupancy_field = OccupancyField(map)

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import data[col][0] [as 别名]
    def __init__(self, test = False):
        
        self.test = test
        if self.test:
            print "Running particle filter in test mode"
        else:
            print "Running particle filter"
        self.initialized = False        # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')           # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"   # the frame of the robot base
        self.map_frame = "map"          # the name of the map coordinate frame
        self.odom_frame = "odom"        # the name of the odometry coordinate frame
        self.scan_topic = "scan"        # the topic where we will get laser scans from 

        self.n_particles = 20          # the number of particles to use

        self.d_thresh = 0.1             # the amount of linear movement before performing an update
        self.a_thresh = math.pi/6       # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0   # maximum penalty to assess in the likelihood field model
        self.scan_count = 0 

        self.robot_pose =  Pose(position=Point(x=.38, y=.6096,z=0), orientation=Quaternion(x=0, y=0, z=0, w=1))        

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
        self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # laser_subscriber listens for data from the lidar
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []
        self.ang_spread = 10.0/180.0 * math.pi
        self.lin_spread = .1
        self.current_odom_xy_theta = []

        #Set up constants for Guassian Probability
        self.variance = .1
        self.gauss_constant = math.sqrt(2*math.pi)
        self.expected_value = 0 

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid

        # We make a fake approximate map of an empty 2 by 2 square for testing to keep it simple.
        # If this worked better, we'd expand to a real OccupancyGrid, but we never got it to work better.
        map = OccupancyGrid()
        map.header.frame_id = '/odom'
        map.info.map_load_time = rospy.Time.now()
        map.info.resolution = .1 # The map resolution [m/cell]
        map.info.width = 288
        map.info.height = 288
        
        map.data = [[0 for _ in range(map.info.height)] for _ in range(map.info.width)] 
        
        for row in range(map.info.height):
            map.data[0][row] = 1
            map.data[map.info.width-1][row] = 1
        for col in range(map.info.width):
            map.data[col][0] = 1
            map.data[col][map.info.height -1] = 1

        self.occupancy_field = OccupancyField(map)
        if self.test:
            print "Initialized"
        self.initialized = True