Skip to content
Snippets Groups Projects
Commit b3b3e0a0 authored by Joakim Ericson's avatar Joakim Ericson
Browse files

updated the code in mainHRP_SLAM to the modifications done to run the demo

parent ce285a54
No related branches found
No related tags found
No related merge requests found
mainHRP_SLAM.py 100644 → 100755
......@@ -8,7 +8,7 @@ from nav_msgs.msg import Odometry
from nav_msgs.msg import OccupancyGrid
import subprocess
import os
import threading
class Mower(object):
def __init__(self):
......@@ -17,7 +17,7 @@ class Mower(object):
self.pub_twist=rospy.Publisher('/cmd_vel',Twist,queue_size=1)
rospy.Subscriber('/scan',LaserScan,self.lidar_msg)
rospy.Subscriber('/odom',Odometry,self.placePos)
rospy.Subscriber('/map', OccupancyGrid,self.loadMap)
#rospy.Subscriber('/map', OccupancyGrid,self.loadMap)
self.twist = Twist()
self.f = np.vectorize(lambda x: 30 if x.__abs__()>30 else x)
self.reset()
......@@ -28,8 +28,13 @@ class Mower(object):
self.side=0
self.devnull = open(os.devnull, 'w')
subprocess.Popen(["rosrun","urg_node","urg_node","_ip_address:=192.168.1.11"])
self.proc = subprocess.Popen(["rosrun","gmapping","slam_gmapping","_temporalUpdate:=2","_map_update_interval:=2"],stdout=self.devnull, stderr=self.devnull)
subprocess.Popen(["rosrun","tf","static_transform_publisher","0","0","0","0","0","0","base_link","laser","100"])
time.sleep(10)
print("Starting")
#self.proc = subprocess.Popen(["rosrun","gmapping","slam_gmapping","_temporalUpdate:=2","_map_update_interval:=2"],stdout=self.devnull, stderr=self.devnull)
t1 = threading.Thread(target=self.loadMap)
t1.start()
time.sleep(10)
#subprocess.Popen(["rosrun","tf","static_transform_publisher","0","0","0","0","0","0","base_link","laser","100"])
def isClear(self,x,y):
dx=x-self.x
......@@ -225,7 +230,8 @@ class Mower(object):
def loadMap(self,data):
def loadMap(self):
while(True):
# Origin is at [2000][2000], resolution 0.05
x=self.x
y=self.y
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment