diff --git a/Added_summer/README b/Added_summer/README
new file mode 100644
index 0000000000000000000000000000000000000000..405549aeee13394fcd3f6febdc9b36472f79e58c
--- /dev/null
+++ b/Added_summer/README
@@ -0,0 +1,6 @@
+The file “toPose.py” is what we had to try to change crazyflie position data to the pose message that the mower uses. Not tested as we never got a crazyflie up and running.
+
+
+The main.py is the script that should start most of the system automatically.
+
+MainHRP_SLAM.py is the most up to date version of the system that runs on the mower to control the mower and avoid obstacles.
\ No newline at end of file
diff --git a/Added_summer/main.py b/Added_summer/main.py
new file mode 100644
index 0000000000000000000000000000000000000000..8f446262b51afac2bb5dcc7e3008c857a50d4a25
--- /dev/null
+++ b/Added_summer/main.py
@@ -0,0 +1,47 @@
+gimport mainHRP_SLAM
+from pexpect import pxssh
+import socket
+import fcntl
+import struct
+import subprocess
+import os
+import time
+
+def main():
+
+ b=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ ifname='wlan0'
+ ip=socket.inet_ntoa(fcntl.ioctl(b.fileno(),0x8915,struct.pack('256s',ifname[:15]))[20:24])
+ os.environ["ROS_MASTER_URI"] = "http://"+ip+":11311"
+ os.environ["ROS_IP"] = ip
+ os.environ["ROS_HOSTNAME"] = ip
+
+ p1=subprocess.Popen('roscore')
+ time.sleep(5)
+ subprocess.Popen(["rosrun","urg_node","urg_node","_ip_address:=192.168.1.11"])
+ try:
+ s=pxssh.pxssh()
+ time.sleep(1)
+ s.login('192.168.100.1','ubuntu',password='ubuntu',auto_prompt_reset=False,login_timeout=50)
+
+ s.sendline('export ROS_MASTER_URI=http://' + ip + ':11311')
+ time.sleep(1)
+ s.sendline('roslaunch am_driver_safe automower_hrp.launch')
+ time.sleep(1)
+ s.close()
+ except:
+ print "Mower already online!"
+# time.sleep(30)
+ return resume()
+
+def resume():
+ b=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ ifname='wlan0'
+ ip=socket.inet_ntoa(fcntl.ioctl(b.fileno(),0x8915,struct.pack('256s',ifname[:15]))[20:24])
+ os.environ["ROS_MASTER_URI"] = "http://"+ip+":11311"
+ os.environ["ROS_IP"] = ip
+ os.environ["ROS_HOSTNAME"] = ip
+ mower = mainHRP_SLAM.Mower()
+ return mower
+
+
diff --git a/Added_summer/mainHRP_SLAM.py b/Added_summer/mainHRP_SLAM.py
new file mode 100644
index 0000000000000000000000000000000000000000..f415d2e8537be2e15c3fae0d95e4f1e5c1a62108
--- /dev/null
+++ b/Added_summer/mainHRP_SLAM.py
@@ -0,0 +1,498 @@
+import time
+import rospy
+from geometry_msgs.msg import Twist
+import numpy as np
+import math
+from sensor_msgs.msg import LaserScan
+from nav_msgs.msg import Odometry
+from nav_msgs.msg import OccupancyGrid
+from am_driver.msg import SensorStatus
+from std_msgs.msg import UInt16
+import subprocess
+import os
+import threading
+from std_msgs.msg import Vector3
+class Mower(object):
+
+ def __init__(self):
+ self.corrDX = 0
+ self.corrDY = 0
+ self.corrDA = 0
+ self.stop = 0
+ self.moving=0
+ self.operationalMode = 0;
+ self.sensorStatus = 0;
+ self.x=0
+ self.y=0
+ self.angle=0
+ self.ranges=np.zeros([1080,1])
+ self.map = -1*np.ones([4000,4000])
+ self.rawMap=-1*np.ones(4000*4000)
+ 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('/vectorCorrPose',Vector3,self.correction)
+ self.posePub = rospy.Publisher('/poseCorrected',Vector3,queue_size=10)
+ #rospy.Subscriber('/map', OccupancyGrid,self.loadMap)
+ self.twist = Twist()
+ self.f = np.vectorize(lambda x: 30 if x.__abs__()>30 else x)
+ self.reset()
+ rospy.init_node('neural')
+ self.Generation=0
+ self.h=0.1
+ self.r=rospy.Rate(10)
+ self.side=0
+ self.devnull = open(os.devnull, 'w')
+ 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)
+ rospy.Subscriber('/sensor_status',SensorStatus,self.callback_sensor_status)
+ self.pub_mode = rospy.Publisher('/cmd_mode', UInt16, queue_size=1)
+ 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 correction(self,data):
+ self.corrDX, self.corrDY, self.corrDA = data
+
+ def isClear(self,x,y):
+ dx=x-self.x
+ dy=y-self.y
+ vx = (dy/np.sqrt(dx*dx+dy*dy)*20).round()
+ vy = (-dx/np.sqrt(dx*dx+dy*dy)*20).round()
+ #v1x = (0.25*dy/np.sqrt(dx*dx+dy*dy)*20).round()
+ #v1y = (-0.25*dx/np.sqrt(dx*dx+dy*dy)*20).round()
+ #v2x = (0.1*dy/np.sqrt(dx*dx+dy*dy)*20).round()
+ #v2y = (-0.1*dx/np.sqrt(dx*dx+dy*dy)*20).round()
+ steps=(np.max([dx.__abs__(),dy.__abs__()])*20*5).round()
+ xSpace=(np.linspace(self.x,x,steps)*20).round().astype(np.int8)
+ ySpace=(np.linspace(self.y,y,steps)*20).round().astype(np.int8)
+ obstacle = False
+ for j in range(-40,40):
+ i=-1
+ while(i<steps-1 and not obstacle):
+ i=i+1
+ obstacleTemp=self.map[2000+xSpace[i]+vx*j*0.01][2000+ySpace[i]+vy*j*0.01]>50
+ if(obstacleTemp):
+ obstacle=True
+ return obstacle,i
+
+ def moveTo(self,x,y):
+ self.moving=1
+ print(x)
+ print(y)
+ print(self.side)
+ oldX=self.x
+ oldY=self.y
+ dx=x-self.x
+ dy=y-self.y
+ steps=(np.max([dx.__abs__(),dy.__abs__()])*20*5).round()
+ xSpace=np.linspace(self.x,x,steps)
+ ySpace=np.linspace(self.y,y,steps)
+ obstacle,i = self.isClear(x,y)
+ vx = dy/np.sqrt(dx*dx+dy*dy)
+ vy = -dx/np.sqrt(dx*dx+dy*dy)
+ ddx = dx/np.sqrt(dx*dx+dy*dy)
+ ddy = dy/np.sqrt(dx*dx+dy*dy)
+
+ if(obstacle):
+ print('Not possible, trying an alternate route...')
+ start = time.time()
+ j=-1
+ altFind=False
+ while(not altFind):
+ j=j+1
+ #print(xSpace[i]+vx*2*j)
+ #print(ySpace[i]+vy*2*j)
+ obs1,i1=self.isClear(xSpace[i]+vx*2*j*0.1,ySpace[i]+vy*2*j*0.1)
+ obs2,i2=self.isClear(xSpace[i]+vx*2*(j*0.1+1),ySpace[i]+vy*2*(j*0.1+1))
+ obs3,i3=self.isClear(xSpace[i]-vx*2*j*0.1,ySpace[i]-vy*2*j*0.1)
+ obs4,i4=self.isClear(xSpace[i]-vx*2*(j*0.1+1),ySpace[i]-vy*2*(j*0.1+1))
+ #print(obs1)
+ #print(obs2)
+ #print(obs3)
+ #print(obs4)
+ #print(not self.side==-1)
+ #print(not self.side==1)
+ if(not obs1 and not obs2 and not self.side<0):
+ print('Right')
+ self.side=1
+ self.moveTo(xSpace[i]+vx*2*j*0.1+vx,ySpace[i]+vy*2*j*0.1+vy)
+ altFind=True
+ elif(not obs3 and not obs4 and not self.side>0):
+ print('Left')
+ self.side=-1
+ self.moveTo(xSpace[i]-vx*2*j*0.1-vx,ySpace[i]-vy*2*j*0.1-vy)
+ altFind=True
+ elif(j>200):
+ self.setSpeed(-0.3)
+ time.sleep(5)
+ self.setSpeed(0)
+ altFind=True
+
+ time.sleep(1)
+ self.side=0
+ self.moveTo(x,y)
+ else:
+ PAng = 0.2
+ TiAng = 2
+ TdAng = 5
+ TrAng= 0.7
+ NAng = 2
+ BAng = 1
+
+ P = 0.2
+ Ti = 1
+ Tr = 1
+ Td = 0
+ N = 1
+ B = 1
+
+ IAng = 0
+ I = 0
+ DAng = 0
+ D = 0
+ adAng = (2*TdAng-NAng*self.h)/(2*TdAng+NAng*self.h)
+ ad = (2*Td-N*self.h)/(2*Td+N*self.h)
+ bdAng = 2*PAng*TdAng*NAng/(2*TdAng+NAng*self.h)
+ bd = 2*P*Td*N/(2*Td+N*self.h)
+ angleOld = np.copy(self.angle)
+ old = np.sqrt(dx*dx+dy*dy)
+ uAngOld=0
+ outOld=0
+ while(dx.__abs__()>0.15 or dy.__abs__()>0.15):
+
+ angleGoal=np.fmod(np.arctan(-dx/dy)+np.pi/2,np.pi)-np.pi*0.5*(1-np.sign(dy))
+ dAngle=np.fmod(angleGoal-self.angle+np.pi,2*np.pi)-np.pi
+ #print(dAngle)
+ DAng=0
+ IAng=0
+ while(dAngle.__abs__()>0.1):
+ angle = np.copy(self.angle)
+ dAngle = np.fmod(angleGoal-angle+np.pi,2*np.pi)-np.pi
+ #print(angleGoal)
+ #print(self.angle)
+ #self.setAngSpeed(-np.sign(dAngle)*0.5)
+ #time.sleep(0.1)
+
+ DAng = adAng*DAng-bdAng*(angle-angleOld)
+ vAng = PAng*(BAng*angleGoal-angle)+IAng+DAng
+ outAng = np.clip(vAng,-0.5,0.5)
+ self.setAngSpeed(outAng)
+ IAng = IAng + PAng*self.h/TiAng*dAngle+self.h/TrAng*(outAng-vAng)
+ angleOld = np.copy(angle)
+ self.r.sleep()
+ dAngle = angleGoal-self.angle
+ uAngOld=outAng
+
+ print('rot done')
+ self.setAngSpeed(0)
+ time.sleep(1)
+ start = time.time()
+ elapsed=time.time()-start
+ #self.setSpeed(1)
+ DAng=0
+ IAng=0
+ D=0
+ I=0
+ while((dx.__abs__()>0.15 or dy.__abs__()>0.15) and elapsed<30):
+ #print(elapsed)
+ #print(angleGoal)
+ #print(self.angle)
+ dx=x-self.x
+ dy=y-self.y
+ if dy is 0:
+ dy=0.001
+ angleGoal=np.fmod(np.arctan(-dx/dy)+np.pi/2,np.pi)-np.pi*0.5*(1-np.sign(dy))
+ elapsed=time.time()-start
+ time.sleep(0.1)
+ angle = np.copy(self.angle)
+ dAngle = angleGoal-angle
+ #print(dAngle)
+ #self.setAngSpeed(-np.sign(dAngle)*0.5)
+ #time.sleep(0.1)
+
+ DAng = adAng*DAng-bdAng*(angle-angleOld)
+ vAng = PAng*(BAng*angleGoal-angle)+IAng+DAng
+ outAng = np.clip(vAng,-0.5,0.5)
+ self.setAngSpeed(outAng)
+ IAng = IAng + PAng*self.h/TiAng*dAngle+self.h/TrAng*(outAng-vAng)
+ angleOld = np.copy(angle)
+ uAngOld=outAng
+ #print(x)
+ #print(self.x)
+ #print(y)
+ #print(self.y)
+ Y = np.sqrt(dx*dx+dy*dy)
+ Dy=0-Y
+ D = ad*D-bd*(Y - old)
+ v = P*(B*0-Y)+I+D
+ out = np.clip(v,-0.3,0.3)
+ self.setSpeed(-out)
+ I = I + P*self.h/Ti*(Dy)+self.h/Tr*(out-v)
+ old = np.copy(Y)
+ outOld=out
+ obs,ind = self.isClear(x,y)
+ print(obs)
+ if np.array(self.ranges).__abs__().min()<0.7 or obs:
+ print "Collision"
+ if np.array(self.ranges[0:361]).__abs__().min()<0.7:
+ angSpeed=0.5
+ elif np.array(self.ranges[720:1081]).__abs__.min()<0.7:
+ angSpeed=-0.5
+ else:
+ angSpeed=0
+ self.setAngSpeed(0)
+ time.sleep(0.05)
+ self.setSpeed(0.075)
+ time.sleep(0.5)
+ self.setAngSpeed(angSpeed)
+ self.setSpeed(-0.15)
+ time.sleep(2)
+ self.setSpeed(0.075)
+ time.sleep(0.5)
+ self.setAngSpeed(0)
+ time.sleep(0.2)
+ self.setSpeed(0)
+ self.moveTo(x,y)
+ if self.stop is 1:
+ dx=0
+ dy=0
+ self.stop=0
+ self.setSpeed(0)
+ #self.setSpeed(0)
+ self.r.sleep()
+
+ self.setSpeed(0)
+ time.sleep(0.1)
+ self.setAngSpeed(0)
+ time.sleep(1)
+
+
+
+ def loadMap(self):
+ while(True):
+ # Origin is at [2000][2000], resolution 0.05
+ x=self.x
+ y=self.y
+ angle=self.angle
+ ranges=np.array(self.ranges)
+ #print('new scan')
+ for u in range(0,271):
+ i=u*4
+ if not ranges[i]<0:
+ if ranges[i]>20:
+ ranges[i]=21
+ dx = np.cos(angle+np.pi/180*135*i/540-135*np.pi/180)
+ dy = np.sin(angle+np.pi/180*135*i/540-135*np.pi/180)
+ steps=(np.round(ranges[i]*20)).astype(np.int16)
+ #print(steps)
+ if steps>0:
+ xSpace=np.linspace(np.round(x*20),np.round((x+dx*ranges[i])*20),steps).round().astype(np.int16)
+ ySpace=np.linspace(np.round(y*20),np.round((y+dy*ranges[i])*20),steps).round().astype(np.int16)
+ #print(xSpace)
+ #print(ySpace)
+ for j in range(0,steps):
+ self.map[2000+xSpace[j],2000+ySpace[j]]=0
+ if not ranges[i]>20:
+ #print('notSkipped')
+ self.map[2000+xSpace[-1],2000+ySpace[-1]]=100
+ #print('scan done')
+ times = 0
+ while(self.operationalMode is not 1):
+ if self.moving is 0:
+ if times is 0:
+ print "Not in manual mode, trying to change that."
+ if times>15:
+ times = 0
+ else:
+ times = self.times+1
+ self.pub_mode.publish(272)
+ self.pub_mode.publish(144)
+ time.sleep(2)
+ else:
+ self.stop=1
+
+
+ def placePos(self,data):
+ self.x = data.pose.pose.position.x-self.corrDX
+ self.y = data.pose.pose.position.y-self.corrDY
+ q0 = data.pose.pose.orientation.w
+ q1 = data.pose.pose.orientation.x
+ q2 = data.pose.pose.orientation.y
+ q3 = data.pose.pose.orientation.z
+ self.angle = np.arctan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-self.corrDA
+ self.posePub.publish(Vector3(self.x,self.y,self.angle))
+
+ def lidar_msg(self,data):
+ self.ranges=data.ranges
+
+ def setSpeed(self, speed):
+ self.twist.linear.x = np.clip(speed,-2,2)
+ self.pub_twist.publish(self.twist)
+
+ def setAngSpeed(self, angSpeed):
+ self.twist.angular.z = np.clip(angSpeed,-1,1)
+ self.pub_twist.publish(self.twist)
+
+ def getRanges(self):
+ return self.f(np.asarray(list(self.ranges)))
+
+ def callback_sensor_status(self,data):
+ if (self.operationalMode != data.operationalMode) or (self.sensorStatus != data.sensorStatus):
+ self.operationalMode = data.operationalMode
+ self.sensorStatus = data.sensorStatus
+
+
+ def load(self,arr,gen=0):
+ self.NN = arr
+ self.Generation=gen
+
+ def sample(self):
+ r = rospy.Rate(3)
+ emptySample=np.zeros(256)
+ self.sampleNodes=np.zeros(256)
+ self.sampleNodes=np.append([self.sampleNodes],[self.sampleNodes],axis=0)
+ start = time.time()
+ while((time.time()-start)<180):
+ filledSample = emptySample.copy()
+ filledSample[0:109]=(self.getRanges())
+ filledSample[109]=1
+ filledSample[-2]=(self.twist.linear.x)
+ filledSample[-1]=(self.twist.angular.z)
+ self.sampleNodes=np.append(self.sampleNodes,[filledSample.copy()],axis=0)
+ r.sleep()
+ self.sampleNodes = np.delete(self.sampleNodes,0,0)
+ self.sampleNodes = np.delete(self.sampleNodes,0,0)
+
+
+ def train(self):
+ learnrate=0.00002
+ NN = (np.random.uniform(-0.001,0.001,[256,256]))
+ #print(NN)
+ sumError = 1000000
+ #oldError = 0
+ iteration = 0
+ deltaError=1000000
+ while(deltaError.__abs__()>0.005):
+ iteration = iteration+1
+ oldError = sumError
+ sumError=0
+ guessedSample=self.sampleNodes.copy()
+ print(iteration)
+ for i in range(1,self.sampleNodes.__len__()):
+ guessedSample[i]=NN.dot(guessedSample[i-1])
+ deltaNNtot = NN*0
+ errors = guessedSample*0
+ for i in range(0,self.sampleNodes.__len__()):
+ deltaNN = NN*0
+ outError = guessedSample[-1-i]-self.sampleNodes[-1-i]
+ #outError[:(outError.__len__()-2)]=0
+ sumError=sumError+outError.__abs__().sum()
+ #outError = outError*guessedSample[-1-i]
+ #print(outError)
+ #deltaNN[-1]=deltaNN[-1]+np.transpose(NN).dot(outError)*guessedSample[-1-i]
+ #print(deltaNN[-1])
+ #print(outError)
+ #print(guessedSample[-1-i])
+ for j in range(0,deltaNN.__len__()):
+ deltaNN[j]= outError[j]*NN[j].__abs__()
+
+
+ deltaNNtot = deltaNNtot+deltaNN
+
+ #print('3')
+ #print(deltaNNtot)
+
+
+ print(sumError)
+ deltaError = oldError-sumError
+ if deltaError<0:
+ learnrate = learnrate*0.9
+ NN = NN-deltaNNtot*learnrate
+ print(deltaError)
+ oldError = sumError
+ self.NN=np.array([NN,NN])
+ for i in range(0,8):
+ self.NN=np.append(self.NN,[NN],axis=0)
+
+
+ def reset(self):
+ self.NN = (np.random.uniform(-0.00001,0.00001,[10,256,256]))
+
+ def run(self):
+ try:
+ r = rospy.Rate(3)
+ while not rospy.is_shutdown():
+ score = np.zeros(10)
+ self.Generation = self.Generation+1
+ liveGeneration = True
+ while(liveGeneration):
+ score = np.zeros(10)
+ for name in range(0,10):
+ alive=True
+ self.state=np.zeros(256)
+ startX=np.copy(self.x)
+ startY=np.copy(self.y)
+ finalX=-startX
+ finalY=-startY
+ startTime = time.time()
+ while(alive):
+ self.state[0:109]=(self.getRanges())
+ if self.state[0:109].__abs__().min()<1.2:
+ alive=False
+ score[name] = -5
+ self.state[109]=1
+ self.state[110]=self.x
+ self.state[111]=self.y
+ self.state[112]=finalX
+ self.state[113]=finalY
+ self.state=self.NN[name].dot(self.state)
+ self.setSpeed(self.state[-2])
+ time.sleep(0.1)
+ self.setAngSpeed(self.state[-1])
+ if (time.time()-startTime)>60:
+ alive = False
+ if (self.x-finalX).__abs__()<1 and (self.y-finalY).__abs__()<1:
+ score[name]=500
+ r.sleep()
+ print('Death of')
+ self.setSpeed(0)
+ time.sleep(0.1)
+ self.setAngSpeed(0)
+ time.sleep(0.2)
+ endTime = time.time()
+ score[name] = score[name]-(startTime-endTime)*0.05-(self.x-finalX)/(startX-finalX)*(self.x-finalX)/(startX-finalX)-(self.y-finalY)/(startY-finalY)*(self.y-finalY)/(startY-finalY)
+ self.setAngSpeed(1)
+ time.sleep(3)
+ self.setAngSpeed(0)
+ time.sleep(0.2)
+ self.setSpeed(-0.5)
+ time.sleep(18)
+ self.setSpeed(1)
+ time.sleep(7.5)
+ self.setSpeed(0)
+ print('Gen')
+ print(self.Generation)
+ print('Name')
+ print(name)
+ print('Score')
+ print(score[name])
+
+ indices = (-score).argsort()
+ self.Generation = self.Generation+1
+ for i in range(0,5):
+ for k in range(0,1000):
+ i1 = np.round(np.random.uniform(-0.5,255.5))
+ i2 = np.round(np.random.uniform(-0.5,255.5))
+ i3 = np.round(np.random.uniform(-0.5,255.5))
+ i4 = np.round(np.random.uniform(-0.5,255.5))
+
+ self.NN[indices[i]][i1][i2]=self.NN[indices[i]][i1][i2]+((np.random.uniform(-0.1,0.1)))
+ self.NN[indices[-i-1]][i3][i4]=self.NN[indices[-i-1]][i3][i4]+((np.random.uniform(-0.1,0.1)))
+
+ except rospy.exceptions.ROSInterruptException:
+ self.setSpeed(0)
+ self.setAngSpeed(0)
+ pass
diff --git a/Added_summer/toPose.py b/Added_summer/toPose.py
new file mode 100755
index 0000000000000000000000000000000000000000..08e443bbd52efae1cf98f56612e53bb6eff70de4
--- /dev/null
+++ b/Added_summer/toPose.py
@@ -0,0 +1,43 @@
+import rospy
+import numpy as np
+from rts_project.msg import GenericLogData
+from std_msgs.msg import Vector3
+from geometry_msgs.msg import Twist
+
+class toPose():
+
+ def __init__(self):
+ self.convert = rospy.Publisher('/vectorCorrPose', Vector3, queue_size=10)
+ self.sub = rospy.Subscriber('/crazyflie/log_pos', GenericLogData, self.get_pos)
+ self.speed = rospy.Subscriber('/cmd_vel',Twist,self.veloc)
+ self.oldX = 0
+ self.oldY = 0
+ self.oldZ = 0
+ self.wz=0
+ self.vx=0
+ self.theta=0
+
+ def get_pos(self, msg):
+ self.oldX = self.x
+ self.oldY = self.y
+ self.oldZ = self.z
+ self.x, self.y, self.z = msg.values
+ dx = self.x-self.oldX
+ dy = self.y-self.oldY
+ dz = self.z-self.oldZ
+ if self.wz is 0.0:
+ self.theta = np.acos(dx)
+ if np.sign(dy)<0:
+ self.theta = self.theta + np.pi
+
+ self.convert.publish(Vector3(self.x,self.y,self.theta))
+
+ def veloc(self,data):
+ self.vx = data.linear.x
+ self.wz = data.angular.z
+
+def main():
+ rospy.init_node('flieConversion')
+
+if __name__ == '__main__':
+ main()