Revision history - ROS Answers: Open Source Q&A Forum

hi, As long as the accurate poses of the robot are not known, you are not able to build a consistent map without using a SLAM a framework. Because all the measurements and controls (Range and odometry) are inherently uncertain. you can build a local map without changing the robot pose or barely accurate map with small movements.

hi, As long as the accurate poses of the robot are not known, you are not able to build a consistent map without using a SLAM a framework. Because all the measurements and controls (Range and odometry) are inherently uncertain. you can build a local map without changing the robot pose or barely accurate map with small ~~movements.~~ movements.

hi, As long as the accurate poses of the robot are not known, you are not able to build a consistent map without using a SLAM framework. Because all the measurements and controls (Range and odometry) are inherently uncertain. you can build a local map without changing the robot pose or a barely accurate map with small movements.

hi, As long as the accurate poses of the robot are not known, you are not able to build a consistent map without using a SLAM framework. Because all the measurements and controls (Range and odometry) are inherently uncertain. you can build a local map without changing the robot pose or a barely accurate map with small ~~movements.~~movements. Use follwing python codes to get started.

build local map using localmap.py

import bresenham
from math import sin, cos, pi,tan, atan2,log
import math
from itertools import groupby
from operator import itemgetter
import tf
import rospy
import numpy as np
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import PointStamped

class localmap:
    def __init__(self, height, width, resolution,morigin):
        self.height=height
        self.width=width
        self.resolution=resolution
        self.punknown=-1.0
        self.localmap=[self.punknown]*int(self.width/self.resolution)*int(self.height/self.resolution)
        self.logodds=[0.0]*int(self.width/self.resolution)*int(self.height/self.resolution)
        self.origin=int(math.ceil(morigin[0]/resolution))+int(math.ceil(width/resolution)*math.ceil(morigin[1]/resolution))
        self.pfree=log(0.3/0.7)
        self.pocc=log(0.9/0.1)
        self.prior=log(0.5/0.5)
        self.max_logodd=100.0
        self.max_logodd_belief=10.0
        self.max_scan_range=1.0
        self.map_origin=morigin



    def updatemap(self,scandata,angle_min,angle_max,angle_increment,range_min,range_max,pose):

        robot_origin=int(pose[0])+int(math.ceil(self.width/self.resolution)*pose[1])
        centreray=len(scandata)/2+1
        for i in range(len(scandata)):
            if not math.isnan(scandata[i]):
                beta=(i-centreray)*angle_increment
                px=int(float(scandata[i])*cos(beta-pose[2])/self.resolution)
                py=int(float(scandata[i])*sin(beta-pose[2])/self.resolution)

                l = bresenham.bresenham([0,0],[px,py])
                for j in range(len(l.path)):                    
                    lpx=self.map_origin[0]+pose[0]+l.path[j][0]*self.resolution
                    lpy=self.map_origin[1]+pose[1]+l.path[j][1]*self.resolution

                    if (0<=lpx<self.width and 0<=lpy<self.height):
                        index=self.origin+int(l.path[j][0]+math.ceil(self.width/self.resolution)*l.path[j][1])
                        if scandata[i]<self.max_scan_range*range_max:
                            if(j<len(l.path)-1):self.logodds[index]+=self.pfree
                            else:self.logodds[index]+=self.pocc
                        else:self.logodds[index]+=self.pfree                        
                        if self.logodds[index]>self.max_logodd:self.logodds[index]=self.max_logodd
                        elif self.logodds[index]<-self.max_logodd:self.logodds[index]=-self.max_logodd
                        if self.logodds[index]>self.max_logodd_belief:self.localmap[index]=100
                        else:self.localmap[index]=0 
                        self.localmap[self.origin]=100.0

use main function main.py

#!/usr/bin/env python
import rospy
import roslib
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String
from nav_msgs.msg import OccupancyGrid
import tf
import math
from math import sin, cos, pi,tan, atan2
import numpy as np
from pylab import *
from itertools import groupby
from operator import itemgetter
import matplotlib.pyplot as plt
from scipy import interpolate

from localmap import localmap
pose=[0.0,0.0,0.0]
#***********************************************************************
def handle_robot_pose(parent, child, pose):
    br = tf.TransformBroadcaster()
    br.sendTransform((pose[0], pose[1], 0), tf.transformations.quaternion_from_euler(0, 0, pose[2]), rospy.Time.now(), child,parent)

#***********************************************************************
def odometryCb(msg):
    global pose
    x=msg.pose.pose.position.x
    y=msg.pose.pose.position.y
    q0 = msg.pose.pose.orientation.w
    q1 = msg.pose.pose.orientation.x
    q2 = msg.pose.pose.orientation.y
    q3 = msg.pose.pose.orientation.z
    theta=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))
    pose=[x,y,theta]

#*********************************************************************** 
def scanCb(msg):
    print pose
    py,px=[],[]
    scandata=msg.ranges
    angle_min=msg.angle_min
    angle_max=msg.angle_max
    angle_increment=msg.angle_increment
    range_min=msg.range_min
    range_max=msg.range_max 
    m.updatemap(scandata,angle_min,angle_max,angle_increment,range_min,range_max,pose)
    handle_robot_pose("map", "odom", pose)

#***********************************************************************    
def mappublisher(m,height, width, resolution,morigin):
    msg = OccupancyGrid()
    msg.header.frame_id='map'
    msg.info.resolution = resolution
    msg.info.width      = math.ceil(width/resolution)
    msg.info.height     = math.ceil(height/resolution)
    msg.info.origin.position.x=-morigin[0]
    msg.info.origin.position.y=-morigin[1]
    msg.data=m  
    mappub.publish(msg)

if __name__ == "__main__":

    rospy.init_node('main', anonymous=True) #make node 
    rospy.Subscriber('/odom',Odometry,odometryCb)
    rospy.Subscriber("/scan", LaserScan, scanCb)
    mappub= rospy.Publisher('/map',OccupancyGrid,queue_size=1)

    rate = rospy.Rate(10) # 100hz   

    height, width, resolution=10,10,0.05
    morigin=[width/2.0,height/2.0]
    m=localmap(height, width, resolution,morigin)

    while not rospy.is_shutdown():
        mappublisher(m.localmap,height, width, resolution,morigin)
        rate.sleep()

Use supplementary script bresenham.py

class bresenham:
    def __init__(self, start, end):
        self.start = list(start)
        self.end = list(end)
        self.path = []
        self.toggle=0

        if start[0]-end[0]+start[1]-end[1]==0:
            return None

        self.steep = abs(self.end[1]-self.start[1]) > abs(self.end[0]-self.start[0])

        if self.steep:
            self.start = self.swap(self.start[0],self.start[1])
            self.end = self.swap(self.end[0],self.end[1])

        if self.start[0] > self.end[0]:
            self.toggle=1
            #print 'flippin and floppin'
            _x0 = int(self.start[0])
            _x1 = int(self.end[0])
            self.start[0] = _x1
            self.end[0] = _x0

            _y0 = int(self.start[1])
            _y1 = int(self.end[1])
            self.start[1] = _y1
            self.end[1] = _y0

        dx = self.end[0] - self.start[0]
        dy = abs(self.end[1] - self.start[1])
        error = 0
        derr = dy/float(dx)

        ystep = 0
        y = self.start[1]

        if self.start[1] < self.end[1]: ystep = 1
        else: ystep = -1

        for x in range(self.start[0],self.end[0]+1):
            if self.steep:
                self.path.append((y,x))
            else:
                self.path.append((x,y))

            error += derr

            if error >= 0.5:
                y += ystep
                error -= 1.0

        if self.toggle==1:
            self.path.reverse()

    def swap(self,n1,n2):
        return [n2,n1]

Revision history [back]