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. 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 ...
(more)
What do you mean by "without slam packages"? Are you trying to implement your own SLAM, or are you just wanting to make a very primitive map?
i want just to create a simple map ( local map )for navigation to implement some demonstration