jhy
/
RobotKernal-UESTC
forked from logzhan/RobotKernal-UESTC
1
0
Fork 0
Code Pull Requests Activity

Compare commits

merge into: jhy:main
Branches Tags
jhy:main
jhy:master
jhy:LRD_Develop
logzhan:LRD_Develop
logzhan:main
...
pull from: jhy:master
Branches Tags
jhy:main
jhy:master
jhy:LRD_Develop
logzhan:LRD_Develop
logzhan:main

No commits in common. "main" and "master" have entirely different histories.
main ... master

140 changed files with 7960 additions and 8552 deletions
Show Stats Expand all files Collapse all files

0
Code/MowingRobot/pibot_ros/pibot_init_env.sh Executable file → Normal file
View File

34
Code/MowingRobot/pibot_ros/pypibot/pypibot/__init__.py Executable file → Normal file
View File

@ -1,17 +1,17 @@
#!/usr/bin/python #!/usr/bin/python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
import pypibot.assistant import pypibot.assistant
import pypibot.log as Logger import pypibot.log as Logger
import pypibot.err import pypibot.err
log=Logger.log log=Logger.log
import sys import sys
def createNamedLog(name): def createNamedLog(name):
return Logger.NamedLog(name) return Logger.NamedLog(name)
class Object(): class Object():
pass pass
isDebug="-d" in sys.argv isDebug="-d" in sys.argv
import platform import platform
isWindows=False isWindows=False
if platform.system()=='Windows': if platform.system()=='Windows':
isWindows=True isWindows=True

468
Code/MowingRobot/pibot_ros/pypibot/pypibot/assistant.py Executable file → Normal file
View File

@ -1,234 +1,234 @@
#!/usr/bin/python #!/usr/bin/python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
import os, sys, inspect import os, sys, inspect
import datetime import datetime
import signal import signal
import threading import threading
import time import time
# function: get directory of current script, if script is built # function: get directory of current script, if script is built
# into an executable file, get directory of the excutable file # into an executable file, get directory of the excutable file
def current_file_directory(): def current_file_directory():
path = os.path.realpath(sys.path[0]) # interpreter starter's path path = os.path.realpath(sys.path[0]) # interpreter starter's path
if os.path.isfile(path): # starter is excutable file if os.path.isfile(path): # starter is excutable file
path = os.path.dirname(path) path = os.path.dirname(path)
path = os.path.abspath(path) # return excutable file's directory path = os.path.abspath(path) # return excutable file's directory
else: # starter is python script else: # starter is python script
caller_file = inspect.stack()[0][1] # function caller's filename caller_file = inspect.stack()[0][1] # function caller's filename
path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory
if path[-1]!=os.path.sep:path+=os.path.sep if path[-1]!=os.path.sep:path+=os.path.sep
return path return path
"""格式化字符串""" """格式化字符串"""
def formatString(string,*argv): def formatString(string,*argv):
string=string%argv string=string%argv
if string.find('$(scriptpath)')>=0: if string.find('$(scriptpath)')>=0:
string=string.replace('$(scriptpath)',current_file_directory()) string=string.replace('$(scriptpath)',current_file_directory())
if string.find('$(filenumber2)')>=0: if string.find('$(filenumber2)')>=0:
i=0 i=0
path="" path=""
while True: while True:
path=string.replace('$(scriptpath)',"%02d"%i) path=string.replace('$(scriptpath)',"%02d"%i)
if not os.path.lexists(path):break if not os.path.lexists(path):break
i+=1 i+=1
string=path string=path
#8位日期（20140404） #8位日期（20140404）
if string.find('$(Date8)')>=0: if string.find('$(Date8)')>=0:
now=datetime.datetime.now() now=datetime.datetime.now()
string=string.replace('$(Date8)', now.strftime("%Y%m%d")) string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
#6位日期（140404） #6位日期（140404）
if string.find('$(Date6)')>=0: if string.find('$(Date6)')>=0:
now=datetime.datetime.now() now=datetime.datetime.now()
string=string.replace('$(Date6)', now.strftime("%y%m%d")) string=string.replace('$(Date6)', now.strftime("%y%m%d"))
#6位时间（121212） #6位时间（121212）
if string.find('$(Time6)')>=0: if string.find('$(Time6)')>=0:
now=datetime.datetime.now() now=datetime.datetime.now()
string=string.replace('$(Time6)', now.strftime("%H%M%S")) string=string.replace('$(Time6)', now.strftime("%H%M%S"))
#4位时间（1212） #4位时间（1212）
if string.find('$(Time4)')>=0: if string.find('$(Time4)')>=0:
now=datetime.datetime.now() now=datetime.datetime.now()
string=string.replace('$(Time4)', now.strftime("%H%M")) string=string.replace('$(Time4)', now.strftime("%H%M"))
#文件编号2位（必须在最后） #文件编号2位（必须在最后）
if string.find('$(filenumber2)')>=0: if string.find('$(filenumber2)')>=0:
i=0 i=0
path="" path=""
while True: while True:
path=string.replace('$(filenumber2)',"%02d"%i) path=string.replace('$(filenumber2)',"%02d"%i)
if not os.path.lexists(path):break if not os.path.lexists(path):break
i+=1 i+=1
string=path string=path
#文件编号3位（必须在最后） #文件编号3位（必须在最后）
if string.find('$(filenumber3)')>=0: if string.find('$(filenumber3)')>=0:
i=0 i=0
path="" path=""
while True: while True:
path=string.replace('$(filenumber3)',"%03d"%i) path=string.replace('$(filenumber3)',"%03d"%i)
if not os.path.lexists(path):break if not os.path.lexists(path):break
i+=1 i+=1
string=path string=path
return string return string
""" """
取得进程列表 取得进程列表
格式：(PID,cmd) 格式：(PID,cmd)
""" """
def getProcessList(): def getProcessList():
processList = [] processList = []
try: try:
for line in os.popen("ps xa"): for line in os.popen("ps xa"):
fields = line.split() fields = line.split()
# spid = fields[0] # spid = fields[0]
pid = 0 pid = 0
try:pid = int(fields[0]) try:pid = int(fields[0])
except:None except:None
cmd = line[27:-1] cmd = line[27:-1]
# print "PS:%d,%s"%(npid,process) # print "PS:%d,%s"%(npid,process)
if pid != 0 and len(cmd) > 0: if pid != 0 and len(cmd) > 0:
processList.append((pid, cmd)) processList.append((pid, cmd))
except Exception as e: except Exception as e:
print("getProcessList except:%s" % (e)) print("getProcessList except:%s" % (e))
return processList return processList
def killCommand(cmd): def killCommand(cmd):
try: try:
processList = getProcessList() processList = getProcessList()
for p in processList: for p in processList:
if p[1].find(cmd) != -1: if p[1].find(cmd) != -1:
pid = p[0] pid = p[0]
os.kill(pid, signal.SIGKILL) os.kill(pid, signal.SIGKILL)
except Exception as e: except Exception as e:
print("killCommand ‘%s’ except:%s" % (cmd,e)) print("killCommand ‘%s’ except:%s" % (cmd,e))
def check_pid(pid): def check_pid(pid):
""" Check For the existence of a unix pid. """ """ Check For the existence of a unix pid. """
if pid == 0:return False if pid == 0:return False
try: try:
os.kill(pid, 0) os.kill(pid, 0)
except OSError: except OSError:
return False return False
else: else:
return True return True
SF=formatString SF=formatString
#全局异常捕获 #全局异常捕获
def excepthook(excType, excValue, tb): def excepthook(excType, excValue, tb):
'''''write the unhandle exception to log''' '''''write the unhandle exception to log'''
from log import log from log import log
import traceback import traceback
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb))) log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
sys.exit(-1) sys.exit(-1)
#sys.__excepthook__(type, value, trace) #sys.__excepthook__(type, value, trace)
#sys.__excepthook__(excType, excValue, tb) #sys.__excepthook__(excType, excValue, tb)
_defaultGlobalExcept=sys.excepthook _defaultGlobalExcept=sys.excepthook
def enableGlobalExcept(enable=True): def enableGlobalExcept(enable=True):
if enable: if enable:
sys.excepthook = excepthook sys.excepthook = excepthook
else: else:
sys.excepthook=_defaultGlobalExcept sys.excepthook=_defaultGlobalExcept
# 默认启动全局异常处理 # 默认启动全局异常处理
enableGlobalExcept() enableGlobalExcept()
#创建线程 #创建线程
def createThread(name,target,args=(),autoRun=True,daemon=True): def createThread(name,target,args=(),autoRun=True,daemon=True):
from log import log from log import log
def threadProc(): def threadProc():
log.i("thread %s started!",name) log.i("thread %s started!",name)
try: try:
target(*args) target(*args)
log.i("thread %s ended!",name) log.i("thread %s ended!",name)
except Exception as e: except Exception as e:
log.e("thread %s crash!err=%s",name,e) log.e("thread %s crash!err=%s",name,e)
thd=threading.Thread(name=name,target=threadProc) thd=threading.Thread(name=name,target=threadProc)
thd.setDaemon(daemon) thd.setDaemon(daemon)
if autoRun:thd.start() if autoRun:thd.start()
return thd return thd
#定时器 #定时器
class Timer(): class Timer():
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"): def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
self.name=name self.name=name
self.first=first self.first=first
self.period=period self.period=period
self.args=args self.args=args
self.timer_proc=timer_proc self.timer_proc=timer_proc
self.thdWork=None self.thdWork=None
self.stopFlag=False self.stopFlag=False
from log import NamedLog from log import NamedLog
self.log=NamedLog(name) self.log=NamedLog(name)
def run(self): def run(self):
if self.first>0: if self.first>0:
time.sleep(self.first) time.sleep(self.first)
try: try:
self.timer_proc(*self.args) self.timer_proc(*self.args)
except Exception as e: except Exception as e:
self.log.error("timer proc crash!err=%s",e) self.log.error("timer proc crash!err=%s",e)
while self.period>0 and not self.stopFlag: while self.period>0 and not self.stopFlag:
time.sleep(self.period) time.sleep(self.period)
try: try:
self.timer_proc(*self.args) self.timer_proc(*self.args)
except Exception as e: except Exception as e:
self.log.error("timer proc crash!err=%s",e) self.log.error("timer proc crash!err=%s",e)
def start(self): def start(self):
if self.isAlive(): if self.isAlive():
self.log.d("already running!") self.log.d("already running!")
return True return True
self.stopFlag=False self.stopFlag=False
self.thdWork=threading.Thread(name=self.name,target=self.run) self.thdWork=threading.Thread(name=self.name,target=self.run)
self.thdWork.setDaemon(True) self.thdWork.setDaemon(True)
self.thdWork.start() self.thdWork.start()
def stop(self,timeout=3): def stop(self,timeout=3):
if self.isAlive(): if self.isAlive():
self.stopFlag=True self.stopFlag=True
try: try:
self.thdWork.join(timeout) self.thdWork.join(timeout)
except Exception as e: except Exception as e:
self.log.e("stop timeout!") self.log.e("stop timeout!")
def isAlive(self): def isAlive(self):
return self.thdWork and self.thdWork.isAlive() return self.thdWork and self.thdWork.isAlive()
#计时器 #计时器
class Ticker(): class Ticker():
def __init__(self): def __init__(self):
self.reset() self.reset()
# 片段,可以判断时长是否在一个特定毫秒段内 # 片段,可以判断时长是否在一个特定毫秒段内
self.section=[] self.section=[]
def reset(self): def reset(self):
self.tick=time.time() self.tick=time.time()
self.end=0 self.end=0
def stop(self): def stop(self):
self.end=time.time() self.end=time.time()
@property @property
def ticker(self): def ticker(self):
if self.end==0: if self.end==0:
return int((time.time()-self.tick)*1000) return int((time.time()-self.tick)*1000)
else: else:
return int((self.end-self.tick)*1000) return int((self.end-self.tick)*1000)
def addSection(self,a,b): def addSection(self,a,b):
a,b=int(a),int(b) a,b=int(a),int(b)
assert a<b assert a<b
self.section.append((a,b)) self.section.append((a,b))
def removeSection(self,a,b): def removeSection(self,a,b):
a,b=int(a),int(b) a,b=int(a),int(b)
self.section.remove((a,b)) self.section.remove((a,b))
def removeAllSectioin(self): def removeAllSectioin(self):
self.section=[] self.section=[]
def inSection(self): def inSection(self):
tick=self.ticker tick=self.ticker
for (a,b) in self.section: for (a,b) in self.section:
if tick>=a and tick<=b: if tick>=a and tick<=b:
return True return True
return False return False
def __call__(self): def __call__(self):
return self.ticker return self.ticker
def waitExit(): def waitExit():
import log import log
log.log.i("start waiting to exit...") log.log.i("start waiting to exit...")
try: try:
while(True): while(True):
time.sleep(1) time.sleep(1)
except Exception as e: except Exception as e:
log.log.w("recv exit sign!") log.log.w("recv exit sign!")
def is_python3(): def is_python3():
return sys.hexversion > 0x03000000 return sys.hexversion > 0x03000000

112
Code/MowingRobot/pibot_ros/pypibot/pypibot/configer.py Executable file → Normal file
View File

@ -1,56 +1,56 @@
#!/usr/bin/python #!/usr/bin/python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
import ConfigParser import ConfigParser
from log import PLOG from log import PLOG
import os import os
def getdefaultfilename(): def getdefaultfilename():
pass pass
def openconfiger(filename): def openconfiger(filename):
return configer(filename) return configer(filename)
class configer: class configer:
def __init__(self,fullfilepath): def __init__(self,fullfilepath):
self._filepath=fullfilepath self._filepath=fullfilepath
if not os.path.isdir(os.path.dirname(fullfilepath)): if not os.path.isdir(os.path.dirname(fullfilepath)):
os.makedirs(os.path.dirname(fullfilepath)) os.makedirs(os.path.dirname(fullfilepath))
self._conf=ConfigParser.ConfigParser() self._conf=ConfigParser.ConfigParser()
if os.path.isfile(fullfilepath): if os.path.isfile(fullfilepath):
try: try:
self._conf.readfp(open(fullfilepath,"r")) self._conf.readfp(open(fullfilepath,"r"))
except Exception,e: except Exception,e:
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e)) PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
def save(self): def save(self):
try: try:
self._conf.write(open(self._filepath,"w")) self._conf.write(open(self._filepath,"w"))
except Exception,e: except Exception,e:
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e)) PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
def changeConfValue(self,section,option,value): def changeConfValue(self,section,option,value):
if self._conf.has_section(section): if self._conf.has_section(section):
self._conf.set(section,option,value) self._conf.set(section,option,value)
else: else:
self._conf.add_section(section) self._conf.add_section(section)
self._conf.set(section,option,value) self._conf.set(section,option,value)
def _readvalue(self,fn,section,option,default): def _readvalue(self,fn,section,option,default):
result=default result=default
if self._conf.has_section(section): if self._conf.has_section(section):
if self._conf.has_option(section,option): if self._conf.has_option(section,option):
result=fn(section,option) result=fn(section,option)
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result))) PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
else: else:
self._conf.set(section,option,str(default)) self._conf.set(section,option,str(default))
result=default result=default
else: else:
self._conf.add_section(section) self._conf.add_section(section)
self._conf.set(section,option,str(default)) self._conf.set(section,option,str(default))
result=default result=default
return result return result
def getstr(self,section,option,default=""): def getstr(self,section,option,default=""):
return self._readvalue(self._conf.get, section, option, default) return self._readvalue(self._conf.get, section, option, default)
def getint(self,section,option,default=0): def getint(self,section,option,default=0):
return self._readvalue(self._conf.getint, section, option, default) return self._readvalue(self._conf.getint, section, option, default)
def getfloat(self,section,option,default=0.0): def getfloat(self,section,option,default=0.0):
return self._readvalue(self._conf.getfloat, section, option, default) return self._readvalue(self._conf.getfloat, section, option, default)
def getboolean(self,section,option,default=False): def getboolean(self,section,option,default=False):
return self._readvalue(self._conf.getboolean, section, option, default) return self._readvalue(self._conf.getboolean, section, option, default)

280
Code/MowingRobot/pibot_ros/pypibot/pypibot/daemon.py Executable file → Normal file
View File

@ -1,140 +1,140 @@
#!/usr/bin/env python #!/usr/bin/env python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
import sys, os, time, atexit import sys, os, time, atexit
from signal import SIGTERM from signal import SIGTERM
def check_pid(pid): def check_pid(pid):
""" Check For the existence of a unix pid. """ """ Check For the existence of a unix pid. """
if pid == 0:return False if pid == 0:return False
try: try:
os.kill(pid, 0) os.kill(pid, 0)
except OSError: except OSError:
return False return False
else: else:
return True return True
class daemon: class daemon:
""" """
A generic daemon class. A generic daemon class.
Usage: subclass the Daemon class and override the run() method Usage: subclass the Daemon class and override the run() method
""" """
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'): def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
self.stdin = stdin self.stdin = stdin
self.stdout = stdout self.stdout = stdout
self.stderr = stderr self.stderr = stderr
self.pidfile = pidfile self.pidfile = pidfile
def daemonize(self): def daemonize(self):
""" """
do the UNIX double-fork magic, see Stevens' "Advanced do the UNIX double-fork magic, see Stevens' "Advanced
Programming in the UNIX Environment" for details (ISBN 0201563177) Programming in the UNIX Environment" for details (ISBN 0201563177)
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16 http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
""" """
try: try:
pid = os.fork() pid = os.fork()
if pid > 0: if pid > 0:
# exit first parent # exit first parent
sys.exit(0) sys.exit(0)
except OSError, e: except OSError, e:
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror)) sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
sys.exit(1) sys.exit(1)
# decouple from parent environment # decouple from parent environment
os.chdir("/") os.chdir("/")
os.setsid() os.setsid()
os.umask(0) os.umask(0)
# do second fork # do second fork
try: try:
pid = os.fork() pid = os.fork()
if pid > 0: if pid > 0:
# exit from second parent # exit from second parent
sys.exit(0) sys.exit(0)
except OSError, e: except OSError, e:
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror)) sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
sys.exit(1) sys.exit(1)
# redirect standard file descriptors # redirect standard file descriptors
sys.stdout.flush() sys.stdout.flush()
sys.stderr.flush() sys.stderr.flush()
si = file(self.stdin, 'r') si = file(self.stdin, 'r')
so = file(self.stdout, 'a+') so = file(self.stdout, 'a+')
se = file(self.stderr, 'a+', 0) se = file(self.stderr, 'a+', 0)
os.dup2(si.fileno(), sys.stdin.fileno()) os.dup2(si.fileno(), sys.stdin.fileno())
os.dup2(so.fileno(), sys.stdout.fileno()) os.dup2(so.fileno(), sys.stdout.fileno())
os.dup2(se.fileno(), sys.stderr.fileno()) os.dup2(se.fileno(), sys.stderr.fileno())
# write pidfile # write pidfile
atexit.register(self.delpid) atexit.register(self.delpid)
pid = str(os.getpid()) pid = str(os.getpid())
file(self.pidfile, 'w+').write("%s\n" % pid) file(self.pidfile, 'w+').write("%s\n" % pid)
def delpid(self): def delpid(self):
os.remove(self.pidfile) os.remove(self.pidfile)
def start(self): def start(self):
""" """
Start the daemon Start the daemon
""" """
# Check for a pidfile to see if the daemon already runs # Check for a pidfile to see if the daemon already runs
try: try:
pf = file(self.pidfile, 'r') pf = file(self.pidfile, 'r')
pid = int(pf.read().strip()) pid = int(pf.read().strip())
pf.close() pf.close()
except IOError: except IOError:
pid = None pid = None
if pid and check_pid(pid): if pid and check_pid(pid):
message = "pidfile %s already exist. Daemon already running?\n" message = "pidfile %s already exist. Daemon already running?\n"
sys.stderr.write(message % self.pidfile) sys.stderr.write(message % self.pidfile)
sys.exit(1) sys.exit(1)
print("daemon start...") print("daemon start...")
# Start the daemon # Start the daemon
self.daemonize() self.daemonize()
self.run() self.run()
def stop(self): def stop(self):
""" """
Stop the daemon Stop the daemon
""" """
# Get the pid from the pidfile # Get the pid from the pidfile
try: try:
pf = file(self.pidfile, 'r') pf = file(self.pidfile, 'r')
pid = int(pf.read().strip()) pid = int(pf.read().strip())
pf.close() pf.close()
except IOError: except IOError:
pid = None pid = None
if not pid: if not pid:
message = "pidfile %s does not exist. Daemon not running?\n" message = "pidfile %s does not exist. Daemon not running?\n"
sys.stderr.write(message % self.pidfile) sys.stderr.write(message % self.pidfile)
return # not an error in a restart return # not an error in a restart
# Try killing the daemon process # Try killing the daemon process
try: try:
while 1: while 1:
os.kill(pid, SIGTERM) os.kill(pid, SIGTERM)
time.sleep(0.1) time.sleep(0.1)
except OSError, err: except OSError, err:
err = str(err) err = str(err)
if err.find("No such process") > 0: if err.find("No such process") > 0:
if os.path.exists(self.pidfile): if os.path.exists(self.pidfile):
os.remove(self.pidfile) os.remove(self.pidfile)
else: else:
print(str(err)) print(str(err))
sys.exit(1) sys.exit(1)
def restart(self): def restart(self):
""" """
Restart the daemon Restart the daemon
""" """
self.stop() self.stop()
self.start() self.start()
def run(self): def run(self):
""" """
You should override this method when you subclass Daemon. It will be called after the process has been You should override this method when you subclass Daemon. It will be called after the process has been
daemonized by start() or restart(). daemonized by start() or restart().
""" """

116
Code/MowingRobot/pibot_ros/pypibot/pypibot/err.py Executable file → Normal file
View File

@ -1,58 +1,58 @@
#!/usr/bin/python #!/usr/bin/python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# 异常类 # 异常类
class PibotError(Exception): class PibotError(Exception):
def __init__(self, errcode, errmsg): def __init__(self, errcode, errmsg):
self.errcode = errcode self.errcode = errcode
self.errmsg = errmsg self.errmsg = errmsg
#Exception.__init__(self,self.__str__()) #Exception.__init__(self,self.__str__())
def msg(self, msg): def msg(self, msg):
if not msg is None:return PibotError(self.errcode, msg) if not msg is None:return PibotError(self.errcode, msg)
return PibotError(8,"unknow error") return PibotError(8,"unknow error")
def __str__(self): def __str__(self):
return "PibotError:%s(%d)"%(self.errmsg,self.errcode) return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
@property @property
def message(self): def message(self):
return str(self) return str(self)
# 声明 # 声明
# 成功 # 成功
success=PibotError(0,"null") success=PibotError(0,"null")
# 通用失败 # 通用失败
fail=PibotError(1,"fail") fail=PibotError(1,"fail")
# 参数无效 # 参数无效
invalidParameter=PibotError(2,"invalid parameter") invalidParameter=PibotError(2,"invalid parameter")
# 不支持 # 不支持
noSupport=PibotError(3,"no support") noSupport=PibotError(3,"no support")
# 不存在 # 不存在
noExist=PibotError(4,"no exist") noExist=PibotError(4,"no exist")
# 超时 # 超时
timeout=PibotError(5,"timeout") timeout=PibotError(5,"timeout")
# 繁忙 # 繁忙
busy=PibotError(6,"busy") busy=PibotError(6,"busy")
# 缺少参数 # 缺少参数
missParameter=PibotError(7,"miss parameter") missParameter=PibotError(7,"miss parameter")
# 系统错误(通用错误) # 系统错误(通用错误)
systemError=PibotError(8,"system error") systemError=PibotError(8,"system error")
# 密码错误 # 密码错误
invalidPassword=PibotError(9,"invalid password") invalidPassword=PibotError(9,"invalid password")
# 编码失败 # 编码失败
encodeFailed=PibotError(10,"encode failed") encodeFailed=PibotError(10,"encode failed")
# 数据库操作失败 # 数据库操作失败
dbOpertationFailed=PibotError(11,"db error") dbOpertationFailed=PibotError(11,"db error")
# 已占用 # 已占用
occupied=PibotError(12,"occupied") occupied=PibotError(12,"occupied")
# session不存在 # session不存在
noSession = PibotError(13,'cannot find session') noSession = PibotError(13,'cannot find session')
#没有找到 #没有找到
noFound = PibotError(14, "no found") noFound = PibotError(14, "no found")
#已经存在 #已经存在
existed = PibotError(15, "existed") existed = PibotError(15, "existed")
#已经锁定 #已经锁定
locked = PibotError(16, "locked") locked = PibotError(16, "locked")
#已经过期 #已经过期
expired = PibotError(17, "is expired") expired = PibotError(17, "is expired")
#无效的参数 #无效的参数
invalidParameter = PibotError(18, "invalid parameter") invalidParameter = PibotError(18, "invalid parameter")

518
Code/MowingRobot/pibot_ros/pypibot/pypibot/log.py Executable file → Normal file
View File

@ -1,259 +1,259 @@
#!/usr/bin/python #!/usr/bin/python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
import sys,os import sys,os
import datetime import datetime
import threading import threading
import pypibot.assistant as assistant import pypibot.assistant as assistant
import platform import platform
if assistant.is_python3(): if assistant.is_python3():
import _thread import _thread
else: else:
import thread import thread
import traceback import traceback
""" """
%a Locale’s abbreviated weekday name. %a Locale’s abbreviated weekday name.
%A Locale’s full weekday name. %A Locale’s full weekday name.
%b Locale’s abbreviated month name. %b Locale’s abbreviated month name.
%B Locale’s full month name. %B Locale’s full month name.
%c Locale’s appropriate date and time representation. %c Locale’s appropriate date and time representation.
%d Day of the month as a decimal number [01,31]. %d Day of the month as a decimal number [01,31].
%H Hour (24-hour clock) as a decimal number [00,23]. %H Hour (24-hour clock) as a decimal number [00,23].
%I Hour (12-hour clock) as a decimal number [01,12]. %I Hour (12-hour clock) as a decimal number [01,12].
%j Day of the year as a decimal number [001,366]. %j Day of the year as a decimal number [001,366].
%m Month as a decimal number [01,12]. %m Month as a decimal number [01,12].
%M Minute as a decimal number [00,59]. %M Minute as a decimal number [00,59].
%p Locale’s equivalent of either AM or PM. (1) %p Locale’s equivalent of either AM or PM. (1)
%S Second as a decimal number [00,61]. (2) %S Second as a decimal number [00,61]. (2)
%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3) %U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3)
%w Weekday as a decimal number [0(Sunday),6]. %w Weekday as a decimal number [0(Sunday),6].
%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3) %W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3)
%x Locale’s appropriate date representation. %x Locale’s appropriate date representation.
%X Locale’s appropriate time representation. %X Locale’s appropriate time representation.
%y Year without century as a decimal number [00,99]. %y Year without century as a decimal number [00,99].
%Y Year with century as a decimal number. %Y Year with century as a decimal number.
%Z Time zone name (no characters if no time zone exists). %Z Time zone name (no characters if no time zone exists).
%% A literal '%' character. %% A literal '%' character.
""" """
isWindows=False isWindows=False
if platform.system()=='Windows': if platform.system()=='Windows':
isWindows=True isWindows=True
defaultEncodeing="utf8" defaultEncodeing="utf8"
if "-utf8" in sys.argv: if "-utf8" in sys.argv:
defaultEncodeing="utf-8" defaultEncodeing="utf-8"
if "-gbk" in sys.argv: if "-gbk" in sys.argv:
defaultEncodeing="gbk" defaultEncodeing="gbk"
TRACE=5 TRACE=5
DEBUG=4 DEBUG=4
INFORMATION=3 INFORMATION=3
WARNING=2 WARNING=2
ERROR=1 ERROR=1
NONE=0 NONE=0
MAX_MSG_SIZE = 4096 MAX_MSG_SIZE = 4096
def getLevelFromString(level): def getLevelFromString(level):
level=level.lower() level=level.lower()
if level=='t' or level=='trace':return 5 if level=='t' or level=='trace':return 5
elif level=='d' or level=='debug':return 4 elif level=='d' or level=='debug':return 4
elif level=='i' or level=='info':return 3 elif level=='i' or level=='info':return 3
elif level=='w' or level=='wran':return 2 elif level=='w' or level=='wran':return 2
elif level=='e' or level=='error':return 1 elif level=='e' or level=='error':return 1
else :return 0 else :return 0
def getLevelString(level): def getLevelString(level):
if level==TRACE:return "T" if level==TRACE:return "T"
elif level==DEBUG:return "D" elif level==DEBUG:return "D"
elif level==INFORMATION:return "I" elif level==INFORMATION:return "I"
elif level==WARNING:return "W" elif level==WARNING:return "W"
elif level==ERROR:return "E" elif level==ERROR:return "E"
else:return "N" else:return "N"
class PibotLog: class PibotLog:
def __init__(self): def __init__(self):
self.isEnableControlLog=True self.isEnableControlLog=True
self.fileTemple=None self.fileTemple=None
self.filePath="" self.filePath=""
self.level=5 self.level=5
self._lock=threading.RLock() self._lock=threading.RLock()
self.fd=None self.fd=None
self.fd_day=-1 self.fd_day=-1
def setLevel(self,level): def setLevel(self,level):
self.level=getLevelFromString(level) self.level=getLevelFromString(level)
def enableControllog(self,enable): def enableControllog(self,enable):
self.isEnableControlLog=enable self.isEnableControlLog=enable
def enableFileLog(self,fileName): def enableFileLog(self,fileName):
self.fileTemple=fileName self.fileTemple=fileName
self.updateFilelog() self.updateFilelog()
def updateFilelog(self): def updateFilelog(self):
fn=assistant.SF(self.fileTemple) fn=assistant.SF(self.fileTemple)
if fn!=self.filePath: if fn!=self.filePath:
self.i("new log file:%s",fn) self.i("new log file:%s",fn)
if self.fd: if self.fd:
self.fd.close() self.fd.close()
self.fd=None self.fd=None
self.fd_day=-1 self.fd_day=-1
self.filePath="" self.filePath=""
try: try:
path=os.path.dirname(fn) path=os.path.dirname(fn)
if path!="" and not os.path.isdir(path):os.makedirs(path) if path!="" and not os.path.isdir(path):os.makedirs(path)
self.fd=open(fn,mode="w") self.fd=open(fn,mode="w")
try: try:
linkfn = fn.split(".log.", 1)[0] + ".INFO" linkfn = fn.split(".log.", 1)[0] + ".INFO"
if os.path.exists(linkfn): if os.path.exists(linkfn):
os.remove(linkfn) os.remove(linkfn)
(filepath, tempfilename) = os.path.split(fn) (filepath, tempfilename) = os.path.split(fn)
os.symlink(tempfilename, linkfn) os.symlink(tempfilename, linkfn)
except: except:
pass pass
self.fd_day=datetime.datetime.now().day self.fd_day=datetime.datetime.now().day
self.filePath=fn self.filePath=fn
except Exception as e: except Exception as e:
print("open file fail!file=%s,err=%s"%(fn,e)) print("open file fail!file=%s,err=%s"%(fn,e))
def _output(self,level,msg,args): def _output(self,level,msg,args):
if self.level<level:return if self.level<level:return
try: try:
if len(args)>0: if len(args)>0:
t=[] t=[]
for arg in args: for arg in args:
if isinstance(arg,Exception): if isinstance(arg,Exception):
t.append(traceback.format_exc(arg).decode('utf-8')) t.append(traceback.format_exc(arg).decode('utf-8'))
elif isinstance(arg,bytes) : elif isinstance(arg,bytes) :
t.append(arg.decode('utf-8')) t.append(arg.decode('utf-8'))
else: else:
t.append(arg) t.append(arg)
args=tuple(t) args=tuple(t)
try: try:
msg=msg%args msg=msg%args
except: except:
try: try:
for arg in args: for arg in args:
msg=msg+str(arg)+" " msg=msg+str(arg)+" "
except Exception as e: except Exception as e:
msg=msg+"==LOG ERROR==>%s"%(e) msg=msg+"==LOG ERROR==>%s"%(e)
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE] if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
now=datetime.datetime.now() now=datetime.datetime.now()
msg=msg+"\r\n" msg=msg+"\r\n"
if assistant.is_python3(): if assistant.is_python3():
id = _thread.get_ident() id = _thread.get_ident()
else: else:
id = thread.get_ident() id = thread.get_ident()
s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg) s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg)
prefix='' prefix=''
suffix='' suffix=''
if not isWindows: if not isWindows:
suffix='\033[0m' suffix='\033[0m'
if level==TRACE: if level==TRACE:
prefix='\033[0;37m' prefix='\033[0;37m'
elif level==DEBUG: elif level==DEBUG:
prefix='\033[1m' prefix='\033[1m'
elif level==INFORMATION: elif level==INFORMATION:
prefix='\033[0;32m' prefix='\033[0;32m'
elif level==WARNING: elif level==WARNING:
prefix='\033[0;33m' prefix='\033[0;33m'
elif level==ERROR: elif level==ERROR:
prefix='\033[0;31m' prefix='\033[0;31m'
else: else:
pass pass
self._lock.acquire() self._lock.acquire()
try: try:
if self.isEnableControlLog: if self.isEnableControlLog:
sys.stdout.write((prefix+s+suffix)) sys.stdout.write((prefix+s+suffix))
if self.fd: if self.fd:
if self.fd_day!=now.day: if self.fd_day!=now.day:
self.updateFilelog() self.updateFilelog()
if assistant.is_python3(): if assistant.is_python3():
self.fd.write(s) self.fd.write(s)
else: else:
self.fd.write(s.encode('utf-8')) self.fd.write(s.encode('utf-8'))
self.fd.flush() self.fd.flush()
finally: finally:
self._lock.release() self._lock.release()
except Exception as e: except Exception as e:
if assistant.is_python3(): if assistant.is_python3():
print(e) print(e)
else: else:
print("PibotLog._output crash!err=%s"%traceback.format_exc(e)) print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
def trace(self,msg,*args): def trace(self,msg,*args):
self._output(TRACE,msg,args) self._output(TRACE,msg,args)
def t(self,msg,*args): def t(self,msg,*args):
self._output(TRACE,msg,args) self._output(TRACE,msg,args)
def debug(self,msg,*args): def debug(self,msg,*args):
self._output(DEBUG, msg,args) self._output(DEBUG, msg,args)
def d(self,msg,*args): def d(self,msg,*args):
self._output(DEBUG, msg,args) self._output(DEBUG, msg,args)
def info(self,msg,*args): def info(self,msg,*args):
self._output(INFORMATION, msg,args) self._output(INFORMATION, msg,args)
def i(self,msg,*args): def i(self,msg,*args):
self._output(INFORMATION, msg,args) self._output(INFORMATION, msg,args)
def warn(self,msg,*args): def warn(self,msg,*args):
self._output(WARNING, msg,args) self._output(WARNING, msg,args)
def w(self,msg,*args): def w(self,msg,*args):
self._output(WARNING, msg,args) self._output(WARNING, msg,args)
def error(self,msg,*args): def error(self,msg,*args):
self._output(ERROR, msg,args) self._output(ERROR, msg,args)
def e(self,msg,*args): def e(self,msg,*args):
self._output(ERROR, msg,args) self._output(ERROR, msg,args)
def _log(self,level,msg,args): def _log(self,level,msg,args):
self._output(level, msg,args) self._output(level, msg,args)
def createNamedLog(self,name): def createNamedLog(self,name):
return NamedLog(name) return NamedLog(name)
log=PibotLog() log=PibotLog()
class NamedLog: class NamedLog:
def __init__(self,name=None): def __init__(self,name=None):
global log global log
self.name='' self.name=''
if name: if name:
self.name='['+name+']' self.name='['+name+']'
self.log=log self.log=log
def trace(self,msg,*args): def trace(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(TRACE,msg,args) self.log._log(TRACE,msg,args)
def t(self,msg,*args): def t(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(TRACE,msg,args) self.log._log(TRACE,msg,args)
def debug(self,msg,*args): def debug(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(DEBUG, msg,args) self.log._log(DEBUG, msg,args)
def d(self,msg,*args): def d(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(DEBUG, msg,args) self.log._log(DEBUG, msg,args)
def info(self,msg,*args): def info(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(INFORMATION, msg,args) self.log._log(INFORMATION, msg,args)
def i(self,msg,*args): def i(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(INFORMATION, msg,args) self.log._log(INFORMATION, msg,args)
def warn(self,msg,*args): def warn(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(WARNING, msg,args) self.log._log(WARNING, msg,args)
def w(self,msg,*args): def w(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(WARNING, msg,args) self.log._log(WARNING, msg,args)
def error(self,msg,*args): def error(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(ERROR, msg,args) self.log._log(ERROR, msg,args)
def e(self,msg,*args): def e(self,msg,*args):
msg=self.name+msg msg=self.name+msg
self.log._log(ERROR, msg,args) self.log._log(ERROR, msg,args)
if __name__ == "__main__": if __name__ == "__main__":
log.trace("1%s","hello") log.trace("1%s","hello")
log.debug("2%d",12) log.debug("2%d",12)
try: try:
raise Exception("EXC") raise Exception("EXC")
except Exception as e: except Exception as e:
log.info("3%s",e) log.info("3%s",e)
log.warn("1") log.warn("1")
log.error("1") log.error("1")
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log") #log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
log.trace("1") log.trace("1")
log.debug("1") log.debug("1")
log.info("1") log.info("1")
log.warn("1") log.warn("1")
log.error("1") log.error("1")
log=NamedLog("test") log=NamedLog("test")
log.d("1") log.d("1")
log.i("1") log.i("1")
log.warn("1") log.warn("1")
log.error("1=%d,%s",100,e) log.error("1=%d,%s",100,e)

0
Code/MowingRobot/pibot_ros/pypibot/pypibot/mapconvert.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/pypibot/pypibot/proxy.py Executable file → Normal file
View File

480
Code/MowingRobot/pibot_ros/pypibot/transport/dataholder.py Executable file → Normal file
View File

@ -1,240 +1,240 @@
import struct import struct
params_size=29 params_size=29
# main board # main board
class MessageID: class MessageID:
ID_GET_VERSION = 0 ID_GET_VERSION = 0
ID_SET_ROBOT_PARAMETER = 1 ID_SET_ROBOT_PARAMETER = 1
ID_GET_ROBOT_PARAMETER = 2 ID_GET_ROBOT_PARAMETER = 2
ID_INIT_ODOM = 3 ID_INIT_ODOM = 3
ID_SET_VEL = 4 ID_SET_VEL = 4
ID_GET_ODOM = 5 ID_GET_ODOM = 5
ID_GET_PID_DEBUG = 6 ID_GET_PID_DEBUG = 6
ID_GET_IMU = 7 ID_GET_IMU = 7
ID_GET_ENCODER_COUNT = 8 ID_GET_ENCODER_COUNT = 8
ID_SET_MOTOR_PWM = 9 ID_SET_MOTOR_PWM = 9
class RobotMessage: class RobotMessage:
def pack(self): def pack(self):
return b'' return b''
def unpack(self): def unpack(self):
return True return True
class RobotFirmwareInfo(RobotMessage): class RobotFirmwareInfo(RobotMessage):
def __init__(self): def __init__(self):
self.version = '' self.version = ''
self.build_time = '' self.build_time = ''
def unpack(self, data): def unpack(self, data):
try: try:
upk = struct.unpack('16s16s', bytes(data)) upk = struct.unpack('16s16s', bytes(data))
except: except:
return False return False
[self.version, self.build_time] = upk [self.version, self.build_time] = upk
return True return True
class RobotImuType: class RobotImuType:
IMU_TYPE_GY65 = 49 IMU_TYPE_GY65 = 49
IMU_TYPE_GY85 = 69 IMU_TYPE_GY85 = 69
IMU_TYPE_GY87 = 71 IMU_TYPE_GY87 = 71
class RobotModelType: class RobotModelType:
MODEL_TYPE_2WD_DIFF = 1 MODEL_TYPE_2WD_DIFF = 1
MODEL_TYPE_4WD_DIFF = 2 MODEL_TYPE_4WD_DIFF = 2
MODEL_TYPE_3WD_OMNI = 101 MODEL_TYPE_3WD_OMNI = 101
MODEL_TYPE_4WD_OMNI = 102 MODEL_TYPE_4WD_OMNI = 102
MODEL_TYPE_4WD_MECANUM = 201 MODEL_TYPE_4WD_MECANUM = 201
class RobotParameters(): class RobotParameters():
def __init__(self, wheel_diameter=0, \ def __init__(self, wheel_diameter=0, \
wheel_track=0, \ wheel_track=0, \
encoder_resolution=0, \ encoder_resolution=0, \
do_pid_interval=0, \ do_pid_interval=0, \
kp=0, \ kp=0, \
ki=0, \ ki=0, \
kd=0, \ kd=0, \
ko=0, \ ko=0, \
cmd_last_time=0, \ cmd_last_time=0, \
max_v_liner_x=0, \ max_v_liner_x=0, \
max_v_liner_y=0, \ max_v_liner_y=0, \
max_v_angular_z=0, \ max_v_angular_z=0, \
imu_type=0, \ imu_type=0, \
motor_ratio=0, \ motor_ratio=0, \
model_type=0, \ model_type=0, \
motor_nonexchange_flag=255, \ motor_nonexchange_flag=255, \
encoder_nonexchange_flag=255, \ encoder_nonexchange_flag=255, \
): ):
self.wheel_diameter = wheel_diameter self.wheel_diameter = wheel_diameter
self.wheel_track = wheel_track self.wheel_track = wheel_track
self.encoder_resolution = encoder_resolution self.encoder_resolution = encoder_resolution
self.do_pid_interval = do_pid_interval self.do_pid_interval = do_pid_interval
self.kp = kp self.kp = kp
self.ki = ki self.ki = ki
self.kd = kd self.kd = kd
self.ko = ko self.ko = ko
self.cmd_last_time = cmd_last_time self.cmd_last_time = cmd_last_time
self.max_v_liner_x = max_v_liner_x self.max_v_liner_x = max_v_liner_x
self.max_v_liner_y = max_v_liner_y self.max_v_liner_y = max_v_liner_y
self.max_v_angular_z = max_v_angular_z self.max_v_angular_z = max_v_angular_z
self.imu_type = imu_type self.imu_type = imu_type
self.motor_ratio = motor_ratio self.motor_ratio = motor_ratio
self.model_type = model_type self.model_type = model_type
self.motor_nonexchange_flag = motor_nonexchange_flag self.motor_nonexchange_flag = motor_nonexchange_flag
self.encoder_nonexchange_flag = encoder_nonexchange_flag self.encoder_nonexchange_flag = encoder_nonexchange_flag
reserve=b'\xff' reserve=b'\xff'
self.reserve=b'' self.reserve=b''
for i in range(64-params_size): for i in range(64-params_size):
self.reserve+=reserve self.reserve+=reserve
robotParam = RobotParameters() robotParam = RobotParameters()
class GetRobotParameters(RobotMessage): class GetRobotParameters(RobotMessage):
def __init__(self): def __init__(self):
self.param = robotParam self.param = robotParam
def unpack(self, data): def unpack(self, data):
#print(bytes(data), len(bytes(data))) #print(bytes(data), len(bytes(data)))
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data)) upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
[self.param.wheel_diameter, [self.param.wheel_diameter,
self.param.wheel_track, self.param.wheel_track,
self.param.encoder_resolution, self.param.encoder_resolution,
self.param.do_pid_interval, self.param.do_pid_interval,
self.param.kp, self.param.kp,
self.param.ki, self.param.ki,
self.param.kd, self.param.kd,
self.param.ko, self.param.ko,
self.param.cmd_last_time, self.param.cmd_last_time,
self.param.max_v_liner_x, self.param.max_v_liner_x,
self.param.max_v_liner_y, self.param.max_v_liner_y,
self.param.max_v_angular_z, self.param.max_v_angular_z,
self.param.imu_type, self.param.imu_type,
self.param.motor_ratio, self.param.motor_ratio,
self.param.model_type, self.param.model_type,
self.param.motor_nonexchange_flag, self.param.motor_nonexchange_flag,
self.param.encoder_nonexchange_flag, self.param.encoder_nonexchange_flag,
self.param.reserve] = upk self.param.reserve] = upk
return True return True
class SetRobotParameters(RobotMessage): class SetRobotParameters(RobotMessage):
def __init__(self): def __init__(self):
self.param = robotParam self.param = robotParam
def pack(self): def pack(self):
data = [self.param.wheel_diameter, data = [self.param.wheel_diameter,
self.param.wheel_track, self.param.wheel_track,
self.param.encoder_resolution, self.param.encoder_resolution,
self.param.do_pid_interval, self.param.do_pid_interval,
self.param.kp, self.param.kp,
self.param.ki, self.param.ki,
self.param.kd, self.param.kd,
self.param.ko, self.param.ko,
self.param.cmd_last_time, self.param.cmd_last_time,
self.param.max_v_liner_x, self.param.max_v_liner_x,
self.param.max_v_liner_y, self.param.max_v_liner_y,
self.param.max_v_angular_z, self.param.max_v_angular_z,
self.param.imu_type, self.param.imu_type,
self.param.motor_ratio, self.param.motor_ratio,
self.param.model_type, self.param.model_type,
self.param.motor_nonexchange_flag, self.param.motor_nonexchange_flag,
self.param.encoder_nonexchange_flag, self.param.encoder_nonexchange_flag,
self.param.reserve] self.param.reserve]
print(data) print(data)
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data) pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
return pk return pk
def unpack(self, data): def unpack(self, data):
return True return True
class RobotVel(RobotMessage): class RobotVel(RobotMessage):
def __init__(self): def __init__(self):
self.v_liner_x = 0 self.v_liner_x = 0
self.v_liner_y = 0 self.v_liner_y = 0
self.v_angular_z = 0 self.v_angular_z = 0
def pack(self): def pack(self):
data = [self.v_liner_x, data = [self.v_liner_x,
self.v_liner_y, self.v_liner_y,
self.v_angular_z] self.v_angular_z]
pk = struct.pack('3h', *data) pk = struct.pack('3h', *data)
return pk return pk
def unpack(self, data): def unpack(self, data):
return True return True
#todo the rest of the message classes #todo the rest of the message classes
class RobotOdom(RobotMessage): class RobotOdom(RobotMessage):
def __init__(self): def __init__(self):
self.v_liner_x = 0 self.v_liner_x = 0
self.v_liner_y = 0 self.v_liner_y = 0
self.v_angular_z = 0 self.v_angular_z = 0
self.x = 0 self.x = 0
self.y = 0 self.y = 0
self.yaw = 0 self.yaw = 0
def unpack(self, data): def unpack(self, data):
try: try:
upk = struct.unpack('<3H2l1H', bytes(data)) upk = struct.unpack('<3H2l1H', bytes(data))
except: except:
return False return False
[self.v_liner_x, [self.v_liner_x,
self.v_liner_y, self.v_liner_y,
self.v_angular_z, self.v_angular_z,
self.x, self.x,
self.y, self.y,
self.yaw] = upk self.yaw] = upk
return True return True
class RobotPIDData(RobotMessage): class RobotPIDData(RobotMessage):
pass pass
class RobotIMU(RobotMessage): class RobotIMU(RobotMessage):
def __init__(self): def __init__(self):
self.imu = [0]*9 self.imu = [0]*9
def unpack(self, data): def unpack(self, data):
try: try:
upk = struct.unpack('<9f', bytes(data)) upk = struct.unpack('<9f', bytes(data))
except: except:
return False return False
self.imu = upk self.imu = upk
return True return True
class RobotEncoderCount(RobotMessage): class RobotEncoderCount(RobotMessage):
def __init__(self): def __init__(self):
self.encoder = [0]*4 self.encoder = [0]*4
def unpack(self, data): def unpack(self, data):
try: try:
upk = struct.unpack('<4f', bytes(data)) upk = struct.unpack('<4f', bytes(data))
except: except:
return False return False
self.encoder = upk self.encoder = upk
return True return True
class RobotMotorPWM(RobotMessage): class RobotMotorPWM(RobotMessage):
def __init__(self): def __init__(self):
self.pwm = [0]*4 self.pwm = [0]*4
def pack(self): def pack(self):
pk = struct.pack('4h', *self.pwm) pk = struct.pack('4h', *self.pwm)
return pk return pk
def unpack(self, data): def unpack(self, data):
return True return True
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(), BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(), MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(), MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
MessageID.ID_SET_VEL:RobotVel(), MessageID.ID_SET_VEL:RobotVel(),
MessageID.ID_GET_ODOM:RobotOdom(), MessageID.ID_GET_ODOM:RobotOdom(),
MessageID.ID_GET_PID_DEBUG: RobotPIDData(), MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
MessageID.ID_GET_IMU: RobotIMU(), MessageID.ID_GET_IMU: RobotIMU(),
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(), MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(), MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
} }

230
Code/MowingRobot/pibot_ros/pypibot/transport/main.py Executable file → Normal file
View File

@ -1,115 +1,115 @@
import platform import platform
import sys import sys
sys.path.append("..") sys.path.append("..")
import pypibot import pypibot
from pypibot import log from pypibot import log
from transport import Transport from transport import Transport
from dataholder import MessageID from dataholder import MessageID
import params import params
import time import time
import signal import signal
#for linux #for linux
port="/dev/pibot" port="/dev/pibot"
#for windows #for windows
#port="com3" #port="com3"
pypibot.assistant.enableGlobalExcept() pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") #log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i") log.setLevel("i")
run_flag = True run_flag = True
def exit(signum, frame): def exit(signum, frame):
global run_flag global run_flag
run_flag = False run_flag = False
if __name__ == '__main__': if __name__ == '__main__':
signal.signal(signal.SIGINT, exit) signal.signal(signal.SIGINT, exit)
mboard = Transport(port, params.pibotBaud) mboard = Transport(port, params.pibotBaud)
if not mboard.start(): if not mboard.start():
log.error("can not open %s"%port) log.error("can not open %s"%port)
sys.exit() sys.exit()
DataHolder = mboard.getDataHolder() DataHolder = mboard.getDataHolder()
for num in range(0,3): for num in range(0,3):
log.info("****************get robot version*****************") log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION] boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION) p = mboard.request(MessageID.ID_GET_VERSION)
if p: if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break break
else: else:
log.error('read firmware version err\r\n') log.error('read firmware version err\r\n')
import time import time
time.sleep(1) time.sleep(1)
if num == 2: if num == 2:
log.error('please check connection or baudrate\r\n') log.error('please check connection or baudrate\r\n')
sys.exit() sys.exit()
# get robot parameter # get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p: if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \ %(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \ robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \ robotParam.param.wheel_track, \
robotParam.param.encoder_resolution robotParam.param.encoder_resolution
)) ))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \ %(robotParam.param.do_pid_interval, \
robotParam.param.kp, \ robotParam.param.kp, \
robotParam.param.ki, \ robotParam.param.ki, \
robotParam.param.kd, \ robotParam.param.kd, \
robotParam.param.ko)) robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \ log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\ %(robotParam.param.cmd_last_time,\
robotParam.param.imu_type robotParam.param.imu_type
)) ))
log.info("max_v:%d %d %d\r\n" \ log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\ %(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \ robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z robotParam.param.max_v_angular_z
)) ))
log.info("motor flag:%d encoder flag: %d\r\n" \ log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\ %(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag robotParam.param.encoder_nonexchange_flag
)) ))
else: else:
log.error('get params err\r\n') log.error('get params err\r\n')
quit(1) quit(1)
log.info("****************get odom&imu*****************") log.info("****************get odom&imu*****************")
while run_flag: while run_flag:
robotOdom = DataHolder[MessageID.ID_GET_ODOM] robotOdom = DataHolder[MessageID.ID_GET_ODOM]
p = mboard.request(MessageID.ID_GET_ODOM) p = mboard.request(MessageID.ID_GET_ODOM)
if p: if p:
log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \ log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \
robotOdom.v_liner_y, \ robotOdom.v_liner_y, \
robotOdom.v_angular_z, \ robotOdom.v_angular_z, \
robotOdom.x, \ robotOdom.x, \
robotOdom.y, \ robotOdom.y, \
robotOdom.yaw)) robotOdom.yaw))
else: else:
log.error('get odom err') log.error('get odom err')
quit(1) quit(1)
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
p = mboard.request(MessageID.ID_GET_IMU) p = mboard.request(MessageID.ID_GET_IMU)
if p: if p:
log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \ log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \
robotIMU[3], robotIMU[4], robotIMU[5], \ robotIMU[3], robotIMU[4], robotIMU[5], \
robotIMU[6], robotIMU[7], robotIMU[8])) robotIMU[6], robotIMU[7], robotIMU[8]))
else: else:
log.error('get imu err') log.error('get imu err')
quit(1) quit(1)
time.sleep(0.1) time.sleep(0.1)

148
Code/MowingRobot/pibot_ros/pypibot/transport/params.py Executable file → Normal file
View File

@ -1,75 +1,75 @@
import dataholder import dataholder
import os import os
from dataholder import RobotImuType from dataholder import RobotImuType
from dataholder import RobotModelType from dataholder import RobotModelType
pibotModel = os.environ['PIBOT_MODEL'] pibotModel = os.environ['PIBOT_MODEL']
boardType = os.environ['PIBOT_BOARD'] boardType = os.environ['PIBOT_BOARD']
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE'] pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
print(pibotModel) print(pibotModel)
print(boardType) print(boardType)
print(pibotBaud) print(pibotBaud)
pibotParam = dataholder.RobotParameters() pibotParam = dataholder.RobotParameters()
if pibotModel == "apollo" and boardType == "arduino": if pibotModel == "apollo" and boardType == "arduino":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
75, 2500, 0, 10, \ 75, 2500, 0, 10, \
250, 40, 0, 200, \ 250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY85, 90, \ RobotImuType.IMU_TYPE_GY85, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apollo" and boardType == "stm32f1": elif pibotModel == "apollo" and boardType == "stm32f1":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
320, 2700, 0, 10, \ 320, 2700, 0, 10, \
250, 50, 0, 200, \ 250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apollo" and boardType == "stm32f4": elif pibotModel == "apollo" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \ pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
320, 2700, 0, 10, \ 320, 2700, 0, 10, \
250, 40, 0, 200, \ 250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "zeus" and boardType == "stm32f4": elif pibotModel == "zeus" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \ pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
320, 2700, 0, 10, \ 320, 2700, 0, 10, \
250, 50, 50, 250, \ 250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_3WD_OMNI) RobotModelType.MODEL_TYPE_3WD_OMNI)
elif pibotModel == "hades" and boardType == "stm32f4": elif pibotModel == "hades" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \ pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
320, 2700, 0, 10, \ 320, 2700, 0, 10, \
250, 50, 50, 250, \ 250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_4WD_MECANUM) RobotModelType.MODEL_TYPE_4WD_MECANUM)
elif pibotModel == "hadesX" and boardType == "stm32f4": elif pibotModel == "hadesX" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \ pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
250, 2750, 0, 10, \ 250, 2750, 0, 10, \
250, 50, 50, 250, \ 250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 72, \ RobotImuType.IMU_TYPE_GY87, 72, \
RobotModelType.MODEL_TYPE_4WD_MECANUM) RobotModelType.MODEL_TYPE_4WD_MECANUM)
elif pibotModel == "hera" and boardType == "stm32f4": elif pibotModel == "hera" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \ pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
320, 2700, 0, 10, \ 320, 2700, 0, 10, \
250, 50, 50, 250, \ 250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_4WD_DIFF) RobotModelType.MODEL_TYPE_4WD_DIFF)
elif pibotModel == "apolloX" and boardType == "arduino": elif pibotModel == "apolloX" and boardType == "arduino":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
75, 2500, 0, 10, \ 75, 2500, 0, 10, \
250, 40, 0, 200, \ 250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY85, 90, \ RobotImuType.IMU_TYPE_GY85, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apolloX" and boardType == "stm32f1": elif pibotModel == "apolloX" and boardType == "stm32f1":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
250, 1200, 0, 10, \ 250, 1200, 0, 10, \
250, 50, 0, 200, \ 250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apolloX" and boardType == "stm32f4": elif pibotModel == "apolloX" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \ pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
250, 1200, 0, 10, \ 250, 1200, 0, 10, \
250, 50, 0, 200, \ 250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \ RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF) RobotModelType.MODEL_TYPE_2WD_DIFF)

178
Code/MowingRobot/pibot_ros/pypibot/transport/set_default_params.py Executable file → Normal file
View File

@ -1,90 +1,90 @@
import platform import platform
import sys import sys
sys.path.append("..") sys.path.append("..")
import pypibot import pypibot
from pypibot import log from pypibot import log
from transport import Transport from transport import Transport
from dataholder import MessageID from dataholder import MessageID
import params import params
#for linux #for linux
port="/dev/pibot" port="/dev/pibot"
#for windows #for windows
#port="com3" #port="com3"
pypibot.assistant.enableGlobalExcept() pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") #log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i") log.setLevel("i")
if __name__ == '__main__': if __name__ == '__main__':
mboard = Transport(port, params.pibotBaud) mboard = Transport(port, params.pibotBaud)
if not mboard.start(): if not mboard.start():
log.error("can not open %s"%port) log.error("can not open %s"%port)
sys.exit() sys.exit()
DataHolder = mboard.getDataHolder() DataHolder = mboard.getDataHolder()
for num in range(0,3): for num in range(0,3):
log.info("****************get robot version*****************") log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION] boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION) p = mboard.request(MessageID.ID_GET_VERSION)
if p: if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break break
else: else:
log.error('read firmware version err\r\n') log.error('read firmware version err\r\n')
import time import time
time.sleep(1) time.sleep(1)
if num == 2: if num == 2:
log.error('please check connection or baudrate\r\n') log.error('please check connection or baudrate\r\n')
sys.exit() sys.exit()
# set robot parameter # set robot parameter
log.info("****************set robot parameter*****************") log.info("****************set robot parameter*****************")
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER) p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
if p: if p:
log.info('set parameter success') log.info('set parameter success')
else: else:
log.error('set parameter err') log.error('set parameter err')
quit(1) quit(1)
# get robot parameter # get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p: if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \ %(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \ robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \ robotParam.param.wheel_track, \
robotParam.param.encoder_resolution robotParam.param.encoder_resolution
)) ))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \ %(robotParam.param.do_pid_interval, \
robotParam.param.kp, \ robotParam.param.kp, \
robotParam.param.ki, \ robotParam.param.ki, \
robotParam.param.kd, \ robotParam.param.kd, \
robotParam.param.ko)) robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \ log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\ %(robotParam.param.cmd_last_time,\
robotParam.param.imu_type robotParam.param.imu_type
)) ))
log.info("max_v:%d %d %d\r\n" \ log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\ %(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \ robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z robotParam.param.max_v_angular_z
)) ))
log.info("motor flag:%d encoder flag: %d\r\n" \ log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\ %(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag robotParam.param.encoder_nonexchange_flag
)) ))
else: else:
log.error('get param err\r\n') log.error('get param err\r\n')
quit(1) quit(1)

242
Code/MowingRobot/pibot_ros/pypibot/transport/test_motors.py Executable file → Normal file
View File

@ -1,122 +1,122 @@
import platform import platform
import sys import sys
sys.path.append("..") sys.path.append("..")
import pypibot import pypibot
from pypibot import log from pypibot import log
from transport import Transport from transport import Transport
from dataholder import MessageID from dataholder import MessageID
import params import params
import signal import signal
#for linux #for linux
port="/dev/pibot" port="/dev/pibot"
#for windows #for windows
#port="com3" #port="com3"
pypibot.assistant.enableGlobalExcept() pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log") #log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i") log.setLevel("i")
run_flag = True run_flag = True
def exit(signum, frame): def exit(signum, frame):
global run_flag global run_flag
run_flag = False run_flag = False
if __name__ == '__main__': if __name__ == '__main__':
signal.signal(signal.SIGINT, exit) signal.signal(signal.SIGINT, exit)
mboard = Transport(port, params.pibotBaud) mboard = Transport(port, params.pibotBaud)
if not mboard.start(): if not mboard.start():
log.error("can not open %s"%port) log.error("can not open %s"%port)
sys.exit() sys.exit()
pwm=[0]*4 pwm=[0]*4
for i in range(len(sys.argv)-1): for i in range(len(sys.argv)-1):
pwm[i] = int(sys.argv[i+1]) pwm[i] = int(sys.argv[i+1])
DataHolder = mboard.getDataHolder() DataHolder = mboard.getDataHolder()
for num in range(0,3): for num in range(0,3):
log.info("****************get robot version*****************") log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION] boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION) p = mboard.request(MessageID.ID_GET_VERSION)
if p: if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode())) log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break break
else: else:
log.error('read firmware version err\r\n') log.error('read firmware version err\r\n')
import time import time
time.sleep(1) time.sleep(1)
if num == 2: if num == 2:
log.error('please check connection or baudrate\r\n') log.error('please check connection or baudrate\r\n')
sys.exit() sys.exit()
# get robot parameter # get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER] robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER) p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p: if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \ log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \ %(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \ robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \ robotParam.param.wheel_track, \
robotParam.param.encoder_resolution robotParam.param.encoder_resolution
)) ))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \ log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \ %(robotParam.param.do_pid_interval, \
robotParam.param.kp, \ robotParam.param.kp, \
robotParam.param.ki, \ robotParam.param.ki, \
robotParam.param.kd, \ robotParam.param.kd, \
robotParam.param.ko)) robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \ log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\ %(robotParam.param.cmd_last_time,\
robotParam.param.imu_type robotParam.param.imu_type
)) ))
log.info("max_v:%d %d %d\r\n" \ log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\ %(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \ robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z robotParam.param.max_v_angular_z
)) ))
log.info("motor flag:%d encoder flag: %d\r\n" \ log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\ %(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag robotParam.param.encoder_nonexchange_flag
)) ))
else: else:
log.error('get params err\r\n') log.error('get params err\r\n')
quit(1) quit(1)
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
p = mboard.request(MessageID.ID_SET_MOTOR_PWM) p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
if p: if p:
log.info('set pwm success') log.info('set pwm success')
else: else:
log.error('set pwm err') log.error('set pwm err')
quit(1) quit(1)
log.info("****************get encoder count*****************") log.info("****************get encoder count*****************")
while run_flag: while run_flag:
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT) p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
if p: if p:
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder]))) log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
else: else:
log.error('get encoder count err') log.error('get encoder count err')
quit(1) quit(1)
import time import time
time.sleep(0.5) time.sleep(0.5)
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4 DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
p = mboard.request(MessageID.ID_SET_MOTOR_PWM) p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
if p: if p:
log.info('set pwm success') log.info('set pwm success')
else: else:
log.error('set pwm err') log.error('set pwm err')
quit(1) quit(1)

394
Code/MowingRobot/pibot_ros/pypibot/transport/transport.py Executable file → Normal file
View File

@ -1,197 +1,197 @@
import sys import sys
sys.path.append("..") sys.path.append("..")
import pypibot import pypibot
from pypibot import log from pypibot import log
from pypibot import assistant from pypibot import assistant
import serial import serial
import threading import threading
import struct import struct
import time import time
from dataholder import MessageID, BoardDataDict from dataholder import MessageID, BoardDataDict
FIX_HEAD = 0x5a FIX_HEAD = 0x5a
class Recstate(): class Recstate():
WAITING_HD = 0 WAITING_HD = 0
WAITING_MSG_ID = 1 WAITING_MSG_ID = 1
RECEIVE_LEN = 2 RECEIVE_LEN = 2
RECEIVE_PACKAGE = 3 RECEIVE_PACKAGE = 3
RECEIVE_CHECK = 4 RECEIVE_CHECK = 4
def checksum(d): def checksum(d):
sum = 0 sum = 0
if assistant.is_python3(): if assistant.is_python3():
for i in d: for i in d:
sum += i sum += i
sum = sum&0xff sum = sum&0xff
else: else:
for i in d: for i in d:
sum += ord(i) sum += ord(i)
sum = sum&0xff sum = sum&0xff
return sum return sum
class Transport: class Transport:
def __init__(self, port, baudrate=921600): def __init__(self, port, baudrate=921600):
self._Port = port self._Port = port
self._Baudrate = baudrate self._Baudrate = baudrate
self._KeepRunning = False self._KeepRunning = False
self.receive_state = Recstate.WAITING_HD self.receive_state = Recstate.WAITING_HD
self.rev_msg = [] self.rev_msg = []
self.rev_data = [] self.rev_data = []
self.wait_event = threading.Event() self.wait_event = threading.Event()
def getDataHolder(self): def getDataHolder(self):
return BoardDataDict return BoardDataDict
def start(self): def start(self):
try: try:
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2) self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
self._KeepRunning = True self._KeepRunning = True
self._ReceiverThread = threading.Thread(target=self._Listen) self._ReceiverThread = threading.Thread(target=self._Listen)
self._ReceiverThread.setDaemon(True) self._ReceiverThread.setDaemon(True)
self._ReceiverThread.start() self._ReceiverThread.start()
return True return True
except: except:
return False return False
def Stop(self): def Stop(self):
self._KeepRunning = False self._KeepRunning = False
time.sleep(0.1) time.sleep(0.1)
self._Serial.close() self._Serial.close()
def _Listen(self): def _Listen(self):
while self._KeepRunning: while self._KeepRunning:
if self.receiveFiniteStates(self._Serial.read()): if self.receiveFiniteStates(self._Serial.read()):
self.packageAnalysis() self.packageAnalysis()
def receiveFiniteStates(self, s): def receiveFiniteStates(self, s):
if len(s) == 0: if len(s) == 0:
return False return False
val = s[0] val = s[0]
val_int = val val_int = val
if not assistant.is_python3(): if not assistant.is_python3():
val_int = ord(val) val_int = ord(val)
if self.receive_state == Recstate.WAITING_HD: if self.receive_state == Recstate.WAITING_HD:
if val_int == FIX_HEAD: if val_int == FIX_HEAD:
log.trace('got head') log.trace('got head')
self.rev_msg = [] self.rev_msg = []
self.rev_data =[] self.rev_data =[]
self.rev_msg.append(val) self.rev_msg.append(val)
self.receive_state = Recstate.WAITING_MSG_ID self.receive_state = Recstate.WAITING_MSG_ID
elif self.receive_state == Recstate.WAITING_MSG_ID: elif self.receive_state == Recstate.WAITING_MSG_ID:
log.trace('got msg id') log.trace('got msg id')
self.rev_msg.append(val) self.rev_msg.append(val)
self.receive_state = Recstate.RECEIVE_LEN self.receive_state = Recstate.RECEIVE_LEN
elif self.receive_state == Recstate.RECEIVE_LEN: elif self.receive_state == Recstate.RECEIVE_LEN:
log.trace('got len:%d', val_int) log.trace('got len:%d', val_int)
self.rev_msg.append(val) self.rev_msg.append(val)
if val_int == 0: if val_int == 0:
self.receive_state = Recstate.RECEIVE_CHECK self.receive_state = Recstate.RECEIVE_CHECK
else: else:
self.receive_state = Recstate.RECEIVE_PACKAGE self.receive_state = Recstate.RECEIVE_PACKAGE
elif self.receive_state == Recstate.RECEIVE_PACKAGE: elif self.receive_state == Recstate.RECEIVE_PACKAGE:
# if assistant.is_python3(): # if assistant.is_python3():
# self.rev_data.append((chr(val)).encode('latin1')) # self.rev_data.append((chr(val)).encode('latin1'))
# else: # else:
self.rev_data.append(val) self.rev_data.append(val)
r = False r = False
if assistant.is_python3(): if assistant.is_python3():
r = len(self.rev_data) == int(self.rev_msg[-1]) r = len(self.rev_data) == int(self.rev_msg[-1])
else: else:
r = len(self.rev_data) == ord(self.rev_msg[-1]) r = len(self.rev_data) == ord(self.rev_msg[-1])
if r: if r:
self.rev_msg.extend(self.rev_data) self.rev_msg.extend(self.rev_data)
self.receive_state = Recstate.RECEIVE_CHECK self.receive_state = Recstate.RECEIVE_CHECK
elif self.receive_state == Recstate.RECEIVE_CHECK: elif self.receive_state == Recstate.RECEIVE_CHECK:
log.trace('got check') log.trace('got check')
self.receive_state = Recstate.WAITING_HD self.receive_state = Recstate.WAITING_HD
if val_int == checksum(self.rev_msg): if val_int == checksum(self.rev_msg):
log.trace('got a complete message') log.trace('got a complete message')
return True return True
else: else:
self.receive_state = Recstate.WAITING_HD self.receive_state = Recstate.WAITING_HD
# continue receiving # continue receiving
return False return False
def packageAnalysis(self): def packageAnalysis(self):
if assistant.is_python3(): if assistant.is_python3():
in_msg_id = int(self.rev_msg[1]) in_msg_id = int(self.rev_msg[1])
else: else:
in_msg_id = ord(self.rev_msg[1]) in_msg_id = ord(self.rev_msg[1])
if assistant.is_python3(): if assistant.is_python3():
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data)) log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data)) r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
else: else:
log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data)) log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data))
r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data)) r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data))
if r: if r:
self.res_id = in_msg_id self.res_id = in_msg_id
if in_msg_id<100: if in_msg_id<100:
self.set_response() self.set_response()
else:#notify else:#notify
log.debug('msg %d'%self.rev_msg[4],'data incoming') log.debug('msg %d'%self.rev_msg[4],'data incoming')
pass pass
else: else:
log.debug ('error unpacking pkg') log.debug ('error unpacking pkg')
def request(self, id, timeout=0.5): def request(self, id, timeout=0.5):
if not self.write(id): if not self.write(id):
log.debug ('Serial send error!') log.debug ('Serial send error!')
return False return False
if self.wait_for_response(timeout): if self.wait_for_response(timeout):
if id == self.res_id: if id == self.res_id:
log.trace ('OK') log.trace ('OK')
else: else:
log.error ('Got unmatched response!') log.error ('Got unmatched response!')
else: else:
log.error ('Request got no response!') log.error ('Request got no response!')
return False return False
# clear response # clear response
self.res_id = None self.res_id = None
return True return True
def write(self, id): def write(self, id):
cmd = self.make_command(id) cmd = self.make_command(id)
if assistant.is_python3(): if assistant.is_python3():
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd)) log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
else: else:
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd)) log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
self._Serial.write(cmd) self._Serial.write(cmd)
return True return True
def wait_for_response(self, timeout): def wait_for_response(self, timeout):
self.wait_event.clear() self.wait_event.clear()
return self.wait_event.wait(timeout) return self.wait_event.wait(timeout)
def set_response(self): def set_response(self):
self.wait_event.set() self.wait_event.set()
def make_command(self, id): def make_command(self, id):
#print(DataDict[id]) #print(DataDict[id])
data = BoardDataDict[id].pack() data = BoardDataDict[id].pack()
l = [FIX_HEAD, id, len(data)] l = [FIX_HEAD, id, len(data)]
head = struct.pack("3B", *l) head = struct.pack("3B", *l)
body = head + data body = head + data
if assistant.is_python3(): if assistant.is_python3():
return body + chr(checksum(body)).encode('latin1') return body + chr(checksum(body)).encode('latin1')
else: else:
return body + chr(checksum(body)) return body + chr(checksum(body))
if __name__ == '__main__': if __name__ == '__main__':
mboard = Transport('com10') mboard = Transport('com10')
if not mboard.start(): if not mboard.start():
import sys import sys
sys.exit() sys.exit()
p = mboard.request(MessageID.ID_GET_VERSION) p = mboard.request(MessageID.ID_GET_VERSION)
log.i("result=%s"%p) log.i("result=%s"%p)
print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n") print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n")

106
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt
View File

@ -1,53 +1,53 @@
cmake_minimum_required(VERSION 3.0.2) cmake_minimum_required(VERSION 3.0.2)
project(FollowingCar) project(FollowingCar)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
rospy rospy
std_msgs std_msgs
message_generation message_generation
geometry_msgs geometry_msgs
pibot_msgs pibot_msgs
) )
# 寻找OpenCV库 # 寻找OpenCV库
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
# 查找 Boost 库 # 查找 Boost 库
find_package(Boost REQUIRED) find_package(Boost REQUIRED)
# catkin_package( # catkin_package(
# # INCLUDE_DIRS include # # INCLUDE_DIRS include
# # LIBRARIES FollowingCar # # LIBRARIES FollowingCar
# # CATKIN_DEPENDS roscpp rospy std_msgs # # CATKIN_DEPENDS roscpp rospy std_msgs
# # DEPENDS system_lib # # DEPENDS system_lib
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
# ) # )
include_directories( include_directories(
# include # include
${OpenCV_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
include include
) )
add_library(${PROJECT_NAME} SHARED add_library(${PROJECT_NAME} SHARED
src/uwb.cpp src/uwb.cpp
src/mapping.cpp src/mapping.cpp
src/align.cpp src/align.cpp
src/Mat.cpp src/Mat.cpp
src/lighthouse.cpp src/lighthouse.cpp
include/senddata.h src/senddata.cpp) include/senddata.h src/senddata.cpp)
generate_messages(DEPENDENCIES std_msgs geometry_msgs) generate_messages(DEPENDENCIES std_msgs geometry_msgs)
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs) catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
add_executable(${PROJECT_NAME}_node src/main.cpp) add_executable(${PROJECT_NAME}_node src/main.cpp)
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME}) target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})

166
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h
View File

@ -1,83 +1,83 @@
/******************** (C) COPYRIGHT 2022 Geek************************************ /******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Mat.h * File Name : Mat.h
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#ifndef _H_MAT_ #ifndef _H_MAT_
#define _H_MAT_ #define _H_MAT_
#define MAT_MAX 15 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> #define MAT_MAX 15 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#include <string.h> #include <string.h>
#define _USE_MATH_DEFINES #define _USE_MATH_DEFINES
#include <math.h> #include <math.h>
class Mat class Mat
{ {
public: public:
Mat(); Mat();
Mat(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD> Mat(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
void Init(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD> void Init(int setm,int setn,int kind);//kind=1<><31>λ<EFBFBD><CEBB>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
void Zero(void); void Zero(void);
//<2F><>Щ<EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>Ϊprivate<74>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><CBB7>㣬<EFBFBD><E3A3AC>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public //<2F><>Щ<EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>Ϊprivate<74>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><CBB7>㣬<EFBFBD><E3A3AC>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public
int m;//<2F><><EFBFBD><EFBFBD> int m;//<2F><><EFBFBD><EFBFBD>
int n;//<2F><><EFBFBD><EFBFBD> int n;//<2F><><EFBFBD><EFBFBD>
double mat[MAT_MAX][MAT_MAX];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double mat[MAT_MAX][MAT_MAX];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD> //<2F><><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
Mat SubMat(int a,int b,int lm,int ln);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD> Mat SubMat(int a,int b,int lm,int ln);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
void FillSubMat(int a,int b,Mat s);//<2F><><EFBFBD><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD> void FillSubMat(int a,int b,Mat s);//<2F><><EFBFBD><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8> //<2F><><EFBFBD><EFBFBD>ר<EFBFBD><D7A8>
double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>Ԫ<EFBFBD>صľ<D8B5><C4BE><EFBFBD>ֵ<EFBFBD><D6B5> double absvec();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>Ԫ<EFBFBD>صľ<D8B5><C4BE><EFBFBD>ֵ<EFBFBD><D6B5>
double Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD> double Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD> friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
//<2F><><EFBFBD><EFBFBD> //<2F><><EFBFBD><EFBFBD>
friend Mat operator *(double k,Mat a); friend Mat operator *(double k,Mat a);
friend Mat operator *(Mat a,double k); friend Mat operator *(Mat a,double k);
friend Mat operator /(Mat a,double k); friend Mat operator /(Mat a,double k);
friend Mat operator *(Mat a,Mat b); friend Mat operator *(Mat a,Mat b);
friend Mat operator +(Mat a,Mat b); friend Mat operator +(Mat a,Mat b);
friend Mat operator -(Mat a,Mat b); friend Mat operator -(Mat a,Mat b);
friend Mat operator ~(Mat a);//ת<><D7AA> friend Mat operator ~(Mat a);//ת<><D7AA>
friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator /(Mat a,Mat b);//a*inv(b)
friend Mat operator %(Mat a,Mat b);//inv(a)*b friend Mat operator %(Mat a,Mat b);//inv(a)*b
//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD> //MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
private: private:
// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD> // Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void RowExchange(int a, int b); void RowExchange(int a, int b);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5> // ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void RowMul(int a,double k); void RowMul(int a,double k);
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD> // <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
void RowAdd(int a,int b, double k); void RowAdd(int a,int b, double k);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ColExchange(int a, int b); void ColExchange(int a, int b);
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5> // ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
void ColMul(int a,double k); void ColMul(int a,double k);
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD> // <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
void ColAdd(int a,int b,double k); void ColAdd(int a,int b,double k);
}; };
#endif #endif

88
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h
View File

@ -1,44 +1,44 @@
#include <cmath> #include <cmath>
#include <ros/ros.h> #include <ros/ros.h>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <utility> #include <utility>
#include <queue> #include <queue>
#include <fstream> #include <fstream>
#include "pibot_msgs/RawImu.h" #include "pibot_msgs/RawImu.h"
#include "type.h" #include "type.h"
#include "uwb.h" #include "uwb.h"
#include "lighthouse.h" #include "lighthouse.h"
#include "Mat.h" #include "Mat.h"
#ifndef ALIGN_H #ifndef ALIGN_H
#define AlIGN_H #define AlIGN_H
namespace uwb_slam{ namespace uwb_slam{
class Align class Align
{ {
public: public:
Align(){ Align(){
}; };
void Run(); void Run();
void wheel_odomCB(const nav_msgs::Odometry& wheel_odom); void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
void imuCB(const pibot_msgs::RawImu& imu); void imuCB(const pibot_msgs::RawImu& imu);
void odomCB(const nav_msgs::Odometry& odom); void odomCB(const nav_msgs::Odometry& odom);
public: public:
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::Subscriber wheel_odom_sub_; ros::Subscriber wheel_odom_sub_;
ros::Subscriber imu_sub_; ros::Subscriber imu_sub_;
ros::Subscriber odom_sub_; ros::Subscriber odom_sub_;
Imu_odom_pose_data imu_odom_; Imu_odom_pose_data imu_odom_;
Uwb_data uwb_data_; Uwb_data uwb_data_;
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime; ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
ros::Time odom_tmp_ ; ros::Time odom_tmp_ ;
bool write_data_ = false; bool write_data_ = false;
cv::Mat img1; cv::Mat img1;
std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue; std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::shared_ptr<uwb_slam::Lighthouse> lighthouse_; std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
}; };
}; };
#endif #endif

58
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h
View File

@ -1,29 +1,29 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <mutex> #include <mutex>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <cstdint> #include <cstdint>
#include "type.h" #include "type.h"
#include <queue> #include <queue>
#include <chrono> #include <chrono>
#ifndef __LIGHTHOUSE_H__ #ifndef __LIGHTHOUSE_H__
#define __LIGHTHOUSE_H__ #define __LIGHTHOUSE_H__
namespace uwb_slam{ namespace uwb_slam{
class Lighthouse{ class Lighthouse{
public: public:
Lighthouse(); Lighthouse();
~Lighthouse(); ~Lighthouse();
void Run(); void Run();
void UDPRead(); void UDPRead();
// Listen PORT // Listen PORT
int PORT = 12345; int PORT = 12345;
int UdpSocket = -1; int UdpSocket = -1;
LightHouseData data; LightHouseData data;
std::mutex mMutexLighthouse; std::mutex mMutexLighthouse;
}; };
}; };
#endif #endif

80
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h
View File

@ -1,40 +1,40 @@
#include <queue> #include <queue>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <mutex> #include <mutex>
#include "uwb.h" #include "uwb.h"
#include "align.h" #include "align.h"
#ifndef MAPPING_H #ifndef MAPPING_H
#define MAPPING_H #define MAPPING_H
namespace uwb_slam{ namespace uwb_slam{
class Mapping class Mapping
{ {
public: public:
const double PIXEL_SCALE = 1.0; //xiangsudian cm const double PIXEL_SCALE = 1.0; //xiangsudian cm
const int AREA_SIZE = 1000; //map size cm const int AREA_SIZE = 1000; //map size cm
Mapping() {}; Mapping() {};
void Run(); void Run();
bool checkQueue(); bool checkQueue();
void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData); void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
void process(); void process();
std::mutex mMutexMap; std::mutex mMutexMap;
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::shared_ptr<uwb_slam::Align> align_; std::shared_ptr<uwb_slam::Align> align_;
private: private:
int realWidth, realHeight; int realWidth, realHeight;
std::queue<std::vector<cv::Point2d>> posDataQueue; std::queue<std::vector<cv::Point2d>> posDataQueue;
std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1}); std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
//cv::Point2d imuPoint = {-1,-1}; //cv::Point2d imuPoint = {-1,-1};
// std::queue<cv::Point2d> posDataQueue; // std::queue<cv::Point2d> posDataQueue;
bool readPos = false; bool readPos = false;
cv::Mat img; cv::Mat img;
}; };
} }
#endif #endif

98
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h
View File

@ -1,50 +1,50 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h> #include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h> #include <geometry_msgs/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
#include <mutex> #include <mutex>
#include "uwb.h" #include "uwb.h"
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include "pibot_msgs/dis_info.h" #include "pibot_msgs/dis_info.h"
#include "pibot_msgs/dis_info_array.h" #include "pibot_msgs/dis_info_array.h"
#ifndef SENDDATA_H #ifndef SENDDATA_H
#define SENDDATA_H #define SENDDATA_H
namespace uwb_slam{ namespace uwb_slam{
class Senddata class Senddata
{ {
public: public:
Senddata(){}; Senddata(){};
void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb); void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
void Run(std::shared_ptr<uwb_slam::Uwb>uwb); void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
void odomCB(const nav_msgs::Odometry& odom); void odomCB(const nav_msgs::Odometry& odom);
void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo); void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo);
std::shared_ptr<uwb_slam::Uwb> uwb_; std::shared_ptr<uwb_slam::Uwb> uwb_;
std::mutex mMutexSend; std::mutex mMutexSend;
private: private:
ros::Publisher position_pub_; ros::Publisher position_pub_;
ros::Publisher cmd_vel_pub_; ros::Publisher cmd_vel_pub_;
ros::Subscriber odom_sub_; ros::Subscriber odom_sub_;
ros::Subscriber obstacles_sub_; ros::Subscriber obstacles_sub_;
ros::NodeHandle nh_; ros::NodeHandle nh_;
nav_msgs::Odometry odom_;//odom的消息类型 nav_msgs::Odometry odom_;//odom的消息类型
nav_msgs::Odometry sub_odom_;//odom的消息类型 nav_msgs::Odometry sub_odom_;//odom的消息类型
geometry_msgs::Twist cmd_vel_; geometry_msgs::Twist cmd_vel_;
bool flag_ = 0; bool flag_ = 0;
}; };
} }
#endif #endif

98
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h
View File

@ -1,50 +1,50 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/time.h> #include <ros/time.h>
#ifndef TYPE_H #ifndef TYPE_H
#define TYPE_H #define TYPE_H
namespace uwb_slam{ namespace uwb_slam{
struct Imu_data struct Imu_data
{ {
ros::Time imu_t_; ros::Time imu_t_;
double a_[3]; double a_[3];
double g_[3]; double g_[3];
double m_[3]; double m_[3];
Imu_data(){}; Imu_data(){};
Imu_data(double ax,double ay,double az, double gx, double gy, double gz, double mx, double my, double mz) Imu_data(double ax,double ay,double az, double gx, double gy, double gz, double mx, double my, double mz)
:a_{ax,ay,az},g_{gx,gy,gz},m_{mx,my,mz}{}; :a_{ax,ay,az},g_{gx,gy,gz},m_{mx,my,mz}{};
}; };
struct Imu_odom_pose_data struct Imu_odom_pose_data
{ {
Imu_data imu_data_; Imu_data imu_data_;
double pose_[3]; double pose_[3];
double quat_[4]; double quat_[4];
double vxy_; double vxy_;
double angle_v_; double angle_v_;
Imu_odom_pose_data(){}; Imu_odom_pose_data(){};
Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){}; Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
}; };
struct Uwb_data struct Uwb_data
{ {
float x_,y_; float x_,y_;
ros::Time uwb_t_; ros::Time uwb_t_;
Uwb_data(){}; Uwb_data(){};
Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){}; Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
}; };
struct LightHouseData struct LightHouseData
{ {
float x_,y_,z_; float x_,y_,z_;
float qw_,qx_,qy_,qz_; float qw_,qx_,qy_,qz_;
ros::Time lighthouse_t_; ros::Time lighthouse_t_;
LightHouseData(){}; LightHouseData(){};
LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t): LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t):
x_(x),y_(y),z_(z),qw_(qw), qx_(qx), qy_(qy), qz_(qz), lighthouse_t_(t){}; x_(x),y_(y),z_(z),qw_(qw), qx_(qx), qy_(qy), qz_(qz), lighthouse_t_(t){};
}; };
} }
#endif #endif

120
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h
View File

@ -1,60 +1,60 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <mutex> #include <mutex>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <cstdint> #include <cstdint>
#include "type.h" #include "type.h"
#include <queue> #include <queue>
#include <chrono> #include <chrono>
#ifndef UWB_H #ifndef UWB_H
#define UWB_H #define UWB_H
#define PI acos(-1) #define PI acos(-1)
namespace uwb_slam{ namespace uwb_slam{
class Uwb class Uwb
{ {
public: public:
Uwb(); Uwb();
void Run(); void Run();
bool checknewdata(); bool checknewdata();
void feed_imu_odom_pose_data(); void feed_imu_odom_pose_data();
void Serread(); void Serread();
public: public:
int pre_seq = -1; int pre_seq = -1;
int cur_seq = -1; int cur_seq = -1;
int AnchorNum = 3; int AnchorNum = 3;
int AnchorPos[3][3]={ int AnchorPos[3][3]={
-240, 400, 113,\ -240, 400, 113,\
240, 400, 113,\ 240, 400, 113,\
-400, -240, 113 -400, -240, 113
};//基站坐标，序号+三维坐标 };//基站坐标，序号+三维坐标
int d[3]; int d[3];
int aoa[3]; int aoa[3];
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_; // std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
Uwb_data uwb_data_; Uwb_data uwb_data_;
// ros_merge_test::RawImu sub_imu_; // ros_merge_test::RawImu sub_imu_;
// std::queue<Imu_odom_pose_data > imu_odom_queue_; // std::queue<Imu_odom_pose_data > imu_odom_queue_;
// std::queue<Uwb_data> uwb_data_queue_; // std::queue<Uwb_data> uwb_data_queue_;
std::mutex mMutexUwb; std::mutex mMutexUwb;
//boost::asio::io_service io; //boost::asio::io_service io;
//boost::asio::serial_port s_port; //boost::asio::serial_port s_port;
// Imu_odom_pose_data imu_odom_pose_data_; // Imu_odom_pose_data imu_odom_pose_data_;
}; };
}; };
#endif #endif

150
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml
View File

@ -1,75 +1,75 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>FollowingCar</name> <name>FollowingCar</name>
<version>0.0.0</version> <version>0.0.0</version>
<description>The FollowingCar package</description> <description>The FollowingCar package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: --> <!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="luoruidi@todo.todo">luoruidi</maintainer> <maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
<!-- One license tag required, multiple allowed, one license per tag --> <!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: --> <!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license> <license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: --> <!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/FollowingCar</url> --> <!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be --> <!-- Authors do not have to be maintainers, but could be -->
<!-- Example: --> <!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> --> <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies --> <!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies --> <!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: --> <!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies --> <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> --> <!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: --> <!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> --> <!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> --> <!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: --> <!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> --> <!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: --> <!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> --> <!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: --> <!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: --> <!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> --> <!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: --> <!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> --> <!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: --> <!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> --> <!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>pibot_msgs</build_depend> <build_depend>pibot_msgs</build_depend>
<build_depend>geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend> <build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend> <build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend> <exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend> <exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend> <exec_depend>message_runtime</exec_depend>
<exec_depend>geometry_msgs</exec_depend> <exec_depend>geometry_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->
</export> </export>
</package> </package>

736
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp
View File

@ -1,368 +1,368 @@
/******************** (C) COPYRIGHT 2022 Geek************************************ /******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Mat.cpp * File Name : Mat.cpp
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#include "Mat.h" #include "Mat.h"
double mind(double a,double b) double mind(double a,double b)
{ {
double c = a; double c = a;
if(b < c){ if(b < c){
c = b; c = b;
} }
return c; return c;
} }
int mini(int a,int b) int mini(int a,int b)
{ {
int c=a; int c=a;
if(b<c) if(b<c)
{ {
c=b; c=b;
} }
return c; return c;
} }
//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF> //<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
Mat::Mat() Mat::Mat()
{ {
Init(1,1,0); Init(1,1,0);
} }
Mat::Mat(int setm,int setn,int kind) Mat::Mat(int setm,int setn,int kind)
{ {
Init(setm,setn,kind); Init(setm,setn,kind);
} }
void Mat::Init(int setm,int setn,int kind) void Mat::Init(int setm,int setn,int kind)
{ {
this->m = setm; this->m = setm;
this->n = setn; this->n = setn;
if((kind==0)||(kind==1)) if((kind==0)||(kind==1))
{ {
memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double)); memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
} }
if(kind==1) if(kind==1)
{ {
int x; int x;
//Cԭ<43>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档 //Cԭ<43>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档
int xend = mini(this->m, this->n); int xend = mini(this->m, this->n);
for(x=0;x < xend;x++){ for(x=0;x < xend;x++){
mat[x][x] = 1; mat[x][x] = 1;
} }
} }
} }
void Mat::Zero() { void Mat::Zero() {
} }
Mat Mat::SubMat(int a,int b,int lm,int ln) Mat Mat::SubMat(int a,int b,int lm,int ln)
{ {
int aend=a+lm-1; int aend=a+lm-1;
int bend=b+ln-1; int bend=b+ln-1;
Mat s(lm,ln,-1); Mat s(lm,ln,-1);
int x,y; int x,y;
for(x=0;x<lm;x++) for(x=0;x<lm;x++)
{ {
for(y=0;y<ln;y++) for(y=0;y<ln;y++)
{ {
s.mat[x][y]=mat[a+x][b+y]; s.mat[x][y]=mat[a+x][b+y];
} }
} }
return s; return s;
} }
void Mat::FillSubMat(int a,int b,Mat s) void Mat::FillSubMat(int a,int b,Mat s)
{ {
int x,y; int x,y;
for(x=0;x<s.m;x++) for(x=0;x<s.m;x++)
{ {
for(y=0;y<s.n;y++) for(y=0;y<s.n;y++)
{ {
mat[a+x][b+y]=s.mat[x][y]; mat[a+x][b+y]=s.mat[x][y];
} }
} }
} }
Mat operator *(double k, Mat a) Mat operator *(double k, Mat a)
{ {
Mat b(a.m,a.n,-1); Mat b(a.m,a.n,-1);
int x,y; int x,y;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<a.n;y++) for(y=0;y<a.n;y++)
{ {
b.mat[x][y]=k*a.mat[x][y]; b.mat[x][y]=k*a.mat[x][y];
} }
} }
return b; return b;
} }
Mat operator *(Mat a,double k) Mat operator *(Mat a,double k)
{ {
return k*a; return k*a;
} }
Mat operator /(Mat a,double k) Mat operator /(Mat a,double k)
{ {
return (1/k)*a; return (1/k)*a;
} }
Mat operator *(Mat a,Mat b) Mat operator *(Mat a,Mat b)
{ {
Mat c(a.m,b.n,-1); Mat c(a.m,b.n,-1);
int x,y,z; int x,y,z;
double s; double s;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<b.n;y++) for(y=0;y<b.n;y++)
{ {
s=0; s=0;
for(z=0;z<a.n;z++) for(z=0;z<a.n;z++)
{ {
s=s+a.mat[x][z]*b.mat[z][y]; s=s+a.mat[x][z]*b.mat[z][y];
} }
c.mat[x][y]=s; c.mat[x][y]=s;
} }
} }
return c; return c;
} }
Mat operator +(Mat a,Mat b) Mat operator +(Mat a,Mat b)
{ {
Mat c=a; Mat c=a;
int x,y; int x,y;
for(x=0;x<c.m;x++) for(x=0;x<c.m;x++)
{ {
for(y=0;y<c.n;y++) for(y=0;y<c.n;y++)
{ {
c.mat[x][y]+=b.mat[x][y]; c.mat[x][y]+=b.mat[x][y];
} }
} }
return c; return c;
} }
Mat operator -(Mat a,Mat b) Mat operator -(Mat a,Mat b)
{ {
Mat c=a; Mat c=a;
int x,y; int x,y;
for(x=0;x<c.m;x++) for(x=0;x<c.m;x++)
{ {
for(y=0;y<c.n;y++) for(y=0;y<c.n;y++)
{ {
c.mat[x][y]-=b.mat[x][y]; c.mat[x][y]-=b.mat[x][y];
} }
} }
return c; return c;
} }
Mat operator ~(Mat a) Mat operator ~(Mat a)
{ {
Mat b(a.n,a.m,-1); Mat b(a.n,a.m,-1);
int x,y; int x,y;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
for(y=0;y<a.n;y++) for(y=0;y<a.n;y++)
{ {
b.mat[y][x]=a.mat[x][y]; b.mat[y][x]=a.mat[x][y];
} }
} }
return b; return b;
} }
Mat operator /(Mat a,Mat b) Mat operator /(Mat a,Mat b)
{ {
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA> //<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
int x,xb; int x,xb;
for(x=0;x<b.n;x++) for(x=0;x<b.n;x++)
{ {
//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
for(xb=x;xb<b.n;xb++) for(xb=x;xb<b.n;xb++)
{ {
sxb=fabs(b.mat[x][xb]); sxb=fabs(b.mat[x][xb]);
if(sxb>s) if(sxb>s)
{ {
p=xb; p=xb;
s=sxb; s=sxb;
} }
} }
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(x!=p) if(x!=p)
{ {
a.ColExchange(x,p); a.ColExchange(x,p);
b.ColExchange(x,p); b.ColExchange(x,p);
} }
//<2F><>һ<EFBFBD>й<EFBFBD>һ //<2F><>һ<EFBFBD>й<EFBFBD>һ
double k=1/b.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double k=1/b.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
a.ColMul(x,k); a.ColMul(x,k);
b.ColMul(x,k); b.ColMul(x,k);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
for(xb=0;xb<b.n;xb++) for(xb=0;xb<b.n;xb++)
{ {
if(xb!=x) if(xb!=x)
{ {
k=(-b.mat[x][xb]); k=(-b.mat[x][xb]);
a.ColAdd(xb,x,k); a.ColAdd(xb,x,k);
b.ColAdd(xb,x,k); b.ColAdd(xb,x,k);
} }
} }
} }
return a; return a;
} }
Mat operator %(Mat a,Mat b) Mat operator %(Mat a,Mat b)
{ {
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA> //<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
int x,xb; int x,xb;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
//<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ѵ<EFBFBD><D1B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>ʼԪ<CABC><D4AA><EFBFBD><EFBFBD><EFBFBD>
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
for(xb=x;xb<a.m;xb++) for(xb=x;xb<a.m;xb++)
{ {
sxb=fabs(a.mat[xb][x]); sxb=fabs(a.mat[xb][x]);
if(sxb>s) if(sxb>s)
{ {
p=xb; p=xb;
s=sxb; s=sxb;
} }
} }
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(x!=p) if(x!=p)
{ {
a.RowExchange(x,p); a.RowExchange(x,p);
b.RowExchange(x,p); b.RowExchange(x,p);
} }
//<2F><>һ<EFBFBD>й<EFBFBD>һ //<2F><>һ<EFBFBD>й<EFBFBD>һ
double k=1/a.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double k=1/a.mat[x][x];//<2F><>һ<EFBFBD>䲻ҪǶ<D2AA>׵<EFBFBD><D7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>²<EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
a.RowMul(x,k); a.RowMul(x,k);
b.RowMul(x,k); b.RowMul(x,k);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD> //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
for(xb=0;xb<a.m;xb++) for(xb=0;xb<a.m;xb++)
{ {
if(xb!=x) if(xb!=x)
{ {
k=(-a.mat[xb][x]); k=(-a.mat[xb][x]);
a.RowAdd(xb,x,k); a.RowAdd(xb,x,k);
b.RowAdd(xb,x,k); b.RowAdd(xb,x,k);
} }
} }
} }
return b; return b;
} }
void Mat::RowExchange(int a, int b) void Mat::RowExchange(int a, int b)
{ {
double s[MAT_MAX]; double s[MAT_MAX];
int ncpy=n*sizeof(double); int ncpy=n*sizeof(double);
memcpy(s,mat[a],ncpy); memcpy(s,mat[a],ncpy);
memcpy(mat[a],mat[b],ncpy); memcpy(mat[a],mat[b],ncpy);
memcpy(mat[b],s,ncpy); memcpy(mat[b],s,ncpy);
} }
void Mat::RowMul(int a,double k) void Mat::RowMul(int a,double k)
{ {
int y; int y;
for(y=0;y<n;y++) for(y=0;y<n;y++)
{ {
mat[a][y]= mat[a][y]*k; mat[a][y]= mat[a][y]*k;
} }
} }
void Mat::RowAdd(int a,int b,double k) void Mat::RowAdd(int a,int b,double k)
{ {
int y; int y;
for(y=0;y<n;y++) for(y=0;y<n;y++)
{ {
mat[a][y]= mat[a][y]+ mat[b][y]*k; mat[a][y]= mat[a][y]+ mat[b][y]*k;
} }
} }
void Mat::ColExchange(int a, int b) void Mat::ColExchange(int a, int b)
{ {
double s; double s;
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
s=mat[x][a]; s=mat[x][a];
mat[x][a]=mat[x][b]; mat[x][a]=mat[x][b];
mat[x][b]=s; mat[x][b]=s;
} }
} }
void Mat::ColMul(int a,double k) void Mat::ColMul(int a,double k)
{ {
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
mat[x][a]=mat[x][a]*k; mat[x][a]=mat[x][a]*k;
} }
} }
void Mat::ColAdd(int a,int b,double k) void Mat::ColAdd(int a,int b,double k)
{ {
int x; int x;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
mat[x][a]=mat[x][a]+mat[x][b]*k; mat[x][a]=mat[x][a]+mat[x][b]*k;
} }
} }
double Mat::Sqrt() double Mat::Sqrt()
{ {
int x; int x;
double numx; double numx;
double s=0; double s=0;
for(x=0;x<m;x++) for(x=0;x<m;x++)
{ {
numx=mat[x][0]; numx=mat[x][0];
s+=(numx*numx); s+=(numx*numx);
} }
return s; return s;
} }
double Mat::absvec() double Mat::absvec()
{ {
return sqrt(Sqrt()); return sqrt(Sqrt());
} }
Mat operator ^(Mat a, Mat b) Mat operator ^(Mat a, Mat b)
{ {
double ax=a.mat[0][0]; double ax=a.mat[0][0];
double ay=a.mat[1][0]; double ay=a.mat[1][0];
double az=a.mat[2][0]; double az=a.mat[2][0];
double bx=b.mat[0][0]; double bx=b.mat[0][0];
double by=b.mat[1][0]; double by=b.mat[1][0];
double bz=b.mat[2][0]; double bz=b.mat[2][0];
Mat c(3,1,-1); Mat c(3,1,-1);
c.mat[0][0]=ay*bz-az*by; c.mat[0][0]=ay*bz-az*by;
c.mat[1][0]=(-(ax*bz-az*bx)); c.mat[1][0]=(-(ax*bz-az*bx));
c.mat[2][0]=ax*by-ay*bx; c.mat[2][0]=ax*by-ay*bx;
return c; return c;
} }

188
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp
View File

@ -1,94 +1,94 @@
#include "align.h" #include "align.h"
namespace uwb_slam{ namespace uwb_slam{
void Align::Run() void Align::Run()
{ {
imuDataRxTime = ros::Time::now(); imuDataRxTime = ros::Time::now();
uwbDataRxTime = ros::Time::now(); uwbDataRxTime = ros::Time::now();
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this); wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this); imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this); odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
std::ofstream outfile("data.txt",std::ofstream::out); std::ofstream outfile("data.txt",std::ofstream::out);
if(outfile.is_open()) { if(outfile.is_open()) {
std::cout << "start saving data" << std::endl; std::cout << "start saving data" << std::endl;
outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\ outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
<< "aX,aY,aZ,"\ << "aX,aY,aZ,"\
<< "gX,gY,gZ"\ << "gX,gY,gZ"\
<< "mX,mY,mZ,"\ << "mX,mY,mZ,"\
<< "qW,qX,qY,qZ,"\ << "qW,qX,qY,qZ,"\
<< "vXY,angleV,"\ << "vXY,angleV,"\
<< "imuPosX,imuPosY,"\ << "imuPosX,imuPosY,"\
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"\ << "lightHousePosX,lightHousePosY,lightHousePosZ,"\
<< "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\ << "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
<< "d1,d2,d3\n"; << "d1,d2,d3\n";
} else { } else {
std::cout<<"file can not open"<<std::endl; std::cout<<"file can not open"<<std::endl;
} }
while(1){ while(1){
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){ if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
//std::cout << "imu received" << std::endl; //std::cout << "imu received" << std::endl;
imuDataRxTime = imu_odom_.imu_data_.imu_t_; imuDataRxTime = imu_odom_.imu_data_.imu_t_;
odomDataRxTime = odom_tmp_; odomDataRxTime = odom_tmp_;
uwbDataRxTime = uwb_->uwb_data_.uwb_t_; uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\ outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
<< imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\ << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\
<< imu_odom_.imu_data_.g_[0] << "," << imu_odom_.imu_data_.g_[1] << "," << imu_odom_.imu_data_.g_[2] << ","\ << imu_odom_.imu_data_.g_[0] << "," << imu_odom_.imu_data_.g_[1] << "," << imu_odom_.imu_data_.g_[2] << ","\
<< imu_odom_.imu_data_.m_[0] << "," << imu_odom_.imu_data_.m_[1] << "," << imu_odom_.imu_data_.m_[2] << ","\ << imu_odom_.imu_data_.m_[0] << "," << imu_odom_.imu_data_.m_[1] << "," << imu_odom_.imu_data_.m_[2] << ","\
<< imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\ << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
<< imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\ << imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
<< imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\ << imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\ << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
<< lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\ << lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n"; << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
} }
} }
// outfile.close(); // outfile.close();
// std::cout<< "Data written to file." << std::endl; // std::cout<< "Data written to file." << std::endl;
} }
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom) void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
{ {
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x; imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z; imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
return; return;
} }
void Align::imuCB(const pibot_msgs::RawImu& imu) void Align::imuCB(const pibot_msgs::RawImu& imu)
{ {
imu_odom_.imu_data_.imu_t_ = imu.header.stamp; imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x; imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y; imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z; imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;
imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x; imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x;
imu_odom_.imu_data_.g_[1] = imu.raw_angular_velocity.y; imu_odom_.imu_data_.g_[1] = imu.raw_angular_velocity.y;
imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z; imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z;
imu_odom_.imu_data_.m_[0] = imu.raw_magnetic_field.x; imu_odom_.imu_data_.m_[0] = imu.raw_magnetic_field.x;
imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y; imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y;
imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z; imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
return; return;
} }
void Align::odomCB(const nav_msgs::Odometry& odom) void Align::odomCB(const nav_msgs::Odometry& odom)
{ {
odom_tmp_ = odom.header.stamp; odom_tmp_ = odom.header.stamp;
imu_odom_.pose_[0] = odom.pose.pose.position.x; imu_odom_.pose_[0] = odom.pose.pose.position.x;
imu_odom_.pose_[1] = odom.pose.pose.position.y; imu_odom_.pose_[1] = odom.pose.pose.position.y;
imu_odom_.pose_[2] = odom.pose.pose.position.z; imu_odom_.pose_[2] = odom.pose.pose.position.z;
imu_odom_.quat_[0] = odom.pose.pose.orientation.w; imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
imu_odom_.quat_[1] = odom.pose.pose.orientation.x; imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
imu_odom_.quat_[2] = odom.pose.pose.orientation.y; imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
imu_odom_.quat_[3] = odom.pose.pose.orientation.z; imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
} }
}; };

156
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp
View File

@ -1,79 +1,79 @@
#include "lighthouse.h" #include "lighthouse.h"
#include <cmath> #include <cmath>
#include <string> #include <string>
namespace uwb_slam{ namespace uwb_slam{
Lighthouse::Lighthouse(){ Lighthouse::Lighthouse(){
std::cout << "Start Run. " << std::endl; std::cout << "Start Run. " << std::endl;
// 创建UDP套接字 // 创建UDP套接字
UdpSocket = socket(AF_INET, SOCK_DGRAM, 0); UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
if (UdpSocket == -1) { if (UdpSocket == -1) {
std::cerr << "Error creating UDP socket." << std::endl; std::cerr << "Error creating UDP socket." << std::endl;
return; return;
} }
std::cout << "Creating UDP socket sucess. " << std::endl; std::cout << "Creating UDP socket sucess. " << std::endl;
// 设置套接字地址结构 // 设置套接字地址结构
sockaddr_in udpAddress; sockaddr_in udpAddress;
std::memset(&udpAddress, 0, sizeof(udpAddress)); std::memset(&udpAddress, 0, sizeof(udpAddress));
udpAddress.sin_family = AF_INET; udpAddress.sin_family = AF_INET;
udpAddress.sin_addr.s_addr = htonl(INADDR_ANY); udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
udpAddress.sin_port = htons(PORT); udpAddress.sin_port = htons(PORT);
// 将套接字绑定到地址 // 将套接字绑定到地址
if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) { if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
std::cerr << "Error binding UDP socket." << std::endl; std::cerr << "Error binding UDP socket." << std::endl;
close(UdpSocket); close(UdpSocket);
} }
} }
Lighthouse::~Lighthouse(){ Lighthouse::~Lighthouse(){
close(UdpSocket); close(UdpSocket);
} }
void Lighthouse::Run() { void Lighthouse::Run() {
while(1){ while(1){
// 这是一个阻塞线程 // 这是一个阻塞线程
this->UDPRead(); this->UDPRead();
} }
} }
void Lighthouse::UDPRead(){ void Lighthouse::UDPRead(){
char buffer[1024]; char buffer[1024];
ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr); ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
if (bytesRead == -1) { if (bytesRead == -1) {
std::cerr << "Error receiving data." << std::endl; std::cerr << "Error receiving data." << std::endl;
return; return;
} }
std::string data(buffer); std::string data(buffer);
float x,y,z,qw,qx,qy,qz; float x,y,z,qw,qx,qy,qz;
qw = 1.0; qw = 1.0;
sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz); sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
mMutexLighthouse.lock(); mMutexLighthouse.lock();
this->data.x_ = x; this->data.x_ = x;
this->data.y_ = y; this->data.y_ = y;
this->data.z_ = z; this->data.z_ = z;
this->data.qw_ = qw; this->data.qw_ = qw;
this->data.qx_ = qx; this->data.qx_ = qx;
this->data.qy_ = qy; this->data.qy_ = qy;
this->data.qz_ = qz; this->data.qz_ = qz;
mMutexLighthouse.unlock(); mMutexLighthouse.unlock();
// 打印接收到的消息 // 打印接收到的消息
buffer[bytesRead] = '\0'; buffer[bytesRead] = '\0';
//std::cout << "Received: " << buffer << std::endl; //std::cout << "Received: " << buffer << std::endl;
} }
}; };

120
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp
View File

@ -1,60 +1,60 @@
#include "Mat.h" #include "Mat.h"
// #include "align.h" // #include "align.h"
#include "mapping.h" #include "mapping.h"
#include <iostream> #include <iostream>
#include <ros/ros.h> #include <ros/ros.h>
#include <thread> #include <thread>
#include "senddata.h" #include "senddata.h"
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>(); std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>(); std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>(); std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>(); std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>(); std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
mp->uwb_ = uwb; mp->uwb_ = uwb;
mp->align_ = align; mp->align_ = align;
align->uwb_ =uwb; align->uwb_ =uwb;
align->lighthouse_ = lighthouse; align->lighthouse_ = lighthouse;
sender->uwb_ = uwb; sender->uwb_ = uwb;
// // control data fllow in system // // control data fllow in system
// std::thread rose_trd ([&system]() { // std::thread rose_trd ([&system]() {
// system->Run(); // system->Run();
// }); // });
// uwb serried read // uwb serried read
std::thread uwb_trd([&uwb]() { std::thread uwb_trd([&uwb]() {
uwb->Run(); uwb->Run();
}); });
// build map // build map
// std::thread map_trd ([&mp]() { // std::thread map_trd ([&mp]() {
// mp->Run(); // mp->Run();
// }); // });
std::thread sender_trd ([&sender, uwb]() { std::thread sender_trd ([&sender, uwb]() {
sender->Run(uwb); sender->Run(uwb);
}); });
// std::thread align_trd ([&align]() { // std::thread align_trd ([&align]() {
// align->Run(); // align->Run();
// }); // });
// std::thread lighthouse_trd ([&lighthouse]() { // std::thread lighthouse_trd ([&lighthouse]() {
// lighthouse->Run(); // lighthouse->Run();
// }); // });
ros::spin(); ros::spin();
} }

298
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp
View File

@ -1,149 +1,149 @@
#include "mapping.h" #include "mapping.h"
#include <mutex> #include <mutex>
#include <unistd.h>// 包含 usleep() 函数所在的头文件 #include <unistd.h>// 包含 usleep() 函数所在的头文件
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
namespace uwb_slam namespace uwb_slam
{ {
bool Mapping::checkQueue() bool Mapping::checkQueue()
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
return !posDataQueue.empty(); return !posDataQueue.empty();
} }
void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData) void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
posDataQueue.push({imuPosData, uwbPosData, syncPosData}); posDataQueue.push({imuPosData, uwbPosData, syncPosData});
} }
void Mapping::process() void Mapping::process()
{ {
{ {
//std::unique_lock<std::mutex> lock(mMutexMap); //std::unique_lock<std::mutex> lock(mMutexMap);
//std::cout << "SIZE: " <<posDataQueue.size() << std::endl; //std::cout << "SIZE: " <<posDataQueue.size() << std::endl;
posData = posDataQueue.front(); posData = posDataQueue.front();
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl; //std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
posDataQueue.pop(); posDataQueue.pop();
} }
/*生成图*/ /*生成图*/
int imuPosPointX = posData[0].x / PIXEL_SCALE + ( fmod(posData[0].x ,PIXEL_SCALE) != 0) + realWidth / 2; int imuPosPointX = posData[0].x / PIXEL_SCALE + ( fmod(posData[0].x ,PIXEL_SCALE) != 0) + realWidth / 2;
int imuPosPointY = posData[0].y / PIXEL_SCALE + ( fmod(posData[0].y ,PIXEL_SCALE) != 0) + realHeight / 2; int imuPosPointY = posData[0].y / PIXEL_SCALE + ( fmod(posData[0].y ,PIXEL_SCALE) != 0) + realHeight / 2;
int uwbPosPointX = posData[1].x / PIXEL_SCALE + ( fmod(posData[1].x ,PIXEL_SCALE) != 0) + realWidth / 2; int uwbPosPointX = posData[1].x / PIXEL_SCALE + ( fmod(posData[1].x ,PIXEL_SCALE) != 0) + realWidth / 2;
int uwbPosPointY = posData[1].y / PIXEL_SCALE + ( fmod(posData[1].y ,PIXEL_SCALE) != 0) + realHeight / 2; int uwbPosPointY = posData[1].y / PIXEL_SCALE + ( fmod(posData[1].y ,PIXEL_SCALE) != 0) + realHeight / 2;
int syncPosPointX = posData[2].x / PIXEL_SCALE + ( fmod(posData[2].x ,PIXEL_SCALE) != 0) + realWidth / 2; int syncPosPointX = posData[2].x / PIXEL_SCALE + ( fmod(posData[2].x ,PIXEL_SCALE) != 0) + realWidth / 2;
int syncPosPointY = posData[2].y / PIXEL_SCALE + ( fmod(posData[2].y ,PIXEL_SCALE) != 0) + realHeight / 2; int syncPosPointY = posData[2].y / PIXEL_SCALE + ( fmod(posData[2].y ,PIXEL_SCALE) != 0) + realHeight / 2;
// std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y; // std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
// std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x; // std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
// std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl; // std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
// std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY; // std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
// std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY; // std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
// std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl; // std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
// img.at<unsigned char>(imuPosPointY, imuPosPointX) = 0; // img.at<unsigned char>(imuPosPointY, imuPosPointX) = 0;
img.at<cv::Vec3b>(imuPosPointY, imuPosPointX) = cv::Vec3b(0,0,255); //imu trace is red img.at<cv::Vec3b>(imuPosPointY, imuPosPointX) = cv::Vec3b(0,0,255); //imu trace is red
img.at<cv::Vec3b>(uwbPosPointY, uwbPosPointX) = cv::Vec3b(0,255,0); //uwb trace is green img.at<cv::Vec3b>(uwbPosPointY, uwbPosPointX) = cv::Vec3b(0,255,0); //uwb trace is green
img.at<cv::Vec3b>(syncPosPointY, syncPosPointX) = cv::Vec3b(255,0,0); //sync trace is blue img.at<cv::Vec3b>(syncPosPointY, syncPosPointX) = cv::Vec3b(255,0,0); //sync trace is blue
} }
void Mapping::Run() void Mapping::Run()
{ {
//int key = cv::waitKey(0);//等待用户按下按键 //int key = cv::waitKey(0);//等待用户按下按键
//std::cout << key << std::endl; //std::cout << key << std::endl;
realWidth = AREA_SIZE / PIXEL_SCALE; realWidth = AREA_SIZE / PIXEL_SCALE;
realHeight = AREA_SIZE / PIXEL_SCALE; realHeight = AREA_SIZE / PIXEL_SCALE;
img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255)); img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
//cankao //cankao
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10) for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10) for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
for (int j=realHeight/2-1; j<=realHeight/2+1; j+=1) for (int j=realHeight/2-1; j<=realHeight/2+1; j+=1)
for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1) for (int i=realWidth/2-1; i<=realWidth/2+1; i+=1)
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
int i = 0, j = 0; int i = 0, j = 0;
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0); img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
cv::imshow("Image",img); cv::imshow("Image",img);
// bool printFlag = 0; // bool printFlag = 0;
// std::ofstream outfile("data.txt",std::ofstream::out); // std::ofstream outfile("data.txt",std::ofstream::out);
// if(outfile.is_open()) { // if(outfile.is_open()) {
// std::cout << "start saving data" << key << std::endl; // std::cout << "start saving data" << key << std::endl;
// printFlag = 1; // printFlag = 1;
// } else { // } else {
// std::cout<<"file can not open"<<std::endl; // std::cout<<"file can not open"<<std::endl;
// } // }
/* /*
std::cout << "waiting" <<std::endl; std::cout << "waiting" <<std::endl;
int key = cv::waitKey(0); int key = cv::waitKey(0);
if (key == 'q' || key == 27) { if (key == 'q' || key == 27) {
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y)); this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
readPos = true; readPos = true;
std::cout << "non" << key << std::endl; std::cout << "non" << key << std::endl;
cv::destroyAllWindows(); cv::destroyAllWindows();
} }
*/ */
while(1){ while(1){
int key = cv::waitKey(0); int key = cv::waitKey(0);
if (key == 'q' ) { if (key == 'q' ) {
readPos = true; readPos = true;
std::cout << "start mapping" << key << std::endl; std::cout << "start mapping" << key << std::endl;
//cv::destroyAllWindows(); //cv::destroyAllWindows();
} }
while( readPos )//按下空格键 while( readPos )//按下空格键
{ {
int key2 = cv::waitKey(1); int key2 = cv::waitKey(1);
if (key2 == 'q') { if (key2 == 'q') {
//TODO: save //TODO: save
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径 std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径
cv::imwrite(pngimage,img); cv::imwrite(pngimage,img);
readPos = false; readPos = false;
// outfile.close(); // outfile.close();
// printFlag = 0; // printFlag = 0;
// std::cout<< "Data written to file." << std::endl; // std::cout<< "Data written to file." << std::endl;
break; break;
} }
//this->feedPos(cv::Point2d(align_->imuPos.mat[0][0], align_->imuPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0])); //this->feedPos(cv::Point2d(align_->imuPos.mat[0][0], align_->imuPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
//this->feedPos(cv::Point2d(uwb_->x, uwb_->y)); //this->feedPos(cv::Point2d(uwb_->x, uwb_->y));
//uwb xieru //uwb xieru
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl; //std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
if(checkQueue()) if(checkQueue())
{ {
//std::cout << " start process" << std::endl; //std::cout << " start process" << std::endl;
process(); process();
//std::cout << " end process" << std::endl; //std::cout << " end process" << std::endl;
} }
} }
// std::cout << "out" << key << std::endl; // std::cout << "out" << key << std::endl;
} }
//std::string pngimage="../Map/pngimage.png";//保存的图片文件路径 //std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
//cv::imwrite(pngimage,img); //cv::imwrite(pngimage,img);
/*ros 发送图片给导航 */ /*ros 发送图片给导航 */
} }
} // namespace uwb_slam } // namespace uwb_slam

292
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp
View File

@ -1,147 +1,147 @@
#include "senddata.h" #include "senddata.h"
#define angleLimit 20 #define angleLimit 20
#define disLimit 200 #define disLimit 200
namespace uwb_slam namespace uwb_slam
{ {
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){ void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
// position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50); // position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50); cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1)); obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1));
// odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); // odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
while(ros::ok()){ while(ros::ok()){
// publishOdometry(uwb); // publishOdometry(uwb);
if (flag_) if (flag_)
{ {
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = -1; cmd_vel_.angular.z = -1;
for (int i = 0; i < 17; ++i) { for (int i = 0; i < 17; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0.2; cmd_vel_.linear.x = 0.2;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
for (int i = 0; i < 20; ++i) { for (int i = 0; i < 20; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = 1; cmd_vel_.angular.z = 1;
for (int i = 0; i < 16; ++i) { for (int i = 0; i < 16; ++i) {
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
loop_rate.sleep(); loop_rate.sleep();
} }
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
flag_ = 0; flag_ = 0;
} }
else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000) else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180) else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
{ {
float angularSpeed = (float)uwb->aoa[0] / 100 + 1; float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
cmd_vel_.angular.z = angularSpeed; cmd_vel_.angular.z = angularSpeed;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->aoa[0] < -angleLimit) else if(uwb->aoa[0] < -angleLimit)
{ {
float angularSpeed = (float)uwb->aoa[0] / 100 - 1; float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
cmd_vel_.angular.z = angularSpeed; cmd_vel_.angular.z = angularSpeed;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
else if(uwb->d[0] > disLimit) else if(uwb->d[0] > disLimit)
{ {
float lineSpeed = (float)uwb->d[0] / 1000 + 0.1; float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = lineSpeed; cmd_vel_.linear.x = lineSpeed;
} }
else if(uwb->d[0] < disLimit - 30) else if(uwb->d[0] < disLimit - 30)
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = -0.2; cmd_vel_.linear.x = -0.2;
} }
else else
{ {
cmd_vel_.angular.z = 0; cmd_vel_.angular.z = 0;
cmd_vel_.linear.x = 0; cmd_vel_.linear.x = 0;
} }
cmd_vel_pub_.publish(cmd_vel_); cmd_vel_pub_.publish(cmd_vel_);
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
} }
// void Senddata::odomCB(const nav_msgs::Odometry& odom){ // void Senddata::odomCB(const nav_msgs::Odometry& odom){
// sub_odom_ = odom; // sub_odom_ = odom;
// return; // return;
// } // }
void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo) void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
{ {
for (const auto& obstacle_info :stereo->dis) for (const auto& obstacle_info :stereo->dis)
{ {
float distance = obstacle_info.distance; float distance = obstacle_info.distance;
// float width = obstacle_info.width; // float width = obstacle_info.width;
// float height = obstacle_info.height; // float height = obstacle_info.height;
// float angle = obstacle_info.angle; // float angle = obstacle_info.angle;
// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0) // if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
if(distance>5&&distance<45) if(distance>5&&distance<45)
{ {
flag_ = 1; flag_ = 1;
} }
} }
return; return;
} }
// void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb) // void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
// { // {
// std::mutex mMutexSend; // std::mutex mMutexSend;
// ros::Time current_time = ros::Time::now(); // ros::Time current_time = ros::Time::now();
// // 设置 Odometry 消息的头部信息 // // 设置 Odometry 消息的头部信息
// odom_.header.stamp = current_time; // odom_.header.stamp = current_time;
// odom_.header.frame_id = "odom"; // 设置坐标系为 "map" // odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
// odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link" // odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
// // 填充 Odometry 消息的位置信息 // // 填充 Odometry 消息的位置信息
// odom_.pose.pose.position.x = uwb->uwb_data_.x_; // odom_.pose.pose.position.x = uwb->uwb_data_.x_;
// odom_.pose.pose.position.y = uwb->uwb_data_.y_; // odom_.pose.pose.position.y = uwb->uwb_data_.y_;
// odom_.pose.pose.position.z = 0.0; // odom_.pose.pose.position.z = 0.0;
// // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态） // // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
// // tf2::Quaternion quat; // // tf2::Quaternion quat;
// // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0 // // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
// // odom.pose.pose.orientation.x = quat.x(); // // odom.pose.pose.orientation.x = quat.x();
// // odom.pose.pose.orientation.y = quat.y(); // // odom.pose.pose.orientation.y = quat.y();
// // odom.pose.pose.orientation.z = quat.z(); // // odom.pose.pose.orientation.z = quat.z();
// // odom.pose.pose.orientation.w = quat.w(); // // odom.pose.pose.orientation.w = quat.w();
// odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x; // odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
// odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y; // odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
// odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z; // odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
// odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w; // odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
// // 发布 Odometry 消息 // // 发布 Odometry 消息
// position_pub_.publish(odom_); // position_pub_.publish(odom_);
// } // }
} // namespace uwb_slam } // namespace uwb_slam

242
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp
View File

@ -1,121 +1,121 @@
#include "uwb.h" #include "uwb.h"
#include <cmath> #include <cmath>
#include "Mat.h" #include "Mat.h"
#define CARHEIGHT 20 #define CARHEIGHT 20
#define DISINDEX 3 #define DISINDEX 3
#define AOAINDEX 15 #define AOAINDEX 15
#define DATALENGTH 27 #define DATALENGTH 27
namespace uwb_slam{ namespace uwb_slam{
Uwb::Uwb(){ Uwb::Uwb(){
} }
void Uwb::Run() { void Uwb::Run() {
while(1){ while(1){
this->Serread(); this->Serread();
} }
} }
void Uwb::Serread(){ void Uwb::Serread(){
try { try {
boost::asio::io_service io; boost::asio::io_service io;
boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径 boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径
serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率 serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位 serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位 serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位 serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
uint8_t tmpdata[DATALENGTH]; uint8_t tmpdata[DATALENGTH];
// std::cerr << "befor read" << std::endl; // std::cerr << "befor read" << std::endl;
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据 size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
// std::cerr << "after read" << std::endl; // std::cerr << "after read" << std::endl;
this->uwb_data_.uwb_t_ = ros::Time::now(); this->uwb_data_.uwb_t_ = ros::Time::now();
for (int i=0;i< bytesRead;i++) for (int i=0;i< bytesRead;i++)
{ {
std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " "; std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
} }
std::cout << std::endl; std::cout << std::endl;
memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0])); memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0]));
memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], sizeof(d[0])); memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], sizeof(d[0]));
memcpy(&this->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0])); memcpy(&this->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0]));
memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[0])); memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[0]));
memcpy(&this->aoa[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0])); memcpy(&this->aoa[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0]));
memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], sizeof(aoa[0])); memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], sizeof(aoa[0]));
//std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl; //std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
// if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) { // if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
// return; // return;
// } // }
// for(int i=0; i<3; i++) // for(int i=0; i<3; i++)
// { // {
// this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT)); // this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
// } // }
std::cout<<"d: "<<this->d[0]<<" aoa: "<<this->aoa[0]<<std::endl; std::cout<<"d: "<<this->d[0]<<" aoa: "<<this->aoa[0]<<std::endl;
d[0] = ((((-1.4229e-07 * d[0]) + 1.8784e-04) * d[0]) + 0.9112) * d[0] + 2.4464; d[0] = ((((-1.4229e-07 * d[0]) + 1.8784e-04) * d[0]) + 0.9112) * d[0] + 2.4464;
// d[1] = ((((-2.3004e-07 * d[1]) + 3.2942e-04) * d[1]) + 0.8385) * d[1] + 6.2770; // d[1] = ((((-2.3004e-07 * d[1]) + 3.2942e-04) * d[1]) + 0.8385) * d[1] + 6.2770;
// d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566; // d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566;
} catch (const std::exception& ex) { } catch (const std::exception& ex) {
std::cerr << "Exception: " << ex.what() << std::endl; std::cerr << "Exception: " << ex.what() << std::endl;
} }
} }
void fusion() void fusion()
{ {
} }
}; };
// bool Uwb::checknewdata() // bool Uwb::checknewdata()
// { // {
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// return !v_buffer_imu_odom_pose_data_.empty(); // return !v_buffer_imu_odom_pose_data_.empty();
// } // }
// void Uwb::Run() { // void Uwb::Run() {
// while(1) // while(1)
// { // {
// if(checknewdata()) // if(checknewdata())
// { // {
// { // {
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front(); // Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
// v_buffer_imu_odom_pose_data_.pop(); // v_buffer_imu_odom_pose_data_.pop();
// } // }
// } // }
// } // }
// } // }
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){ // void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
// std::unique_lock<std::mutex> lock(mMutexUwb); // std::unique_lock<std::mutex> lock(mMutexUwb);
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data); // v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
// } // }

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/gripper_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/one_side_gripper_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_action_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/setup.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/__init__.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/arbotix.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/ax12.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/controllers.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/diff_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/follow_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/io.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/joints.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/linear_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/publishers.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/servo_controller.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/ir_ranger.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/max_sonar.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/setup.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/__init__.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/sensors.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_angular.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_linear.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/get_pos_demo.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/launch_demo.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/marker_server.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_goal.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_multi_goals.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/teleop_twist_keyboard.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/transform_utils.py Executable file → Normal file
View File

54
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_3dsensor/config/astra_camera.yaml
View File

@ -1,27 +1,27 @@
image_width: 640 image_width: 640
image_height: 480 image_height: 480
camera_name: camera camera_name: camera
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [577.54679, 0. , 310.24326, data: [577.54679, 0. , 310.24326,
0. , 578.63325, 253.65539, 0. , 578.63325, 253.65539,
0. , 0. , 1. ] 0. , 0. , 1. ]
camera_model: plumb_bob camera_model: plumb_bob
distortion_model: plumb_bob distortion_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [0.125197, -0.196591, 0.006816, -0.006225, 0.000000] data: [0.125197, -0.196591, 0.006816, -0.006225, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [1., 0., 0., data: [1., 0., 0.,
0., 1., 0., 0., 1., 0.,
0., 0., 1.] 0., 0., 1.]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [590.55457, 0. , 306.57339, 0. , data: [590.55457, 0. , 306.57339, 0. ,
0. , 592.83978, 256.43008, 0. , 0. , 592.83978, 256.43008, 0. ,
0. , 0. , 1. , 0. ] 0. , 0. , 1. , 0. ]

40
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_3dsensor/config/astra_depth.yaml
View File

@ -1,20 +1,20 @@
image_width: 640 image_width: 640
image_height: 480 image_height: 480
camera_name: depth_Astra_Orbbec camera_name: depth_Astra_Orbbec
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [582.795354, 0.000000, 326.415982, 0.000000, 584.395006, 249.989410, 0.000000, 0.000000, 1.000000] data: [582.795354, 0.000000, 326.415982, 0.000000, 584.395006, 249.989410, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob distortion_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [-0.068613, 0.174404, 0.001015, 0.006240, 0.000000] data: [-0.068613, 0.174404, 0.001015, 0.006240, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [586.186035, 0.000000, 329.702427, 0.000000, 0.000000, 590.631409, 250.167765, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] data: [586.186035, 0.000000, 329.702427, 0.000000, 0.000000, 590.631409, 250.167765, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

62
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/include/pibot_bringup/simple_dataframe_master.h
View File

@ -1,32 +1,32 @@
#ifndef PIBOT_SIMPLE_DATAFRAME_MASTER_H_ #ifndef PIBOT_SIMPLE_DATAFRAME_MASTER_H_
#define PIBOT_SIMPLE_DATAFRAME_MASTER_H_ #define PIBOT_SIMPLE_DATAFRAME_MASTER_H_
#include "simple_dataframe.h" #include "simple_dataframe.h"
#include <string.h> #include <string.h>
class Transport; class Transport;
class Simple_dataframe : public Dataframe class Simple_dataframe : public Dataframe
{ {
public: public:
Simple_dataframe(Transport* trans=0); Simple_dataframe(Transport* trans=0);
~Simple_dataframe(); ~Simple_dataframe();
void register_notify(const MESSAGE_ID id, Notify* _nf) { void register_notify(const MESSAGE_ID id, Notify* _nf) {
} }
bool data_recv(unsigned char c); bool data_recv(unsigned char c);
bool data_parse(); bool data_parse();
bool init(); bool init();
bool interact(const MESSAGE_ID id); bool interact(const MESSAGE_ID id);
private: private:
bool recv_proc(); bool recv_proc();
private: private:
bool send_message(const MESSAGE_ID id); bool send_message(const MESSAGE_ID id);
bool send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len); bool send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len);
bool send_message(Message* msg); bool send_message(Message* msg);
private: private:
Message active_rx_msg; Message active_rx_msg;
RECEIVE_STATE recv_state; RECEIVE_STATE recv_state;
Transport* trans; Transport* trans;
}; };
#endif #endif

60
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/include/pibot_bringup/transport.h
View File

@ -1,30 +1,30 @@
#ifndef TRANSPORT_H_ #ifndef TRANSPORT_H_
#define TRANSPORT_H_ #define TRANSPORT_H_
#include <iostream> #include <iostream>
#include <inttypes.h> #include <inttypes.h>
#include <vector> #include <vector>
#include <deque> #include <deque>
#include <queue> #include <queue>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/smart_ptr.hpp> #include <boost/smart_ptr.hpp>
#include <boost/thread.hpp> #include <boost/thread.hpp>
typedef std::vector<uint8_t> Buffer; typedef std::vector<uint8_t> Buffer;
class Transport class Transport
{ {
public: public:
virtual ~Transport(){} virtual ~Transport(){}
virtual bool init()=0; virtual bool init()=0;
virtual void set_timeout(int t)=0; virtual void set_timeout(int t)=0;
virtual bool is_timeout()=0; virtual bool is_timeout()=0;
virtual Buffer read() = 0; virtual Buffer read() = 0;
virtual void write(Buffer &data) = 0; virtual void write(Buffer &data) = 0;
}; };
#endif /* TRANSPORT_BASE_H_ */ #endif /* TRANSPORT_BASE_H_ */

14
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/view_model.launch
View File

@ -1,7 +1,7 @@
<launch> <launch>
<node <node
name="rviz" name="rviz"
pkg="rviz" pkg="rviz"
type="rviz" type="rviz"
args="-d $(find pibot_description)/urdf.rviz" /> args="-d $(find pibot_description)/urdf.rviz" />
</launch> </launch>

0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py Executable file → Normal file
View File

38
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/launch/gazebo.launch
View File

@ -1,20 +1,20 @@
<launch> <launch>
<include <include
file="$(find gazebo_ros)/launch/empty_world.launch" /> file="$(find gazebo_ros)/launch/empty_world.launch" />
<node <node
name="tf_footprint_base" name="tf_footprint_base"
pkg="tf" pkg="tf"
type="static_transform_publisher" type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" /> args="0 0 0 0 0 0 base_link base_footprint 40" />
<node <node
name="spawn_model" name="spawn_model"
pkg="gazebo_ros" pkg="gazebo_ros"
type="spawn_model" type="spawn_model"
args="-file $(find pi_bot.SLDASM)/robots/pi_bot.SLDASM.urdf -urdf -model pi_bot.SLDASM" args="-file $(find pi_bot.SLDASM)/robots/pi_bot.SLDASM.urdf -urdf -model pi_bot.SLDASM"
output="screen" /> output="screen" />
<node <node
name="fake_joint_calibration" name="fake_joint_calibration"
pkg="rostopic" pkg="rostopic"
type="rostopic" type="rostopic"
args="pub /calibrated std_msgs/Bool true" /> args="pub /calibrated std_msgs/Bool true" />
</launch> </launch>

10
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/manifest.xml
View File

@ -1,6 +1,6 @@
<package> <package>
<description brief="pi_bot.SLDASM">pi_bot.SLDASM</description> <description brief="pi_bot.SLDASM">pi_bot.SLDASM</description>
<depend package="gazebo" /> <depend package="gazebo" />
<author>me</author> <author>me</author>
<license>BSD</license> <license>BSD</license>
</package> </package>

116
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/apollo.urdf.xacro
View File

@ -1,58 +1,58 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
<xacro:macro name="base"> <xacro:macro name="base">
<!-- Chassis a.k.a base_link --> <!-- Chassis a.k.a base_link -->
<link name="base_link"> <link name="base_link">
<visual name="visual_base"> <visual name="visual_base">
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/apollo_base.stl"/> <mesh filename="package://pibot_description/meshes/base/apollo_base.stl"/>
<!-- <mesh filename="file://home/firefly/pibot_ros/ros_ws/src/pibot_description/meshes/base/apollo_base.stl"/> --> <!-- <mesh filename="file://home/firefly/pibot_ros/ros_ws/src/pibot_description/meshes/base/apollo_base.stl"/> -->
</geometry> </geometry>
<material name="material_black"/> <material name="material_black"/>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<!-- <mesh filename="file://home/firefly/pibot_ros/ros_ws/src/pibot_description/meshes/base/apollo_base.stl"/> --> <!-- <mesh filename="file://home/firefly/pibot_ros/ros_ws/src/pibot_description/meshes/base/apollo_base.stl"/> -->
<mesh filename="package://pibot_description/meshes/base/apollo_base.stl"/> <mesh filename="package://pibot_description/meshes/base/apollo_base.stl"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" /> <origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
<mass value="1.1852" /> <mass value="1.1852" />
<inertia <inertia
ixx="0.0044133" ixx="0.0044133"
ixy="2.4428E-08" ixy="2.4428E-08"
ixz="-0.00050667" ixz="-0.00050667"
iyy="0.0048259" iyy="0.0048259"
iyz="-9.0612E-09" iyz="-9.0612E-09"
izz="0.0079062" /> izz="0.0079062" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<!-- LIDAR Pos--> <!-- LIDAR Pos-->
<xacro:property name="lidar_joint_xyz" value="0 0 0.175"/> <xacro:property name="lidar_joint_xyz" value="0 0 0.175"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
<!-- Camera Pos --> <!-- Camera Pos -->
<xacro:property name="camera_pos_rear" value="false"/> <xacro:property name="camera_pos_rear" value="false"/>
<xacro:if value="${camera_pos_rear}"> <xacro:if value="${camera_pos_rear}">
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="0 0 0.25"/> <xacro:property name="camera_joint_xyz" value="0 0 0.25"/>
<xacro:property name="camera_pillar_height" value="0.07"/> <xacro:property name="camera_pillar_height" value="0.07"/>
</xacro:if> </xacro:if>
<xacro:unless value="${camera_pos_rear}"> <xacro:unless value="${camera_pos_rear}">
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="0.145 0 0.215"/> <xacro:property name="camera_joint_xyz" value="0.145 0 0.215"/>
<xacro:property name="camera_pillar_height" value="0.04"/> <xacro:property name="camera_pillar_height" value="0.04"/>
</xacro:unless> </xacro:unless>
<xacro:property name="camera_joint_rpy" value="0 0 0"/> <xacro:property name="camera_joint_rpy" value="0 0 0"/>
</robot> </robot>

134
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/apolloX.urdf.xacro
View File

@ -1,67 +1,67 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
<xacro:macro name="base"> <xacro:macro name="base">
<!-- Chassis a.k.a base_link --> <!-- Chassis a.k.a base_link -->
<link name="base_link"> <link name="base_link">
<visual name="visual_base"> <visual name="visual_base">
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/> <mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
</geometry> </geometry>
<material name="material_black"/> <material name="material_black"/>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/> <mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" /> <origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
<mass value="1.1852" /> <mass value="1.1852" />
<inertia <inertia
ixx="0.0044133" ixx="0.0044133"
ixy="2.4428E-08" ixy="2.4428E-08"
ixz="-0.00050667" ixz="-0.00050667"
iyy="0.0048259" iyy="0.0048259"
iyz="-9.0612E-09" iyz="-9.0612E-09"
izz="0.0079062" /> izz="0.0079062" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<!-- apolloX use omni wheel/universal wheel --> <!-- apolloX use omni wheel/universal wheel -->
<xacro:property name="use_omni_wheel" value="false"/> <xacro:property name="use_omni_wheel" value="false"/>
<!-- LIDAR Pos--> <!-- LIDAR Pos-->
<xacro:if value="${use_omni_wheel}"> <xacro:if value="${use_omni_wheel}">
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.13"/> <xacro:property name="lidar_joint_xyz" value="0.13 0 0.13"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
</xacro:if> </xacro:if>
<xacro:unless value="${use_omni_wheel}"> <xacro:unless value="${use_omni_wheel}">
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.185"/> <xacro:property name="lidar_joint_xyz" value="0.13 0 0.185"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
</xacro:unless> </xacro:unless>
<!-- Camera Pos --> <!-- Camera Pos -->
<xacro:property name="camera_pos_rear" value="true"/> <xacro:property name="camera_pos_rear" value="true"/>
<xacro:if value="${camera_pos_rear}"> <xacro:if value="${camera_pos_rear}">
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="0 0 0.27"/> <xacro:property name="camera_joint_xyz" value="0 0 0.27"/>
<xacro:property name="camera_pillar_height" value="0.08"/> <xacro:property name="camera_pillar_height" value="0.08"/>
</xacro:if> </xacro:if>
<xacro:unless value="${camera_pos_rear}"> <xacro:unless value="${camera_pos_rear}">
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="0.28 0 0.23"/> <xacro:property name="camera_joint_xyz" value="0.28 0 0.23"/>
<xacro:property name="camera_pillar_height" value="0.04"/> <xacro:property name="camera_pillar_height" value="0.04"/>
</xacro:unless> </xacro:unless>
<xacro:property name="camera_joint_rpy" value="0 0 0"/> <xacro:property name="camera_joint_rpy" value="0 0 0"/>
</robot> </robot>

92
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/hades.urdf.xacro
View File

@ -1,46 +1,46 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
<xacro:macro name="base"> <xacro:macro name="base">
<!-- Chassis a.k.a base_link --> <!-- Chassis a.k.a base_link -->
<link name="base_link"> <link name="base_link">
<visual name="visual_base"> <visual name="visual_base">
<origin xyz="0 0 0.063" rpy="0 0 0" /> <origin xyz="0 0 0.063" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/> <mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
</geometry> </geometry>
<material name="material_dark_grey"/> <material name="material_dark_grey"/>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/> <mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" /> <origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
<mass value="1.1852" /> <mass value="1.1852" />
<inertia <inertia
ixx="0.0044133" ixx="0.0044133"
ixy="2.4428E-08" ixy="2.4428E-08"
ixz="-0.00050667" ixz="-0.00050667"
iyy="0.0048259" iyy="0.0048259"
iyz="-9.0612E-09" iyz="-9.0612E-09"
izz="0.0079062" /> izz="0.0079062" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<!-- LIDAR Pos--> <!-- LIDAR Pos-->
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/> <xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
<!-- Camera Pos--> <!-- Camera Pos-->
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="-0.10 0 0.26"/> <xacro:property name="camera_joint_xyz" value="-0.10 0 0.26"/>
<xacro:property name="camera_joint_rpy" value="0 0 0"/> <xacro:property name="camera_joint_rpy" value="0 0 0"/>
<xacro:property name="camera_pillar_height" value="0.08"/> <xacro:property name="camera_pillar_height" value="0.08"/>
</robot> </robot>

92
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/hera.urdf.xacro
View File

@ -1,46 +1,46 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
<xacro:macro name="base"> <xacro:macro name="base">
<!-- Chassis a.k.a base_link --> <!-- Chassis a.k.a base_link -->
<link name="base_link"> <link name="base_link">
<visual name="visual_base"> <visual name="visual_base">
<origin xyz="0 0 0.063" rpy="0 0 0" /> <origin xyz="0 0 0.063" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/> <mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
</geometry> </geometry>
<material name="material_dark_grey"/> <material name="material_dark_grey"/>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/> <mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" /> <origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
<mass value="1.1852" /> <mass value="1.1852" />
<inertia <inertia
ixx="0.0044133" ixx="0.0044133"
ixy="2.4428E-08" ixy="2.4428E-08"
ixz="-0.00050667" ixz="-0.00050667"
iyy="0.0048259" iyy="0.0048259"
iyz="-9.0612E-09" iyz="-9.0612E-09"
izz="0.0079062" /> izz="0.0079062" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<!-- LIDAR Pos--> <!-- LIDAR Pos-->
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/> <xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
<!-- Camera Pos--> <!-- Camera Pos-->
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="-0.10 0 0.26"/> <xacro:property name="camera_joint_xyz" value="-0.10 0 0.26"/>
<xacro:property name="camera_joint_rpy" value="0 0 0"/> <xacro:property name="camera_joint_rpy" value="0 0 0"/>
<xacro:property name="camera_pillar_height" value="0.08"/> <xacro:property name="camera_pillar_height" value="0.08"/>
</robot> </robot>

78
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/pibot.urdf.xacro
View File

@ -1,39 +1,39 @@
<robot name="pibot_robot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot_robot" xmlns:xacro="https://ros.org/wiki/xacro">
<!-- xacro includes --> <!-- xacro includes -->
<xacro:include filename="$(find pibot_description)/urdf/components/pibot_properties.xacro"/> <xacro:include filename="$(find pibot_description)/urdf/components/pibot_properties.xacro"/>
<xacro:arg name="model" default="apollo" /> <xacro:arg name="model" default="apollo" />
<xacro:arg name="lidar" default="none" /> <xacro:arg name="lidar" default="none" />
<xacro:arg name="pb_3dsensor" default="none" /> <xacro:arg name="pb_3dsensor" default="none" />
<xacro:macro name="pibot_robot"> <xacro:macro name="pibot_robot">
<!-- Footprint --> <!-- Footprint -->
<link name="base_footprint"/> <link name="base_footprint"/>
<!-- Joint from base_footprint to base link(chassis) --> <!-- Joint from base_footprint to base link(chassis) -->
<joint name="base_footprint_joint" type="fixed"> <joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_link" /> <parent link="base_link" />
<child link="base_footprint" /> <child link="base_footprint" />
</joint> </joint>
<!-- Base --> <!-- Base -->
<xacro:include filename="$(find pibot_description)/urdf/$(arg model).urdf.xacro"/> <xacro:include filename="$(find pibot_description)/urdf/$(arg model).urdf.xacro"/>
<xacro:base></xacro:base> <xacro:base></xacro:base>
<!-- LIDAR --> <!-- LIDAR -->
<xacro:property name="pibot_lidar" value="$(arg lidar)"/> <xacro:property name="pibot_lidar" value="$(arg lidar)"/>
<xacro:unless value="${pibot_lidar == 'none'}"> <xacro:unless value="${pibot_lidar == 'none'}">
<xacro:include filename="$(find pibot_description)/urdf/accessories/lidar.urdf.xacro"/> <xacro:include filename="$(find pibot_description)/urdf/accessories/lidar.urdf.xacro"/>
<xacro:lidar lidar_enabled="true" lidar_joint_xyz="${lidar_joint_xyz}" lidar_joint_rpy="${lidar_joint_rpy}"></xacro:lidar> <xacro:lidar lidar_enabled="true" lidar_joint_xyz="${lidar_joint_xyz}" lidar_joint_rpy="${lidar_joint_rpy}"></xacro:lidar>
</xacro:unless> </xacro:unless>
<!-- Camera --> <!-- Camera -->
<xacro:property name="pibot_3dsensor" value="$(arg pb_3dsensor)"/> <xacro:property name="pibot_3dsensor" value="$(arg pb_3dsensor)"/>
<xacro:unless value="${pibot_3dsensor == 'none'}"> <xacro:unless value="${pibot_3dsensor == 'none'}">
<xacro:include filename="$(find pibot_description)/urdf/accessories/camera.urdf.xacro"/> <xacro:include filename="$(find pibot_description)/urdf/accessories/camera.urdf.xacro"/>
<xacro:camera camera_enabled="true" camera_joint_xyz="${camera_joint_xyz}" camera_joint_rpy="${camera_joint_rpy}"></xacro:camera> <xacro:camera camera_enabled="true" camera_joint_xyz="${camera_joint_xyz}" camera_joint_rpy="${camera_joint_rpy}"></xacro:camera>
</xacro:unless> </xacro:unless>
</xacro:macro> </xacro:macro>
</robot> </robot>

92
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_description/urdf/zeus.urdf.xacro
View File

@ -1,46 +1,46 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro"> <robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
<xacro:macro name="base"> <xacro:macro name="base">
<!-- Chassis a.k.a base_link --> <!-- Chassis a.k.a base_link -->
<link name="base_link"> <link name="base_link">
<visual name="visual_base"> <visual name="visual_base">
<origin xyz="0 0 0.065" rpy="0 0 0.5235987755982988" /> <origin xyz="0 0 0.065" rpy="0 0 0.5235987755982988" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/> <mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
</geometry> </geometry>
<material name="material_black"/> <material name="material_black"/>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/> <mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" /> <origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
<mass value="1.1852" /> <mass value="1.1852" />
<inertia <inertia
ixx="0.0044133" ixx="0.0044133"
ixy="2.4428E-08" ixy="2.4428E-08"
ixz="-0.00050667" ixz="-0.00050667"
iyy="0.0048259" iyy="0.0048259"
iyz="-9.0612E-09" iyz="-9.0612E-09"
izz="0.0079062" /> izz="0.0079062" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<!-- LIDAR Pose--> <!-- LIDAR Pose-->
<xacro:property name="lidar_joint_xyz" value="0 0 0.18"/> <xacro:property name="lidar_joint_xyz" value="0 0 0.18"/>
<xacro:property name="lidar_joint_rpy" value="0 0 0"/> <xacro:property name="lidar_joint_rpy" value="0 0 0"/>
<!-- Camera Pos--> <!-- Camera Pos-->
<xacro:property name="camera_pillar_enabled" value="true"/> <xacro:property name="camera_pillar_enabled" value="true"/>
<xacro:property name="camera_joint_xyz" value="-0.08 0 0.27"/> <xacro:property name="camera_joint_xyz" value="-0.08 0 0.27"/>
<xacro:property name="camera_joint_rpy" value="0 0 0"/> <xacro:property name="camera_joint_rpy" value="0 0 0"/>
<xacro:property name="camera_pillar_height" value="0.085"/> <xacro:property name="camera_pillar_height" value="0.085"/>
</robot> </robot>

20
Code/MowingRobot/pibot_ros/ros_ws/src/rplidar_ros/launch/rplidar.launch
View File

@ -1,10 +1,10 @@
<launch> <launch>
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/> <param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 --> <param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 -->
<!--param name="serial_baudrate" type="int" value="256000"--><!--A3 --> <!--param name="serial_baudrate" type="int" value="256000"--><!--A3 -->
<param name="frame_id" type="string" value="laser"/> <param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/> <param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/> <param name="angle_compensate" type="bool" value="true"/>
</node> </node>
</launch> </launch>

20
Code/MowingRobot/pibot_ros/ros_ws/src/rplidar_ros/launch/rplidar_a3.launch
View File

@ -1,10 +1,10 @@
<launch> <launch>
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/> <param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="256000"/><!--A3 --> <param name="serial_baudrate" type="int" value="256000"/><!--A3 -->
<param name="frame_id" type="string" value="laser"/> <param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/> <param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/> <param name="angle_compensate" type="bool" value="true"/>
<param name="scan_mode" type="string" value="Sensitivity"/> <param name="scan_mode" type="string" value="Sensitivity"/>
</node> </node>
</launch> </launch>

384
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/CMakeLists.txt
View File

@ -1,192 +1,192 @@
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(simple_follower) project(simple_follower)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
rospy rospy
std_msgs std_msgs
sensor_msgs sensor_msgs
geometry_msgs geometry_msgs
message_generation message_generation
) )
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures ## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed ## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup() # catkin_python_setup()
################################################ ################################################
## Declare ROS messages, services and actions ## ## Declare ROS messages, services and actions ##
################################################ ################################################
## To declare and build messages, services or actions from within this ## To declare and build messages, services or actions from within this
## package, follow these steps: ## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in ## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml: ## * In the file package.xml:
## * add a build_depend tag for "message_generation" ## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless: ## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime" ## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt): ## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to ## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...) ## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to ## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...) ## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed ## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed ## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below ## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
add_message_files( add_message_files(
FILES FILES
position.msg position.msg
) )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
# add_service_files( # add_service_files(
# FILES # FILES
# Service1.srv # Service1.srv
# Service2.srv # Service2.srv
# ) # )
## Generate actions in the 'action' folder ## Generate actions in the 'action' folder
# add_action_files( # add_action_files(
# FILES # FILES
# Action1.action # Action1.action
# Action2.action # Action2.action
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
generate_messages( generate_messages(
DEPENDENCIES DEPENDENCIES
std_msgs std_msgs
sensor_msgs sensor_msgs
geometry_msgs geometry_msgs
) )
################################################ ################################################
## Declare ROS dynamic reconfigure parameters ## ## Declare ROS dynamic reconfigure parameters ##
################################################ ################################################
## To declare and build dynamic reconfigure parameters within this ## To declare and build dynamic reconfigure parameters within this
## package, follow these steps: ## package, follow these steps:
## * In the file package.xml: ## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure" ## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt): ## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to ## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...) ## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below ## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed ## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder ## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options( # generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg # cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg # cfg/DynReconf2.cfg
# ) # )
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
################################### ###################################
## The catkin_package macro generates cmake config files for your package ## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects ## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files ## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need ## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package( catkin_package(
# INCLUDE_DIRS include # INCLUDE_DIRS include
# LIBRARIES simple_follower # LIBRARIES simple_follower
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
# DEPENDS system_lib # DEPENDS system_lib
) )
########### ###########
## Build ## ## Build ##
########### ###########
## Specify additional locations of header files ## Specify additional locations of header files
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
# include_directories(include) # include_directories(include)
include_directories( include_directories(
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
) )
## Declare a C++ library ## Declare a C++ library
# add_library(simple_follower # add_library(simple_follower
# src/${PROJECT_NAME}/simple_follower.cpp # src/${PROJECT_NAME}/simple_follower.cpp
# ) # )
## Add cmake target dependencies of the library ## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries ## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure ## either from message generation or dynamic reconfigure
# add_dependencies(simple_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_dependencies(simple_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable ## Declare a C++ executable
# add_executable(simple_follower_node src/simple_follower_node.cpp) # add_executable(simple_follower_node src/simple_follower_node.cpp)
## Add cmake target dependencies of the executable ## Add cmake target dependencies of the executable
## same as for the library above ## same as for the library above
# add_dependencies(simple_follower_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_dependencies(simple_follower_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against ## Specify libraries to link a library or executable target against
# target_link_libraries(simple_follower_node # target_link_libraries(simple_follower_node
# ${catkin_LIBRARIES} # ${catkin_LIBRARIES}
# ) # )
############# #############
## Install ## ## Install ##
############# #############
# all install targets should use catkin DESTINATION variables # all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation ## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination ## in contrast to setup.py, you can choose the destination
# install(PROGRAMS # install(PROGRAMS
# scripts/my_python_script # scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# ) # )
## Mark executables and/or libraries for installation ## Mark executables and/or libraries for installation
# install(TARGETS simple_follower simple_follower_node # install(TARGETS simple_follower simple_follower_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# ) # )
## Mark cpp header files for installation ## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/ # install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h" # FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE # PATTERN ".svn" EXCLUDE
# ) # )
## Mark other files for installation (e.g. launch and bag files, etc.) ## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES # install(FILES
# # myfile1 # # myfile1
# # myfile2 # # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# ) # )
############# #############
## Testing ## ## Testing ##
############# #############
## Add gtest based cpp test target and link libraries ## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_follower.cpp) # catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_follower.cpp)
# if(TARGET ${PROJECT_NAME}-test) # if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif() # endif()
## Add folders to be run by python nosetests ## Add folders to be run by python nosetests
# catkin_add_nosetests(test) # catkin_add_nosetests(test)

8
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/launch/laser_follower.launch
View File

@ -1,4 +1,4 @@
<launch> <launch>
<include file='$(find simple_follower)/launch/nodes/laserTracker.launch' /> <include file='$(find simple_follower)/launch/nodes/laserTracker.launch' />
<include file='$(find simple_follower)/launch/nodes/follower.launch' /> <include file='$(find simple_follower)/launch/nodes/follower.launch' />
</launch> </launch>

26
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/launch/nodes/follower.launch
View File

@ -1,13 +1,13 @@
<launch> <launch>
<node name='follower' pkg="simple_follower" type="follower.py"> <node name='follower' pkg="simple_follower" type="follower.py">
<!-- switchMode: (if true one button press will change betwenn active, not active. If false the button will have to be kept pressed all the time to be active --> <!-- switchMode: (if true one button press will change betwenn active, not active. If false the button will have to be kept pressed all the time to be active -->
<param name="switchMode" value="True" type="bool" /> <param name="switchMode" value="True" type="bool" />
<!-- maximal speed (angular and linear both), tartet distance: the robot will try to keep this fixed distance --> <!-- maximal speed (angular and linear both), tartet distance: the robot will try to keep this fixed distance -->
<param name='maxSpeed' value='0.4' type='double' /> <param name='maxSpeed' value='0.4' type='double' />
<param name='targetDist' value='0.4' type='double' /> <param name='targetDist' value='0.4' type='double' />
<!-- index of the button in the buttons field of the joy message from the ps3 controller, that switches active/inactive --> <!-- index of the button in the buttons field of the joy message from the ps3 controller, that switches active/inactive -->
<param name='controllButtonIndex' value='-4' type='int' /> <param name='controllButtonIndex' value='-4' type='int' />
<rosparam ns='PID_controller' command='load' file='$(find simple_follower)/parameters/PID_param.yaml' /> <rosparam ns='PID_controller' command='load' file='$(find simple_follower)/parameters/PID_param.yaml' />
</node> </node>
</launch> </launch>

16
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/launch/nodes/laserTracker.launch
View File

@ -1,8 +1,8 @@
<launch> <launch>
<node name='laser_tracker' pkg="simple_follower" type="laserTracker.py"> <node name='laser_tracker' pkg="simple_follower" type="laserTracker.py">
<!-- This is to avoid treating noise in the laser ranges as objects --> <!-- This is to avoid treating noise in the laser ranges as objects -->
<!-- we check for all range measurements around the currently closest distance measurement (size of this window specified by winSize) if they where similarly close in the last scan (threshold specified by deltaDist--> <!-- we check for all range measurements around the currently closest distance measurement (size of this window specified by winSize) if they where similarly close in the last scan (threshold specified by deltaDist-->
<param name="winSize" value="2" type="int" /> <param name="winSize" value="2" type="int" />
<param name="deltaDist" value="0.2" type="double" /> <param name="deltaDist" value="0.2" type="double" />
</node> </node>
</launch> </launch>

34
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/launch/nodes/visualTracker.launch
View File

@ -1,17 +1,17 @@
<launch> <launch>
<node name='visual_tracker' pkg="simple_follower" type="visualTracker.py"> <node name='visual_tracker' pkg="simple_follower" type="visualTracker.py">
<!-- color or the target in HSV color space --> <!-- color or the target in HSV color space -->
<rosparam ns='target'> <rosparam ns='target'>
upper : [0, 120, 100] upper : [0, 120, 100]
lower : [9, 255, 255] lower : [9, 255, 255]
</rosparam> </rosparam>
<rosparam ns='pictureDimensions'> <rosparam ns='pictureDimensions'>
<!-- Picture dimensions in pixel --> <!-- Picture dimensions in pixel -->
pictureHeight: 480 pictureHeight: 480
pictureWidth: 640 pictureWidth: 640
<!-- Viewing angle of the camera in one direction in Radians --> <!-- Viewing angle of the camera in one direction in Radians -->
verticalAngle: 0.43196898986859655 verticalAngle: 0.43196898986859655
horizontalAngle: 0.5235987755982988 horizontalAngle: 0.5235987755982988
</rosparam> </rosparam>
</node> </node>
</launch> </launch>

8
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/launch/visual_follower.launch
View File

@ -1,4 +1,4 @@
<launch> <launch>
<include file='$(find simple_follower)/launch/nodes/visualTracker.launch' /> <include file='$(find simple_follower)/launch/nodes/visualTracker.launch' />
<include file='$(find simple_follower)/launch/nodes/follower.launch' /> <include file='$(find simple_follower)/launch/nodes/follower.launch' />
</launch> </launch>

6
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/msg/position.msg
View File

@ -1,3 +1,3 @@
float32 angleX float32 angleX
float32 angleY float32 angleY
float32 distance float32 distance

122
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/package.xml
View File

@ -1,61 +1,61 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>simple_follower</name> <name>simple_follower</name>
<version>0.0.0</version> <version>0.0.0</version>
<description>Makes a mobile robot follow a target. either using rgb-d or laser range finder</description> <description>Makes a mobile robot follow a target. either using rgb-d or laser range finder</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: --> <!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="chutter@uos.de">clemens</maintainer> <maintainer email="chutter@uos.de">clemens</maintainer>
<!-- One license tag required, multiple allowed, one license per tag --> <!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: --> <!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license> <license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag --> <!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: --> <!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/first_experiments</url> --> <!-- <url type="website">http://wiki.ros.org/first_experiments</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag --> <!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be --> <!-- Authors do not have to be maintianers, but could be -->
<!-- Example: --> <!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> --> <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies --> <!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies --> <!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: --> <!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: --> <!-- Use build_depend for packages you need at compile time: -->
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
<!-- Use buildtool_depend for build tool packages: --> <!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: --> <!-- Use run_depend for packages you need at runtime: -->
<!-- Use test_depend for packages you need only for testing: --> <!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> --> <!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend>
<run_depend>message_runtime</run_depend> <run_depend>message_runtime</run_depend>
<run_depend>message_generation</run_depend> <run_depend>message_generation</run_depend>
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend> <run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend> <run_depend>std_msgs</run_depend>
<run_depend>sensor_msgs</run_depend> <run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend> <run_depend>geometry_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->
</export> </export>
</package> </package>

6
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/parameters/PID_param.yaml
View File

@ -1,3 +1,3 @@
P: [0.7, 0.6] P: [0.7, 0.6]
I: [0.07,0.04] I: [0.07,0.04]
D: [0.00, 0.000] D: [0.00, 0.000]

0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/follower.py Executable file → Normal file
View File

0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/laserTracker.py Executable file → Normal file
View File

228
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/testCode.py Executable file → Normal file
View File

@ -1,114 +1,114 @@
import numpy as np import numpy as np
import copy import copy
from unsafe_runaway import * from unsafe_runaway import *
import time import time
from nose.tools import assert_raises from nose.tools import assert_raises
class mockup: class mockup:
pass pass
def makeLaserData(): def makeLaserData():
laser_data = mockup() laser_data = mockup()
laser_data.range_max = 5.6 laser_data.range_max = 5.6
laser_data.angle_min = -2.1 laser_data.angle_min = -2.1
laser_data.angle_increment = 0.06136 laser_data.angle_increment = 0.06136
laser_data.ranges = list(1+np.random.rand(69)*5) laser_data.ranges = list(1+np.random.rand(69)*5)
return laser_data return laser_data
class Test_simpleTracker: class Test_simpleTracker:
def setUp(self): def setUp(self):
self.tracker = simpleTracker() self.tracker = simpleTracker()
def test_ignor_first_scan(self): def test_ignor_first_scan(self):
laser_data = makeLaserData() laser_data = makeLaserData()
tracker = simpleTracker() tracker = simpleTracker()
assert_raises(UserWarning, self.tracker.registerScan,laser_data) assert_raises(UserWarning, self.tracker.registerScan,laser_data)
def test_unmuted(self): def test_unmuted(self):
laser_data = makeLaserData() laser_data = makeLaserData()
backup = copy.copy(laser_data) backup = copy.copy(laser_data)
try: try:
angle, distance = self.tracker.registerScan(laser_data) angle, distance = self.tracker.registerScan(laser_data)
except: except:
pass pass
assert backup.ranges == laser_data.ranges assert backup.ranges == laser_data.ranges
#print(laser_data.ranges) #print(laser_data.ranges)
#print('angle: {}, dist: {}'.format(angle, distance)) #print('angle: {}, dist: {}'.format(angle, distance))
def test_nan(self): def test_nan(self):
laser_data = makeLaserData() laser_data = makeLaserData()
assert_raises(UserWarning, self.tracker.registerScan,laser_data) assert_raises(UserWarning, self.tracker.registerScan,laser_data)
laser_data.ranges[12] = float('nan') laser_data.ranges[12] = float('nan')
angle, dist=self.tracker.registerScan(laser_data) angle, dist=self.tracker.registerScan(laser_data)
#print('angle: {}, dist: {}'.format(angle, dist)) #print('angle: {}, dist: {}'.format(angle, dist))
def test_only_nan(self): def test_only_nan(self):
laser_data = makeLaserData() laser_data = makeLaserData()
laser_data.ranges = [float('nan') for _ in laser_data.ranges] laser_data.ranges = [float('nan') for _ in laser_data.ranges]
assert_raises(UserWarning, self.tracker.registerScan,laser_data) assert_raises(UserWarning, self.tracker.registerScan,laser_data)
assert_raises(UserWarning, self.tracker.registerScan,laser_data) assert_raises(UserWarning, self.tracker.registerScan,laser_data)
def test_real_real_min(self): def test_real_real_min(self):
laser_data = makeLaserData() laser_data = makeLaserData()
laser_data.ranges[-1]=0.5 #real min laser_data.ranges[-1]=0.5 #real min
assert_raises(UserWarning, self.tracker.registerScan,laser_data) assert_raises(UserWarning, self.tracker.registerScan,laser_data)
laser_data.ranges[-1]=0.6 laser_data.ranges[-1]=0.6
laser_data.ranges[42]=0.1 #fake min laser_data.ranges[42]=0.1 #fake min
ang, dist = self.tracker.registerScan(laser_data) ang, dist = self.tracker.registerScan(laser_data)
assert dist == 0.6 assert dist == 0.6
#print('ang: {}, target: {}'.format(ang, (laser_data.angle_min+ 23*laser_data.angle_increment))) #print('ang: {}, target: {}'.format(ang, (laser_data.angle_min+ 23*laser_data.angle_increment)))
assert ang == laser_data.angle_min+ 68*laser_data.angle_increment assert ang == laser_data.angle_min+ 68*laser_data.angle_increment
class Test_PID: class Test_PID:
def setUp(self): def setUp(self):
pass pass
def test_convergence(self): def test_convergence(self):
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001) self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
x =np.array([23, 12]) x =np.array([23, 12])
for i in range(20): for i in range(20):
update= self.pid.update(x) update= self.pid.update(x)
print('added {} to current x {}'.format(update, x)) print('added {} to current x {}'.format(update, x))
x = x+update x = x+update
time.sleep(0.1) time.sleep(0.1)
assert np.all(abs(x-[0,30])<=0.01) assert np.all(abs(x-[0,30])<=0.01)
def test_convergence_differentParamShape(self): def test_convergence_differentParamShape(self):
self.pid = simplePID([0,30],0.8, 0.001, 0.0001) self.pid = simplePID([0,30],0.8, 0.001, 0.0001)
x =np.array([23, 12]) x =np.array([23, 12])
for i in range(20): for i in range(20):
update= self.pid.update(x) update= self.pid.update(x)
print('added {} to current x {}'.format(update, x)) print('added {} to current x {}'.format(update, x))
x = x+update x = x+update
time.sleep(0.1) time.sleep(0.1)
assert np.all(abs(x-[0,30])<=0.01) assert np.all(abs(x-[0,30])<=0.01)
def test_raises_unequal_param_shape_at_creation(self): def test_raises_unequal_param_shape_at_creation(self):
assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7, 0.1], 0.001, 0.0001) assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7, 0.1], 0.001, 0.0001)
assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7], 0.001, 0.0001) assert_raises(TypeError, simplePID, [0,30],[0.8, 0.7], 0.001, 0.0001)
assert_raises(TypeError, simplePID, 0,[0.8, 0.7], 0.001, 0.0001) assert_raises(TypeError, simplePID, 0,[0.8, 0.7], 0.001, 0.0001)
assert_raises(TypeError, simplePID, 0, [0.8, 0.7], [0.001, 0.001], [0.0001, 0,0001]) assert_raises(TypeError, simplePID, 0, [0.8, 0.7], [0.001, 0.001], [0.0001, 0,0001])
_ = simplePID([0,30],[0.8, 0.7], [0.001, 0.001], [0.0001, 0.0001]) _ = simplePID([0,30],[0.8, 0.7], [0.001, 0.001], [0.0001, 0.0001])
_ = simplePID([0,30],0.8, 0.001, 0.0001) _ = simplePID([0,30],0.8, 0.001, 0.0001)
_ = simplePID(0,0.8, 0.001, 0.0001) _ = simplePID(0,0.8, 0.001, 0.0001)
def test_raise_incompatable_input(self): def test_raise_incompatable_input(self):
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001) self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
_ = assert_raises(TypeError, self.pid.update, 3) _ = assert_raises(TypeError, self.pid.update, 3)
x =np.array([23, 12]) x =np.array([23, 12])
for i in range(50): for i in range(50):
update= self.pid.update(x) update= self.pid.update(x)
print('added {} to current x {}'.format(update, x)) print('added {} to current x {}'.format(update, x))
x = x+update x = x+update
time.sleep(0.1) time.sleep(0.1)
assert np.all(abs(x-[0,30])<=0.001) assert np.all(abs(x-[0,30])<=0.001)

0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/visualTracker.py Executable file → Normal file
View File

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_更新.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 38 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_预测.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 36 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理1.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 18 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 42 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 36 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理4.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 34 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理1.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 80 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理2.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 27 KiB

BIN
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理3.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 26 KiB

Some files were not shown because too many files have changed in this diff Show More