Ask Your Question
0

create 2d occupancy grid map by laser data

asked 2018-03-22 05:12:04 -0500

abdelkrim gravatar image

updated 2018-03-26 06:48:10 -0500

i want to create 2d occupancy grid map manually without using slam packages help me please with tutorials or documentation to learn how i can do that thank you

edit retag flag offensive close merge delete

Comments

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?

allenh1 gravatar imageallenh1 ( 2018-03-26 09:45:34 -0500 )edit

i want just to create a simple map ( local map )for navigation to implement some demonstration

abdelkrim gravatar imageabdelkrim ( 2018-03-26 11:20:57 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-03-26 13:53:10 -0500

Gayan Brahmanage gravatar image

updated 2018-03-26 16:22:33 -0500

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)
edit flag offensive delete link more

Comments

that is exactly what i am looking for just to create a local map with small movements it s enough for me . can you help me how i can do that

abdelkrim gravatar imageabdelkrim ( 2018-03-26 16:00:28 -0500 )edit

See the updated answer Good luck!

Gayan Brahmanage gravatar imageGayan Brahmanage ( 2018-03-26 16:26:14 -0500 )edit

i will start with this script and reply you later thank you very much

abdelkrim gravatar imageabdelkrim ( 2018-03-26 16:28:57 -0500 )edit

it is very helpful. thanks! could you mention that where did you get these code?

suho0515 gravatar imagesuho0515 ( 2019-08-08 22:02:44 -0500 )edit

this was my own code, I think the original question has been posted a long time ago, Unfortunately, I don't have these codes anymore. Please try this code. This is a simple code with some math.

Gayan Brahmanage gravatar imageGayan Brahmanage ( 2019-08-08 22:44:52 -0500 )edit

Thanks again, i'm gonna try it! can i ask you something? if i understand right, the "main.py" get "scan data", "odom data" and send these data to localmap function. after calculating "somthing" in localmap function, finally they publish map data. so... what "localmap.py" do? just drawing map? and, is localization or navigation algorithm working in "bresenham.py"?

sorry for too many question, but i really do wanna understand your work well.
if there is paper or article about this algorithms could you mention it?

thanks again Gayan Brahmanage!

suho0515 gravatar imagesuho0515 ( 2019-08-08 23:50:58 -0500 )edit

You can follow this tutorial to understand the math behind the code

Gayan Brahmanage gravatar imageGayan Brahmanage ( 2019-08-09 00:14:36 -0500 )edit

Thank you very much :)

suho0515 gravatar imagesuho0515 ( 2019-08-09 00:52:24 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-03-22 05:12:04 -0500

Seen: 1,982 times

Last updated: Mar 26 '18