forked from logzhan/RobotKernal-UESTC
Compare commits
No commits in common. "main" and "master" have entirely different histories.
|
|
@ -1,17 +1,17 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import pypibot.assistant
|
||||
import pypibot.log as Logger
|
||||
import pypibot.err
|
||||
log=Logger.log
|
||||
import sys
|
||||
def createNamedLog(name):
|
||||
return Logger.NamedLog(name)
|
||||
class Object():
|
||||
pass
|
||||
isDebug="-d" in sys.argv
|
||||
|
||||
import platform
|
||||
isWindows=False
|
||||
if platform.system()=='Windows':
|
||||
isWindows=True
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import pypibot.assistant
|
||||
import pypibot.log as Logger
|
||||
import pypibot.err
|
||||
log=Logger.log
|
||||
import sys
|
||||
def createNamedLog(name):
|
||||
return Logger.NamedLog(name)
|
||||
class Object():
|
||||
pass
|
||||
isDebug="-d" in sys.argv
|
||||
|
||||
import platform
|
||||
isWindows=False
|
||||
if platform.system()=='Windows':
|
||||
isWindows=True
|
||||
|
|
|
|||
|
|
@ -1,234 +1,234 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import os, sys, inspect
|
||||
import datetime
|
||||
import signal
|
||||
import threading
|
||||
import time
|
||||
# function: get directory of current script, if script is built
|
||||
# into an executable file, get directory of the excutable file
|
||||
def current_file_directory():
|
||||
path = os.path.realpath(sys.path[0]) # interpreter starter's path
|
||||
if os.path.isfile(path): # starter is excutable file
|
||||
path = os.path.dirname(path)
|
||||
path = os.path.abspath(path) # return excutable file's directory
|
||||
else: # starter is python script
|
||||
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
|
||||
if path[-1]!=os.path.sep:path+=os.path.sep
|
||||
return path
|
||||
|
||||
"""格式化字符串"""
|
||||
def formatString(string,*argv):
|
||||
string=string%argv
|
||||
if string.find('$(scriptpath)')>=0:
|
||||
string=string.replace('$(scriptpath)',current_file_directory())
|
||||
if string.find('$(filenumber2)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(scriptpath)',"%02d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
#8位日期（20140404）
|
||||
if string.find('$(Date8)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
|
||||
#6位日期（140404）
|
||||
if string.find('$(Date6)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Date6)', now.strftime("%y%m%d"))
|
||||
#6位时间（121212）
|
||||
if string.find('$(Time6)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Time6)', now.strftime("%H%M%S"))
|
||||
#4位时间（1212）
|
||||
if string.find('$(Time4)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Time4)', now.strftime("%H%M"))
|
||||
#文件编号2位（必须在最后）
|
||||
if string.find('$(filenumber2)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(filenumber2)',"%02d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
#文件编号3位（必须在最后）
|
||||
if string.find('$(filenumber3)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(filenumber3)',"%03d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
return string
|
||||
|
||||
"""
|
||||
取得进程列表
|
||||
格式：(PID,cmd)
|
||||
"""
|
||||
def getProcessList():
|
||||
processList = []
|
||||
try:
|
||||
for line in os.popen("ps xa"):
|
||||
fields = line.split()
|
||||
# spid = fields[0]
|
||||
pid = 0
|
||||
try:pid = int(fields[0])
|
||||
except:None
|
||||
cmd = line[27:-1]
|
||||
# print "PS:%d,%s"%(npid,process)
|
||||
if pid != 0 and len(cmd) > 0:
|
||||
processList.append((pid, cmd))
|
||||
except Exception as e:
|
||||
print("getProcessList except:%s" % (e))
|
||||
return processList
|
||||
def killCommand(cmd):
|
||||
try:
|
||||
processList = getProcessList()
|
||||
for p in processList:
|
||||
if p[1].find(cmd) != -1:
|
||||
pid = p[0]
|
||||
os.kill(pid, signal.SIGKILL)
|
||||
except Exception as e:
|
||||
print("killCommand ‘%s’ except:%s" % (cmd,e))
|
||||
|
||||
def check_pid(pid):
|
||||
""" Check For the existence of a unix pid. """
|
||||
if pid == 0:return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
SF=formatString
|
||||
|
||||
#全局异常捕获
|
||||
def excepthook(excType, excValue, tb):
|
||||
'''''write the unhandle exception to log'''
|
||||
from log import log
|
||||
import traceback
|
||||
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
|
||||
sys.exit(-1)
|
||||
#sys.__excepthook__(type, value, trace)
|
||||
#sys.__excepthook__(excType, excValue, tb)
|
||||
|
||||
_defaultGlobalExcept=sys.excepthook
|
||||
|
||||
def enableGlobalExcept(enable=True):
|
||||
if enable:
|
||||
sys.excepthook = excepthook
|
||||
else:
|
||||
sys.excepthook=_defaultGlobalExcept
|
||||
# 默认启动全局异常处理
|
||||
enableGlobalExcept()
|
||||
#创建线程
|
||||
def createThread(name,target,args=(),autoRun=True,daemon=True):
|
||||
from log import log
|
||||
def threadProc():
|
||||
log.i("thread %s started!",name)
|
||||
try:
|
||||
target(*args)
|
||||
log.i("thread %s ended!",name)
|
||||
except Exception as e:
|
||||
log.e("thread %s crash!err=%s",name,e)
|
||||
thd=threading.Thread(name=name,target=threadProc)
|
||||
thd.setDaemon(daemon)
|
||||
if autoRun:thd.start()
|
||||
return thd
|
||||
|
||||
|
||||
#定时器
|
||||
class Timer():
|
||||
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
|
||||
self.name=name
|
||||
self.first=first
|
||||
self.period=period
|
||||
self.args=args
|
||||
self.timer_proc=timer_proc
|
||||
self.thdWork=None
|
||||
self.stopFlag=False
|
||||
from log import NamedLog
|
||||
self.log=NamedLog(name)
|
||||
def run(self):
|
||||
if self.first>0:
|
||||
time.sleep(self.first)
|
||||
try:
|
||||
self.timer_proc(*self.args)
|
||||
except Exception as e:
|
||||
self.log.error("timer proc crash!err=%s",e)
|
||||
while self.period>0 and not self.stopFlag:
|
||||
time.sleep(self.period)
|
||||
try:
|
||||
self.timer_proc(*self.args)
|
||||
except Exception as e:
|
||||
self.log.error("timer proc crash!err=%s",e)
|
||||
def start(self):
|
||||
if self.isAlive():
|
||||
self.log.d("already running!")
|
||||
return True
|
||||
self.stopFlag=False
|
||||
self.thdWork=threading.Thread(name=self.name,target=self.run)
|
||||
self.thdWork.setDaemon(True)
|
||||
self.thdWork.start()
|
||||
def stop(self,timeout=3):
|
||||
if self.isAlive():
|
||||
self.stopFlag=True
|
||||
try:
|
||||
self.thdWork.join(timeout)
|
||||
except Exception as e:
|
||||
self.log.e("stop timeout!")
|
||||
|
||||
def isAlive(self):
|
||||
return self.thdWork and self.thdWork.isAlive()
|
||||
#计时器
|
||||
class Ticker():
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
# 片段,可以判断时长是否在一个特定毫秒段内
|
||||
self.section=[]
|
||||
def reset(self):
|
||||
self.tick=time.time()
|
||||
self.end=0
|
||||
def stop(self):
|
||||
self.end=time.time()
|
||||
@property
|
||||
def ticker(self):
|
||||
if self.end==0:
|
||||
return int((time.time()-self.tick)*1000)
|
||||
else:
|
||||
return int((self.end-self.tick)*1000)
|
||||
def addSection(self,a,b):
|
||||
a,b=int(a),int(b)
|
||||
assert a<b
|
||||
self.section.append((a,b))
|
||||
def removeSection(self,a,b):
|
||||
a,b=int(a),int(b)
|
||||
self.section.remove((a,b))
|
||||
def removeAllSectioin(self):
|
||||
self.section=[]
|
||||
def inSection(self):
|
||||
tick=self.ticker
|
||||
for (a,b) in self.section:
|
||||
if tick>=a and tick<=b:
|
||||
return True
|
||||
return False
|
||||
def __call__(self):
|
||||
return self.ticker
|
||||
def waitExit():
|
||||
import log
|
||||
log.log.i("start waiting to exit...")
|
||||
try:
|
||||
while(True):
|
||||
time.sleep(1)
|
||||
except Exception as e:
|
||||
log.log.w("recv exit sign!")
|
||||
|
||||
def is_python3():
|
||||
return sys.hexversion > 0x03000000
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import os, sys, inspect
|
||||
import datetime
|
||||
import signal
|
||||
import threading
|
||||
import time
|
||||
# function: get directory of current script, if script is built
|
||||
# into an executable file, get directory of the excutable file
|
||||
def current_file_directory():
|
||||
path = os.path.realpath(sys.path[0]) # interpreter starter's path
|
||||
if os.path.isfile(path): # starter is excutable file
|
||||
path = os.path.dirname(path)
|
||||
path = os.path.abspath(path) # return excutable file's directory
|
||||
else: # starter is python script
|
||||
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
|
||||
if path[-1]!=os.path.sep:path+=os.path.sep
|
||||
return path
|
||||
|
||||
"""格式化字符串"""
|
||||
def formatString(string,*argv):
|
||||
string=string%argv
|
||||
if string.find('$(scriptpath)')>=0:
|
||||
string=string.replace('$(scriptpath)',current_file_directory())
|
||||
if string.find('$(filenumber2)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(scriptpath)',"%02d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
#8位日期（20140404）
|
||||
if string.find('$(Date8)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
|
||||
#6位日期（140404）
|
||||
if string.find('$(Date6)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Date6)', now.strftime("%y%m%d"))
|
||||
#6位时间（121212）
|
||||
if string.find('$(Time6)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Time6)', now.strftime("%H%M%S"))
|
||||
#4位时间（1212）
|
||||
if string.find('$(Time4)')>=0:
|
||||
now=datetime.datetime.now()
|
||||
string=string.replace('$(Time4)', now.strftime("%H%M"))
|
||||
#文件编号2位（必须在最后）
|
||||
if string.find('$(filenumber2)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(filenumber2)',"%02d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
#文件编号3位（必须在最后）
|
||||
if string.find('$(filenumber3)')>=0:
|
||||
i=0
|
||||
path=""
|
||||
while True:
|
||||
path=string.replace('$(filenumber3)',"%03d"%i)
|
||||
if not os.path.lexists(path):break
|
||||
i+=1
|
||||
string=path
|
||||
return string
|
||||
|
||||
"""
|
||||
取得进程列表
|
||||
格式：(PID,cmd)
|
||||
"""
|
||||
def getProcessList():
|
||||
processList = []
|
||||
try:
|
||||
for line in os.popen("ps xa"):
|
||||
fields = line.split()
|
||||
# spid = fields[0]
|
||||
pid = 0
|
||||
try:pid = int(fields[0])
|
||||
except:None
|
||||
cmd = line[27:-1]
|
||||
# print "PS:%d,%s"%(npid,process)
|
||||
if pid != 0 and len(cmd) > 0:
|
||||
processList.append((pid, cmd))
|
||||
except Exception as e:
|
||||
print("getProcessList except:%s" % (e))
|
||||
return processList
|
||||
def killCommand(cmd):
|
||||
try:
|
||||
processList = getProcessList()
|
||||
for p in processList:
|
||||
if p[1].find(cmd) != -1:
|
||||
pid = p[0]
|
||||
os.kill(pid, signal.SIGKILL)
|
||||
except Exception as e:
|
||||
print("killCommand ‘%s’ except:%s" % (cmd,e))
|
||||
|
||||
def check_pid(pid):
|
||||
""" Check For the existence of a unix pid. """
|
||||
if pid == 0:return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
SF=formatString
|
||||
|
||||
#全局异常捕获
|
||||
def excepthook(excType, excValue, tb):
|
||||
'''''write the unhandle exception to log'''
|
||||
from log import log
|
||||
import traceback
|
||||
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
|
||||
sys.exit(-1)
|
||||
#sys.__excepthook__(type, value, trace)
|
||||
#sys.__excepthook__(excType, excValue, tb)
|
||||
|
||||
_defaultGlobalExcept=sys.excepthook
|
||||
|
||||
def enableGlobalExcept(enable=True):
|
||||
if enable:
|
||||
sys.excepthook = excepthook
|
||||
else:
|
||||
sys.excepthook=_defaultGlobalExcept
|
||||
# 默认启动全局异常处理
|
||||
enableGlobalExcept()
|
||||
#创建线程
|
||||
def createThread(name,target,args=(),autoRun=True,daemon=True):
|
||||
from log import log
|
||||
def threadProc():
|
||||
log.i("thread %s started!",name)
|
||||
try:
|
||||
target(*args)
|
||||
log.i("thread %s ended!",name)
|
||||
except Exception as e:
|
||||
log.e("thread %s crash!err=%s",name,e)
|
||||
thd=threading.Thread(name=name,target=threadProc)
|
||||
thd.setDaemon(daemon)
|
||||
if autoRun:thd.start()
|
||||
return thd
|
||||
|
||||
|
||||
#定时器
|
||||
class Timer():
|
||||
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
|
||||
self.name=name
|
||||
self.first=first
|
||||
self.period=period
|
||||
self.args=args
|
||||
self.timer_proc=timer_proc
|
||||
self.thdWork=None
|
||||
self.stopFlag=False
|
||||
from log import NamedLog
|
||||
self.log=NamedLog(name)
|
||||
def run(self):
|
||||
if self.first>0:
|
||||
time.sleep(self.first)
|
||||
try:
|
||||
self.timer_proc(*self.args)
|
||||
except Exception as e:
|
||||
self.log.error("timer proc crash!err=%s",e)
|
||||
while self.period>0 and not self.stopFlag:
|
||||
time.sleep(self.period)
|
||||
try:
|
||||
self.timer_proc(*self.args)
|
||||
except Exception as e:
|
||||
self.log.error("timer proc crash!err=%s",e)
|
||||
def start(self):
|
||||
if self.isAlive():
|
||||
self.log.d("already running!")
|
||||
return True
|
||||
self.stopFlag=False
|
||||
self.thdWork=threading.Thread(name=self.name,target=self.run)
|
||||
self.thdWork.setDaemon(True)
|
||||
self.thdWork.start()
|
||||
def stop(self,timeout=3):
|
||||
if self.isAlive():
|
||||
self.stopFlag=True
|
||||
try:
|
||||
self.thdWork.join(timeout)
|
||||
except Exception as e:
|
||||
self.log.e("stop timeout!")
|
||||
|
||||
def isAlive(self):
|
||||
return self.thdWork and self.thdWork.isAlive()
|
||||
#计时器
|
||||
class Ticker():
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
# 片段,可以判断时长是否在一个特定毫秒段内
|
||||
self.section=[]
|
||||
def reset(self):
|
||||
self.tick=time.time()
|
||||
self.end=0
|
||||
def stop(self):
|
||||
self.end=time.time()
|
||||
@property
|
||||
def ticker(self):
|
||||
if self.end==0:
|
||||
return int((time.time()-self.tick)*1000)
|
||||
else:
|
||||
return int((self.end-self.tick)*1000)
|
||||
def addSection(self,a,b):
|
||||
a,b=int(a),int(b)
|
||||
assert a<b
|
||||
self.section.append((a,b))
|
||||
def removeSection(self,a,b):
|
||||
a,b=int(a),int(b)
|
||||
self.section.remove((a,b))
|
||||
def removeAllSectioin(self):
|
||||
self.section=[]
|
||||
def inSection(self):
|
||||
tick=self.ticker
|
||||
for (a,b) in self.section:
|
||||
if tick>=a and tick<=b:
|
||||
return True
|
||||
return False
|
||||
def __call__(self):
|
||||
return self.ticker
|
||||
def waitExit():
|
||||
import log
|
||||
log.log.i("start waiting to exit...")
|
||||
try:
|
||||
while(True):
|
||||
time.sleep(1)
|
||||
except Exception as e:
|
||||
log.log.w("recv exit sign!")
|
||||
|
||||
def is_python3():
|
||||
return sys.hexversion > 0x03000000
|
||||
|
|
|
|||
|
|
@ -1,56 +1,56 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import ConfigParser
|
||||
from log import PLOG
|
||||
import os
|
||||
def getdefaultfilename():
|
||||
pass
|
||||
def openconfiger(filename):
|
||||
return configer(filename)
|
||||
class configer:
|
||||
def __init__(self,fullfilepath):
|
||||
self._filepath=fullfilepath
|
||||
if not os.path.isdir(os.path.dirname(fullfilepath)):
|
||||
os.makedirs(os.path.dirname(fullfilepath))
|
||||
self._conf=ConfigParser.ConfigParser()
|
||||
if os.path.isfile(fullfilepath):
|
||||
try:
|
||||
self._conf.readfp(open(fullfilepath,"r"))
|
||||
except Exception,e:
|
||||
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
|
||||
def save(self):
|
||||
try:
|
||||
self._conf.write(open(self._filepath,"w"))
|
||||
except Exception,e:
|
||||
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
|
||||
|
||||
def changeConfValue(self,section,option,value):
|
||||
if self._conf.has_section(section):
|
||||
self._conf.set(section,option,value)
|
||||
else:
|
||||
self._conf.add_section(section)
|
||||
self._conf.set(section,option,value)
|
||||
|
||||
def _readvalue(self,fn,section,option,default):
|
||||
result=default
|
||||
if self._conf.has_section(section):
|
||||
if self._conf.has_option(section,option):
|
||||
result=fn(section,option)
|
||||
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
|
||||
else:
|
||||
self._conf.set(section,option,str(default))
|
||||
result=default
|
||||
else:
|
||||
self._conf.add_section(section)
|
||||
self._conf.set(section,option,str(default))
|
||||
result=default
|
||||
return result
|
||||
def getstr(self,section,option,default=""):
|
||||
return self._readvalue(self._conf.get, section, option, default)
|
||||
def getint(self,section,option,default=0):
|
||||
return self._readvalue(self._conf.getint, section, option, default)
|
||||
def getfloat(self,section,option,default=0.0):
|
||||
return self._readvalue(self._conf.getfloat, section, option, default)
|
||||
def getboolean(self,section,option,default=False):
|
||||
return self._readvalue(self._conf.getboolean, section, option, default)
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import ConfigParser
|
||||
from log import PLOG
|
||||
import os
|
||||
def getdefaultfilename():
|
||||
pass
|
||||
def openconfiger(filename):
|
||||
return configer(filename)
|
||||
class configer:
|
||||
def __init__(self,fullfilepath):
|
||||
self._filepath=fullfilepath
|
||||
if not os.path.isdir(os.path.dirname(fullfilepath)):
|
||||
os.makedirs(os.path.dirname(fullfilepath))
|
||||
self._conf=ConfigParser.ConfigParser()
|
||||
if os.path.isfile(fullfilepath):
|
||||
try:
|
||||
self._conf.readfp(open(fullfilepath,"r"))
|
||||
except Exception,e:
|
||||
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
|
||||
def save(self):
|
||||
try:
|
||||
self._conf.write(open(self._filepath,"w"))
|
||||
except Exception,e:
|
||||
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
|
||||
|
||||
def changeConfValue(self,section,option,value):
|
||||
if self._conf.has_section(section):
|
||||
self._conf.set(section,option,value)
|
||||
else:
|
||||
self._conf.add_section(section)
|
||||
self._conf.set(section,option,value)
|
||||
|
||||
def _readvalue(self,fn,section,option,default):
|
||||
result=default
|
||||
if self._conf.has_section(section):
|
||||
if self._conf.has_option(section,option):
|
||||
result=fn(section,option)
|
||||
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
|
||||
else:
|
||||
self._conf.set(section,option,str(default))
|
||||
result=default
|
||||
else:
|
||||
self._conf.add_section(section)
|
||||
self._conf.set(section,option,str(default))
|
||||
result=default
|
||||
return result
|
||||
def getstr(self,section,option,default=""):
|
||||
return self._readvalue(self._conf.get, section, option, default)
|
||||
def getint(self,section,option,default=0):
|
||||
return self._readvalue(self._conf.getint, section, option, default)
|
||||
def getfloat(self,section,option,default=0.0):
|
||||
return self._readvalue(self._conf.getfloat, section, option, default)
|
||||
def getboolean(self,section,option,default=False):
|
||||
return self._readvalue(self._conf.getboolean, section, option, default)
|
||||
|
|
|
|||
|
|
@ -1,140 +1,140 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import sys, os, time, atexit
|
||||
from signal import SIGTERM
|
||||
|
||||
def check_pid(pid):
|
||||
""" Check For the existence of a unix pid. """
|
||||
if pid == 0:return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
class daemon:
|
||||
"""
|
||||
A generic daemon class.
|
||||
|
||||
Usage: subclass the Daemon class and override the run() method
|
||||
"""
|
||||
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
|
||||
self.stdin = stdin
|
||||
self.stdout = stdout
|
||||
self.stderr = stderr
|
||||
self.pidfile = pidfile
|
||||
|
||||
def daemonize(self):
|
||||
"""
|
||||
do the UNIX double-fork magic, see Stevens' "Advanced
|
||||
Programming in the UNIX Environment" for details (ISBN 0201563177)
|
||||
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
|
||||
"""
|
||||
try:
|
||||
pid = os.fork()
|
||||
if pid > 0:
|
||||
# exit first parent
|
||||
sys.exit(0)
|
||||
except OSError, e:
|
||||
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||
sys.exit(1)
|
||||
|
||||
# decouple from parent environment
|
||||
os.chdir("/")
|
||||
os.setsid()
|
||||
os.umask(0)
|
||||
|
||||
# do second fork
|
||||
try:
|
||||
pid = os.fork()
|
||||
if pid > 0:
|
||||
# exit from second parent
|
||||
sys.exit(0)
|
||||
except OSError, e:
|
||||
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||
sys.exit(1)
|
||||
|
||||
# redirect standard file descriptors
|
||||
sys.stdout.flush()
|
||||
sys.stderr.flush()
|
||||
si = file(self.stdin, 'r')
|
||||
so = file(self.stdout, 'a+')
|
||||
se = file(self.stderr, 'a+', 0)
|
||||
os.dup2(si.fileno(), sys.stdin.fileno())
|
||||
os.dup2(so.fileno(), sys.stdout.fileno())
|
||||
os.dup2(se.fileno(), sys.stderr.fileno())
|
||||
|
||||
# write pidfile
|
||||
atexit.register(self.delpid)
|
||||
pid = str(os.getpid())
|
||||
file(self.pidfile, 'w+').write("%s\n" % pid)
|
||||
|
||||
def delpid(self):
|
||||
os.remove(self.pidfile)
|
||||
|
||||
def start(self):
|
||||
"""
|
||||
Start the daemon
|
||||
"""
|
||||
# Check for a pidfile to see if the daemon already runs
|
||||
try:
|
||||
pf = file(self.pidfile, 'r')
|
||||
pid = int(pf.read().strip())
|
||||
pf.close()
|
||||
except IOError:
|
||||
pid = None
|
||||
|
||||
if pid and check_pid(pid):
|
||||
message = "pidfile %s already exist. Daemon already running?\n"
|
||||
sys.stderr.write(message % self.pidfile)
|
||||
sys.exit(1)
|
||||
print("daemon start...")
|
||||
# Start the daemon
|
||||
self.daemonize()
|
||||
self.run()
|
||||
|
||||
def stop(self):
|
||||
"""
|
||||
Stop the daemon
|
||||
"""
|
||||
# Get the pid from the pidfile
|
||||
try:
|
||||
pf = file(self.pidfile, 'r')
|
||||
pid = int(pf.read().strip())
|
||||
pf.close()
|
||||
except IOError:
|
||||
pid = None
|
||||
|
||||
if not pid:
|
||||
message = "pidfile %s does not exist. Daemon not running?\n"
|
||||
sys.stderr.write(message % self.pidfile)
|
||||
return # not an error in a restart
|
||||
|
||||
# Try killing the daemon process
|
||||
try:
|
||||
while 1:
|
||||
os.kill(pid, SIGTERM)
|
||||
time.sleep(0.1)
|
||||
except OSError, err:
|
||||
err = str(err)
|
||||
if err.find("No such process") > 0:
|
||||
if os.path.exists(self.pidfile):
|
||||
os.remove(self.pidfile)
|
||||
else:
|
||||
print(str(err))
|
||||
sys.exit(1)
|
||||
|
||||
def restart(self):
|
||||
"""
|
||||
Restart the daemon
|
||||
"""
|
||||
self.stop()
|
||||
self.start()
|
||||
|
||||
def run(self):
|
||||
"""
|
||||
You should override this method when you subclass Daemon. It will be called after the process has been
|
||||
daemonized by start() or restart().
|
||||
"""
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import sys, os, time, atexit
|
||||
from signal import SIGTERM
|
||||
|
||||
def check_pid(pid):
|
||||
""" Check For the existence of a unix pid. """
|
||||
if pid == 0:return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
class daemon:
|
||||
"""
|
||||
A generic daemon class.
|
||||
|
||||
Usage: subclass the Daemon class and override the run() method
|
||||
"""
|
||||
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
|
||||
self.stdin = stdin
|
||||
self.stdout = stdout
|
||||
self.stderr = stderr
|
||||
self.pidfile = pidfile
|
||||
|
||||
def daemonize(self):
|
||||
"""
|
||||
do the UNIX double-fork magic, see Stevens' "Advanced
|
||||
Programming in the UNIX Environment" for details (ISBN 0201563177)
|
||||
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
|
||||
"""
|
||||
try:
|
||||
pid = os.fork()
|
||||
if pid > 0:
|
||||
# exit first parent
|
||||
sys.exit(0)
|
||||
except OSError, e:
|
||||
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||
sys.exit(1)
|
||||
|
||||
# decouple from parent environment
|
||||
os.chdir("/")
|
||||
os.setsid()
|
||||
os.umask(0)
|
||||
|
||||
# do second fork
|
||||
try:
|
||||
pid = os.fork()
|
||||
if pid > 0:
|
||||
# exit from second parent
|
||||
sys.exit(0)
|
||||
except OSError, e:
|
||||
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||
sys.exit(1)
|
||||
|
||||
# redirect standard file descriptors
|
||||
sys.stdout.flush()
|
||||
sys.stderr.flush()
|
||||
si = file(self.stdin, 'r')
|
||||
so = file(self.stdout, 'a+')
|
||||
se = file(self.stderr, 'a+', 0)
|
||||
os.dup2(si.fileno(), sys.stdin.fileno())
|
||||
os.dup2(so.fileno(), sys.stdout.fileno())
|
||||
os.dup2(se.fileno(), sys.stderr.fileno())
|
||||
|
||||
# write pidfile
|
||||
atexit.register(self.delpid)
|
||||
pid = str(os.getpid())
|
||||
file(self.pidfile, 'w+').write("%s\n" % pid)
|
||||
|
||||
def delpid(self):
|
||||
os.remove(self.pidfile)
|
||||
|
||||
def start(self):
|
||||
"""
|
||||
Start the daemon
|
||||
"""
|
||||
# Check for a pidfile to see if the daemon already runs
|
||||
try:
|
||||
pf = file(self.pidfile, 'r')
|
||||
pid = int(pf.read().strip())
|
||||
pf.close()
|
||||
except IOError:
|
||||
pid = None
|
||||
|
||||
if pid and check_pid(pid):
|
||||
message = "pidfile %s already exist. Daemon already running?\n"
|
||||
sys.stderr.write(message % self.pidfile)
|
||||
sys.exit(1)
|
||||
print("daemon start...")
|
||||
# Start the daemon
|
||||
self.daemonize()
|
||||
self.run()
|
||||
|
||||
def stop(self):
|
||||
"""
|
||||
Stop the daemon
|
||||
"""
|
||||
# Get the pid from the pidfile
|
||||
try:
|
||||
pf = file(self.pidfile, 'r')
|
||||
pid = int(pf.read().strip())
|
||||
pf.close()
|
||||
except IOError:
|
||||
pid = None
|
||||
|
||||
if not pid:
|
||||
message = "pidfile %s does not exist. Daemon not running?\n"
|
||||
sys.stderr.write(message % self.pidfile)
|
||||
return # not an error in a restart
|
||||
|
||||
# Try killing the daemon process
|
||||
try:
|
||||
while 1:
|
||||
os.kill(pid, SIGTERM)
|
||||
time.sleep(0.1)
|
||||
except OSError, err:
|
||||
err = str(err)
|
||||
if err.find("No such process") > 0:
|
||||
if os.path.exists(self.pidfile):
|
||||
os.remove(self.pidfile)
|
||||
else:
|
||||
print(str(err))
|
||||
sys.exit(1)
|
||||
|
||||
def restart(self):
|
||||
"""
|
||||
Restart the daemon
|
||||
"""
|
||||
self.stop()
|
||||
self.start()
|
||||
|
||||
def run(self):
|
||||
"""
|
||||
You should override this method when you subclass Daemon. It will be called after the process has been
|
||||
daemonized by start() or restart().
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -1,58 +1,58 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
# 异常类
|
||||
class PibotError(Exception):
|
||||
def __init__(self, errcode, errmsg):
|
||||
self.errcode = errcode
|
||||
self.errmsg = errmsg
|
||||
#Exception.__init__(self,self.__str__())
|
||||
|
||||
def msg(self, msg):
|
||||
if not msg is None:return PibotError(self.errcode, msg)
|
||||
return PibotError(8,"unknow error")
|
||||
def __str__(self):
|
||||
return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
|
||||
@property
|
||||
def message(self):
|
||||
return str(self)
|
||||
# 声明
|
||||
# 成功
|
||||
success=PibotError(0,"null")
|
||||
# 通用失败
|
||||
fail=PibotError(1,"fail")
|
||||
# 参数无效
|
||||
invalidParameter=PibotError(2,"invalid parameter")
|
||||
# 不支持
|
||||
noSupport=PibotError(3,"no support")
|
||||
# 不存在
|
||||
noExist=PibotError(4,"no exist")
|
||||
# 超时
|
||||
timeout=PibotError(5,"timeout")
|
||||
# 繁忙
|
||||
busy=PibotError(6,"busy")
|
||||
# 缺少参数
|
||||
missParameter=PibotError(7,"miss parameter")
|
||||
# 系统错误(通用错误)
|
||||
systemError=PibotError(8,"system error")
|
||||
# 密码错误
|
||||
invalidPassword=PibotError(9,"invalid password")
|
||||
# 编码失败
|
||||
encodeFailed=PibotError(10,"encode failed")
|
||||
# 数据库操作失败
|
||||
dbOpertationFailed=PibotError(11,"db error")
|
||||
# 已占用
|
||||
occupied=PibotError(12,"occupied")
|
||||
# session不存在
|
||||
noSession = PibotError(13,'cannot find session')
|
||||
#没有找到
|
||||
noFound = PibotError(14, "no found")
|
||||
#已经存在
|
||||
existed = PibotError(15, "existed")
|
||||
#已经锁定
|
||||
locked = PibotError(16, "locked")
|
||||
#已经过期
|
||||
expired = PibotError(17, "is expired")
|
||||
#无效的参数
|
||||
invalidParameter = PibotError(18, "invalid parameter")
|
||||
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
# 异常类
|
||||
class PibotError(Exception):
|
||||
def __init__(self, errcode, errmsg):
|
||||
self.errcode = errcode
|
||||
self.errmsg = errmsg
|
||||
#Exception.__init__(self,self.__str__())
|
||||
|
||||
def msg(self, msg):
|
||||
if not msg is None:return PibotError(self.errcode, msg)
|
||||
return PibotError(8,"unknow error")
|
||||
def __str__(self):
|
||||
return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
|
||||
@property
|
||||
def message(self):
|
||||
return str(self)
|
||||
# 声明
|
||||
# 成功
|
||||
success=PibotError(0,"null")
|
||||
# 通用失败
|
||||
fail=PibotError(1,"fail")
|
||||
# 参数无效
|
||||
invalidParameter=PibotError(2,"invalid parameter")
|
||||
# 不支持
|
||||
noSupport=PibotError(3,"no support")
|
||||
# 不存在
|
||||
noExist=PibotError(4,"no exist")
|
||||
# 超时
|
||||
timeout=PibotError(5,"timeout")
|
||||
# 繁忙
|
||||
busy=PibotError(6,"busy")
|
||||
# 缺少参数
|
||||
missParameter=PibotError(7,"miss parameter")
|
||||
# 系统错误(通用错误)
|
||||
systemError=PibotError(8,"system error")
|
||||
# 密码错误
|
||||
invalidPassword=PibotError(9,"invalid password")
|
||||
# 编码失败
|
||||
encodeFailed=PibotError(10,"encode failed")
|
||||
# 数据库操作失败
|
||||
dbOpertationFailed=PibotError(11,"db error")
|
||||
# 已占用
|
||||
occupied=PibotError(12,"occupied")
|
||||
# session不存在
|
||||
noSession = PibotError(13,'cannot find session')
|
||||
#没有找到
|
||||
noFound = PibotError(14, "no found")
|
||||
#已经存在
|
||||
existed = PibotError(15, "existed")
|
||||
#已经锁定
|
||||
locked = PibotError(16, "locked")
|
||||
#已经过期
|
||||
expired = PibotError(17, "is expired")
|
||||
#无效的参数
|
||||
invalidParameter = PibotError(18, "invalid parameter")
|
||||
|
||||
|
|
|
|||
|
|
@ -1,259 +1,259 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import sys,os
|
||||
import datetime
|
||||
import threading
|
||||
import pypibot.assistant as assistant
|
||||
import platform
|
||||
if assistant.is_python3():
|
||||
import _thread
|
||||
else:
|
||||
import thread
|
||||
import traceback
|
||||
"""
|
||||
%a Locale’s abbreviated weekday name.
|
||||
%A Locale’s full weekday name.
|
||||
%b Locale’s abbreviated month name.
|
||||
%B Locale’s full month name.
|
||||
%c Locale’s appropriate date and time representation.
|
||||
%d Day of the month as a decimal number [01,31].
|
||||
%H Hour (24-hour clock) as a decimal number [00,23].
|
||||
%I Hour (12-hour clock) as a decimal number [01,12].
|
||||
%j Day of the year as a decimal number [001,366].
|
||||
%m Month as a decimal number [01,12].
|
||||
%M Minute as a decimal number [00,59].
|
||||
%p Locale’s equivalent of either AM or PM. (1)
|
||||
%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)
|
||||
%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)
|
||||
%x Locale’s appropriate date representation.
|
||||
%X Locale’s appropriate time representation.
|
||||
%y Year without century as a decimal number [00,99].
|
||||
%Y Year with century as a decimal number.
|
||||
%Z Time zone name (no characters if no time zone exists).
|
||||
%% A literal '%' character.
|
||||
|
||||
"""
|
||||
isWindows=False
|
||||
if platform.system()=='Windows':
|
||||
isWindows=True
|
||||
defaultEncodeing="utf8"
|
||||
if "-utf8" in sys.argv:
|
||||
defaultEncodeing="utf-8"
|
||||
if "-gbk" in sys.argv:
|
||||
defaultEncodeing="gbk"
|
||||
|
||||
TRACE=5
|
||||
DEBUG=4
|
||||
INFORMATION=3
|
||||
WARNING=2
|
||||
ERROR=1
|
||||
NONE=0
|
||||
|
||||
MAX_MSG_SIZE = 4096
|
||||
|
||||
def getLevelFromString(level):
|
||||
level=level.lower()
|
||||
if level=='t' or level=='trace':return 5
|
||||
elif level=='d' or level=='debug':return 4
|
||||
elif level=='i' or level=='info':return 3
|
||||
elif level=='w' or level=='wran':return 2
|
||||
elif level=='e' or level=='error':return 1
|
||||
else :return 0
|
||||
def getLevelString(level):
|
||||
if level==TRACE:return "T"
|
||||
elif level==DEBUG:return "D"
|
||||
elif level==INFORMATION:return "I"
|
||||
elif level==WARNING:return "W"
|
||||
elif level==ERROR:return "E"
|
||||
else:return "N"
|
||||
class PibotLog:
|
||||
def __init__(self):
|
||||
self.isEnableControlLog=True
|
||||
self.fileTemple=None
|
||||
self.filePath=""
|
||||
self.level=5
|
||||
self._lock=threading.RLock()
|
||||
self.fd=None
|
||||
self.fd_day=-1
|
||||
def setLevel(self,level):
|
||||
self.level=getLevelFromString(level)
|
||||
def enableControllog(self,enable):
|
||||
self.isEnableControlLog=enable
|
||||
def enableFileLog(self,fileName):
|
||||
self.fileTemple=fileName
|
||||
self.updateFilelog()
|
||||
def updateFilelog(self):
|
||||
fn=assistant.SF(self.fileTemple)
|
||||
if fn!=self.filePath:
|
||||
self.i("new log file:%s",fn)
|
||||
if self.fd:
|
||||
self.fd.close()
|
||||
self.fd=None
|
||||
self.fd_day=-1
|
||||
self.filePath=""
|
||||
try:
|
||||
path=os.path.dirname(fn)
|
||||
if path!="" and not os.path.isdir(path):os.makedirs(path)
|
||||
self.fd=open(fn,mode="w")
|
||||
try:
|
||||
linkfn = fn.split(".log.", 1)[0] + ".INFO"
|
||||
if os.path.exists(linkfn):
|
||||
os.remove(linkfn)
|
||||
(filepath, tempfilename) = os.path.split(fn)
|
||||
os.symlink(tempfilename, linkfn)
|
||||
except:
|
||||
pass
|
||||
self.fd_day=datetime.datetime.now().day
|
||||
self.filePath=fn
|
||||
except Exception as e:
|
||||
print("open file fail!file=%s,err=%s"%(fn,e))
|
||||
def _output(self,level,msg,args):
|
||||
if self.level<level:return
|
||||
try:
|
||||
if len(args)>0:
|
||||
t=[]
|
||||
for arg in args:
|
||||
if isinstance(arg,Exception):
|
||||
t.append(traceback.format_exc(arg).decode('utf-8'))
|
||||
elif isinstance(arg,bytes) :
|
||||
t.append(arg.decode('utf-8'))
|
||||
else:
|
||||
t.append(arg)
|
||||
args=tuple(t)
|
||||
try:
|
||||
msg=msg%args
|
||||
except:
|
||||
try:
|
||||
for arg in args:
|
||||
msg=msg+str(arg)+" "
|
||||
except Exception as e:
|
||||
msg=msg+"==LOG ERROR==>%s"%(e)
|
||||
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
|
||||
now=datetime.datetime.now()
|
||||
msg=msg+"\r\n"
|
||||
if assistant.is_python3():
|
||||
id = _thread.get_ident()
|
||||
else:
|
||||
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)
|
||||
prefix=''
|
||||
suffix=''
|
||||
if not isWindows:
|
||||
suffix='\033[0m'
|
||||
if level==TRACE:
|
||||
prefix='\033[0;37m'
|
||||
elif level==DEBUG:
|
||||
prefix='\033[1m'
|
||||
elif level==INFORMATION:
|
||||
prefix='\033[0;32m'
|
||||
elif level==WARNING:
|
||||
prefix='\033[0;33m'
|
||||
elif level==ERROR:
|
||||
prefix='\033[0;31m'
|
||||
else:
|
||||
pass
|
||||
self._lock.acquire()
|
||||
try:
|
||||
if self.isEnableControlLog:
|
||||
sys.stdout.write((prefix+s+suffix))
|
||||
if self.fd:
|
||||
if self.fd_day!=now.day:
|
||||
self.updateFilelog()
|
||||
if assistant.is_python3():
|
||||
self.fd.write(s)
|
||||
else:
|
||||
self.fd.write(s.encode('utf-8'))
|
||||
self.fd.flush()
|
||||
finally:
|
||||
self._lock.release()
|
||||
except Exception as e:
|
||||
if assistant.is_python3():
|
||||
print(e)
|
||||
else:
|
||||
print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
|
||||
|
||||
def trace(self,msg,*args):
|
||||
self._output(TRACE,msg,args)
|
||||
def t(self,msg,*args):
|
||||
self._output(TRACE,msg,args)
|
||||
def debug(self,msg,*args):
|
||||
self._output(DEBUG, msg,args)
|
||||
def d(self,msg,*args):
|
||||
self._output(DEBUG, msg,args)
|
||||
def info(self,msg,*args):
|
||||
self._output(INFORMATION, msg,args)
|
||||
def i(self,msg,*args):
|
||||
self._output(INFORMATION, msg,args)
|
||||
def warn(self,msg,*args):
|
||||
self._output(WARNING, msg,args)
|
||||
def w(self,msg,*args):
|
||||
self._output(WARNING, msg,args)
|
||||
def error(self,msg,*args):
|
||||
self._output(ERROR, msg,args)
|
||||
def e(self,msg,*args):
|
||||
self._output(ERROR, msg,args)
|
||||
def _log(self,level,msg,args):
|
||||
self._output(level, msg,args)
|
||||
def createNamedLog(self,name):
|
||||
return NamedLog(name)
|
||||
log=PibotLog()
|
||||
class NamedLog:
|
||||
def __init__(self,name=None):
|
||||
global log
|
||||
self.name=''
|
||||
if name:
|
||||
self.name='['+name+']'
|
||||
self.log=log
|
||||
def trace(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(TRACE,msg,args)
|
||||
def t(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(TRACE,msg,args)
|
||||
def debug(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(DEBUG, msg,args)
|
||||
def d(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(DEBUG, msg,args)
|
||||
def info(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(INFORMATION, msg,args)
|
||||
def i(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(INFORMATION, msg,args)
|
||||
def warn(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(WARNING, msg,args)
|
||||
def w(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(WARNING, msg,args)
|
||||
def error(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(ERROR, msg,args)
|
||||
def e(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(ERROR, msg,args)
|
||||
|
||||
if __name__ == "__main__":
|
||||
log.trace("1%s","hello")
|
||||
log.debug("2%d",12)
|
||||
try:
|
||||
raise Exception("EXC")
|
||||
except Exception as e:
|
||||
log.info("3%s",e)
|
||||
log.warn("1")
|
||||
log.error("1")
|
||||
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
|
||||
log.trace("1")
|
||||
log.debug("1")
|
||||
log.info("1")
|
||||
log.warn("1")
|
||||
log.error("1")
|
||||
log=NamedLog("test")
|
||||
log.d("1")
|
||||
log.i("1")
|
||||
log.warn("1")
|
||||
log.error("1=%d,%s",100,e)
|
||||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
import sys,os
|
||||
import datetime
|
||||
import threading
|
||||
import pypibot.assistant as assistant
|
||||
import platform
|
||||
if assistant.is_python3():
|
||||
import _thread
|
||||
else:
|
||||
import thread
|
||||
import traceback
|
||||
"""
|
||||
%a Locale’s abbreviated weekday name.
|
||||
%A Locale’s full weekday name.
|
||||
%b Locale’s abbreviated month name.
|
||||
%B Locale’s full month name.
|
||||
%c Locale’s appropriate date and time representation.
|
||||
%d Day of the month as a decimal number [01,31].
|
||||
%H Hour (24-hour clock) as a decimal number [00,23].
|
||||
%I Hour (12-hour clock) as a decimal number [01,12].
|
||||
%j Day of the year as a decimal number [001,366].
|
||||
%m Month as a decimal number [01,12].
|
||||
%M Minute as a decimal number [00,59].
|
||||
%p Locale’s equivalent of either AM or PM. (1)
|
||||
%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)
|
||||
%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)
|
||||
%x Locale’s appropriate date representation.
|
||||
%X Locale’s appropriate time representation.
|
||||
%y Year without century as a decimal number [00,99].
|
||||
%Y Year with century as a decimal number.
|
||||
%Z Time zone name (no characters if no time zone exists).
|
||||
%% A literal '%' character.
|
||||
|
||||
"""
|
||||
isWindows=False
|
||||
if platform.system()=='Windows':
|
||||
isWindows=True
|
||||
defaultEncodeing="utf8"
|
||||
if "-utf8" in sys.argv:
|
||||
defaultEncodeing="utf-8"
|
||||
if "-gbk" in sys.argv:
|
||||
defaultEncodeing="gbk"
|
||||
|
||||
TRACE=5
|
||||
DEBUG=4
|
||||
INFORMATION=3
|
||||
WARNING=2
|
||||
ERROR=1
|
||||
NONE=0
|
||||
|
||||
MAX_MSG_SIZE = 4096
|
||||
|
||||
def getLevelFromString(level):
|
||||
level=level.lower()
|
||||
if level=='t' or level=='trace':return 5
|
||||
elif level=='d' or level=='debug':return 4
|
||||
elif level=='i' or level=='info':return 3
|
||||
elif level=='w' or level=='wran':return 2
|
||||
elif level=='e' or level=='error':return 1
|
||||
else :return 0
|
||||
def getLevelString(level):
|
||||
if level==TRACE:return "T"
|
||||
elif level==DEBUG:return "D"
|
||||
elif level==INFORMATION:return "I"
|
||||
elif level==WARNING:return "W"
|
||||
elif level==ERROR:return "E"
|
||||
else:return "N"
|
||||
class PibotLog:
|
||||
def __init__(self):
|
||||
self.isEnableControlLog=True
|
||||
self.fileTemple=None
|
||||
self.filePath=""
|
||||
self.level=5
|
||||
self._lock=threading.RLock()
|
||||
self.fd=None
|
||||
self.fd_day=-1
|
||||
def setLevel(self,level):
|
||||
self.level=getLevelFromString(level)
|
||||
def enableControllog(self,enable):
|
||||
self.isEnableControlLog=enable
|
||||
def enableFileLog(self,fileName):
|
||||
self.fileTemple=fileName
|
||||
self.updateFilelog()
|
||||
def updateFilelog(self):
|
||||
fn=assistant.SF(self.fileTemple)
|
||||
if fn!=self.filePath:
|
||||
self.i("new log file:%s",fn)
|
||||
if self.fd:
|
||||
self.fd.close()
|
||||
self.fd=None
|
||||
self.fd_day=-1
|
||||
self.filePath=""
|
||||
try:
|
||||
path=os.path.dirname(fn)
|
||||
if path!="" and not os.path.isdir(path):os.makedirs(path)
|
||||
self.fd=open(fn,mode="w")
|
||||
try:
|
||||
linkfn = fn.split(".log.", 1)[0] + ".INFO"
|
||||
if os.path.exists(linkfn):
|
||||
os.remove(linkfn)
|
||||
(filepath, tempfilename) = os.path.split(fn)
|
||||
os.symlink(tempfilename, linkfn)
|
||||
except:
|
||||
pass
|
||||
self.fd_day=datetime.datetime.now().day
|
||||
self.filePath=fn
|
||||
except Exception as e:
|
||||
print("open file fail!file=%s,err=%s"%(fn,e))
|
||||
def _output(self,level,msg,args):
|
||||
if self.level<level:return
|
||||
try:
|
||||
if len(args)>0:
|
||||
t=[]
|
||||
for arg in args:
|
||||
if isinstance(arg,Exception):
|
||||
t.append(traceback.format_exc(arg).decode('utf-8'))
|
||||
elif isinstance(arg,bytes) :
|
||||
t.append(arg.decode('utf-8'))
|
||||
else:
|
||||
t.append(arg)
|
||||
args=tuple(t)
|
||||
try:
|
||||
msg=msg%args
|
||||
except:
|
||||
try:
|
||||
for arg in args:
|
||||
msg=msg+str(arg)+" "
|
||||
except Exception as e:
|
||||
msg=msg+"==LOG ERROR==>%s"%(e)
|
||||
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
|
||||
now=datetime.datetime.now()
|
||||
msg=msg+"\r\n"
|
||||
if assistant.is_python3():
|
||||
id = _thread.get_ident()
|
||||
else:
|
||||
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)
|
||||
prefix=''
|
||||
suffix=''
|
||||
if not isWindows:
|
||||
suffix='\033[0m'
|
||||
if level==TRACE:
|
||||
prefix='\033[0;37m'
|
||||
elif level==DEBUG:
|
||||
prefix='\033[1m'
|
||||
elif level==INFORMATION:
|
||||
prefix='\033[0;32m'
|
||||
elif level==WARNING:
|
||||
prefix='\033[0;33m'
|
||||
elif level==ERROR:
|
||||
prefix='\033[0;31m'
|
||||
else:
|
||||
pass
|
||||
self._lock.acquire()
|
||||
try:
|
||||
if self.isEnableControlLog:
|
||||
sys.stdout.write((prefix+s+suffix))
|
||||
if self.fd:
|
||||
if self.fd_day!=now.day:
|
||||
self.updateFilelog()
|
||||
if assistant.is_python3():
|
||||
self.fd.write(s)
|
||||
else:
|
||||
self.fd.write(s.encode('utf-8'))
|
||||
self.fd.flush()
|
||||
finally:
|
||||
self._lock.release()
|
||||
except Exception as e:
|
||||
if assistant.is_python3():
|
||||
print(e)
|
||||
else:
|
||||
print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
|
||||
|
||||
def trace(self,msg,*args):
|
||||
self._output(TRACE,msg,args)
|
||||
def t(self,msg,*args):
|
||||
self._output(TRACE,msg,args)
|
||||
def debug(self,msg,*args):
|
||||
self._output(DEBUG, msg,args)
|
||||
def d(self,msg,*args):
|
||||
self._output(DEBUG, msg,args)
|
||||
def info(self,msg,*args):
|
||||
self._output(INFORMATION, msg,args)
|
||||
def i(self,msg,*args):
|
||||
self._output(INFORMATION, msg,args)
|
||||
def warn(self,msg,*args):
|
||||
self._output(WARNING, msg,args)
|
||||
def w(self,msg,*args):
|
||||
self._output(WARNING, msg,args)
|
||||
def error(self,msg,*args):
|
||||
self._output(ERROR, msg,args)
|
||||
def e(self,msg,*args):
|
||||
self._output(ERROR, msg,args)
|
||||
def _log(self,level,msg,args):
|
||||
self._output(level, msg,args)
|
||||
def createNamedLog(self,name):
|
||||
return NamedLog(name)
|
||||
log=PibotLog()
|
||||
class NamedLog:
|
||||
def __init__(self,name=None):
|
||||
global log
|
||||
self.name=''
|
||||
if name:
|
||||
self.name='['+name+']'
|
||||
self.log=log
|
||||
def trace(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(TRACE,msg,args)
|
||||
def t(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(TRACE,msg,args)
|
||||
def debug(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(DEBUG, msg,args)
|
||||
def d(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(DEBUG, msg,args)
|
||||
def info(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(INFORMATION, msg,args)
|
||||
def i(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(INFORMATION, msg,args)
|
||||
def warn(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(WARNING, msg,args)
|
||||
def w(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(WARNING, msg,args)
|
||||
def error(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(ERROR, msg,args)
|
||||
def e(self,msg,*args):
|
||||
msg=self.name+msg
|
||||
self.log._log(ERROR, msg,args)
|
||||
|
||||
if __name__ == "__main__":
|
||||
log.trace("1%s","hello")
|
||||
log.debug("2%d",12)
|
||||
try:
|
||||
raise Exception("EXC")
|
||||
except Exception as e:
|
||||
log.info("3%s",e)
|
||||
log.warn("1")
|
||||
log.error("1")
|
||||
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
|
||||
log.trace("1")
|
||||
log.debug("1")
|
||||
log.info("1")
|
||||
log.warn("1")
|
||||
log.error("1")
|
||||
log=NamedLog("test")
|
||||
log.d("1")
|
||||
log.i("1")
|
||||
log.warn("1")
|
||||
log.error("1=%d,%s",100,e)
|
||||
|
|
|
|||
|
|
@ -1,240 +1,240 @@
|
|||
import struct
|
||||
|
||||
params_size=29
|
||||
|
||||
# main board
|
||||
class MessageID:
|
||||
ID_GET_VERSION = 0
|
||||
ID_SET_ROBOT_PARAMETER = 1
|
||||
ID_GET_ROBOT_PARAMETER = 2
|
||||
ID_INIT_ODOM = 3
|
||||
ID_SET_VEL = 4
|
||||
ID_GET_ODOM = 5
|
||||
ID_GET_PID_DEBUG = 6
|
||||
ID_GET_IMU = 7
|
||||
ID_GET_ENCODER_COUNT = 8
|
||||
ID_SET_MOTOR_PWM = 9
|
||||
|
||||
class RobotMessage:
|
||||
def pack(self):
|
||||
return b''
|
||||
|
||||
def unpack(self):
|
||||
return True
|
||||
|
||||
class RobotFirmwareInfo(RobotMessage):
|
||||
def __init__(self):
|
||||
self.version = ''
|
||||
self.build_time = ''
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('16s16s', bytes(data))
|
||||
except:
|
||||
return False
|
||||
[self.version, self.build_time] = upk
|
||||
return True
|
||||
|
||||
class RobotImuType:
|
||||
IMU_TYPE_GY65 = 49
|
||||
IMU_TYPE_GY85 = 69
|
||||
IMU_TYPE_GY87 = 71
|
||||
|
||||
class RobotModelType:
|
||||
MODEL_TYPE_2WD_DIFF = 1
|
||||
MODEL_TYPE_4WD_DIFF = 2
|
||||
MODEL_TYPE_3WD_OMNI = 101
|
||||
MODEL_TYPE_4WD_OMNI = 102
|
||||
MODEL_TYPE_4WD_MECANUM = 201
|
||||
|
||||
class RobotParameters():
|
||||
def __init__(self, wheel_diameter=0, \
|
||||
wheel_track=0, \
|
||||
encoder_resolution=0, \
|
||||
do_pid_interval=0, \
|
||||
kp=0, \
|
||||
ki=0, \
|
||||
kd=0, \
|
||||
ko=0, \
|
||||
cmd_last_time=0, \
|
||||
max_v_liner_x=0, \
|
||||
max_v_liner_y=0, \
|
||||
max_v_angular_z=0, \
|
||||
imu_type=0, \
|
||||
motor_ratio=0, \
|
||||
model_type=0, \
|
||||
motor_nonexchange_flag=255, \
|
||||
encoder_nonexchange_flag=255, \
|
||||
):
|
||||
self.wheel_diameter = wheel_diameter
|
||||
self.wheel_track = wheel_track
|
||||
self.encoder_resolution = encoder_resolution
|
||||
self.do_pid_interval = do_pid_interval
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.ko = ko
|
||||
self.cmd_last_time = cmd_last_time
|
||||
self.max_v_liner_x = max_v_liner_x
|
||||
self.max_v_liner_y = max_v_liner_y
|
||||
self.max_v_angular_z = max_v_angular_z
|
||||
self.imu_type = imu_type
|
||||
self.motor_ratio = motor_ratio
|
||||
self.model_type = model_type
|
||||
self.motor_nonexchange_flag = motor_nonexchange_flag
|
||||
self.encoder_nonexchange_flag = encoder_nonexchange_flag
|
||||
reserve=b'\xff'
|
||||
self.reserve=b''
|
||||
for i in range(64-params_size):
|
||||
self.reserve+=reserve
|
||||
robotParam = RobotParameters()
|
||||
|
||||
class GetRobotParameters(RobotMessage):
|
||||
def __init__(self):
|
||||
self.param = robotParam
|
||||
|
||||
def unpack(self, data):
|
||||
#print(bytes(data), len(bytes(data)))
|
||||
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
|
||||
|
||||
[self.param.wheel_diameter,
|
||||
self.param.wheel_track,
|
||||
self.param.encoder_resolution,
|
||||
self.param.do_pid_interval,
|
||||
self.param.kp,
|
||||
self.param.ki,
|
||||
self.param.kd,
|
||||
self.param.ko,
|
||||
self.param.cmd_last_time,
|
||||
self.param.max_v_liner_x,
|
||||
self.param.max_v_liner_y,
|
||||
self.param.max_v_angular_z,
|
||||
self.param.imu_type,
|
||||
self.param.motor_ratio,
|
||||
self.param.model_type,
|
||||
self.param.motor_nonexchange_flag,
|
||||
self.param.encoder_nonexchange_flag,
|
||||
self.param.reserve] = upk
|
||||
return True
|
||||
|
||||
class SetRobotParameters(RobotMessage):
|
||||
def __init__(self):
|
||||
self.param = robotParam
|
||||
|
||||
def pack(self):
|
||||
data = [self.param.wheel_diameter,
|
||||
self.param.wheel_track,
|
||||
self.param.encoder_resolution,
|
||||
self.param.do_pid_interval,
|
||||
self.param.kp,
|
||||
self.param.ki,
|
||||
self.param.kd,
|
||||
self.param.ko,
|
||||
self.param.cmd_last_time,
|
||||
self.param.max_v_liner_x,
|
||||
self.param.max_v_liner_y,
|
||||
self.param.max_v_angular_z,
|
||||
self.param.imu_type,
|
||||
self.param.motor_ratio,
|
||||
self.param.model_type,
|
||||
self.param.motor_nonexchange_flag,
|
||||
self.param.encoder_nonexchange_flag,
|
||||
self.param.reserve]
|
||||
|
||||
print(data)
|
||||
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
class RobotVel(RobotMessage):
|
||||
def __init__(self):
|
||||
self.v_liner_x = 0
|
||||
self.v_liner_y = 0
|
||||
self.v_angular_z = 0
|
||||
|
||||
def pack(self):
|
||||
data = [self.v_liner_x,
|
||||
self.v_liner_y,
|
||||
self.v_angular_z]
|
||||
pk = struct.pack('3h', *data)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
#todo the rest of the message classes
|
||||
class RobotOdom(RobotMessage):
|
||||
def __init__(self):
|
||||
self.v_liner_x = 0
|
||||
self.v_liner_y = 0
|
||||
self.v_angular_z = 0
|
||||
self.x = 0
|
||||
self.y = 0
|
||||
self.yaw = 0
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<3H2l1H', bytes(data))
|
||||
except:
|
||||
return False
|
||||
[self.v_liner_x,
|
||||
self.v_liner_y,
|
||||
self.v_angular_z,
|
||||
self.x,
|
||||
self.y,
|
||||
self.yaw] = upk
|
||||
return True
|
||||
|
||||
class RobotPIDData(RobotMessage):
|
||||
pass
|
||||
|
||||
class RobotIMU(RobotMessage):
|
||||
def __init__(self):
|
||||
self.imu = [0]*9
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<9f', bytes(data))
|
||||
except:
|
||||
return False
|
||||
|
||||
self.imu = upk
|
||||
return True
|
||||
|
||||
class RobotEncoderCount(RobotMessage):
|
||||
def __init__(self):
|
||||
self.encoder = [0]*4
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<4f', bytes(data))
|
||||
except:
|
||||
return False
|
||||
|
||||
self.encoder = upk
|
||||
return True
|
||||
|
||||
class RobotMotorPWM(RobotMessage):
|
||||
def __init__(self):
|
||||
self.pwm = [0]*4
|
||||
|
||||
def pack(self):
|
||||
pk = struct.pack('4h', *self.pwm)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
|
||||
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
|
||||
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
|
||||
MessageID.ID_SET_VEL:RobotVel(),
|
||||
MessageID.ID_GET_ODOM:RobotOdom(),
|
||||
MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
|
||||
MessageID.ID_GET_IMU: RobotIMU(),
|
||||
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
|
||||
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
|
||||
}
|
||||
|
||||
import struct
|
||||
|
||||
params_size=29
|
||||
|
||||
# main board
|
||||
class MessageID:
|
||||
ID_GET_VERSION = 0
|
||||
ID_SET_ROBOT_PARAMETER = 1
|
||||
ID_GET_ROBOT_PARAMETER = 2
|
||||
ID_INIT_ODOM = 3
|
||||
ID_SET_VEL = 4
|
||||
ID_GET_ODOM = 5
|
||||
ID_GET_PID_DEBUG = 6
|
||||
ID_GET_IMU = 7
|
||||
ID_GET_ENCODER_COUNT = 8
|
||||
ID_SET_MOTOR_PWM = 9
|
||||
|
||||
class RobotMessage:
|
||||
def pack(self):
|
||||
return b''
|
||||
|
||||
def unpack(self):
|
||||
return True
|
||||
|
||||
class RobotFirmwareInfo(RobotMessage):
|
||||
def __init__(self):
|
||||
self.version = ''
|
||||
self.build_time = ''
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('16s16s', bytes(data))
|
||||
except:
|
||||
return False
|
||||
[self.version, self.build_time] = upk
|
||||
return True
|
||||
|
||||
class RobotImuType:
|
||||
IMU_TYPE_GY65 = 49
|
||||
IMU_TYPE_GY85 = 69
|
||||
IMU_TYPE_GY87 = 71
|
||||
|
||||
class RobotModelType:
|
||||
MODEL_TYPE_2WD_DIFF = 1
|
||||
MODEL_TYPE_4WD_DIFF = 2
|
||||
MODEL_TYPE_3WD_OMNI = 101
|
||||
MODEL_TYPE_4WD_OMNI = 102
|
||||
MODEL_TYPE_4WD_MECANUM = 201
|
||||
|
||||
class RobotParameters():
|
||||
def __init__(self, wheel_diameter=0, \
|
||||
wheel_track=0, \
|
||||
encoder_resolution=0, \
|
||||
do_pid_interval=0, \
|
||||
kp=0, \
|
||||
ki=0, \
|
||||
kd=0, \
|
||||
ko=0, \
|
||||
cmd_last_time=0, \
|
||||
max_v_liner_x=0, \
|
||||
max_v_liner_y=0, \
|
||||
max_v_angular_z=0, \
|
||||
imu_type=0, \
|
||||
motor_ratio=0, \
|
||||
model_type=0, \
|
||||
motor_nonexchange_flag=255, \
|
||||
encoder_nonexchange_flag=255, \
|
||||
):
|
||||
self.wheel_diameter = wheel_diameter
|
||||
self.wheel_track = wheel_track
|
||||
self.encoder_resolution = encoder_resolution
|
||||
self.do_pid_interval = do_pid_interval
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.ko = ko
|
||||
self.cmd_last_time = cmd_last_time
|
||||
self.max_v_liner_x = max_v_liner_x
|
||||
self.max_v_liner_y = max_v_liner_y
|
||||
self.max_v_angular_z = max_v_angular_z
|
||||
self.imu_type = imu_type
|
||||
self.motor_ratio = motor_ratio
|
||||
self.model_type = model_type
|
||||
self.motor_nonexchange_flag = motor_nonexchange_flag
|
||||
self.encoder_nonexchange_flag = encoder_nonexchange_flag
|
||||
reserve=b'\xff'
|
||||
self.reserve=b''
|
||||
for i in range(64-params_size):
|
||||
self.reserve+=reserve
|
||||
robotParam = RobotParameters()
|
||||
|
||||
class GetRobotParameters(RobotMessage):
|
||||
def __init__(self):
|
||||
self.param = robotParam
|
||||
|
||||
def unpack(self, data):
|
||||
#print(bytes(data), len(bytes(data)))
|
||||
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
|
||||
|
||||
[self.param.wheel_diameter,
|
||||
self.param.wheel_track,
|
||||
self.param.encoder_resolution,
|
||||
self.param.do_pid_interval,
|
||||
self.param.kp,
|
||||
self.param.ki,
|
||||
self.param.kd,
|
||||
self.param.ko,
|
||||
self.param.cmd_last_time,
|
||||
self.param.max_v_liner_x,
|
||||
self.param.max_v_liner_y,
|
||||
self.param.max_v_angular_z,
|
||||
self.param.imu_type,
|
||||
self.param.motor_ratio,
|
||||
self.param.model_type,
|
||||
self.param.motor_nonexchange_flag,
|
||||
self.param.encoder_nonexchange_flag,
|
||||
self.param.reserve] = upk
|
||||
return True
|
||||
|
||||
class SetRobotParameters(RobotMessage):
|
||||
def __init__(self):
|
||||
self.param = robotParam
|
||||
|
||||
def pack(self):
|
||||
data = [self.param.wheel_diameter,
|
||||
self.param.wheel_track,
|
||||
self.param.encoder_resolution,
|
||||
self.param.do_pid_interval,
|
||||
self.param.kp,
|
||||
self.param.ki,
|
||||
self.param.kd,
|
||||
self.param.ko,
|
||||
self.param.cmd_last_time,
|
||||
self.param.max_v_liner_x,
|
||||
self.param.max_v_liner_y,
|
||||
self.param.max_v_angular_z,
|
||||
self.param.imu_type,
|
||||
self.param.motor_ratio,
|
||||
self.param.model_type,
|
||||
self.param.motor_nonexchange_flag,
|
||||
self.param.encoder_nonexchange_flag,
|
||||
self.param.reserve]
|
||||
|
||||
print(data)
|
||||
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
class RobotVel(RobotMessage):
|
||||
def __init__(self):
|
||||
self.v_liner_x = 0
|
||||
self.v_liner_y = 0
|
||||
self.v_angular_z = 0
|
||||
|
||||
def pack(self):
|
||||
data = [self.v_liner_x,
|
||||
self.v_liner_y,
|
||||
self.v_angular_z]
|
||||
pk = struct.pack('3h', *data)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
#todo the rest of the message classes
|
||||
class RobotOdom(RobotMessage):
|
||||
def __init__(self):
|
||||
self.v_liner_x = 0
|
||||
self.v_liner_y = 0
|
||||
self.v_angular_z = 0
|
||||
self.x = 0
|
||||
self.y = 0
|
||||
self.yaw = 0
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<3H2l1H', bytes(data))
|
||||
except:
|
||||
return False
|
||||
[self.v_liner_x,
|
||||
self.v_liner_y,
|
||||
self.v_angular_z,
|
||||
self.x,
|
||||
self.y,
|
||||
self.yaw] = upk
|
||||
return True
|
||||
|
||||
class RobotPIDData(RobotMessage):
|
||||
pass
|
||||
|
||||
class RobotIMU(RobotMessage):
|
||||
def __init__(self):
|
||||
self.imu = [0]*9
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<9f', bytes(data))
|
||||
except:
|
||||
return False
|
||||
|
||||
self.imu = upk
|
||||
return True
|
||||
|
||||
class RobotEncoderCount(RobotMessage):
|
||||
def __init__(self):
|
||||
self.encoder = [0]*4
|
||||
|
||||
def unpack(self, data):
|
||||
try:
|
||||
upk = struct.unpack('<4f', bytes(data))
|
||||
except:
|
||||
return False
|
||||
|
||||
self.encoder = upk
|
||||
return True
|
||||
|
||||
class RobotMotorPWM(RobotMessage):
|
||||
def __init__(self):
|
||||
self.pwm = [0]*4
|
||||
|
||||
def pack(self):
|
||||
pk = struct.pack('4h', *self.pwm)
|
||||
return pk
|
||||
|
||||
def unpack(self, data):
|
||||
return True
|
||||
|
||||
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
|
||||
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
|
||||
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
|
||||
MessageID.ID_SET_VEL:RobotVel(),
|
||||
MessageID.ID_GET_ODOM:RobotOdom(),
|
||||
MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
|
||||
MessageID.ID_GET_IMU: RobotIMU(),
|
||||
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
|
||||
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,115 +1,115 @@
|
|||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
import time
|
||||
import signal
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
run_flag = True
|
||||
|
||||
def exit(signum, frame):
|
||||
global run_flag
|
||||
run_flag = False
|
||||
|
||||
if __name__ == '__main__':
|
||||
signal.signal(signal.SIGINT, exit)
|
||||
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get params err\r\n')
|
||||
quit(1)
|
||||
|
||||
log.info("****************get odom&imu*****************")
|
||||
while run_flag:
|
||||
robotOdom = DataHolder[MessageID.ID_GET_ODOM]
|
||||
p = mboard.request(MessageID.ID_GET_ODOM)
|
||||
if p:
|
||||
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_angular_z, \
|
||||
robotOdom.x, \
|
||||
robotOdom.y, \
|
||||
robotOdom.yaw))
|
||||
else:
|
||||
log.error('get odom err')
|
||||
quit(1)
|
||||
|
||||
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
|
||||
p = mboard.request(MessageID.ID_GET_IMU)
|
||||
if p:
|
||||
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[6], robotIMU[7], robotIMU[8]))
|
||||
else:
|
||||
log.error('get imu err')
|
||||
quit(1)
|
||||
|
||||
time.sleep(0.1)
|
||||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
import time
|
||||
import signal
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
run_flag = True
|
||||
|
||||
def exit(signum, frame):
|
||||
global run_flag
|
||||
run_flag = False
|
||||
|
||||
if __name__ == '__main__':
|
||||
signal.signal(signal.SIGINT, exit)
|
||||
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get params err\r\n')
|
||||
quit(1)
|
||||
|
||||
log.info("****************get odom&imu*****************")
|
||||
while run_flag:
|
||||
robotOdom = DataHolder[MessageID.ID_GET_ODOM]
|
||||
p = mboard.request(MessageID.ID_GET_ODOM)
|
||||
if p:
|
||||
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_angular_z, \
|
||||
robotOdom.x, \
|
||||
robotOdom.y, \
|
||||
robotOdom.yaw))
|
||||
else:
|
||||
log.error('get odom err')
|
||||
quit(1)
|
||||
|
||||
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
|
||||
p = mboard.request(MessageID.ID_GET_IMU)
|
||||
if p:
|
||||
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[6], robotIMU[7], robotIMU[8]))
|
||||
else:
|
||||
log.error('get imu err')
|
||||
quit(1)
|
||||
|
||||
time.sleep(0.1)
|
||||
|
|
|
|||
|
|
@ -1,75 +1,75 @@
|
|||
import dataholder
|
||||
import os
|
||||
from dataholder import RobotImuType
|
||||
from dataholder import RobotModelType
|
||||
|
||||
pibotModel = os.environ['PIBOT_MODEL']
|
||||
boardType = os.environ['PIBOT_BOARD']
|
||||
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
|
||||
|
||||
print(pibotModel)
|
||||
print(boardType)
|
||||
print(pibotBaud)
|
||||
|
||||
pibotParam = dataholder.RobotParameters()
|
||||
|
||||
if pibotModel == "apollo" and boardType == "arduino":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
75, 2500, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apollo" and boardType == "stm32f1":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apollo" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "zeus" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_3WD_OMNI)
|
||||
elif pibotModel == "hades" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||
elif pibotModel == "hadesX" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
|
||||
250, 2750, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 72, \
|
||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||
elif pibotModel == "hera" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_4WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "arduino":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
75, 2500, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "stm32f1":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
250, 1200, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
250, 1200, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
import dataholder
|
||||
import os
|
||||
from dataholder import RobotImuType
|
||||
from dataholder import RobotModelType
|
||||
|
||||
pibotModel = os.environ['PIBOT_MODEL']
|
||||
boardType = os.environ['PIBOT_BOARD']
|
||||
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
|
||||
|
||||
print(pibotModel)
|
||||
print(boardType)
|
||||
print(pibotBaud)
|
||||
|
||||
pibotParam = dataholder.RobotParameters()
|
||||
|
||||
if pibotModel == "apollo" and boardType == "arduino":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
75, 2500, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apollo" and boardType == "stm32f1":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apollo" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "zeus" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_3WD_OMNI)
|
||||
elif pibotModel == "hades" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||
elif pibotModel == "hadesX" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
|
||||
250, 2750, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 72, \
|
||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||
elif pibotModel == "hera" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
|
||||
320, 2700, 0, 10, \
|
||||
250, 50, 50, 250, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_4WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "arduino":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
75, 2500, 0, 10, \
|
||||
250, 40, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "stm32f1":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
250, 1200, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
elif pibotModel == "apolloX" and boardType == "stm32f4":
|
||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||
250, 1200, 0, 10, \
|
||||
250, 50, 0, 200, \
|
||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||
178
Code/MowingRobot/pibot_ros/pypibot/transport/set_default_params.py
Executable file → Normal file
178
Code/MowingRobot/pibot_ros/pypibot/transport/set_default_params.py
Executable file → Normal file
|
|
@ -1,90 +1,90 @@
|
|||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
if __name__ == '__main__':
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# set robot parameter
|
||||
log.info("****************set robot parameter*****************")
|
||||
|
||||
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info('set parameter success')
|
||||
else:
|
||||
log.error('set parameter err')
|
||||
quit(1)
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get param err\r\n')
|
||||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
if __name__ == '__main__':
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# set robot parameter
|
||||
log.info("****************set robot parameter*****************")
|
||||
|
||||
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info('set parameter success')
|
||||
else:
|
||||
log.error('set parameter err')
|
||||
quit(1)
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get param err\r\n')
|
||||
quit(1)
|
||||
|
|
@ -1,122 +1,122 @@
|
|||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
import signal
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
run_flag = True
|
||||
|
||||
def exit(signum, frame):
|
||||
global run_flag
|
||||
run_flag = False
|
||||
|
||||
if __name__ == '__main__':
|
||||
signal.signal(signal.SIGINT, exit)
|
||||
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
pwm=[0]*4
|
||||
for i in range(len(sys.argv)-1):
|
||||
pwm[i] = int(sys.argv[i+1])
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get params err\r\n')
|
||||
quit(1)
|
||||
|
||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||
if p:
|
||||
log.info('set pwm success')
|
||||
else:
|
||||
log.error('set pwm err')
|
||||
quit(1)
|
||||
|
||||
log.info("****************get encoder count*****************")
|
||||
while run_flag:
|
||||
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
|
||||
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
|
||||
if p:
|
||||
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
|
||||
else:
|
||||
log.error('get encoder count err')
|
||||
quit(1)
|
||||
|
||||
import time
|
||||
time.sleep(0.5)
|
||||
|
||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||
if p:
|
||||
log.info('set pwm success')
|
||||
else:
|
||||
log.error('set pwm err')
|
||||
import platform
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from transport import Transport
|
||||
from dataholder import MessageID
|
||||
import params
|
||||
import signal
|
||||
|
||||
#for linux
|
||||
port="/dev/pibot"
|
||||
|
||||
#for windows
|
||||
#port="com3"
|
||||
|
||||
pypibot.assistant.enableGlobalExcept()
|
||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||
log.setLevel("i")
|
||||
|
||||
run_flag = True
|
||||
|
||||
def exit(signum, frame):
|
||||
global run_flag
|
||||
run_flag = False
|
||||
|
||||
if __name__ == '__main__':
|
||||
signal.signal(signal.SIGINT, exit)
|
||||
|
||||
mboard = Transport(port, params.pibotBaud)
|
||||
if not mboard.start():
|
||||
log.error("can not open %s"%port)
|
||||
sys.exit()
|
||||
|
||||
pwm=[0]*4
|
||||
for i in range(len(sys.argv)-1):
|
||||
pwm[i] = int(sys.argv[i+1])
|
||||
|
||||
DataHolder = mboard.getDataHolder()
|
||||
|
||||
for num in range(0,3):
|
||||
log.info("****************get robot version*****************")
|
||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
if p:
|
||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||
break
|
||||
else:
|
||||
log.error('read firmware version err\r\n')
|
||||
import time
|
||||
time.sleep(1)
|
||||
if num == 2:
|
||||
log.error('please check connection or baudrate\r\n')
|
||||
sys.exit()
|
||||
|
||||
# get robot parameter
|
||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||
if p:
|
||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||
%(robotParam.param.model_type, \
|
||||
robotParam.param.wheel_diameter, \
|
||||
robotParam.param.wheel_track, \
|
||||
robotParam.param.encoder_resolution
|
||||
))
|
||||
|
||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||
%(robotParam.param.do_pid_interval, \
|
||||
robotParam.param.kp, \
|
||||
robotParam.param.ki, \
|
||||
robotParam.param.kd, \
|
||||
robotParam.param.ko))
|
||||
|
||||
log.info("cmd_last_time:%d imu_type:%d" \
|
||||
%(robotParam.param.cmd_last_time,\
|
||||
robotParam.param.imu_type
|
||||
))
|
||||
|
||||
log.info("max_v:%d %d %d\r\n" \
|
||||
%(robotParam.param.max_v_liner_x,\
|
||||
robotParam.param.max_v_liner_y, \
|
||||
robotParam.param.max_v_angular_z
|
||||
))
|
||||
|
||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||
%(robotParam.param.motor_nonexchange_flag,\
|
||||
robotParam.param.encoder_nonexchange_flag
|
||||
))
|
||||
else:
|
||||
log.error('get params err\r\n')
|
||||
quit(1)
|
||||
|
||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||
if p:
|
||||
log.info('set pwm success')
|
||||
else:
|
||||
log.error('set pwm err')
|
||||
quit(1)
|
||||
|
||||
log.info("****************get encoder count*****************")
|
||||
while run_flag:
|
||||
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
|
||||
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
|
||||
if p:
|
||||
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
|
||||
else:
|
||||
log.error('get encoder count err')
|
||||
quit(1)
|
||||
|
||||
import time
|
||||
time.sleep(0.5)
|
||||
|
||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
|
||||
|
||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||
if p:
|
||||
log.info('set pwm success')
|
||||
else:
|
||||
log.error('set pwm err')
|
||||
quit(1)
|
||||
|
|
@ -1,197 +1,197 @@
|
|||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from pypibot import assistant
|
||||
|
||||
import serial
|
||||
import threading
|
||||
import struct
|
||||
import time
|
||||
from dataholder import MessageID, BoardDataDict
|
||||
FIX_HEAD = 0x5a
|
||||
|
||||
class Recstate():
|
||||
WAITING_HD = 0
|
||||
WAITING_MSG_ID = 1
|
||||
RECEIVE_LEN = 2
|
||||
RECEIVE_PACKAGE = 3
|
||||
RECEIVE_CHECK = 4
|
||||
|
||||
def checksum(d):
|
||||
sum = 0
|
||||
if assistant.is_python3():
|
||||
for i in d:
|
||||
sum += i
|
||||
sum = sum&0xff
|
||||
else:
|
||||
for i in d:
|
||||
sum += ord(i)
|
||||
sum = sum&0xff
|
||||
return sum
|
||||
|
||||
|
||||
class Transport:
|
||||
def __init__(self, port, baudrate=921600):
|
||||
self._Port = port
|
||||
self._Baudrate = baudrate
|
||||
self._KeepRunning = False
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
self.rev_msg = []
|
||||
self.rev_data = []
|
||||
self.wait_event = threading.Event()
|
||||
|
||||
def getDataHolder(self):
|
||||
return BoardDataDict
|
||||
|
||||
def start(self):
|
||||
try:
|
||||
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
|
||||
self._KeepRunning = True
|
||||
self._ReceiverThread = threading.Thread(target=self._Listen)
|
||||
self._ReceiverThread.setDaemon(True)
|
||||
self._ReceiverThread.start()
|
||||
return True
|
||||
except:
|
||||
return False
|
||||
|
||||
def Stop(self):
|
||||
self._KeepRunning = False
|
||||
time.sleep(0.1)
|
||||
self._Serial.close()
|
||||
|
||||
def _Listen(self):
|
||||
while self._KeepRunning:
|
||||
if self.receiveFiniteStates(self._Serial.read()):
|
||||
self.packageAnalysis()
|
||||
|
||||
def receiveFiniteStates(self, s):
|
||||
if len(s) == 0:
|
||||
return False
|
||||
val = s[0]
|
||||
val_int = val
|
||||
if not assistant.is_python3():
|
||||
val_int = ord(val)
|
||||
|
||||
if self.receive_state == Recstate.WAITING_HD:
|
||||
if val_int == FIX_HEAD:
|
||||
log.trace('got head')
|
||||
self.rev_msg = []
|
||||
self.rev_data =[]
|
||||
self.rev_msg.append(val)
|
||||
self.receive_state = Recstate.WAITING_MSG_ID
|
||||
elif self.receive_state == Recstate.WAITING_MSG_ID:
|
||||
log.trace('got msg id')
|
||||
self.rev_msg.append(val)
|
||||
self.receive_state = Recstate.RECEIVE_LEN
|
||||
elif self.receive_state == Recstate.RECEIVE_LEN:
|
||||
log.trace('got len:%d', val_int)
|
||||
self.rev_msg.append(val)
|
||||
if val_int == 0:
|
||||
self.receive_state = Recstate.RECEIVE_CHECK
|
||||
else:
|
||||
self.receive_state = Recstate.RECEIVE_PACKAGE
|
||||
elif self.receive_state == Recstate.RECEIVE_PACKAGE:
|
||||
# if assistant.is_python3():
|
||||
# self.rev_data.append((chr(val)).encode('latin1'))
|
||||
# else:
|
||||
self.rev_data.append(val)
|
||||
r = False
|
||||
if assistant.is_python3():
|
||||
r = len(self.rev_data) == int(self.rev_msg[-1])
|
||||
else:
|
||||
r = len(self.rev_data) == ord(self.rev_msg[-1])
|
||||
|
||||
if r:
|
||||
self.rev_msg.extend(self.rev_data)
|
||||
self.receive_state = Recstate.RECEIVE_CHECK
|
||||
elif self.receive_state == Recstate.RECEIVE_CHECK:
|
||||
log.trace('got check')
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
if val_int == checksum(self.rev_msg):
|
||||
log.trace('got a complete message')
|
||||
return True
|
||||
else:
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
|
||||
# continue receiving
|
||||
return False
|
||||
|
||||
def packageAnalysis(self):
|
||||
if assistant.is_python3():
|
||||
in_msg_id = int(self.rev_msg[1])
|
||||
else:
|
||||
in_msg_id = ord(self.rev_msg[1])
|
||||
if assistant.is_python3():
|
||||
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
|
||||
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
|
||||
else:
|
||||
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))
|
||||
if r:
|
||||
self.res_id = in_msg_id
|
||||
if in_msg_id<100:
|
||||
self.set_response()
|
||||
else:#notify
|
||||
log.debug('msg %d'%self.rev_msg[4],'data incoming')
|
||||
pass
|
||||
else:
|
||||
log.debug ('error unpacking pkg')
|
||||
|
||||
def request(self, id, timeout=0.5):
|
||||
if not self.write(id):
|
||||
log.debug ('Serial send error!')
|
||||
return False
|
||||
if self.wait_for_response(timeout):
|
||||
if id == self.res_id:
|
||||
log.trace ('OK')
|
||||
else:
|
||||
log.error ('Got unmatched response!')
|
||||
else:
|
||||
log.error ('Request got no response!')
|
||||
return False
|
||||
# clear response
|
||||
self.res_id = None
|
||||
return True
|
||||
|
||||
def write(self, id):
|
||||
cmd = self.make_command(id)
|
||||
if assistant.is_python3():
|
||||
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
|
||||
else:
|
||||
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
|
||||
self._Serial.write(cmd)
|
||||
return True
|
||||
|
||||
def wait_for_response(self, timeout):
|
||||
self.wait_event.clear()
|
||||
return self.wait_event.wait(timeout)
|
||||
|
||||
def set_response(self):
|
||||
self.wait_event.set()
|
||||
|
||||
def make_command(self, id):
|
||||
#print(DataDict[id])
|
||||
data = BoardDataDict[id].pack()
|
||||
l = [FIX_HEAD, id, len(data)]
|
||||
head = struct.pack("3B", *l)
|
||||
body = head + data
|
||||
|
||||
if assistant.is_python3():
|
||||
return body + chr(checksum(body)).encode('latin1')
|
||||
else:
|
||||
return body + chr(checksum(body))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
mboard = Transport('com10')
|
||||
if not mboard.start():
|
||||
import sys
|
||||
sys.exit()
|
||||
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
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")
|
||||
|
||||
|
||||
import sys
|
||||
sys.path.append("..")
|
||||
import pypibot
|
||||
from pypibot import log
|
||||
from pypibot import assistant
|
||||
|
||||
import serial
|
||||
import threading
|
||||
import struct
|
||||
import time
|
||||
from dataholder import MessageID, BoardDataDict
|
||||
FIX_HEAD = 0x5a
|
||||
|
||||
class Recstate():
|
||||
WAITING_HD = 0
|
||||
WAITING_MSG_ID = 1
|
||||
RECEIVE_LEN = 2
|
||||
RECEIVE_PACKAGE = 3
|
||||
RECEIVE_CHECK = 4
|
||||
|
||||
def checksum(d):
|
||||
sum = 0
|
||||
if assistant.is_python3():
|
||||
for i in d:
|
||||
sum += i
|
||||
sum = sum&0xff
|
||||
else:
|
||||
for i in d:
|
||||
sum += ord(i)
|
||||
sum = sum&0xff
|
||||
return sum
|
||||
|
||||
|
||||
class Transport:
|
||||
def __init__(self, port, baudrate=921600):
|
||||
self._Port = port
|
||||
self._Baudrate = baudrate
|
||||
self._KeepRunning = False
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
self.rev_msg = []
|
||||
self.rev_data = []
|
||||
self.wait_event = threading.Event()
|
||||
|
||||
def getDataHolder(self):
|
||||
return BoardDataDict
|
||||
|
||||
def start(self):
|
||||
try:
|
||||
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
|
||||
self._KeepRunning = True
|
||||
self._ReceiverThread = threading.Thread(target=self._Listen)
|
||||
self._ReceiverThread.setDaemon(True)
|
||||
self._ReceiverThread.start()
|
||||
return True
|
||||
except:
|
||||
return False
|
||||
|
||||
def Stop(self):
|
||||
self._KeepRunning = False
|
||||
time.sleep(0.1)
|
||||
self._Serial.close()
|
||||
|
||||
def _Listen(self):
|
||||
while self._KeepRunning:
|
||||
if self.receiveFiniteStates(self._Serial.read()):
|
||||
self.packageAnalysis()
|
||||
|
||||
def receiveFiniteStates(self, s):
|
||||
if len(s) == 0:
|
||||
return False
|
||||
val = s[0]
|
||||
val_int = val
|
||||
if not assistant.is_python3():
|
||||
val_int = ord(val)
|
||||
|
||||
if self.receive_state == Recstate.WAITING_HD:
|
||||
if val_int == FIX_HEAD:
|
||||
log.trace('got head')
|
||||
self.rev_msg = []
|
||||
self.rev_data =[]
|
||||
self.rev_msg.append(val)
|
||||
self.receive_state = Recstate.WAITING_MSG_ID
|
||||
elif self.receive_state == Recstate.WAITING_MSG_ID:
|
||||
log.trace('got msg id')
|
||||
self.rev_msg.append(val)
|
||||
self.receive_state = Recstate.RECEIVE_LEN
|
||||
elif self.receive_state == Recstate.RECEIVE_LEN:
|
||||
log.trace('got len:%d', val_int)
|
||||
self.rev_msg.append(val)
|
||||
if val_int == 0:
|
||||
self.receive_state = Recstate.RECEIVE_CHECK
|
||||
else:
|
||||
self.receive_state = Recstate.RECEIVE_PACKAGE
|
||||
elif self.receive_state == Recstate.RECEIVE_PACKAGE:
|
||||
# if assistant.is_python3():
|
||||
# self.rev_data.append((chr(val)).encode('latin1'))
|
||||
# else:
|
||||
self.rev_data.append(val)
|
||||
r = False
|
||||
if assistant.is_python3():
|
||||
r = len(self.rev_data) == int(self.rev_msg[-1])
|
||||
else:
|
||||
r = len(self.rev_data) == ord(self.rev_msg[-1])
|
||||
|
||||
if r:
|
||||
self.rev_msg.extend(self.rev_data)
|
||||
self.receive_state = Recstate.RECEIVE_CHECK
|
||||
elif self.receive_state == Recstate.RECEIVE_CHECK:
|
||||
log.trace('got check')
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
if val_int == checksum(self.rev_msg):
|
||||
log.trace('got a complete message')
|
||||
return True
|
||||
else:
|
||||
self.receive_state = Recstate.WAITING_HD
|
||||
|
||||
# continue receiving
|
||||
return False
|
||||
|
||||
def packageAnalysis(self):
|
||||
if assistant.is_python3():
|
||||
in_msg_id = int(self.rev_msg[1])
|
||||
else:
|
||||
in_msg_id = ord(self.rev_msg[1])
|
||||
if assistant.is_python3():
|
||||
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
|
||||
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
|
||||
else:
|
||||
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))
|
||||
if r:
|
||||
self.res_id = in_msg_id
|
||||
if in_msg_id<100:
|
||||
self.set_response()
|
||||
else:#notify
|
||||
log.debug('msg %d'%self.rev_msg[4],'data incoming')
|
||||
pass
|
||||
else:
|
||||
log.debug ('error unpacking pkg')
|
||||
|
||||
def request(self, id, timeout=0.5):
|
||||
if not self.write(id):
|
||||
log.debug ('Serial send error!')
|
||||
return False
|
||||
if self.wait_for_response(timeout):
|
||||
if id == self.res_id:
|
||||
log.trace ('OK')
|
||||
else:
|
||||
log.error ('Got unmatched response!')
|
||||
else:
|
||||
log.error ('Request got no response!')
|
||||
return False
|
||||
# clear response
|
||||
self.res_id = None
|
||||
return True
|
||||
|
||||
def write(self, id):
|
||||
cmd = self.make_command(id)
|
||||
if assistant.is_python3():
|
||||
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
|
||||
else:
|
||||
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
|
||||
self._Serial.write(cmd)
|
||||
return True
|
||||
|
||||
def wait_for_response(self, timeout):
|
||||
self.wait_event.clear()
|
||||
return self.wait_event.wait(timeout)
|
||||
|
||||
def set_response(self):
|
||||
self.wait_event.set()
|
||||
|
||||
def make_command(self, id):
|
||||
#print(DataDict[id])
|
||||
data = BoardDataDict[id].pack()
|
||||
l = [FIX_HEAD, id, len(data)]
|
||||
head = struct.pack("3B", *l)
|
||||
body = head + data
|
||||
|
||||
if assistant.is_python3():
|
||||
return body + chr(checksum(body)).encode('latin1')
|
||||
else:
|
||||
return body + chr(checksum(body))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
mboard = Transport('com10')
|
||||
if not mboard.start():
|
||||
import sys
|
||||
sys.exit()
|
||||
|
||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||
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")
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,53 +1,53 @@
|
|||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(FollowingCar)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
message_generation
|
||||
geometry_msgs
|
||||
pibot_msgs
|
||||
)
|
||||
|
||||
# 寻找OpenCV库
|
||||
find_package(OpenCV REQUIRED)
|
||||
# 查找 Boost 库
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
# catkin_package(
|
||||
# # INCLUDE_DIRS include
|
||||
# # LIBRARIES FollowingCar
|
||||
# # CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# # DEPENDS system_lib
|
||||
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
|
||||
# )
|
||||
|
||||
|
||||
include_directories(
|
||||
# include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
include
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
|
||||
src/uwb.cpp
|
||||
src/mapping.cpp
|
||||
src/align.cpp
|
||||
src/Mat.cpp
|
||||
src/lighthouse.cpp
|
||||
include/senddata.h src/senddata.cpp)
|
||||
|
||||
|
||||
|
||||
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
||||
|
||||
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
|
||||
add_executable(${PROJECT_NAME}_node src/main.cpp)
|
||||
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})
|
||||
|
||||
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(FollowingCar)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
message_generation
|
||||
geometry_msgs
|
||||
pibot_msgs
|
||||
)
|
||||
|
||||
# 寻找OpenCV库
|
||||
find_package(OpenCV REQUIRED)
|
||||
# 查找 Boost 库
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
# catkin_package(
|
||||
# # INCLUDE_DIRS include
|
||||
# # LIBRARIES FollowingCar
|
||||
# # CATKIN_DEPENDS roscpp rospy std_msgs
|
||||
# # DEPENDS system_lib
|
||||
# CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
|
||||
# )
|
||||
|
||||
|
||||
include_directories(
|
||||
# include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
include
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
|
||||
src/uwb.cpp
|
||||
src/mapping.cpp
|
||||
src/align.cpp
|
||||
src/Mat.cpp
|
||||
src/lighthouse.cpp
|
||||
include/senddata.h src/senddata.cpp)
|
||||
|
||||
|
||||
|
||||
generate_messages(DEPENDENCIES std_msgs geometry_msgs)
|
||||
|
||||
catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)
|
||||
add_executable(${PROJECT_NAME}_node src/main.cpp)
|
||||
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES} pthread ${PROJECT_NAME})
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,83 +1,83 @@
|
|||
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||
* File Name : Mat.h
|
||||
* Current Version : V1.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2022.09.14
|
||||
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#ifndef _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>
|
||||
|
||||
|
||||
#include <string.h>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
class Mat
|
||||
{
|
||||
public:
|
||||
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>
|
||||
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);
|
||||
//<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 n;//<2F><><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>
|
||||
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>
|
||||
|
||||
//<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 Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
|
||||
friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD>
|
||||
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,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,Mat b);//a*inv(b)
|
||||
friend Mat operator %(Mat a,Mat b);//inv(a)*b
|
||||
|
||||
//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
private:
|
||||
// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
void RowExchange(int a, int b);
|
||||
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||
void RowMul(int a,double k);
|
||||
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||
void RowAdd(int a,int b, double k);
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
void ColExchange(int a, int b);
|
||||
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||
void ColMul(int a,double k);
|
||||
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||
void ColAdd(int a,int b,double k);
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||
* File Name : Mat.h
|
||||
* Current Version : V1.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2022.09.14
|
||||
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#ifndef _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>
|
||||
|
||||
|
||||
#include <string.h>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
class Mat
|
||||
{
|
||||
public:
|
||||
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>
|
||||
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);
|
||||
//<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 n;//<2F><><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>
|
||||
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>
|
||||
|
||||
//<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 Sqrt();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><C6BD>
|
||||
friend Mat operator ^(Mat a,Mat b);//<2F><><EFBFBD>
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD>
|
||||
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,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,Mat b);//a*inv(b)
|
||||
friend Mat operator %(Mat a,Mat b);//inv(a)*b
|
||||
|
||||
//MAT inv();//<2F><><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
private:
|
||||
// Ϊ<><CEAA><EFBFBD>ø<EFBFBD>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<D2BB><D0A9><EFBFBD><EFBFBD>
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
void RowExchange(int a, int b);
|
||||
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||
void RowMul(int a,double k);
|
||||
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||
void RowAdd(int a,int b, double k);
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
void ColExchange(int a, int b);
|
||||
// ijһ<C4B3>г<EFBFBD><D0B3><EFBFBD>ϵ<EFBFBD><CFB5>
|
||||
void ColMul(int a,double k);
|
||||
// <20><>ijһ<C4B3>мӼ<D0BC><D3BC><EFBFBD>һ<EFBFBD>еı<D0B5><C4B1><EFBFBD>
|
||||
void ColAdd(int a,int b,double k);
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,44 +1,44 @@
|
|||
#include <cmath>
|
||||
#include <ros/ros.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <utility>
|
||||
#include <queue>
|
||||
#include <fstream>
|
||||
#include "pibot_msgs/RawImu.h"
|
||||
#include "type.h"
|
||||
#include "uwb.h"
|
||||
#include "lighthouse.h"
|
||||
#include "Mat.h"
|
||||
|
||||
#ifndef ALIGN_H
|
||||
#define AlIGN_H
|
||||
namespace uwb_slam{
|
||||
class Align
|
||||
{
|
||||
public:
|
||||
Align(){
|
||||
|
||||
};
|
||||
void Run();
|
||||
void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
|
||||
void imuCB(const pibot_msgs::RawImu& imu);
|
||||
void odomCB(const nav_msgs::Odometry& odom);
|
||||
|
||||
public:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Subscriber wheel_odom_sub_;
|
||||
ros::Subscriber imu_sub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
Imu_odom_pose_data imu_odom_;
|
||||
Uwb_data uwb_data_;
|
||||
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
|
||||
ros::Time odom_tmp_ ;
|
||||
bool write_data_ = false;
|
||||
cv::Mat img1;
|
||||
std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
|
||||
};
|
||||
};
|
||||
#endif
|
||||
#include <cmath>
|
||||
#include <ros/ros.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <utility>
|
||||
#include <queue>
|
||||
#include <fstream>
|
||||
#include "pibot_msgs/RawImu.h"
|
||||
#include "type.h"
|
||||
#include "uwb.h"
|
||||
#include "lighthouse.h"
|
||||
#include "Mat.h"
|
||||
|
||||
#ifndef ALIGN_H
|
||||
#define AlIGN_H
|
||||
namespace uwb_slam{
|
||||
class Align
|
||||
{
|
||||
public:
|
||||
Align(){
|
||||
|
||||
};
|
||||
void Run();
|
||||
void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
|
||||
void imuCB(const pibot_msgs::RawImu& imu);
|
||||
void odomCB(const nav_msgs::Odometry& odom);
|
||||
|
||||
public:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Subscriber wheel_odom_sub_;
|
||||
ros::Subscriber imu_sub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
Imu_odom_pose_data imu_odom_;
|
||||
Uwb_data uwb_data_;
|
||||
ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
|
||||
ros::Time odom_tmp_ ;
|
||||
bool write_data_ = false;
|
||||
cv::Mat img1;
|
||||
std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
|
||||
};
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,29 +1,29 @@
|
|||
#include <ros/ros.h>
|
||||
#include <mutex>
|
||||
#include <boost/asio.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include "type.h"
|
||||
#include <queue>
|
||||
#include <chrono>
|
||||
#ifndef __LIGHTHOUSE_H__
|
||||
#define __LIGHTHOUSE_H__
|
||||
|
||||
namespace uwb_slam{
|
||||
class Lighthouse{
|
||||
public:
|
||||
Lighthouse();
|
||||
~Lighthouse();
|
||||
void Run();
|
||||
void UDPRead();
|
||||
// Listen PORT
|
||||
int PORT = 12345;
|
||||
int UdpSocket = -1;
|
||||
|
||||
LightHouseData data;
|
||||
|
||||
std::mutex mMutexLighthouse;
|
||||
};
|
||||
};
|
||||
#endif
|
||||
#include <ros/ros.h>
|
||||
#include <mutex>
|
||||
#include <boost/asio.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include "type.h"
|
||||
#include <queue>
|
||||
#include <chrono>
|
||||
#ifndef __LIGHTHOUSE_H__
|
||||
#define __LIGHTHOUSE_H__
|
||||
|
||||
namespace uwb_slam{
|
||||
class Lighthouse{
|
||||
public:
|
||||
Lighthouse();
|
||||
~Lighthouse();
|
||||
void Run();
|
||||
void UDPRead();
|
||||
// Listen PORT
|
||||
int PORT = 12345;
|
||||
int UdpSocket = -1;
|
||||
|
||||
LightHouseData data;
|
||||
|
||||
std::mutex mMutexLighthouse;
|
||||
};
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,40 +1,40 @@
|
|||
#include <queue>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#include "align.h"
|
||||
|
||||
#ifndef MAPPING_H
|
||||
#define MAPPING_H
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
class Mapping
|
||||
{
|
||||
public:
|
||||
const double PIXEL_SCALE = 1.0; //xiangsudian cm
|
||||
const int AREA_SIZE = 1000; //map size cm
|
||||
Mapping() {};
|
||||
void Run();
|
||||
bool checkQueue();
|
||||
void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
|
||||
void process();
|
||||
std::mutex mMutexMap;
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
std::shared_ptr<uwb_slam::Align> align_;
|
||||
|
||||
private:
|
||||
int realWidth, realHeight;
|
||||
std::queue<std::vector<cv::Point2d>> posDataQueue;
|
||||
std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
|
||||
//cv::Point2d imuPoint = {-1,-1};
|
||||
// std::queue<cv::Point2d> posDataQueue;
|
||||
|
||||
|
||||
bool readPos = false;
|
||||
cv::Mat img;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
#include <queue>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#include "align.h"
|
||||
|
||||
#ifndef MAPPING_H
|
||||
#define MAPPING_H
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
class Mapping
|
||||
{
|
||||
public:
|
||||
const double PIXEL_SCALE = 1.0; //xiangsudian cm
|
||||
const int AREA_SIZE = 1000; //map size cm
|
||||
Mapping() {};
|
||||
void Run();
|
||||
bool checkQueue();
|
||||
void feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData);
|
||||
void process();
|
||||
std::mutex mMutexMap;
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
std::shared_ptr<uwb_slam::Align> align_;
|
||||
|
||||
private:
|
||||
int realWidth, realHeight;
|
||||
std::queue<std::vector<cv::Point2d>> posDataQueue;
|
||||
std::vector<cv::Point2d> posData = std::vector<cv::Point2d>(3, {-1, -1});
|
||||
//cv::Point2d imuPoint = {-1,-1};
|
||||
// std::queue<cv::Point2d> posDataQueue;
|
||||
|
||||
|
||||
bool readPos = false;
|
||||
cv::Mat img;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,50 +1,50 @@
|
|||
#include <ros/ros.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "pibot_msgs/dis_info.h"
|
||||
#include "pibot_msgs/dis_info_array.h"
|
||||
|
||||
#ifndef SENDDATA_H
|
||||
#define SENDDATA_H
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
|
||||
class Senddata
|
||||
{
|
||||
public:
|
||||
Senddata(){};
|
||||
void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||
void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||
void odomCB(const nav_msgs::Odometry& odom);
|
||||
void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo);
|
||||
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
|
||||
|
||||
std::mutex mMutexSend;
|
||||
|
||||
private:
|
||||
ros::Publisher position_pub_;
|
||||
ros::Publisher cmd_vel_pub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
ros::Subscriber obstacles_sub_;
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
nav_msgs::Odometry odom_;//odom的消息类型
|
||||
nav_msgs::Odometry sub_odom_;//odom的消息类型
|
||||
geometry_msgs::Twist cmd_vel_;
|
||||
|
||||
bool flag_ = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <mutex>
|
||||
#include "uwb.h"
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "pibot_msgs/dis_info.h"
|
||||
#include "pibot_msgs/dis_info_array.h"
|
||||
|
||||
#ifndef SENDDATA_H
|
||||
#define SENDDATA_H
|
||||
|
||||
|
||||
namespace uwb_slam{
|
||||
|
||||
class Senddata
|
||||
{
|
||||
public:
|
||||
Senddata(){};
|
||||
void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||
void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
|
||||
void odomCB(const nav_msgs::Odometry& odom);
|
||||
void stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo);
|
||||
|
||||
std::shared_ptr<uwb_slam::Uwb> uwb_;
|
||||
|
||||
|
||||
std::mutex mMutexSend;
|
||||
|
||||
private:
|
||||
ros::Publisher position_pub_;
|
||||
ros::Publisher cmd_vel_pub_;
|
||||
ros::Subscriber odom_sub_;
|
||||
ros::Subscriber obstacles_sub_;
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
nav_msgs::Odometry odom_;//odom的消息类型
|
||||
nav_msgs::Odometry sub_odom_;//odom的消息类型
|
||||
geometry_msgs::Twist cmd_vel_;
|
||||
|
||||
bool flag_ = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -1,50 +1,50 @@
|
|||
#include <ros/ros.h>
|
||||
#include <ros/time.h>
|
||||
#ifndef TYPE_H
|
||||
#define TYPE_H
|
||||
|
||||
namespace uwb_slam{
|
||||
struct Imu_data
|
||||
{
|
||||
ros::Time imu_t_;
|
||||
double a_[3];
|
||||
double g_[3];
|
||||
double m_[3];
|
||||
Imu_data(){};
|
||||
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}{};
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct Imu_odom_pose_data
|
||||
{
|
||||
Imu_data imu_data_;
|
||||
double pose_[3];
|
||||
double quat_[4];
|
||||
double vxy_;
|
||||
double angle_v_;
|
||||
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){};
|
||||
};
|
||||
|
||||
struct Uwb_data
|
||||
{
|
||||
float x_,y_;
|
||||
ros::Time uwb_t_;
|
||||
Uwb_data(){};
|
||||
Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
|
||||
};
|
||||
|
||||
struct LightHouseData
|
||||
{
|
||||
float x_,y_,z_;
|
||||
float qw_,qx_,qy_,qz_;
|
||||
ros::Time lighthouse_t_;
|
||||
LightHouseData(){};
|
||||
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){};
|
||||
};
|
||||
|
||||
}
|
||||
#include <ros/ros.h>
|
||||
#include <ros/time.h>
|
||||
#ifndef TYPE_H
|
||||
#define TYPE_H
|
||||
|
||||
namespace uwb_slam{
|
||||
struct Imu_data
|
||||
{
|
||||
ros::Time imu_t_;
|
||||
double a_[3];
|
||||
double g_[3];
|
||||
double m_[3];
|
||||
Imu_data(){};
|
||||
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}{};
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct Imu_odom_pose_data
|
||||
{
|
||||
Imu_data imu_data_;
|
||||
double pose_[3];
|
||||
double quat_[4];
|
||||
double vxy_;
|
||||
double angle_v_;
|
||||
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){};
|
||||
};
|
||||
|
||||
struct Uwb_data
|
||||
{
|
||||
float x_,y_;
|
||||
ros::Time uwb_t_;
|
||||
Uwb_data(){};
|
||||
Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
|
||||
};
|
||||
|
||||
struct LightHouseData
|
||||
{
|
||||
float x_,y_,z_;
|
||||
float qw_,qx_,qy_,qz_;
|
||||
ros::Time lighthouse_t_;
|
||||
LightHouseData(){};
|
||||
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){};
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,60 +1,60 @@
|
|||
#include <ros/ros.h>
|
||||
#include <mutex>
|
||||
#include <boost/asio.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include "type.h"
|
||||
#include <queue>
|
||||
#include <chrono>
|
||||
#ifndef UWB_H
|
||||
#define UWB_H
|
||||
|
||||
#define PI acos(-1)
|
||||
namespace uwb_slam{
|
||||
|
||||
class Uwb
|
||||
{
|
||||
public:
|
||||
Uwb();
|
||||
void Run();
|
||||
bool checknewdata();
|
||||
void feed_imu_odom_pose_data();
|
||||
void Serread();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
int pre_seq = -1;
|
||||
int cur_seq = -1;
|
||||
int AnchorNum = 3;
|
||||
int AnchorPos[3][3]={
|
||||
-240, 400, 113,\
|
||||
240, 400, 113,\
|
||||
-400, -240, 113
|
||||
};//基站坐标，序号+三维坐标
|
||||
int d[3];
|
||||
int aoa[3];
|
||||
|
||||
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
|
||||
|
||||
|
||||
Uwb_data uwb_data_;
|
||||
// ros_merge_test::RawImu sub_imu_;
|
||||
// std::queue<Imu_odom_pose_data > imu_odom_queue_;
|
||||
// std::queue<Uwb_data> uwb_data_queue_;
|
||||
std::mutex mMutexUwb;
|
||||
//boost::asio::io_service io;
|
||||
//boost::asio::serial_port s_port;
|
||||
|
||||
// Imu_odom_pose_data imu_odom_pose_data_;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
#include <ros/ros.h>
|
||||
#include <mutex>
|
||||
#include <boost/asio.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <cstdint>
|
||||
#include "type.h"
|
||||
#include <queue>
|
||||
#include <chrono>
|
||||
#ifndef UWB_H
|
||||
#define UWB_H
|
||||
|
||||
#define PI acos(-1)
|
||||
namespace uwb_slam{
|
||||
|
||||
class Uwb
|
||||
{
|
||||
public:
|
||||
Uwb();
|
||||
void Run();
|
||||
bool checknewdata();
|
||||
void feed_imu_odom_pose_data();
|
||||
void Serread();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
int pre_seq = -1;
|
||||
int cur_seq = -1;
|
||||
int AnchorNum = 3;
|
||||
int AnchorPos[3][3]={
|
||||
-240, 400, 113,\
|
||||
240, 400, 113,\
|
||||
-400, -240, 113
|
||||
};//基站坐标，序号+三维坐标
|
||||
int d[3];
|
||||
int aoa[3];
|
||||
|
||||
// std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
|
||||
|
||||
|
||||
Uwb_data uwb_data_;
|
||||
// ros_merge_test::RawImu sub_imu_;
|
||||
// std::queue<Imu_odom_pose_data > imu_odom_queue_;
|
||||
// std::queue<Uwb_data> uwb_data_queue_;
|
||||
std::mutex mMutexUwb;
|
||||
//boost::asio::io_service io;
|
||||
//boost::asio::serial_port s_port;
|
||||
|
||||
// Imu_odom_pose_data imu_odom_pose_data_;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,75 +1,75 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>FollowingCar</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The FollowingCar package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>pibot_msgs</build_depend>
|
||||
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>FollowingCar</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The FollowingCar package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="luoruidi@todo.todo">luoruidi</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>pibot_msgs</build_depend>
|
||||
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,368 +1,368 @@
|
|||
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||
* File Name : Mat.cpp
|
||||
* Current Version : V1.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2022.09.14
|
||||
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include "Mat.h"
|
||||
|
||||
double mind(double a,double b)
|
||||
{
|
||||
double c = a;
|
||||
if(b < c){
|
||||
c = b;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
int mini(int a,int b)
|
||||
{
|
||||
int c=a;
|
||||
if(b<c)
|
||||
{
|
||||
c=b;
|
||||
}
|
||||
return c;
|
||||
|
||||
}
|
||||
|
||||
//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
|
||||
Mat::Mat()
|
||||
{
|
||||
Init(1,1,0);
|
||||
}
|
||||
|
||||
Mat::Mat(int setm,int setn,int kind)
|
||||
{
|
||||
Init(setm,setn,kind);
|
||||
}
|
||||
|
||||
void Mat::Init(int setm,int setn,int kind)
|
||||
{
|
||||
this->m = setm;
|
||||
this->n = setn;
|
||||
if((kind==0)||(kind==1))
|
||||
{
|
||||
memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
|
||||
}
|
||||
|
||||
if(kind==1)
|
||||
{
|
||||
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>档
|
||||
int xend = mini(this->m, this->n);
|
||||
for(x=0;x < xend;x++){
|
||||
mat[x][x] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
void Mat::Zero() {
|
||||
|
||||
}
|
||||
|
||||
Mat Mat::SubMat(int a,int b,int lm,int ln)
|
||||
{
|
||||
|
||||
int aend=a+lm-1;
|
||||
int bend=b+ln-1;
|
||||
|
||||
|
||||
Mat s(lm,ln,-1);
|
||||
int x,y;
|
||||
for(x=0;x<lm;x++)
|
||||
{
|
||||
for(y=0;y<ln;y++)
|
||||
{
|
||||
s.mat[x][y]=mat[a+x][b+y];
|
||||
}
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
void Mat::FillSubMat(int a,int b,Mat s)
|
||||
{
|
||||
int x,y;
|
||||
for(x=0;x<s.m;x++)
|
||||
{
|
||||
for(y=0;y<s.n;y++)
|
||||
{
|
||||
mat[a+x][b+y]=s.mat[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Mat operator *(double k, Mat a)
|
||||
{
|
||||
Mat b(a.m,a.n,-1);
|
||||
int x,y;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<a.n;y++)
|
||||
{
|
||||
b.mat[x][y]=k*a.mat[x][y];
|
||||
}
|
||||
}
|
||||
return b;
|
||||
}
|
||||
Mat operator *(Mat a,double k)
|
||||
{
|
||||
return k*a;
|
||||
}
|
||||
Mat operator /(Mat a,double k)
|
||||
{
|
||||
return (1/k)*a;
|
||||
}
|
||||
Mat operator *(Mat a,Mat b)
|
||||
{
|
||||
Mat c(a.m,b.n,-1);
|
||||
int x,y,z;
|
||||
double s;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<b.n;y++)
|
||||
{
|
||||
s=0;
|
||||
for(z=0;z<a.n;z++)
|
||||
{
|
||||
s=s+a.mat[x][z]*b.mat[z][y];
|
||||
}
|
||||
c.mat[x][y]=s;
|
||||
}
|
||||
}
|
||||
return c;
|
||||
|
||||
}
|
||||
|
||||
Mat operator +(Mat a,Mat b)
|
||||
{
|
||||
|
||||
Mat c=a;
|
||||
int x,y;
|
||||
for(x=0;x<c.m;x++)
|
||||
{
|
||||
for(y=0;y<c.n;y++)
|
||||
{
|
||||
c.mat[x][y]+=b.mat[x][y];
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
Mat operator -(Mat a,Mat b)
|
||||
{
|
||||
|
||||
Mat c=a;
|
||||
int x,y;
|
||||
for(x=0;x<c.m;x++)
|
||||
{
|
||||
for(y=0;y<c.n;y++)
|
||||
{
|
||||
c.mat[x][y]-=b.mat[x][y];
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
Mat operator ~(Mat a)
|
||||
{
|
||||
Mat b(a.n,a.m,-1);
|
||||
int x,y;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<a.n;y++)
|
||||
{
|
||||
b.mat[y][x]=a.mat[x][y];
|
||||
|
||||
}
|
||||
}
|
||||
return b;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Mat operator /(Mat a,Mat b)
|
||||
{
|
||||
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||
|
||||
int x,xb;
|
||||
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>
|
||||
double s=0;
|
||||
int p=x;
|
||||
double sxb;
|
||||
for(xb=x;xb<b.n;xb++)
|
||||
{
|
||||
sxb=fabs(b.mat[x][xb]);
|
||||
if(sxb>s)
|
||||
{
|
||||
p=xb;
|
||||
s=sxb;
|
||||
}
|
||||
}
|
||||
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if(x!=p)
|
||||
{
|
||||
a.ColExchange(x,p);
|
||||
b.ColExchange(x,p);
|
||||
}
|
||||
|
||||
//<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>
|
||||
a.ColMul(x,k);
|
||||
b.ColMul(x,k);
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||
for(xb=0;xb<b.n;xb++)
|
||||
{
|
||||
if(xb!=x)
|
||||
{
|
||||
k=(-b.mat[x][xb]);
|
||||
a.ColAdd(xb,x,k);
|
||||
b.ColAdd(xb,x,k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
Mat operator %(Mat a,Mat b)
|
||||
{
|
||||
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||
int x,xb;
|
||||
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>
|
||||
double s=0;
|
||||
int p=x;
|
||||
double sxb;
|
||||
for(xb=x;xb<a.m;xb++)
|
||||
{
|
||||
sxb=fabs(a.mat[xb][x]);
|
||||
if(sxb>s)
|
||||
{
|
||||
p=xb;
|
||||
s=sxb;
|
||||
}
|
||||
}
|
||||
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if(x!=p)
|
||||
{
|
||||
a.RowExchange(x,p);
|
||||
b.RowExchange(x,p);
|
||||
}
|
||||
|
||||
//<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>
|
||||
a.RowMul(x,k);
|
||||
b.RowMul(x,k);
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||
for(xb=0;xb<a.m;xb++)
|
||||
{
|
||||
if(xb!=x)
|
||||
{
|
||||
k=(-a.mat[xb][x]);
|
||||
a.RowAdd(xb,x,k);
|
||||
b.RowAdd(xb,x,k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return b;
|
||||
}
|
||||
|
||||
|
||||
void Mat::RowExchange(int a, int b)
|
||||
{
|
||||
double s[MAT_MAX];
|
||||
int ncpy=n*sizeof(double);
|
||||
memcpy(s,mat[a],ncpy);
|
||||
memcpy(mat[a],mat[b],ncpy);
|
||||
memcpy(mat[b],s,ncpy);
|
||||
}
|
||||
void Mat::RowMul(int a,double k)
|
||||
{
|
||||
int y;
|
||||
for(y=0;y<n;y++)
|
||||
{
|
||||
mat[a][y]= mat[a][y]*k;
|
||||
}
|
||||
}
|
||||
void Mat::RowAdd(int a,int b,double k)
|
||||
{
|
||||
int y;
|
||||
for(y=0;y<n;y++)
|
||||
{
|
||||
mat[a][y]= mat[a][y]+ mat[b][y]*k;
|
||||
}
|
||||
}
|
||||
|
||||
void Mat::ColExchange(int a, int b)
|
||||
{
|
||||
double s;
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
s=mat[x][a];
|
||||
mat[x][a]=mat[x][b];
|
||||
mat[x][b]=s;
|
||||
}
|
||||
}
|
||||
void Mat::ColMul(int a,double k)
|
||||
{
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
mat[x][a]=mat[x][a]*k;
|
||||
}
|
||||
}
|
||||
void Mat::ColAdd(int a,int b,double k)
|
||||
{
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
mat[x][a]=mat[x][a]+mat[x][b]*k;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
double Mat::Sqrt()
|
||||
{
|
||||
int x;
|
||||
double numx;
|
||||
double s=0;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
numx=mat[x][0];
|
||||
s+=(numx*numx);
|
||||
}
|
||||
return s;
|
||||
}
|
||||
double Mat::absvec()
|
||||
{
|
||||
return sqrt(Sqrt());
|
||||
}
|
||||
|
||||
Mat operator ^(Mat a, Mat b)
|
||||
{
|
||||
double ax=a.mat[0][0];
|
||||
double ay=a.mat[1][0];
|
||||
double az=a.mat[2][0];
|
||||
double bx=b.mat[0][0];
|
||||
double by=b.mat[1][0];
|
||||
double bz=b.mat[2][0];
|
||||
|
||||
Mat c(3,1,-1);
|
||||
c.mat[0][0]=ay*bz-az*by;
|
||||
c.mat[1][0]=(-(ax*bz-az*bx));
|
||||
c.mat[2][0]=ax*by-ay*bx;
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||
* File Name : Mat.cpp
|
||||
* Current Version : V1.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2022.09.14
|
||||
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include "Mat.h"
|
||||
|
||||
double mind(double a,double b)
|
||||
{
|
||||
double c = a;
|
||||
if(b < c){
|
||||
c = b;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
int mini(int a,int b)
|
||||
{
|
||||
int c=a;
|
||||
if(b<c)
|
||||
{
|
||||
c=b;
|
||||
}
|
||||
return c;
|
||||
|
||||
}
|
||||
|
||||
//<2F><>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5>ù<EFBFBD><C3B9>캯<EFBFBD><ECBAAF>
|
||||
Mat::Mat()
|
||||
{
|
||||
Init(1,1,0);
|
||||
}
|
||||
|
||||
Mat::Mat(int setm,int setn,int kind)
|
||||
{
|
||||
Init(setm,setn,kind);
|
||||
}
|
||||
|
||||
void Mat::Init(int setm,int setn,int kind)
|
||||
{
|
||||
this->m = setm;
|
||||
this->n = setn;
|
||||
if((kind==0)||(kind==1))
|
||||
{
|
||||
memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
|
||||
}
|
||||
|
||||
if(kind==1)
|
||||
{
|
||||
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>档
|
||||
int xend = mini(this->m, this->n);
|
||||
for(x=0;x < xend;x++){
|
||||
mat[x][x] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
void Mat::Zero() {
|
||||
|
||||
}
|
||||
|
||||
Mat Mat::SubMat(int a,int b,int lm,int ln)
|
||||
{
|
||||
|
||||
int aend=a+lm-1;
|
||||
int bend=b+ln-1;
|
||||
|
||||
|
||||
Mat s(lm,ln,-1);
|
||||
int x,y;
|
||||
for(x=0;x<lm;x++)
|
||||
{
|
||||
for(y=0;y<ln;y++)
|
||||
{
|
||||
s.mat[x][y]=mat[a+x][b+y];
|
||||
}
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
void Mat::FillSubMat(int a,int b,Mat s)
|
||||
{
|
||||
int x,y;
|
||||
for(x=0;x<s.m;x++)
|
||||
{
|
||||
for(y=0;y<s.n;y++)
|
||||
{
|
||||
mat[a+x][b+y]=s.mat[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Mat operator *(double k, Mat a)
|
||||
{
|
||||
Mat b(a.m,a.n,-1);
|
||||
int x,y;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<a.n;y++)
|
||||
{
|
||||
b.mat[x][y]=k*a.mat[x][y];
|
||||
}
|
||||
}
|
||||
return b;
|
||||
}
|
||||
Mat operator *(Mat a,double k)
|
||||
{
|
||||
return k*a;
|
||||
}
|
||||
Mat operator /(Mat a,double k)
|
||||
{
|
||||
return (1/k)*a;
|
||||
}
|
||||
Mat operator *(Mat a,Mat b)
|
||||
{
|
||||
Mat c(a.m,b.n,-1);
|
||||
int x,y,z;
|
||||
double s;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<b.n;y++)
|
||||
{
|
||||
s=0;
|
||||
for(z=0;z<a.n;z++)
|
||||
{
|
||||
s=s+a.mat[x][z]*b.mat[z][y];
|
||||
}
|
||||
c.mat[x][y]=s;
|
||||
}
|
||||
}
|
||||
return c;
|
||||
|
||||
}
|
||||
|
||||
Mat operator +(Mat a,Mat b)
|
||||
{
|
||||
|
||||
Mat c=a;
|
||||
int x,y;
|
||||
for(x=0;x<c.m;x++)
|
||||
{
|
||||
for(y=0;y<c.n;y++)
|
||||
{
|
||||
c.mat[x][y]+=b.mat[x][y];
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
Mat operator -(Mat a,Mat b)
|
||||
{
|
||||
|
||||
Mat c=a;
|
||||
int x,y;
|
||||
for(x=0;x<c.m;x++)
|
||||
{
|
||||
for(y=0;y<c.n;y++)
|
||||
{
|
||||
c.mat[x][y]-=b.mat[x][y];
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
Mat operator ~(Mat a)
|
||||
{
|
||||
Mat b(a.n,a.m,-1);
|
||||
int x,y;
|
||||
for(x=0;x<a.m;x++)
|
||||
{
|
||||
for(y=0;y<a.n;y++)
|
||||
{
|
||||
b.mat[y][x]=a.mat[x][y];
|
||||
|
||||
}
|
||||
}
|
||||
return b;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Mat operator /(Mat a,Mat b)
|
||||
{
|
||||
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||
|
||||
int x,xb;
|
||||
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>
|
||||
double s=0;
|
||||
int p=x;
|
||||
double sxb;
|
||||
for(xb=x;xb<b.n;xb++)
|
||||
{
|
||||
sxb=fabs(b.mat[x][xb]);
|
||||
if(sxb>s)
|
||||
{
|
||||
p=xb;
|
||||
s=sxb;
|
||||
}
|
||||
}
|
||||
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if(x!=p)
|
||||
{
|
||||
a.ColExchange(x,p);
|
||||
b.ColExchange(x,p);
|
||||
}
|
||||
|
||||
//<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>
|
||||
a.ColMul(x,k);
|
||||
b.ColMul(x,k);
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||
for(xb=0;xb<b.n;xb++)
|
||||
{
|
||||
if(xb!=x)
|
||||
{
|
||||
k=(-b.mat[x][xb]);
|
||||
a.ColAdd(xb,x,k);
|
||||
b.ColAdd(xb,x,k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
Mat operator %(Mat a,Mat b)
|
||||
{
|
||||
//<2F><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA>
|
||||
int x,xb;
|
||||
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>
|
||||
double s=0;
|
||||
int p=x;
|
||||
double sxb;
|
||||
for(xb=x;xb<a.m;xb++)
|
||||
{
|
||||
sxb=fabs(a.mat[xb][x]);
|
||||
if(sxb>s)
|
||||
{
|
||||
p=xb;
|
||||
s=sxb;
|
||||
}
|
||||
}
|
||||
//ͬʱ<CDAC>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
if(x!=p)
|
||||
{
|
||||
a.RowExchange(x,p);
|
||||
b.RowExchange(x,p);
|
||||
}
|
||||
|
||||
//<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>
|
||||
a.RowMul(x,k);
|
||||
b.RowMul(x,k);
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
|
||||
for(xb=0;xb<a.m;xb++)
|
||||
{
|
||||
if(xb!=x)
|
||||
{
|
||||
k=(-a.mat[xb][x]);
|
||||
a.RowAdd(xb,x,k);
|
||||
b.RowAdd(xb,x,k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return b;
|
||||
}
|
||||
|
||||
|
||||
void Mat::RowExchange(int a, int b)
|
||||
{
|
||||
double s[MAT_MAX];
|
||||
int ncpy=n*sizeof(double);
|
||||
memcpy(s,mat[a],ncpy);
|
||||
memcpy(mat[a],mat[b],ncpy);
|
||||
memcpy(mat[b],s,ncpy);
|
||||
}
|
||||
void Mat::RowMul(int a,double k)
|
||||
{
|
||||
int y;
|
||||
for(y=0;y<n;y++)
|
||||
{
|
||||
mat[a][y]= mat[a][y]*k;
|
||||
}
|
||||
}
|
||||
void Mat::RowAdd(int a,int b,double k)
|
||||
{
|
||||
int y;
|
||||
for(y=0;y<n;y++)
|
||||
{
|
||||
mat[a][y]= mat[a][y]+ mat[b][y]*k;
|
||||
}
|
||||
}
|
||||
|
||||
void Mat::ColExchange(int a, int b)
|
||||
{
|
||||
double s;
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
s=mat[x][a];
|
||||
mat[x][a]=mat[x][b];
|
||||
mat[x][b]=s;
|
||||
}
|
||||
}
|
||||
void Mat::ColMul(int a,double k)
|
||||
{
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
mat[x][a]=mat[x][a]*k;
|
||||
}
|
||||
}
|
||||
void Mat::ColAdd(int a,int b,double k)
|
||||
{
|
||||
int x;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
mat[x][a]=mat[x][a]+mat[x][b]*k;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
double Mat::Sqrt()
|
||||
{
|
||||
int x;
|
||||
double numx;
|
||||
double s=0;
|
||||
for(x=0;x<m;x++)
|
||||
{
|
||||
numx=mat[x][0];
|
||||
s+=(numx*numx);
|
||||
}
|
||||
return s;
|
||||
}
|
||||
double Mat::absvec()
|
||||
{
|
||||
return sqrt(Sqrt());
|
||||
}
|
||||
|
||||
Mat operator ^(Mat a, Mat b)
|
||||
{
|
||||
double ax=a.mat[0][0];
|
||||
double ay=a.mat[1][0];
|
||||
double az=a.mat[2][0];
|
||||
double bx=b.mat[0][0];
|
||||
double by=b.mat[1][0];
|
||||
double bz=b.mat[2][0];
|
||||
|
||||
Mat c(3,1,-1);
|
||||
c.mat[0][0]=ay*bz-az*by;
|
||||
c.mat[1][0]=(-(ax*bz-az*bx));
|
||||
c.mat[2][0]=ax*by-ay*bx;
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,94 +1,94 @@
|
|||
#include "align.h"
|
||||
|
||||
namespace uwb_slam{
|
||||
void Align::Run()
|
||||
{
|
||||
imuDataRxTime = ros::Time::now();
|
||||
uwbDataRxTime = ros::Time::now();
|
||||
|
||||
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
|
||||
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
|
||||
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
|
||||
|
||||
std::ofstream outfile("data.txt",std::ofstream::out);
|
||||
if(outfile.is_open()) {
|
||||
std::cout << "start saving data" << std::endl;
|
||||
outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
|
||||
<< "aX,aY,aZ,"\
|
||||
<< "gX,gY,gZ"\
|
||||
<< "mX,mY,mZ,"\
|
||||
<< "qW,qX,qY,qZ,"\
|
||||
<< "vXY,angleV,"\
|
||||
<< "imuPosX,imuPosY,"\
|
||||
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"\
|
||||
<< "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
|
||||
<< "d1,d2,d3\n";
|
||||
} else {
|
||||
std::cout<<"file can not open"<<std::endl;
|
||||
}
|
||||
|
||||
while(1){
|
||||
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
|
||||
//std::cout << "imu received" << std::endl;
|
||||
imuDataRxTime = imu_odom_.imu_data_.imu_t_;
|
||||
odomDataRxTime = odom_tmp_;
|
||||
uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
|
||||
|
||||
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_.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_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
|
||||
<< imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
|
||||
<< imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
|
||||
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
|
||||
<< lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
|
||||
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
|
||||
}
|
||||
}
|
||||
// outfile.close();
|
||||
// std::cout<< "Data written to file." << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
|
||||
{
|
||||
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
|
||||
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
|
||||
return;
|
||||
}
|
||||
void Align::imuCB(const pibot_msgs::RawImu& imu)
|
||||
{
|
||||
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_[1] = imu.raw_linear_acceleration.y;
|
||||
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_[1] = imu.raw_angular_velocity.y;
|
||||
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_[1] = imu.raw_magnetic_field.y;
|
||||
imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void Align::odomCB(const nav_msgs::Odometry& odom)
|
||||
{
|
||||
odom_tmp_ = odom.header.stamp;
|
||||
imu_odom_.pose_[0] = odom.pose.pose.position.x;
|
||||
imu_odom_.pose_[1] = odom.pose.pose.position.y;
|
||||
imu_odom_.pose_[2] = odom.pose.pose.position.z;
|
||||
imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
|
||||
imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
|
||||
imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
|
||||
imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#include "align.h"
|
||||
|
||||
namespace uwb_slam{
|
||||
void Align::Run()
|
||||
{
|
||||
imuDataRxTime = ros::Time::now();
|
||||
uwbDataRxTime = ros::Time::now();
|
||||
|
||||
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
|
||||
imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
|
||||
odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
|
||||
|
||||
std::ofstream outfile("data.txt",std::ofstream::out);
|
||||
if(outfile.is_open()) {
|
||||
std::cout << "start saving data" << std::endl;
|
||||
outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
|
||||
<< "aX,aY,aZ,"\
|
||||
<< "gX,gY,gZ"\
|
||||
<< "mX,mY,mZ,"\
|
||||
<< "qW,qX,qY,qZ,"\
|
||||
<< "vXY,angleV,"\
|
||||
<< "imuPosX,imuPosY,"\
|
||||
<< "lightHousePosX,lightHousePosY,lightHousePosZ,"\
|
||||
<< "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
|
||||
<< "d1,d2,d3\n";
|
||||
} else {
|
||||
std::cout<<"file can not open"<<std::endl;
|
||||
}
|
||||
|
||||
while(1){
|
||||
if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
|
||||
//std::cout << "imu received" << std::endl;
|
||||
imuDataRxTime = imu_odom_.imu_data_.imu_t_;
|
||||
odomDataRxTime = odom_tmp_;
|
||||
uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
|
||||
|
||||
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_.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_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
|
||||
<< imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
|
||||
<< imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
|
||||
<< lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
|
||||
<< lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
|
||||
<< uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
|
||||
}
|
||||
}
|
||||
// outfile.close();
|
||||
// std::cout<< "Data written to file." << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
|
||||
{
|
||||
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
|
||||
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
|
||||
return;
|
||||
}
|
||||
void Align::imuCB(const pibot_msgs::RawImu& imu)
|
||||
{
|
||||
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_[1] = imu.raw_linear_acceleration.y;
|
||||
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_[1] = imu.raw_angular_velocity.y;
|
||||
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_[1] = imu.raw_magnetic_field.y;
|
||||
imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void Align::odomCB(const nav_msgs::Odometry& odom)
|
||||
{
|
||||
odom_tmp_ = odom.header.stamp;
|
||||
imu_odom_.pose_[0] = odom.pose.pose.position.x;
|
||||
imu_odom_.pose_[1] = odom.pose.pose.position.y;
|
||||
imu_odom_.pose_[2] = odom.pose.pose.position.z;
|
||||
imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
|
||||
imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
|
||||
imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
|
||||
imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,79 +1,79 @@
|
|||
#include "lighthouse.h"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
|
||||
namespace uwb_slam{
|
||||
Lighthouse::Lighthouse(){
|
||||
std::cout << "Start Run. " << std::endl;
|
||||
|
||||
// 创建UDP套接字
|
||||
UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (UdpSocket == -1) {
|
||||
std::cerr << "Error creating UDP socket." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "Creating UDP socket sucess. " << std::endl;
|
||||
|
||||
// 设置套接字地址结构
|
||||
sockaddr_in udpAddress;
|
||||
std::memset(&udpAddress, 0, sizeof(udpAddress));
|
||||
udpAddress.sin_family = AF_INET;
|
||||
udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
udpAddress.sin_port = htons(PORT);
|
||||
|
||||
// 将套接字绑定到地址
|
||||
if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
|
||||
std::cerr << "Error binding UDP socket." << std::endl;
|
||||
close(UdpSocket);
|
||||
}
|
||||
}
|
||||
|
||||
Lighthouse::~Lighthouse(){
|
||||
close(UdpSocket);
|
||||
}
|
||||
|
||||
void Lighthouse::Run() {
|
||||
while(1){
|
||||
// 这是一个阻塞线程
|
||||
this->UDPRead();
|
||||
}
|
||||
}
|
||||
|
||||
void Lighthouse::UDPRead(){
|
||||
char buffer[1024];
|
||||
ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
|
||||
if (bytesRead == -1) {
|
||||
std::cerr << "Error receiving data." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::string data(buffer);
|
||||
|
||||
float x,y,z,qw,qx,qy,qz;
|
||||
|
||||
qw = 1.0;
|
||||
|
||||
sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
|
||||
|
||||
mMutexLighthouse.lock();
|
||||
|
||||
this->data.x_ = x;
|
||||
this->data.y_ = y;
|
||||
this->data.z_ = z;
|
||||
|
||||
this->data.qw_ = qw;
|
||||
this->data.qx_ = qx;
|
||||
this->data.qy_ = qy;
|
||||
this->data.qz_ = qz;
|
||||
|
||||
|
||||
|
||||
mMutexLighthouse.unlock();
|
||||
|
||||
// 打印接收到的消息
|
||||
buffer[bytesRead] = '\0';
|
||||
//std::cout << "Received: " << buffer << std::endl;
|
||||
|
||||
}
|
||||
#include "lighthouse.h"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
|
||||
namespace uwb_slam{
|
||||
Lighthouse::Lighthouse(){
|
||||
std::cout << "Start Run. " << std::endl;
|
||||
|
||||
// 创建UDP套接字
|
||||
UdpSocket = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (UdpSocket == -1) {
|
||||
std::cerr << "Error creating UDP socket." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "Creating UDP socket sucess. " << std::endl;
|
||||
|
||||
// 设置套接字地址结构
|
||||
sockaddr_in udpAddress;
|
||||
std::memset(&udpAddress, 0, sizeof(udpAddress));
|
||||
udpAddress.sin_family = AF_INET;
|
||||
udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
udpAddress.sin_port = htons(PORT);
|
||||
|
||||
// 将套接字绑定到地址
|
||||
if (bind(UdpSocket, (struct sockaddr*)&udpAddress, sizeof(udpAddress)) == -1) {
|
||||
std::cerr << "Error binding UDP socket." << std::endl;
|
||||
close(UdpSocket);
|
||||
}
|
||||
}
|
||||
|
||||
Lighthouse::~Lighthouse(){
|
||||
close(UdpSocket);
|
||||
}
|
||||
|
||||
void Lighthouse::Run() {
|
||||
while(1){
|
||||
// 这是一个阻塞线程
|
||||
this->UDPRead();
|
||||
}
|
||||
}
|
||||
|
||||
void Lighthouse::UDPRead(){
|
||||
char buffer[1024];
|
||||
ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
|
||||
if (bytesRead == -1) {
|
||||
std::cerr << "Error receiving data." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
std::string data(buffer);
|
||||
|
||||
float x,y,z,qw,qx,qy,qz;
|
||||
|
||||
qw = 1.0;
|
||||
|
||||
sscanf(data.c_str(),"%f %f %f %f %f %f",&x,&y,&z,&qx,&qy,&qz);
|
||||
|
||||
mMutexLighthouse.lock();
|
||||
|
||||
this->data.x_ = x;
|
||||
this->data.y_ = y;
|
||||
this->data.z_ = z;
|
||||
|
||||
this->data.qw_ = qw;
|
||||
this->data.qx_ = qx;
|
||||
this->data.qy_ = qy;
|
||||
this->data.qz_ = qz;
|
||||
|
||||
|
||||
|
||||
mMutexLighthouse.unlock();
|
||||
|
||||
// 打印接收到的消息
|
||||
buffer[bytesRead] = '\0';
|
||||
//std::cout << "Received: " << buffer << std::endl;
|
||||
|
||||
}
|
||||
};
|
||||
|
|
@ -1,60 +1,60 @@
|
|||
#include "Mat.h"
|
||||
|
||||
// #include "align.h"
|
||||
#include "mapping.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <thread>
|
||||
#include "senddata.h"
|
||||
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
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::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::Align> align = std::make_shared<uwb_slam::Align>();
|
||||
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
|
||||
|
||||
|
||||
|
||||
|
||||
mp->uwb_ = uwb;
|
||||
mp->align_ = align;
|
||||
align->uwb_ =uwb;
|
||||
align->lighthouse_ = lighthouse;
|
||||
sender->uwb_ = uwb;
|
||||
// // control data fllow in system
|
||||
// std::thread rose_trd ([&system]() {
|
||||
// system->Run();
|
||||
// });
|
||||
|
||||
// uwb serried read
|
||||
std::thread uwb_trd([&uwb]() {
|
||||
uwb->Run();
|
||||
});
|
||||
|
||||
// build map
|
||||
// std::thread map_trd ([&mp]() {
|
||||
// mp->Run();
|
||||
// });
|
||||
|
||||
std::thread sender_trd ([&sender, uwb]() {
|
||||
sender->Run(uwb);
|
||||
});
|
||||
|
||||
// std::thread align_trd ([&align]() {
|
||||
// align->Run();
|
||||
// });
|
||||
|
||||
// std::thread lighthouse_trd ([&lighthouse]() {
|
||||
// lighthouse->Run();
|
||||
// });
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
#include "Mat.h"
|
||||
|
||||
// #include "align.h"
|
||||
#include "mapping.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <thread>
|
||||
#include "senddata.h"
|
||||
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
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::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::Align> align = std::make_shared<uwb_slam::Align>();
|
||||
std::shared_ptr<uwb_slam::Lighthouse> lighthouse = std::make_shared<uwb_slam::Lighthouse>();
|
||||
|
||||
|
||||
|
||||
|
||||
mp->uwb_ = uwb;
|
||||
mp->align_ = align;
|
||||
align->uwb_ =uwb;
|
||||
align->lighthouse_ = lighthouse;
|
||||
sender->uwb_ = uwb;
|
||||
// // control data fllow in system
|
||||
// std::thread rose_trd ([&system]() {
|
||||
// system->Run();
|
||||
// });
|
||||
|
||||
// uwb serried read
|
||||
std::thread uwb_trd([&uwb]() {
|
||||
uwb->Run();
|
||||
});
|
||||
|
||||
// build map
|
||||
// std::thread map_trd ([&mp]() {
|
||||
// mp->Run();
|
||||
// });
|
||||
|
||||
std::thread sender_trd ([&sender, uwb]() {
|
||||
sender->Run(uwb);
|
||||
});
|
||||
|
||||
// std::thread align_trd ([&align]() {
|
||||
// align->Run();
|
||||
// });
|
||||
|
||||
// std::thread lighthouse_trd ([&lighthouse]() {
|
||||
// lighthouse->Run();
|
||||
// });
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,149 +1,149 @@
|
|||
#include "mapping.h"
|
||||
#include <mutex>
|
||||
#include <unistd.h>// 包含 usleep() 函数所在的头文件
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
|
||||
namespace uwb_slam
|
||||
{
|
||||
bool Mapping::checkQueue()
|
||||
{
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
return !posDataQueue.empty();
|
||||
}
|
||||
|
||||
void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
|
||||
{
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
posDataQueue.push({imuPosData, uwbPosData, syncPosData});
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Mapping::process()
|
||||
{
|
||||
{
|
||||
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
//std::cout << "SIZE: " <<posDataQueue.size() << std::endl;
|
||||
posData = posDataQueue.front();
|
||||
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
|
||||
posDataQueue.pop();
|
||||
}
|
||||
/*生成图*/
|
||||
|
||||
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 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 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;
|
||||
// std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
|
||||
// std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
|
||||
// std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
|
||||
|
||||
// std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
|
||||
// std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
|
||||
// std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
|
||||
// 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>(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
|
||||
}
|
||||
|
||||
|
||||
void Mapping::Run()
|
||||
{
|
||||
|
||||
//int key = cv::waitKey(0);//等待用户按下按键
|
||||
//std::cout << key << std::endl;
|
||||
realWidth = AREA_SIZE / PIXEL_SCALE;
|
||||
realHeight = AREA_SIZE / PIXEL_SCALE;
|
||||
|
||||
img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
|
||||
//cankao
|
||||
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
|
||||
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
|
||||
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 i=realWidth/2-1; i<=realWidth/2+1; i+=1)
|
||||
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||
int i = 0, j = 0;
|
||||
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||
|
||||
|
||||
|
||||
cv::imshow("Image",img);
|
||||
|
||||
// bool printFlag = 0;
|
||||
// std::ofstream outfile("data.txt",std::ofstream::out);
|
||||
// if(outfile.is_open()) {
|
||||
// std::cout << "start saving data" << key << std::endl;
|
||||
// printFlag = 1;
|
||||
// } else {
|
||||
// std::cout<<"file can not open"<<std::endl;
|
||||
// }
|
||||
|
||||
/*
|
||||
std::cout << "waiting" <<std::endl;
|
||||
int key = cv::waitKey(0);
|
||||
if (key == 'q' || key == 27) {
|
||||
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
|
||||
readPos = true;
|
||||
std::cout << "non" << key << std::endl;
|
||||
cv::destroyAllWindows();
|
||||
}
|
||||
*/
|
||||
while(1){
|
||||
int key = cv::waitKey(0);
|
||||
if (key == 'q' ) {
|
||||
readPos = true;
|
||||
std::cout << "start mapping" << key << std::endl;
|
||||
|
||||
//cv::destroyAllWindows();
|
||||
}
|
||||
while( readPos )//按下空格键
|
||||
{
|
||||
int key2 = cv::waitKey(1);
|
||||
if (key2 == 'q') {
|
||||
//TODO: save
|
||||
|
||||
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径
|
||||
cv::imwrite(pngimage,img);
|
||||
readPos = false;
|
||||
|
||||
// outfile.close();
|
||||
// printFlag = 0;
|
||||
// std::cout<< "Data written to file." << std::endl;
|
||||
|
||||
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(uwb_->x, uwb_->y));
|
||||
|
||||
//uwb xieru
|
||||
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
|
||||
|
||||
if(checkQueue())
|
||||
{
|
||||
//std::cout << " start process" << std::endl;
|
||||
process();
|
||||
//std::cout << " end process" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
// std::cout << "out" << key << std::endl;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
|
||||
//cv::imwrite(pngimage,img);
|
||||
|
||||
/*ros 发送图片给导航 */
|
||||
}
|
||||
} // namespace uwb_slam
|
||||
|
||||
#include "mapping.h"
|
||||
#include <mutex>
|
||||
#include <unistd.h>// 包含 usleep() 函数所在的头文件
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
|
||||
namespace uwb_slam
|
||||
{
|
||||
bool Mapping::checkQueue()
|
||||
{
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
return !posDataQueue.empty();
|
||||
}
|
||||
|
||||
void Mapping::feedPos(const cv::Point2d & imuPosData, const cv::Point2d & uwbPosData, const cv::Point2d & syncPosData)
|
||||
{
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
posDataQueue.push({imuPosData, uwbPosData, syncPosData});
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Mapping::process()
|
||||
{
|
||||
{
|
||||
|
||||
//std::unique_lock<std::mutex> lock(mMutexMap);
|
||||
//std::cout << "SIZE: " <<posDataQueue.size() << std::endl;
|
||||
posData = posDataQueue.front();
|
||||
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
|
||||
posDataQueue.pop();
|
||||
}
|
||||
/*生成图*/
|
||||
|
||||
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 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 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;
|
||||
// std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
|
||||
// std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
|
||||
// std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
|
||||
|
||||
// std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
|
||||
// std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;
|
||||
// std::cout << " imuPos:" <<imuPosPointX << " " << imuPosPointY << std::endl;
|
||||
// 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>(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
|
||||
}
|
||||
|
||||
|
||||
void Mapping::Run()
|
||||
{
|
||||
|
||||
//int key = cv::waitKey(0);//等待用户按下按键
|
||||
//std::cout << key << std::endl;
|
||||
realWidth = AREA_SIZE / PIXEL_SCALE;
|
||||
realHeight = AREA_SIZE / PIXEL_SCALE;
|
||||
|
||||
img = cv::Mat(realHeight, realWidth, CV_8UC3, cv::Scalar(255,255,255));
|
||||
//cankao
|
||||
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=10)
|
||||
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=10)
|
||||
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 i=realWidth/2-1; i<=realWidth/2+1; i+=1)
|
||||
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||
int i = 0, j = 0;
|
||||
img.at<cv::Vec3b>(j,i)= cv::Vec3b(0,0,0);
|
||||
|
||||
|
||||
|
||||
cv::imshow("Image",img);
|
||||
|
||||
// bool printFlag = 0;
|
||||
// std::ofstream outfile("data.txt",std::ofstream::out);
|
||||
// if(outfile.is_open()) {
|
||||
// std::cout << "start saving data" << key << std::endl;
|
||||
// printFlag = 1;
|
||||
// } else {
|
||||
// std::cout<<"file can not open"<<std::endl;
|
||||
// }
|
||||
|
||||
/*
|
||||
std::cout << "waiting" <<std::endl;
|
||||
int key = cv::waitKey(0);
|
||||
if (key == 'q' || key == 27) {
|
||||
this->feed_uwb_data(cv::Point2d(uwb_->x,uwb_->y));
|
||||
readPos = true;
|
||||
std::cout << "non" << key << std::endl;
|
||||
cv::destroyAllWindows();
|
||||
}
|
||||
*/
|
||||
while(1){
|
||||
int key = cv::waitKey(0);
|
||||
if (key == 'q' ) {
|
||||
readPos = true;
|
||||
std::cout << "start mapping" << key << std::endl;
|
||||
|
||||
//cv::destroyAllWindows();
|
||||
}
|
||||
while( readPos )//按下空格键
|
||||
{
|
||||
int key2 = cv::waitKey(1);
|
||||
if (key2 == 'q') {
|
||||
//TODO: save
|
||||
|
||||
std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/pibot_msgs/Map/output_image.png";//保存的图片文件路径
|
||||
cv::imwrite(pngimage,img);
|
||||
readPos = false;
|
||||
|
||||
// outfile.close();
|
||||
// printFlag = 0;
|
||||
// std::cout<< "Data written to file." << std::endl;
|
||||
|
||||
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(uwb_->x, uwb_->y));
|
||||
|
||||
//uwb xieru
|
||||
//std::cout << "cur_SEQ: " <<uwb_->cur_seq << std::endl;
|
||||
|
||||
if(checkQueue())
|
||||
{
|
||||
//std::cout << " start process" << std::endl;
|
||||
process();
|
||||
//std::cout << " end process" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
// std::cout << "out" << key << std::endl;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//std::string pngimage="../Map/pngimage.png";//保存的图片文件路径
|
||||
//cv::imwrite(pngimage,img);
|
||||
|
||||
/*ros 发送图片给导航 */
|
||||
}
|
||||
} // namespace uwb_slam
|
||||
|
||||
|
|
|
|||
|
|
@ -1,147 +1,147 @@
|
|||
#include "senddata.h"
|
||||
|
||||
#define angleLimit 20
|
||||
#define disLimit 200
|
||||
|
||||
namespace uwb_slam
|
||||
{
|
||||
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
||||
|
||||
ros::Rate loop_rate(10);
|
||||
// position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",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));
|
||||
// odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
||||
while(ros::ok()){
|
||||
// publishOdometry(uwb);
|
||||
if (flag_)
|
||||
{
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = -1;
|
||||
for (int i = 0; i < 17; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0.2;
|
||||
cmd_vel_.angular.z = 0;
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = 1;
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = 0;
|
||||
flag_ = 0;
|
||||
}
|
||||
else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
|
||||
{
|
||||
float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
|
||||
cmd_vel_.angular.z = angularSpeed;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->aoa[0] < -angleLimit)
|
||||
{
|
||||
float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
|
||||
cmd_vel_.angular.z = angularSpeed;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->d[0] > disLimit)
|
||||
{
|
||||
float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = lineSpeed;
|
||||
}
|
||||
else if(uwb->d[0] < disLimit - 30)
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = -0.2;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
// void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
||||
// sub_odom_ = odom;
|
||||
// return;
|
||||
// }
|
||||
void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
|
||||
{
|
||||
for (const auto& obstacle_info :stereo->dis)
|
||||
{
|
||||
float distance = obstacle_info.distance;
|
||||
// float width = obstacle_info.width;
|
||||
// float height = obstacle_info.height;
|
||||
// float angle = obstacle_info.angle;
|
||||
|
||||
// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
|
||||
if(distance>5&&distance<45)
|
||||
{
|
||||
flag_ = 1;
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
|
||||
// {
|
||||
|
||||
// std::mutex mMutexSend;
|
||||
// ros::Time current_time = ros::Time::now();
|
||||
|
||||
// // 设置 Odometry 消息的头部信息
|
||||
// odom_.header.stamp = current_time;
|
||||
// odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
|
||||
// odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
|
||||
|
||||
// // 填充 Odometry 消息的位置信息
|
||||
// odom_.pose.pose.position.x = uwb->uwb_data_.x_;
|
||||
// odom_.pose.pose.position.y = uwb->uwb_data_.y_;
|
||||
// odom_.pose.pose.position.z = 0.0;
|
||||
|
||||
|
||||
// // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
|
||||
// // tf2::Quaternion quat;
|
||||
// // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
|
||||
// // odom.pose.pose.orientation.x = quat.x();
|
||||
// // odom.pose.pose.orientation.y = quat.y();
|
||||
// // odom.pose.pose.orientation.z = quat.z();
|
||||
// // odom.pose.pose.orientation.w = quat.w();
|
||||
|
||||
// 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.z = sub_odom_.pose.pose.orientation.z;
|
||||
// odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
|
||||
|
||||
// // 发布 Odometry 消息
|
||||
// position_pub_.publish(odom_);
|
||||
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#include "senddata.h"
|
||||
|
||||
#define angleLimit 20
|
||||
#define disLimit 200
|
||||
|
||||
namespace uwb_slam
|
||||
{
|
||||
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
|
||||
|
||||
ros::Rate loop_rate(10);
|
||||
// position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",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));
|
||||
// odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
|
||||
while(ros::ok()){
|
||||
// publishOdometry(uwb);
|
||||
if (flag_)
|
||||
{
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = -1;
|
||||
for (int i = 0; i < 17; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0.2;
|
||||
cmd_vel_.angular.z = 0;
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = 1;
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
cmd_vel_.linear.x = 0;
|
||||
cmd_vel_.angular.z = 0;
|
||||
flag_ = 0;
|
||||
}
|
||||
else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
|
||||
{
|
||||
float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
|
||||
cmd_vel_.angular.z = angularSpeed;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->aoa[0] < -angleLimit)
|
||||
{
|
||||
float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
|
||||
cmd_vel_.angular.z = angularSpeed;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
else if(uwb->d[0] > disLimit)
|
||||
{
|
||||
float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = lineSpeed;
|
||||
}
|
||||
else if(uwb->d[0] < disLimit - 30)
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = -0.2;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd_vel_.angular.z = 0;
|
||||
cmd_vel_.linear.x = 0;
|
||||
}
|
||||
cmd_vel_pub_.publish(cmd_vel_);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
// void Senddata::odomCB(const nav_msgs::Odometry& odom){
|
||||
// sub_odom_ = odom;
|
||||
// return;
|
||||
// }
|
||||
void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
|
||||
{
|
||||
for (const auto& obstacle_info :stereo->dis)
|
||||
{
|
||||
float distance = obstacle_info.distance;
|
||||
// float width = obstacle_info.width;
|
||||
// float height = obstacle_info.height;
|
||||
// float angle = obstacle_info.angle;
|
||||
|
||||
// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
|
||||
if(distance>5&&distance<45)
|
||||
{
|
||||
flag_ = 1;
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
|
||||
// {
|
||||
|
||||
// std::mutex mMutexSend;
|
||||
// ros::Time current_time = ros::Time::now();
|
||||
|
||||
// // 设置 Odometry 消息的头部信息
|
||||
// odom_.header.stamp = current_time;
|
||||
// odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
|
||||
// odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
|
||||
|
||||
// // 填充 Odometry 消息的位置信息
|
||||
// odom_.pose.pose.position.x = uwb->uwb_data_.x_;
|
||||
// odom_.pose.pose.position.y = uwb->uwb_data_.y_;
|
||||
// odom_.pose.pose.position.z = 0.0;
|
||||
|
||||
|
||||
// // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
|
||||
// // tf2::Quaternion quat;
|
||||
// // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
|
||||
// // odom.pose.pose.orientation.x = quat.x();
|
||||
// // odom.pose.pose.orientation.y = quat.y();
|
||||
// // odom.pose.pose.orientation.z = quat.z();
|
||||
// // odom.pose.pose.orientation.w = quat.w();
|
||||
|
||||
// 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.z = sub_odom_.pose.pose.orientation.z;
|
||||
// odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
|
||||
|
||||
// // 发布 Odometry 消息
|
||||
// position_pub_.publish(odom_);
|
||||
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} // namespace uwb_slam
|
||||
|
|
@ -1,121 +1,121 @@
|
|||
#include "uwb.h"
|
||||
#include <cmath>
|
||||
#include "Mat.h"
|
||||
|
||||
|
||||
#define CARHEIGHT 20
|
||||
#define DISINDEX 3
|
||||
#define AOAINDEX 15
|
||||
#define DATALENGTH 27
|
||||
|
||||
namespace uwb_slam{
|
||||
Uwb::Uwb(){
|
||||
|
||||
}
|
||||
|
||||
void Uwb::Run() {
|
||||
|
||||
while(1){
|
||||
this->Serread();
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Uwb::Serread(){
|
||||
try {
|
||||
boost::asio::io_service io;
|
||||
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::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::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
|
||||
|
||||
uint8_t tmpdata[DATALENGTH];
|
||||
// std::cerr << "befor read" << std::endl;
|
||||
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
|
||||
// std::cerr << "after read" << std::endl;
|
||||
this->uwb_data_.uwb_t_ = ros::Time::now();
|
||||
|
||||
for (int i=0;i< bytesRead;i++)
|
||||
{
|
||||
std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
memcpy(&this->d[0], &tmpdata[DISINDEX], 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->aoa[0], &tmpdata[AOAINDEX], 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]));
|
||||
|
||||
//std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
|
||||
|
||||
// if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
|
||||
// return;
|
||||
// }
|
||||
// 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));
|
||||
// }
|
||||
|
||||
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[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;
|
||||
} catch (const std::exception& ex) {
|
||||
std::cerr << "Exception: " << ex.what() << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void fusion()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
// bool Uwb::checknewdata()
|
||||
// {
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// return !v_buffer_imu_odom_pose_data_.empty();
|
||||
// }
|
||||
|
||||
// void Uwb::Run() {
|
||||
// while(1)
|
||||
// {
|
||||
// if(checknewdata())
|
||||
// {
|
||||
// {
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
|
||||
// v_buffer_imu_odom_pose_data_.pop();
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
|
||||
// }
|
||||
|
||||
#include "uwb.h"
|
||||
#include <cmath>
|
||||
#include "Mat.h"
|
||||
|
||||
|
||||
#define CARHEIGHT 20
|
||||
#define DISINDEX 3
|
||||
#define AOAINDEX 15
|
||||
#define DATALENGTH 27
|
||||
|
||||
namespace uwb_slam{
|
||||
Uwb::Uwb(){
|
||||
|
||||
}
|
||||
|
||||
void Uwb::Run() {
|
||||
|
||||
while(1){
|
||||
this->Serread();
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Uwb::Serread(){
|
||||
try {
|
||||
boost::asio::io_service io;
|
||||
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::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::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
|
||||
|
||||
uint8_t tmpdata[DATALENGTH];
|
||||
// std::cerr << "befor read" << std::endl;
|
||||
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
|
||||
// std::cerr << "after read" << std::endl;
|
||||
this->uwb_data_.uwb_t_ = ros::Time::now();
|
||||
|
||||
for (int i=0;i< bytesRead;i++)
|
||||
{
|
||||
std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
memcpy(&this->d[0], &tmpdata[DISINDEX], 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->aoa[0], &tmpdata[AOAINDEX], 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]));
|
||||
|
||||
//std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
|
||||
|
||||
// if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
|
||||
// return;
|
||||
// }
|
||||
// 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));
|
||||
// }
|
||||
|
||||
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[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;
|
||||
} catch (const std::exception& ex) {
|
||||
std::cerr << "Exception: " << ex.what() << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void fusion()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
// bool Uwb::checknewdata()
|
||||
// {
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// return !v_buffer_imu_odom_pose_data_.empty();
|
||||
// }
|
||||
|
||||
// void Uwb::Run() {
|
||||
// while(1)
|
||||
// {
|
||||
// if(checknewdata())
|
||||
// {
|
||||
// {
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
|
||||
// v_buffer_imu_odom_pose_data_.pop();
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
|
||||
// std::unique_lock<std::mutex> lock(mMutexUwb);
|
||||
// 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
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/gripper_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/one_side_gripper_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/one_side_gripper_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_action_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_action_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/setup.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/setup.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/__init__.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/__init__.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/arbotix.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/arbotix.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/ax12.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/ax12.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/controllers.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/controllers.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/diff_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/diff_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/follow_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/follow_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/io.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/io.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/joints.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/joints.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/linear_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/linear_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/publishers.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/publishers.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/servo_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/servo_controller.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/ir_ranger.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/ir_ranger.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/max_sonar.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/bin/max_sonar.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/setup.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/setup.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/__init__.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/__init__.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/sensors.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/sensors.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_angular.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_angular.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_linear.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/calibrate_linear.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/marker_server.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/marker_server.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_goal.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_goal.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_multi_goals.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/navigation_multi_goals.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/teleop_twist_keyboard.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/teleop_twist_keyboard.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/transform_utils.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot/scripts/transform_utils.py
Executable file → Normal file
|
|
@ -1,27 +1,27 @@
|
|||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: camera
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [577.54679, 0. , 310.24326,
|
||||
0. , 578.63325, 253.65539,
|
||||
0. , 0. , 1. ]
|
||||
camera_model: plumb_bob
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [0.125197, -0.196591, 0.006816, -0.006225, 0.000000]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1., 0., 0.,
|
||||
0., 1., 0.,
|
||||
0., 0., 1.]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [590.55457, 0. , 306.57339, 0. ,
|
||||
0. , 592.83978, 256.43008, 0. ,
|
||||
0. , 0. , 1. , 0. ]
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: camera
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [577.54679, 0. , 310.24326,
|
||||
0. , 578.63325, 253.65539,
|
||||
0. , 0. , 1. ]
|
||||
camera_model: plumb_bob
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [0.125197, -0.196591, 0.006816, -0.006225, 0.000000]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1., 0., 0.,
|
||||
0., 1., 0.,
|
||||
0., 0., 1.]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
cols: 4
|
||||
data: [590.55457, 0. , 306.57339, 0. ,
|
||||
0. , 592.83978, 256.43008, 0. ,
|
||||
0. , 0. , 1. , 0. ]
|
||||
|
|
|
|||
|
|
@ -1,20 +1,20 @@
|
|||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: depth_Astra_Orbbec
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [582.795354, 0.000000, 326.415982, 0.000000, 584.395006, 249.989410, 0.000000, 0.000000, 1.000000]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [-0.068613, 0.174404, 0.001015, 0.006240, 0.000000]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
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]
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
camera_name: depth_Astra_Orbbec
|
||||
camera_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [582.795354, 0.000000, 326.415982, 0.000000, 584.395006, 249.989410, 0.000000, 0.000000, 1.000000]
|
||||
distortion_model: plumb_bob
|
||||
distortion_coefficients:
|
||||
rows: 1
|
||||
cols: 5
|
||||
data: [-0.068613, 0.174404, 0.001015, 0.006240, 0.000000]
|
||||
rectification_matrix:
|
||||
rows: 3
|
||||
cols: 3
|
||||
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
|
||||
projection_matrix:
|
||||
rows: 3
|
||||
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]
|
||||
|
|
|
|||
|
|
@ -1,32 +1,32 @@
|
|||
#ifndef PIBOT_SIMPLE_DATAFRAME_MASTER_H_
|
||||
#define PIBOT_SIMPLE_DATAFRAME_MASTER_H_
|
||||
|
||||
#include "simple_dataframe.h"
|
||||
#include <string.h>
|
||||
|
||||
class Transport;
|
||||
class Simple_dataframe : public Dataframe
|
||||
{
|
||||
public:
|
||||
Simple_dataframe(Transport* trans=0);
|
||||
~Simple_dataframe();
|
||||
void register_notify(const MESSAGE_ID id, Notify* _nf) {
|
||||
}
|
||||
|
||||
bool data_recv(unsigned char c);
|
||||
bool data_parse();
|
||||
bool init();
|
||||
bool interact(const MESSAGE_ID id);
|
||||
private:
|
||||
bool recv_proc();
|
||||
private:
|
||||
bool send_message(const MESSAGE_ID id);
|
||||
bool send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len);
|
||||
bool send_message(Message* msg);
|
||||
private:
|
||||
Message active_rx_msg;
|
||||
|
||||
RECEIVE_STATE recv_state;
|
||||
Transport* trans;
|
||||
};
|
||||
#ifndef PIBOT_SIMPLE_DATAFRAME_MASTER_H_
|
||||
#define PIBOT_SIMPLE_DATAFRAME_MASTER_H_
|
||||
|
||||
#include "simple_dataframe.h"
|
||||
#include <string.h>
|
||||
|
||||
class Transport;
|
||||
class Simple_dataframe : public Dataframe
|
||||
{
|
||||
public:
|
||||
Simple_dataframe(Transport* trans=0);
|
||||
~Simple_dataframe();
|
||||
void register_notify(const MESSAGE_ID id, Notify* _nf) {
|
||||
}
|
||||
|
||||
bool data_recv(unsigned char c);
|
||||
bool data_parse();
|
||||
bool init();
|
||||
bool interact(const MESSAGE_ID id);
|
||||
private:
|
||||
bool recv_proc();
|
||||
private:
|
||||
bool send_message(const MESSAGE_ID id);
|
||||
bool send_message(const MESSAGE_ID id, unsigned char* data, unsigned char len);
|
||||
bool send_message(Message* msg);
|
||||
private:
|
||||
Message active_rx_msg;
|
||||
|
||||
RECEIVE_STATE recv_state;
|
||||
Transport* trans;
|
||||
};
|
||||
#endif
|
||||
|
|
@ -1,30 +1,30 @@
|
|||
#ifndef TRANSPORT_H_
|
||||
#define TRANSPORT_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <inttypes.h>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <queue>
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/smart_ptr.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
typedef std::vector<uint8_t> Buffer;
|
||||
|
||||
class Transport
|
||||
{
|
||||
public:
|
||||
virtual ~Transport(){}
|
||||
virtual bool init()=0;
|
||||
virtual void set_timeout(int t)=0;
|
||||
virtual bool is_timeout()=0;
|
||||
virtual Buffer read() = 0;
|
||||
|
||||
virtual void write(Buffer &data) = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* TRANSPORT_BASE_H_ */
|
||||
#ifndef TRANSPORT_H_
|
||||
#define TRANSPORT_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <inttypes.h>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <queue>
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/smart_ptr.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
typedef std::vector<uint8_t> Buffer;
|
||||
|
||||
class Transport
|
||||
{
|
||||
public:
|
||||
virtual ~Transport(){}
|
||||
virtual bool init()=0;
|
||||
virtual void set_timeout(int t)=0;
|
||||
virtual bool is_timeout()=0;
|
||||
virtual Buffer read() = 0;
|
||||
|
||||
virtual void write(Buffer &data) = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* TRANSPORT_BASE_H_ */
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
<launch>
|
||||
<node
|
||||
name="rviz"
|
||||
pkg="rviz"
|
||||
type="rviz"
|
||||
args="-d $(find pibot_description)/urdf.rviz" />
|
||||
</launch>
|
||||
<launch>
|
||||
<node
|
||||
name="rviz"
|
||||
pkg="rviz"
|
||||
type="rviz"
|
||||
args="-d $(find pibot_description)/urdf.rviz" />
|
||||
</launch>
|
||||
|
|
|
|||
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py
Executable file → Normal file
|
|
@ -1,20 +1,20 @@
|
|||
<launch>
|
||||
<include
|
||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||
<node
|
||||
name="tf_footprint_base"
|
||||
pkg="tf"
|
||||
type="static_transform_publisher"
|
||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
<node
|
||||
name="spawn_model"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-file $(find pi_bot.SLDASM)/robots/pi_bot.SLDASM.urdf -urdf -model pi_bot.SLDASM"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
<launch>
|
||||
<include
|
||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||
<node
|
||||
name="tf_footprint_base"
|
||||
pkg="tf"
|
||||
type="static_transform_publisher"
|
||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
<node
|
||||
name="spawn_model"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-file $(find pi_bot.SLDASM)/robots/pi_bot.SLDASM.urdf -urdf -model pi_bot.SLDASM"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
<package>
|
||||
<description brief="pi_bot.SLDASM">pi_bot.SLDASM</description>
|
||||
<depend package="gazebo" />
|
||||
<author>me</author>
|
||||
<license>BSD</license>
|
||||
<package>
|
||||
<description brief="pi_bot.SLDASM">pi_bot.SLDASM</description>
|
||||
<depend package="gazebo" />
|
||||
<author>me</author>
|
||||
<license>BSD</license>
|
||||
</package>
|
||||
|
|
@ -1,58 +1,58 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<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"/> -->
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<!-- <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"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.175"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos -->
|
||||
<xacro:property name="camera_pos_rear" value="false"/>
|
||||
|
||||
<xacro:if value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0 0 0.25"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.07"/>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0.145 0 0.215"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.04"/>
|
||||
</xacro:unless>
|
||||
|
||||
<xacro:property name="camera_joint_rpy" value="0 0 0"/>
|
||||
</robot>
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<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"/> -->
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<!-- <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"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.175"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos -->
|
||||
<xacro:property name="camera_pos_rear" value="false"/>
|
||||
|
||||
<xacro:if value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0 0 0.25"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.07"/>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0.145 0 0.215"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.04"/>
|
||||
</xacro:unless>
|
||||
|
||||
<xacro:property name="camera_joint_rpy" value="0 0 0"/>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,67 +1,67 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- apolloX use omni wheel/universal wheel -->
|
||||
<xacro:property name="use_omni_wheel" value="false"/>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:if value="${use_omni_wheel}">
|
||||
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.13"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${use_omni_wheel}">
|
||||
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.185"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
</xacro:unless>
|
||||
|
||||
<!-- Camera Pos -->
|
||||
<xacro:property name="camera_pos_rear" value="true"/>
|
||||
|
||||
<xacro:if value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0 0 0.27"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</xacro:if>
|
||||
|
||||
<xacro:unless value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0.28 0 0.23"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.04"/>
|
||||
</xacro:unless>
|
||||
|
||||
<xacro:property name="camera_joint_rpy" value="0 0 0"/>
|
||||
|
||||
</robot>
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/apolloX_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- apolloX use omni wheel/universal wheel -->
|
||||
<xacro:property name="use_omni_wheel" value="false"/>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:if value="${use_omni_wheel}">
|
||||
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.13"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${use_omni_wheel}">
|
||||
<xacro:property name="lidar_joint_xyz" value="0.13 0 0.185"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
</xacro:unless>
|
||||
|
||||
<!-- Camera Pos -->
|
||||
<xacro:property name="camera_pos_rear" value="true"/>
|
||||
|
||||
<xacro:if value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0 0 0.27"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</xacro:if>
|
||||
|
||||
<xacro:unless value="${camera_pos_rear}">
|
||||
<xacro:property name="camera_pillar_enabled" value="true"/>
|
||||
<xacro:property name="camera_joint_xyz" value="0.28 0 0.23"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.04"/>
|
||||
</xacro:unless>
|
||||
|
||||
<xacro:property name="camera_joint_rpy" value="0 0 0"/>
|
||||
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,46 +1,46 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.063" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_dark_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</robot>
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.063" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_dark_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hades_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,46 +1,46 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.063" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_dark_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</robot>
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.063" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_dark_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/hera_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pos-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.17"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.08"/>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,39 +1,39 @@
|
|||
|
||||
<robot name="pibot_robot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
<!-- xacro includes -->
|
||||
<xacro:include filename="$(find pibot_description)/urdf/components/pibot_properties.xacro"/>
|
||||
|
||||
<xacro:arg name="model" default="apollo" />
|
||||
<xacro:arg name="lidar" default="none" />
|
||||
<xacro:arg name="pb_3dsensor" default="none" />
|
||||
|
||||
<xacro:macro name="pibot_robot">
|
||||
<!-- Footprint -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<!-- Joint from base_footprint to base link(chassis) -->
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="base_footprint" />
|
||||
</joint>
|
||||
|
||||
<!-- Base -->
|
||||
<xacro:include filename="$(find pibot_description)/urdf/$(arg model).urdf.xacro"/>
|
||||
<xacro:base></xacro:base>
|
||||
|
||||
<!-- LIDAR -->
|
||||
<xacro:property name="pibot_lidar" value="$(arg lidar)"/>
|
||||
<xacro:unless value="${pibot_lidar == 'none'}">
|
||||
<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:unless>
|
||||
|
||||
<!-- Camera -->
|
||||
<xacro:property name="pibot_3dsensor" value="$(arg pb_3dsensor)"/>
|
||||
<xacro:unless value="${pibot_3dsensor == 'none'}">
|
||||
<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:unless>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
||||
<robot name="pibot_robot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
<!-- xacro includes -->
|
||||
<xacro:include filename="$(find pibot_description)/urdf/components/pibot_properties.xacro"/>
|
||||
|
||||
<xacro:arg name="model" default="apollo" />
|
||||
<xacro:arg name="lidar" default="none" />
|
||||
<xacro:arg name="pb_3dsensor" default="none" />
|
||||
|
||||
<xacro:macro name="pibot_robot">
|
||||
<!-- Footprint -->
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<!-- Joint from base_footprint to base link(chassis) -->
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="base_footprint" />
|
||||
</joint>
|
||||
|
||||
<!-- Base -->
|
||||
<xacro:include filename="$(find pibot_description)/urdf/$(arg model).urdf.xacro"/>
|
||||
<xacro:base></xacro:base>
|
||||
|
||||
<!-- LIDAR -->
|
||||
<xacro:property name="pibot_lidar" value="$(arg lidar)"/>
|
||||
<xacro:unless value="${pibot_lidar == 'none'}">
|
||||
<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:unless>
|
||||
|
||||
<!-- Camera -->
|
||||
<xacro:property name="pibot_3dsensor" value="$(arg pb_3dsensor)"/>
|
||||
<xacro:unless value="${pibot_3dsensor == 'none'}">
|
||||
<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:unless>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,46 +1,46 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.065" rpy="0 0 0.5235987755982988" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pose-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.18"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.085"/>
|
||||
</robot>
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="pibot" xmlns:xacro="https://ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="base">
|
||||
<!-- Chassis a.k.a base_link -->
|
||||
<link name="base_link">
|
||||
<visual name="visual_base">
|
||||
<origin xyz="0 0 0.065" rpy="0 0 0.5235987755982988" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
|
||||
</geometry>
|
||||
<material name="material_black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://pibot_description/meshes/base/zeus_base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="-0.021088 -2.5229E-07 -0.0016872" rpy="0 0 0" />
|
||||
<mass value="1.1852" />
|
||||
<inertia
|
||||
ixx="0.0044133"
|
||||
ixy="2.4428E-08"
|
||||
ixz="-0.00050667"
|
||||
iyy="0.0048259"
|
||||
iyz="-9.0612E-09"
|
||||
izz="0.0079062" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
<!-- LIDAR Pose-->
|
||||
<xacro:property name="lidar_joint_xyz" value="0 0 0.18"/>
|
||||
<xacro:property name="lidar_joint_rpy" value="0 0 0"/>
|
||||
|
||||
<!-- Camera Pos-->
|
||||
<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_rpy" value="0 0 0"/>
|
||||
<xacro:property name="camera_pillar_height" value="0.085"/>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
<launch>
|
||||
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
|
||||
<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="256000"--><!--A3 -->
|
||||
<param name="frame_id" type="string" value="laser"/>
|
||||
<param name="inverted" type="bool" value="false"/>
|
||||
<param name="angle_compensate" type="bool" value="true"/>
|
||||
</node>
|
||||
</launch>
|
||||
<launch>
|
||||
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
|
||||
<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="256000"--><!--A3 -->
|
||||
<param name="frame_id" type="string" value="laser"/>
|
||||
<param name="inverted" type="bool" value="false"/>
|
||||
<param name="angle_compensate" type="bool" value="true"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
<launch>
|
||||
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
|
||||
<param name="serial_port" type="string" value="/dev/rplidar"/>
|
||||
<param name="serial_baudrate" type="int" value="256000"/><!--A3 -->
|
||||
<param name="frame_id" type="string" value="laser"/>
|
||||
<param name="inverted" type="bool" value="false"/>
|
||||
<param name="angle_compensate" type="bool" value="true"/>
|
||||
<param name="scan_mode" type="string" value="Sensitivity"/>
|
||||
</node>
|
||||
</launch>
|
||||
<launch>
|
||||
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
|
||||
<param name="serial_port" type="string" value="/dev/rplidar"/>
|
||||
<param name="serial_baudrate" type="int" value="256000"/><!--A3 -->
|
||||
<param name="frame_id" type="string" value="laser"/>
|
||||
<param name="inverted" type="bool" value="false"/>
|
||||
<param name="angle_compensate" type="bool" value="true"/>
|
||||
<param name="scan_mode" type="string" value="Sensitivity"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,192 +1,192 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(simple_follower)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * 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, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * 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
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
position.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES simple_follower
|
||||
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
# include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(simple_follower
|
||||
# src/${PROJECT_NAME}/simple_follower.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(simple_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
# add_executable(simple_follower_node src/simple_follower_node.cpp)
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(simple_follower_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(simple_follower_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS simple_follower simple_follower_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_follower.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(simple_follower)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * 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, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * 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
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
add_message_files(
|
||||
FILES
|
||||
position.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES simple_follower
|
||||
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
# include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(simple_follower
|
||||
# src/${PROJECT_NAME}/simple_follower.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(simple_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
# add_executable(simple_follower_node src/simple_follower_node.cpp)
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(simple_follower_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(simple_follower_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS simple_follower simple_follower_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_follower.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
<launch>
|
||||
<include file='$(find simple_follower)/launch/nodes/laserTracker.launch' />
|
||||
<include file='$(find simple_follower)/launch/nodes/follower.launch' />
|
||||
</launch>
|
||||
<launch>
|
||||
<include file='$(find simple_follower)/launch/nodes/laserTracker.launch' />
|
||||
<include file='$(find simple_follower)/launch/nodes/follower.launch' />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,13 +1,13 @@
|
|||
<launch>
|
||||
<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 -->
|
||||
<param name="switchMode" value="True" type="bool" />
|
||||
<!-- 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='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 -->
|
||||
<param name='controllButtonIndex' value='-4' type='int' />
|
||||
<rosparam ns='PID_controller' command='load' file='$(find simple_follower)/parameters/PID_param.yaml' />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
<launch>
|
||||
<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 -->
|
||||
<param name="switchMode" value="True" type="bool" />
|
||||
<!-- 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='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 -->
|
||||
<param name='controllButtonIndex' value='-4' type='int' />
|
||||
<rosparam ns='PID_controller' command='load' file='$(find simple_follower)/parameters/PID_param.yaml' />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
<launch>
|
||||
<node name='laser_tracker' pkg="simple_follower" type="laserTracker.py">
|
||||
<!-- 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-->
|
||||
<param name="winSize" value="2" type="int" />
|
||||
<param name="deltaDist" value="0.2" type="double" />
|
||||
</node>
|
||||
</launch>
|
||||
<launch>
|
||||
<node name='laser_tracker' pkg="simple_follower" type="laserTracker.py">
|
||||
<!-- 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-->
|
||||
<param name="winSize" value="2" type="int" />
|
||||
<param name="deltaDist" value="0.2" type="double" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,17 +1,17 @@
|
|||
<launch>
|
||||
<node name='visual_tracker' pkg="simple_follower" type="visualTracker.py">
|
||||
<!-- color or the target in HSV color space -->
|
||||
<rosparam ns='target'>
|
||||
upper : [0, 120, 100]
|
||||
lower : [9, 255, 255]
|
||||
</rosparam>
|
||||
<rosparam ns='pictureDimensions'>
|
||||
<!-- Picture dimensions in pixel -->
|
||||
pictureHeight: 480
|
||||
pictureWidth: 640
|
||||
<!-- Viewing angle of the camera in one direction in Radians -->
|
||||
verticalAngle: 0.43196898986859655
|
||||
horizontalAngle: 0.5235987755982988
|
||||
</rosparam>
|
||||
</node>
|
||||
</launch>
|
||||
<launch>
|
||||
<node name='visual_tracker' pkg="simple_follower" type="visualTracker.py">
|
||||
<!-- color or the target in HSV color space -->
|
||||
<rosparam ns='target'>
|
||||
upper : [0, 120, 100]
|
||||
lower : [9, 255, 255]
|
||||
</rosparam>
|
||||
<rosparam ns='pictureDimensions'>
|
||||
<!-- Picture dimensions in pixel -->
|
||||
pictureHeight: 480
|
||||
pictureWidth: 640
|
||||
<!-- Viewing angle of the camera in one direction in Radians -->
|
||||
verticalAngle: 0.43196898986859655
|
||||
horizontalAngle: 0.5235987755982988
|
||||
</rosparam>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
<launch>
|
||||
<include file='$(find simple_follower)/launch/nodes/visualTracker.launch' />
|
||||
<include file='$(find simple_follower)/launch/nodes/follower.launch' />
|
||||
</launch>
|
||||
<launch>
|
||||
<include file='$(find simple_follower)/launch/nodes/visualTracker.launch' />
|
||||
<include file='$(find simple_follower)/launch/nodes/follower.launch' />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
float32 angleX
|
||||
float32 angleY
|
||||
float32 distance
|
||||
float32 angleX
|
||||
float32 angleY
|
||||
float32 distance
|
||||
|
|
|
|||
|
|
@ -1,61 +1,61 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>simple_follower</name>
|
||||
<version>0.0.0</version>
|
||||
<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 -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="chutter@uos.de">clemens</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/first_experiments</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>message_generation</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>simple_follower</name>
|
||||
<version>0.0.0</version>
|
||||
<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 -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="chutter@uos.de">clemens</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/first_experiments</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>message_generation</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
P: [0.7, 0.6]
|
||||
I: [0.07,0.04]
|
||||
D: [0.00, 0.000]
|
||||
P: [0.7, 0.6]
|
||||
I: [0.07,0.04]
|
||||
D: [0.00, 0.000]
|
||||
|
|
|
|||
0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/follower.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/follower.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/laserTracker.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/laserTracker.py
Executable file → Normal file
228
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/testCode.py
Executable file → Normal file
228
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/testCode.py
Executable file → Normal file
|
|
@ -1,114 +1,114 @@
|
|||
import numpy as np
|
||||
import copy
|
||||
from unsafe_runaway import *
|
||||
import time
|
||||
from nose.tools import assert_raises
|
||||
|
||||
|
||||
|
||||
|
||||
class mockup:
|
||||
pass
|
||||
|
||||
|
||||
def makeLaserData():
|
||||
laser_data = mockup()
|
||||
laser_data.range_max = 5.6
|
||||
laser_data.angle_min = -2.1
|
||||
laser_data.angle_increment = 0.06136
|
||||
laser_data.ranges = list(1+np.random.rand(69)*5)
|
||||
return laser_data
|
||||
|
||||
class Test_simpleTracker:
|
||||
|
||||
|
||||
def setUp(self):
|
||||
self.tracker = simpleTracker()
|
||||
|
||||
def test_ignor_first_scan(self):
|
||||
laser_data = makeLaserData()
|
||||
tracker = simpleTracker()
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
|
||||
def test_unmuted(self):
|
||||
laser_data = makeLaserData()
|
||||
backup = copy.copy(laser_data)
|
||||
try:
|
||||
angle, distance = self.tracker.registerScan(laser_data)
|
||||
except:
|
||||
pass
|
||||
assert backup.ranges == laser_data.ranges
|
||||
#print(laser_data.ranges)
|
||||
#print('angle: {}, dist: {}'.format(angle, distance))
|
||||
|
||||
def test_nan(self):
|
||||
laser_data = makeLaserData()
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
laser_data.ranges[12] = float('nan')
|
||||
angle, dist=self.tracker.registerScan(laser_data)
|
||||
#print('angle: {}, dist: {}'.format(angle, dist))
|
||||
|
||||
def test_only_nan(self):
|
||||
laser_data = makeLaserData()
|
||||
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)
|
||||
|
||||
|
||||
|
||||
def test_real_real_min(self):
|
||||
laser_data = makeLaserData()
|
||||
laser_data.ranges[-1]=0.5 #real min
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
laser_data.ranges[-1]=0.6
|
||||
laser_data.ranges[42]=0.1 #fake min
|
||||
ang, dist = self.tracker.registerScan(laser_data)
|
||||
assert dist == 0.6
|
||||
#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
|
||||
|
||||
|
||||
class Test_PID:
|
||||
def setUp(self):
|
||||
pass
|
||||
|
||||
def test_convergence(self):
|
||||
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
|
||||
x =np.array([23, 12])
|
||||
for i in range(20):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
assert np.all(abs(x-[0,30])<=0.01)
|
||||
|
||||
def test_convergence_differentParamShape(self):
|
||||
self.pid = simplePID([0,30],0.8, 0.001, 0.0001)
|
||||
x =np.array([23, 12])
|
||||
for i in range(20):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
assert np.all(abs(x-[0,30])<=0.01)
|
||||
|
||||
|
||||
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.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])
|
||||
_ = 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,0.8, 0.001, 0.0001)
|
||||
|
||||
def test_raise_incompatable_input(self):
|
||||
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
|
||||
_ = assert_raises(TypeError, self.pid.update, 3)
|
||||
x =np.array([23, 12])
|
||||
for i in range(50):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
assert np.all(abs(x-[0,30])<=0.001)
|
||||
import numpy as np
|
||||
import copy
|
||||
from unsafe_runaway import *
|
||||
import time
|
||||
from nose.tools import assert_raises
|
||||
|
||||
|
||||
|
||||
|
||||
class mockup:
|
||||
pass
|
||||
|
||||
|
||||
def makeLaserData():
|
||||
laser_data = mockup()
|
||||
laser_data.range_max = 5.6
|
||||
laser_data.angle_min = -2.1
|
||||
laser_data.angle_increment = 0.06136
|
||||
laser_data.ranges = list(1+np.random.rand(69)*5)
|
||||
return laser_data
|
||||
|
||||
class Test_simpleTracker:
|
||||
|
||||
|
||||
def setUp(self):
|
||||
self.tracker = simpleTracker()
|
||||
|
||||
def test_ignor_first_scan(self):
|
||||
laser_data = makeLaserData()
|
||||
tracker = simpleTracker()
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
|
||||
def test_unmuted(self):
|
||||
laser_data = makeLaserData()
|
||||
backup = copy.copy(laser_data)
|
||||
try:
|
||||
angle, distance = self.tracker.registerScan(laser_data)
|
||||
except:
|
||||
pass
|
||||
assert backup.ranges == laser_data.ranges
|
||||
#print(laser_data.ranges)
|
||||
#print('angle: {}, dist: {}'.format(angle, distance))
|
||||
|
||||
def test_nan(self):
|
||||
laser_data = makeLaserData()
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
laser_data.ranges[12] = float('nan')
|
||||
angle, dist=self.tracker.registerScan(laser_data)
|
||||
#print('angle: {}, dist: {}'.format(angle, dist))
|
||||
|
||||
def test_only_nan(self):
|
||||
laser_data = makeLaserData()
|
||||
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)
|
||||
|
||||
|
||||
|
||||
def test_real_real_min(self):
|
||||
laser_data = makeLaserData()
|
||||
laser_data.ranges[-1]=0.5 #real min
|
||||
assert_raises(UserWarning, self.tracker.registerScan,laser_data)
|
||||
laser_data.ranges[-1]=0.6
|
||||
laser_data.ranges[42]=0.1 #fake min
|
||||
ang, dist = self.tracker.registerScan(laser_data)
|
||||
assert dist == 0.6
|
||||
#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
|
||||
|
||||
|
||||
class Test_PID:
|
||||
def setUp(self):
|
||||
pass
|
||||
|
||||
def test_convergence(self):
|
||||
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
|
||||
x =np.array([23, 12])
|
||||
for i in range(20):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
assert np.all(abs(x-[0,30])<=0.01)
|
||||
|
||||
def test_convergence_differentParamShape(self):
|
||||
self.pid = simplePID([0,30],0.8, 0.001, 0.0001)
|
||||
x =np.array([23, 12])
|
||||
for i in range(20):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
assert np.all(abs(x-[0,30])<=0.01)
|
||||
|
||||
|
||||
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.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])
|
||||
_ = 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,0.8, 0.001, 0.0001)
|
||||
|
||||
def test_raise_incompatable_input(self):
|
||||
self.pid = simplePID([0,30], 0.8, 0.001, 0.0001)
|
||||
_ = assert_raises(TypeError, self.pid.update, 3)
|
||||
x =np.array([23, 12])
|
||||
for i in range(50):
|
||||
update= self.pid.update(x)
|
||||
print('added {} to current x {}'.format(update, x))
|
||||
x = x+update
|
||||
time.sleep(0.1)
|
||||
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
0
Code/MowingRobot/pibot_ros/ros_ws/src/simple_follower/scripts/visualTracker.py
Executable file → Normal file
Binary file not shown.
|
Before Width: | Height: | Size: 38 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 36 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 42 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 36 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 34 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 26 KiB |
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue