Compare commits
No commits in common. "7bb2ee4d5fb58b9a3bad87fdf397d388276c8501" and "afbfe9401f3f8580fed331a7f915b59c8f019fba" have entirely different histories.
7bb2ee4d5f
...
afbfe9401f
Binary file not shown.
|
After Width: | Height: | Size: 108 KiB |
|
|
@ -0,0 +1,93 @@
|
||||||
|
# `PIBot_ROS`使用说明
|
||||||
|
|
||||||
|
> `pibot`是银星合作时购买的小车底盘。其包含直流电机控制、里程计、IMU和上位机通讯等功能。
|
||||||
|
|
||||||
|
## 1、远程连接`RK3588`板子
|
||||||
|
|
||||||
|
我们是通过`RK3588 & RK3566`等高性能的主板控制`PIBot`下位机(`STM32`控制板)。如果直接采用`HDMI`线和`USB`鼠标和键盘进行控制非常的不便; 采用`VNC`等远程桌面的方式对性能的损耗较大，所以采用远程ssh连接一种较好的方式。这里，**我们采用的`vscode`进行ssh远程连接**, 其具有远程图形显示功能以及其他强大的功能。
|
||||||
|
|
||||||
|
`ssh`连接需要知道目标机器的`ip`地址。理论上如果知道开发板的外网`IP`地址，那么就可以在任意位置连接开发板。由于外网固定`IP`难以获取，所以这里我们**采用局域网ssh连接**。由于`windows`平台的渲染窗口和`linux`不兼容，所以**windows平台的vscode是不支持ssh窗口显示的，需要开启虚拟机或者`linux`主机下面的`vscode`才可以显示窗口**。我们在虚拟机`ubuntu`里面使用`vscode`远程`ssh`连接`RK3588`和虚拟机需要用同个网络，远程连接[教程](http://www.autolabor.com.cn/book/ROSTutorials/di-9-zhang-ji-qi-ren-dao-822a28-shi-4f5329/92-vscodeyuan-cheng-kai-fa.html)：
|
||||||
|
|
||||||
|
1. `ip`查询方式终端: 在小车终端里输入 `ifconfig`查看小车的ip地址
|
||||||
|
2. 将教程里步骤三内容改为：
|
||||||
|
|
||||||
|
3. `ssh -X firefly@ip` （-X 为远程显示图形界面需要）
|
||||||
|
4. 密码为：`firefly`
|
||||||
|
|
||||||
|
5. 连接上后选择需要打开的文件夹
|
||||||
|
|
||||||
|
|
||||||
|
## 2、小车驱动编译
|
||||||
|
|
||||||
|
#### 2.1 驱动代码位置
|
||||||
|
|
||||||
|
`PIBot`的ROS支持包的位置在[仓库]([RobotKernal-UESTC](http://logzhan.ticp.io:30000/logzhan/RobotKernal-UESTC))中，其路径为`Code\RK3588\PIbot_ROS`中。我们解压后可以把`PIBot_ROS`移动到`~/`位置，并重新命名为`pibot_ros`（命名可以按照个人习惯），下面的教程以`pibot_ros`为例。
|
||||||
|
|
||||||
|
#### 2.2 3588上编译问题（主要是全覆盖部分）
|
||||||
|
|
||||||
|
`PIBot_ROS/ros_ws/src/ipa_coverage_planning`(全覆盖部分的代码)
|
||||||
|
|
||||||
|
```shell
|
||||||
|
# 进入到ROS的工作空间
|
||||||
|
cd pibot_ros/ros_ws
|
||||||
|
# 在pibot_ros/ros_ws路径下执行catkin_make
|
||||||
|
# ~:/pibot_ros/ros_ws$ catkin_make
|
||||||
|
catkin_make
|
||||||
|
```
|
||||||
|
|
||||||
|
#### 2.3 根据出现的报错缺少的库问题解决
|
||||||
|
|
||||||
|
```shell
|
||||||
|
# noetic 是ubuntu20.04版本名，其他版本需要更换名字
|
||||||
|
sudo apt-get install ros-noetic-xxx
|
||||||
|
# 例如：sudo apt-get install ros-noetic-opengm
|
||||||
|
sudo apt-get install ros-noetic-libdlib
|
||||||
|
sudo apt-get install ros-noetic-cob-navigation
|
||||||
|
sudo apt-get install coinor-*
|
||||||
|
```
|
||||||
|
|
||||||
|
可参考内容：
|
||||||
|
|
||||||
|
> [全覆盖规划算法Coverage Path Planning开源代码ipa_coverage_planning编译-CSDN博客](https://blog.csdn.net/ktigerhero3/article/details/121562049)
|
||||||
|
>
|
||||||
|
> [ROS全覆盖规划算法 Coverage Path Planning 采坑-CSDN博客](https://blog.csdn.net/weixin_42179076/article/details/121164350)
|
||||||
|
|
||||||
|
## 3、使用说明
|
||||||
|
|
||||||
|
#### 3.1 导航的启动
|
||||||
|
|
||||||
|
`PIBot_ROS`的文件解压或重新命名可以按照个人习惯即可，这里以解压后命名为`pibot_ros`为例，介绍命令启动导航。
|
||||||
|
|
||||||
|
```shell
|
||||||
|
# 进入到pibot_ros的工作空间
|
||||||
|
cd ~/pibot_ros/ros_ws
|
||||||
|
# 配置环境变量
|
||||||
|
source ./devel/setup.bash
|
||||||
|
# 启动导航文件
|
||||||
|
# 格式 roslaunch package_name launch_file_name
|
||||||
|
roslaunch pibot_navigation nav.launch
|
||||||
|
```
|
||||||
|
|
||||||
|
#### 3.2 更改导航地图
|
||||||
|
|
||||||
|
根据导航需求更改导航地图，地图路径在下图左下角位置`pgm`为地图格式 `yaml`为配置文件:
|
||||||
|
|
||||||
|
<img src="./Image/ROS_Using (3).png" alt="Untitled" style="zoom:80%;" />
|
||||||
|
|
||||||
|
#### 3.3 小车控制和可视化
|
||||||
|
|
||||||
|
小车的控制和可视化功能需要**开启两个新的终端，分别在终端输入命令**：
|
||||||
|
|
||||||
|
```shell
|
||||||
|
# 小车的键盘控制,在任意路径执行:
|
||||||
|
pibot_control
|
||||||
|
```
|
||||||
|
|
||||||
|
```shell
|
||||||
|
# 导航可视化,在任意路径执行:
|
||||||
|
pibot_view (需要设置从主机 记不清 后面再补充)
|
||||||
|
```
|
||||||
|
|
||||||
|
在这上面可以在地图上给出目标点进行单点导航。
|
||||||
|
|
||||||
|
#### 3.4、全覆盖路径规划的使用
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
# PIBOT ROS Workspace v2.0
|
# PIBOT ROS Workspace v2.0
|
||||||
|
|
||||||
## install ros
|
## 安装ROS
|
||||||
```shell
|
```shell
|
||||||
cd ~/pibot_ros/
|
cd ~/pibot_ros/
|
||||||
./pibot_install_ros.sh
|
./pibot_install_ros.sh
|
||||||
34
Code/MowingRobot/pibot_ros/pypibot/pypibot/__init__.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/__init__.py
Executable file → Normal file
34
Code/MowingRobot/pibot_ros/pypibot/pypibot/__init__.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/__init__.py
Executable file → Normal file
|
|
@ -1,17 +1,17 @@
|
||||||
#!/usr/bin/python
|
#!/usr/bin/python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
import pypibot.assistant
|
import pypibot.assistant
|
||||||
import pypibot.log as Logger
|
import pypibot.log as Logger
|
||||||
import pypibot.err
|
import pypibot.err
|
||||||
log=Logger.log
|
log=Logger.log
|
||||||
import sys
|
import sys
|
||||||
def createNamedLog(name):
|
def createNamedLog(name):
|
||||||
return Logger.NamedLog(name)
|
return Logger.NamedLog(name)
|
||||||
class Object():
|
class Object():
|
||||||
pass
|
pass
|
||||||
isDebug="-d" in sys.argv
|
isDebug="-d" in sys.argv
|
||||||
|
|
||||||
import platform
|
import platform
|
||||||
isWindows=False
|
isWindows=False
|
||||||
if platform.system()=='Windows':
|
if platform.system()=='Windows':
|
||||||
isWindows=True
|
isWindows=True
|
||||||
468
Code/MowingRobot/pibot_ros/pypibot/pypibot/assistant.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/assistant.py
Executable file → Normal file
468
Code/MowingRobot/pibot_ros/pypibot/pypibot/assistant.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/assistant.py
Executable file → Normal file
|
|
@ -1,234 +1,234 @@
|
||||||
#!/usr/bin/python
|
#!/usr/bin/python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
import os, sys, inspect
|
import os, sys, inspect
|
||||||
import datetime
|
import datetime
|
||||||
import signal
|
import signal
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
# function: get directory of current script, if script is built
|
# function: get directory of current script, if script is built
|
||||||
# into an executable file, get directory of the excutable file
|
# into an executable file, get directory of the excutable file
|
||||||
def current_file_directory():
|
def current_file_directory():
|
||||||
path = os.path.realpath(sys.path[0]) # interpreter starter's path
|
path = os.path.realpath(sys.path[0]) # interpreter starter's path
|
||||||
if os.path.isfile(path): # starter is excutable file
|
if os.path.isfile(path): # starter is excutable file
|
||||||
path = os.path.dirname(path)
|
path = os.path.dirname(path)
|
||||||
path = os.path.abspath(path) # return excutable file's directory
|
path = os.path.abspath(path) # return excutable file's directory
|
||||||
else: # starter is python script
|
else: # starter is python script
|
||||||
caller_file = inspect.stack()[0][1] # function caller's filename
|
caller_file = inspect.stack()[0][1] # function caller's filename
|
||||||
path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory
|
path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory
|
||||||
if path[-1]!=os.path.sep:path+=os.path.sep
|
if path[-1]!=os.path.sep:path+=os.path.sep
|
||||||
return path
|
return path
|
||||||
|
|
||||||
"""格式化字符串"""
|
"""格式化字符串"""
|
||||||
def formatString(string,*argv):
|
def formatString(string,*argv):
|
||||||
string=string%argv
|
string=string%argv
|
||||||
if string.find('$(scriptpath)')>=0:
|
if string.find('$(scriptpath)')>=0:
|
||||||
string=string.replace('$(scriptpath)',current_file_directory())
|
string=string.replace('$(scriptpath)',current_file_directory())
|
||||||
if string.find('$(filenumber2)')>=0:
|
if string.find('$(filenumber2)')>=0:
|
||||||
i=0
|
i=0
|
||||||
path=""
|
path=""
|
||||||
while True:
|
while True:
|
||||||
path=string.replace('$(scriptpath)',"%02d"%i)
|
path=string.replace('$(scriptpath)',"%02d"%i)
|
||||||
if not os.path.lexists(path):break
|
if not os.path.lexists(path):break
|
||||||
i+=1
|
i+=1
|
||||||
string=path
|
string=path
|
||||||
#8位日期（20140404）
|
#8位日期（20140404）
|
||||||
if string.find('$(Date8)')>=0:
|
if string.find('$(Date8)')>=0:
|
||||||
now=datetime.datetime.now()
|
now=datetime.datetime.now()
|
||||||
string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
|
string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
|
||||||
#6位日期（140404）
|
#6位日期（140404）
|
||||||
if string.find('$(Date6)')>=0:
|
if string.find('$(Date6)')>=0:
|
||||||
now=datetime.datetime.now()
|
now=datetime.datetime.now()
|
||||||
string=string.replace('$(Date6)', now.strftime("%y%m%d"))
|
string=string.replace('$(Date6)', now.strftime("%y%m%d"))
|
||||||
#6位时间（121212）
|
#6位时间（121212）
|
||||||
if string.find('$(Time6)')>=0:
|
if string.find('$(Time6)')>=0:
|
||||||
now=datetime.datetime.now()
|
now=datetime.datetime.now()
|
||||||
string=string.replace('$(Time6)', now.strftime("%H%M%S"))
|
string=string.replace('$(Time6)', now.strftime("%H%M%S"))
|
||||||
#4位时间（1212）
|
#4位时间（1212）
|
||||||
if string.find('$(Time4)')>=0:
|
if string.find('$(Time4)')>=0:
|
||||||
now=datetime.datetime.now()
|
now=datetime.datetime.now()
|
||||||
string=string.replace('$(Time4)', now.strftime("%H%M"))
|
string=string.replace('$(Time4)', now.strftime("%H%M"))
|
||||||
#文件编号2位（必须在最后）
|
#文件编号2位（必须在最后）
|
||||||
if string.find('$(filenumber2)')>=0:
|
if string.find('$(filenumber2)')>=0:
|
||||||
i=0
|
i=0
|
||||||
path=""
|
path=""
|
||||||
while True:
|
while True:
|
||||||
path=string.replace('$(filenumber2)',"%02d"%i)
|
path=string.replace('$(filenumber2)',"%02d"%i)
|
||||||
if not os.path.lexists(path):break
|
if not os.path.lexists(path):break
|
||||||
i+=1
|
i+=1
|
||||||
string=path
|
string=path
|
||||||
#文件编号3位（必须在最后）
|
#文件编号3位（必须在最后）
|
||||||
if string.find('$(filenumber3)')>=0:
|
if string.find('$(filenumber3)')>=0:
|
||||||
i=0
|
i=0
|
||||||
path=""
|
path=""
|
||||||
while True:
|
while True:
|
||||||
path=string.replace('$(filenumber3)',"%03d"%i)
|
path=string.replace('$(filenumber3)',"%03d"%i)
|
||||||
if not os.path.lexists(path):break
|
if not os.path.lexists(path):break
|
||||||
i+=1
|
i+=1
|
||||||
string=path
|
string=path
|
||||||
return string
|
return string
|
||||||
|
|
||||||
"""
|
"""
|
||||||
取得进程列表
|
取得进程列表
|
||||||
格式：(PID,cmd)
|
格式：(PID,cmd)
|
||||||
"""
|
"""
|
||||||
def getProcessList():
|
def getProcessList():
|
||||||
processList = []
|
processList = []
|
||||||
try:
|
try:
|
||||||
for line in os.popen("ps xa"):
|
for line in os.popen("ps xa"):
|
||||||
fields = line.split()
|
fields = line.split()
|
||||||
# spid = fields[0]
|
# spid = fields[0]
|
||||||
pid = 0
|
pid = 0
|
||||||
try:pid = int(fields[0])
|
try:pid = int(fields[0])
|
||||||
except:None
|
except:None
|
||||||
cmd = line[27:-1]
|
cmd = line[27:-1]
|
||||||
# print "PS:%d,%s"%(npid,process)
|
# print "PS:%d,%s"%(npid,process)
|
||||||
if pid != 0 and len(cmd) > 0:
|
if pid != 0 and len(cmd) > 0:
|
||||||
processList.append((pid, cmd))
|
processList.append((pid, cmd))
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print("getProcessList except:%s" % (e))
|
print("getProcessList except:%s" % (e))
|
||||||
return processList
|
return processList
|
||||||
def killCommand(cmd):
|
def killCommand(cmd):
|
||||||
try:
|
try:
|
||||||
processList = getProcessList()
|
processList = getProcessList()
|
||||||
for p in processList:
|
for p in processList:
|
||||||
if p[1].find(cmd) != -1:
|
if p[1].find(cmd) != -1:
|
||||||
pid = p[0]
|
pid = p[0]
|
||||||
os.kill(pid, signal.SIGKILL)
|
os.kill(pid, signal.SIGKILL)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print("killCommand ‘%s’ except:%s" % (cmd,e))
|
print("killCommand ‘%s’ except:%s" % (cmd,e))
|
||||||
|
|
||||||
def check_pid(pid):
|
def check_pid(pid):
|
||||||
""" Check For the existence of a unix pid. """
|
""" Check For the existence of a unix pid. """
|
||||||
if pid == 0:return False
|
if pid == 0:return False
|
||||||
try:
|
try:
|
||||||
os.kill(pid, 0)
|
os.kill(pid, 0)
|
||||||
except OSError:
|
except OSError:
|
||||||
return False
|
return False
|
||||||
else:
|
else:
|
||||||
return True
|
return True
|
||||||
|
|
||||||
SF=formatString
|
SF=formatString
|
||||||
|
|
||||||
#全局异常捕获
|
#全局异常捕获
|
||||||
def excepthook(excType, excValue, tb):
|
def excepthook(excType, excValue, tb):
|
||||||
'''''write the unhandle exception to log'''
|
'''''write the unhandle exception to log'''
|
||||||
from log import log
|
from log import log
|
||||||
import traceback
|
import traceback
|
||||||
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
|
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
|
||||||
sys.exit(-1)
|
sys.exit(-1)
|
||||||
#sys.__excepthook__(type, value, trace)
|
#sys.__excepthook__(type, value, trace)
|
||||||
#sys.__excepthook__(excType, excValue, tb)
|
#sys.__excepthook__(excType, excValue, tb)
|
||||||
|
|
||||||
_defaultGlobalExcept=sys.excepthook
|
_defaultGlobalExcept=sys.excepthook
|
||||||
|
|
||||||
def enableGlobalExcept(enable=True):
|
def enableGlobalExcept(enable=True):
|
||||||
if enable:
|
if enable:
|
||||||
sys.excepthook = excepthook
|
sys.excepthook = excepthook
|
||||||
else:
|
else:
|
||||||
sys.excepthook=_defaultGlobalExcept
|
sys.excepthook=_defaultGlobalExcept
|
||||||
# 默认启动全局异常处理
|
# 默认启动全局异常处理
|
||||||
enableGlobalExcept()
|
enableGlobalExcept()
|
||||||
#创建线程
|
#创建线程
|
||||||
def createThread(name,target,args=(),autoRun=True,daemon=True):
|
def createThread(name,target,args=(),autoRun=True,daemon=True):
|
||||||
from log import log
|
from log import log
|
||||||
def threadProc():
|
def threadProc():
|
||||||
log.i("thread %s started!",name)
|
log.i("thread %s started!",name)
|
||||||
try:
|
try:
|
||||||
target(*args)
|
target(*args)
|
||||||
log.i("thread %s ended!",name)
|
log.i("thread %s ended!",name)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
log.e("thread %s crash!err=%s",name,e)
|
log.e("thread %s crash!err=%s",name,e)
|
||||||
thd=threading.Thread(name=name,target=threadProc)
|
thd=threading.Thread(name=name,target=threadProc)
|
||||||
thd.setDaemon(daemon)
|
thd.setDaemon(daemon)
|
||||||
if autoRun:thd.start()
|
if autoRun:thd.start()
|
||||||
return thd
|
return thd
|
||||||
|
|
||||||
|
|
||||||
#定时器
|
#定时器
|
||||||
class Timer():
|
class Timer():
|
||||||
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
|
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
|
||||||
self.name=name
|
self.name=name
|
||||||
self.first=first
|
self.first=first
|
||||||
self.period=period
|
self.period=period
|
||||||
self.args=args
|
self.args=args
|
||||||
self.timer_proc=timer_proc
|
self.timer_proc=timer_proc
|
||||||
self.thdWork=None
|
self.thdWork=None
|
||||||
self.stopFlag=False
|
self.stopFlag=False
|
||||||
from log import NamedLog
|
from log import NamedLog
|
||||||
self.log=NamedLog(name)
|
self.log=NamedLog(name)
|
||||||
def run(self):
|
def run(self):
|
||||||
if self.first>0:
|
if self.first>0:
|
||||||
time.sleep(self.first)
|
time.sleep(self.first)
|
||||||
try:
|
try:
|
||||||
self.timer_proc(*self.args)
|
self.timer_proc(*self.args)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.log.error("timer proc crash!err=%s",e)
|
self.log.error("timer proc crash!err=%s",e)
|
||||||
while self.period>0 and not self.stopFlag:
|
while self.period>0 and not self.stopFlag:
|
||||||
time.sleep(self.period)
|
time.sleep(self.period)
|
||||||
try:
|
try:
|
||||||
self.timer_proc(*self.args)
|
self.timer_proc(*self.args)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.log.error("timer proc crash!err=%s",e)
|
self.log.error("timer proc crash!err=%s",e)
|
||||||
def start(self):
|
def start(self):
|
||||||
if self.isAlive():
|
if self.isAlive():
|
||||||
self.log.d("already running!")
|
self.log.d("already running!")
|
||||||
return True
|
return True
|
||||||
self.stopFlag=False
|
self.stopFlag=False
|
||||||
self.thdWork=threading.Thread(name=self.name,target=self.run)
|
self.thdWork=threading.Thread(name=self.name,target=self.run)
|
||||||
self.thdWork.setDaemon(True)
|
self.thdWork.setDaemon(True)
|
||||||
self.thdWork.start()
|
self.thdWork.start()
|
||||||
def stop(self,timeout=3):
|
def stop(self,timeout=3):
|
||||||
if self.isAlive():
|
if self.isAlive():
|
||||||
self.stopFlag=True
|
self.stopFlag=True
|
||||||
try:
|
try:
|
||||||
self.thdWork.join(timeout)
|
self.thdWork.join(timeout)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.log.e("stop timeout!")
|
self.log.e("stop timeout!")
|
||||||
|
|
||||||
def isAlive(self):
|
def isAlive(self):
|
||||||
return self.thdWork and self.thdWork.isAlive()
|
return self.thdWork and self.thdWork.isAlive()
|
||||||
#计时器
|
#计时器
|
||||||
class Ticker():
|
class Ticker():
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.reset()
|
self.reset()
|
||||||
# 片段,可以判断时长是否在一个特定毫秒段内
|
# 片段,可以判断时长是否在一个特定毫秒段内
|
||||||
self.section=[]
|
self.section=[]
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.tick=time.time()
|
self.tick=time.time()
|
||||||
self.end=0
|
self.end=0
|
||||||
def stop(self):
|
def stop(self):
|
||||||
self.end=time.time()
|
self.end=time.time()
|
||||||
@property
|
@property
|
||||||
def ticker(self):
|
def ticker(self):
|
||||||
if self.end==0:
|
if self.end==0:
|
||||||
return int((time.time()-self.tick)*1000)
|
return int((time.time()-self.tick)*1000)
|
||||||
else:
|
else:
|
||||||
return int((self.end-self.tick)*1000)
|
return int((self.end-self.tick)*1000)
|
||||||
def addSection(self,a,b):
|
def addSection(self,a,b):
|
||||||
a,b=int(a),int(b)
|
a,b=int(a),int(b)
|
||||||
assert a<b
|
assert a<b
|
||||||
self.section.append((a,b))
|
self.section.append((a,b))
|
||||||
def removeSection(self,a,b):
|
def removeSection(self,a,b):
|
||||||
a,b=int(a),int(b)
|
a,b=int(a),int(b)
|
||||||
self.section.remove((a,b))
|
self.section.remove((a,b))
|
||||||
def removeAllSectioin(self):
|
def removeAllSectioin(self):
|
||||||
self.section=[]
|
self.section=[]
|
||||||
def inSection(self):
|
def inSection(self):
|
||||||
tick=self.ticker
|
tick=self.ticker
|
||||||
for (a,b) in self.section:
|
for (a,b) in self.section:
|
||||||
if tick>=a and tick<=b:
|
if tick>=a and tick<=b:
|
||||||
return True
|
return True
|
||||||
return False
|
return False
|
||||||
def __call__(self):
|
def __call__(self):
|
||||||
return self.ticker
|
return self.ticker
|
||||||
def waitExit():
|
def waitExit():
|
||||||
import log
|
import log
|
||||||
log.log.i("start waiting to exit...")
|
log.log.i("start waiting to exit...")
|
||||||
try:
|
try:
|
||||||
while(True):
|
while(True):
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
log.log.w("recv exit sign!")
|
log.log.w("recv exit sign!")
|
||||||
|
|
||||||
def is_python3():
|
def is_python3():
|
||||||
return sys.hexversion > 0x03000000
|
return sys.hexversion > 0x03000000
|
||||||
112
Code/MowingRobot/pibot_ros/pypibot/pypibot/configer.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/configer.py
Executable file → Normal file
112
Code/MowingRobot/pibot_ros/pypibot/pypibot/configer.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/configer.py
Executable file → Normal file
|
|
@ -1,56 +1,56 @@
|
||||||
#!/usr/bin/python
|
#!/usr/bin/python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
import ConfigParser
|
import ConfigParser
|
||||||
from log import PLOG
|
from log import PLOG
|
||||||
import os
|
import os
|
||||||
def getdefaultfilename():
|
def getdefaultfilename():
|
||||||
pass
|
pass
|
||||||
def openconfiger(filename):
|
def openconfiger(filename):
|
||||||
return configer(filename)
|
return configer(filename)
|
||||||
class configer:
|
class configer:
|
||||||
def __init__(self,fullfilepath):
|
def __init__(self,fullfilepath):
|
||||||
self._filepath=fullfilepath
|
self._filepath=fullfilepath
|
||||||
if not os.path.isdir(os.path.dirname(fullfilepath)):
|
if not os.path.isdir(os.path.dirname(fullfilepath)):
|
||||||
os.makedirs(os.path.dirname(fullfilepath))
|
os.makedirs(os.path.dirname(fullfilepath))
|
||||||
self._conf=ConfigParser.ConfigParser()
|
self._conf=ConfigParser.ConfigParser()
|
||||||
if os.path.isfile(fullfilepath):
|
if os.path.isfile(fullfilepath):
|
||||||
try:
|
try:
|
||||||
self._conf.readfp(open(fullfilepath,"r"))
|
self._conf.readfp(open(fullfilepath,"r"))
|
||||||
except Exception,e:
|
except Exception,e:
|
||||||
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
|
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
|
||||||
def save(self):
|
def save(self):
|
||||||
try:
|
try:
|
||||||
self._conf.write(open(self._filepath,"w"))
|
self._conf.write(open(self._filepath,"w"))
|
||||||
except Exception,e:
|
except Exception,e:
|
||||||
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
|
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
|
||||||
|
|
||||||
def changeConfValue(self,section,option,value):
|
def changeConfValue(self,section,option,value):
|
||||||
if self._conf.has_section(section):
|
if self._conf.has_section(section):
|
||||||
self._conf.set(section,option,value)
|
self._conf.set(section,option,value)
|
||||||
else:
|
else:
|
||||||
self._conf.add_section(section)
|
self._conf.add_section(section)
|
||||||
self._conf.set(section,option,value)
|
self._conf.set(section,option,value)
|
||||||
|
|
||||||
def _readvalue(self,fn,section,option,default):
|
def _readvalue(self,fn,section,option,default):
|
||||||
result=default
|
result=default
|
||||||
if self._conf.has_section(section):
|
if self._conf.has_section(section):
|
||||||
if self._conf.has_option(section,option):
|
if self._conf.has_option(section,option):
|
||||||
result=fn(section,option)
|
result=fn(section,option)
|
||||||
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
|
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
|
||||||
else:
|
else:
|
||||||
self._conf.set(section,option,str(default))
|
self._conf.set(section,option,str(default))
|
||||||
result=default
|
result=default
|
||||||
else:
|
else:
|
||||||
self._conf.add_section(section)
|
self._conf.add_section(section)
|
||||||
self._conf.set(section,option,str(default))
|
self._conf.set(section,option,str(default))
|
||||||
result=default
|
result=default
|
||||||
return result
|
return result
|
||||||
def getstr(self,section,option,default=""):
|
def getstr(self,section,option,default=""):
|
||||||
return self._readvalue(self._conf.get, section, option, default)
|
return self._readvalue(self._conf.get, section, option, default)
|
||||||
def getint(self,section,option,default=0):
|
def getint(self,section,option,default=0):
|
||||||
return self._readvalue(self._conf.getint, section, option, default)
|
return self._readvalue(self._conf.getint, section, option, default)
|
||||||
def getfloat(self,section,option,default=0.0):
|
def getfloat(self,section,option,default=0.0):
|
||||||
return self._readvalue(self._conf.getfloat, section, option, default)
|
return self._readvalue(self._conf.getfloat, section, option, default)
|
||||||
def getboolean(self,section,option,default=False):
|
def getboolean(self,section,option,default=False):
|
||||||
return self._readvalue(self._conf.getboolean, section, option, default)
|
return self._readvalue(self._conf.getboolean, section, option, default)
|
||||||
280
Code/MowingRobot/pibot_ros/pypibot/pypibot/daemon.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/daemon.py
Executable file → Normal file
280
Code/MowingRobot/pibot_ros/pypibot/pypibot/daemon.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/daemon.py
Executable file → Normal file
|
|
@ -1,140 +1,140 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
import sys, os, time, atexit
|
import sys, os, time, atexit
|
||||||
from signal import SIGTERM
|
from signal import SIGTERM
|
||||||
|
|
||||||
def check_pid(pid):
|
def check_pid(pid):
|
||||||
""" Check For the existence of a unix pid. """
|
""" Check For the existence of a unix pid. """
|
||||||
if pid == 0:return False
|
if pid == 0:return False
|
||||||
try:
|
try:
|
||||||
os.kill(pid, 0)
|
os.kill(pid, 0)
|
||||||
except OSError:
|
except OSError:
|
||||||
return False
|
return False
|
||||||
else:
|
else:
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class daemon:
|
class daemon:
|
||||||
"""
|
"""
|
||||||
A generic daemon class.
|
A generic daemon class.
|
||||||
|
|
||||||
Usage: subclass the Daemon class and override the run() method
|
Usage: subclass the Daemon class and override the run() method
|
||||||
"""
|
"""
|
||||||
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
|
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
|
||||||
self.stdin = stdin
|
self.stdin = stdin
|
||||||
self.stdout = stdout
|
self.stdout = stdout
|
||||||
self.stderr = stderr
|
self.stderr = stderr
|
||||||
self.pidfile = pidfile
|
self.pidfile = pidfile
|
||||||
|
|
||||||
def daemonize(self):
|
def daemonize(self):
|
||||||
"""
|
"""
|
||||||
do the UNIX double-fork magic, see Stevens' "Advanced
|
do the UNIX double-fork magic, see Stevens' "Advanced
|
||||||
Programming in the UNIX Environment" for details (ISBN 0201563177)
|
Programming in the UNIX Environment" for details (ISBN 0201563177)
|
||||||
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
|
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
|
||||||
"""
|
"""
|
||||||
try:
|
try:
|
||||||
pid = os.fork()
|
pid = os.fork()
|
||||||
if pid > 0:
|
if pid > 0:
|
||||||
# exit first parent
|
# exit first parent
|
||||||
sys.exit(0)
|
sys.exit(0)
|
||||||
except OSError, e:
|
except OSError, e:
|
||||||
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
|
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
# decouple from parent environment
|
# decouple from parent environment
|
||||||
os.chdir("/")
|
os.chdir("/")
|
||||||
os.setsid()
|
os.setsid()
|
||||||
os.umask(0)
|
os.umask(0)
|
||||||
|
|
||||||
# do second fork
|
# do second fork
|
||||||
try:
|
try:
|
||||||
pid = os.fork()
|
pid = os.fork()
|
||||||
if pid > 0:
|
if pid > 0:
|
||||||
# exit from second parent
|
# exit from second parent
|
||||||
sys.exit(0)
|
sys.exit(0)
|
||||||
except OSError, e:
|
except OSError, e:
|
||||||
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
|
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
# redirect standard file descriptors
|
# redirect standard file descriptors
|
||||||
sys.stdout.flush()
|
sys.stdout.flush()
|
||||||
sys.stderr.flush()
|
sys.stderr.flush()
|
||||||
si = file(self.stdin, 'r')
|
si = file(self.stdin, 'r')
|
||||||
so = file(self.stdout, 'a+')
|
so = file(self.stdout, 'a+')
|
||||||
se = file(self.stderr, 'a+', 0)
|
se = file(self.stderr, 'a+', 0)
|
||||||
os.dup2(si.fileno(), sys.stdin.fileno())
|
os.dup2(si.fileno(), sys.stdin.fileno())
|
||||||
os.dup2(so.fileno(), sys.stdout.fileno())
|
os.dup2(so.fileno(), sys.stdout.fileno())
|
||||||
os.dup2(se.fileno(), sys.stderr.fileno())
|
os.dup2(se.fileno(), sys.stderr.fileno())
|
||||||
|
|
||||||
# write pidfile
|
# write pidfile
|
||||||
atexit.register(self.delpid)
|
atexit.register(self.delpid)
|
||||||
pid = str(os.getpid())
|
pid = str(os.getpid())
|
||||||
file(self.pidfile, 'w+').write("%s\n" % pid)
|
file(self.pidfile, 'w+').write("%s\n" % pid)
|
||||||
|
|
||||||
def delpid(self):
|
def delpid(self):
|
||||||
os.remove(self.pidfile)
|
os.remove(self.pidfile)
|
||||||
|
|
||||||
def start(self):
|
def start(self):
|
||||||
"""
|
"""
|
||||||
Start the daemon
|
Start the daemon
|
||||||
"""
|
"""
|
||||||
# Check for a pidfile to see if the daemon already runs
|
# Check for a pidfile to see if the daemon already runs
|
||||||
try:
|
try:
|
||||||
pf = file(self.pidfile, 'r')
|
pf = file(self.pidfile, 'r')
|
||||||
pid = int(pf.read().strip())
|
pid = int(pf.read().strip())
|
||||||
pf.close()
|
pf.close()
|
||||||
except IOError:
|
except IOError:
|
||||||
pid = None
|
pid = None
|
||||||
|
|
||||||
if pid and check_pid(pid):
|
if pid and check_pid(pid):
|
||||||
message = "pidfile %s already exist. Daemon already running?\n"
|
message = "pidfile %s already exist. Daemon already running?\n"
|
||||||
sys.stderr.write(message % self.pidfile)
|
sys.stderr.write(message % self.pidfile)
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
print("daemon start...")
|
print("daemon start...")
|
||||||
# Start the daemon
|
# Start the daemon
|
||||||
self.daemonize()
|
self.daemonize()
|
||||||
self.run()
|
self.run()
|
||||||
|
|
||||||
def stop(self):
|
def stop(self):
|
||||||
"""
|
"""
|
||||||
Stop the daemon
|
Stop the daemon
|
||||||
"""
|
"""
|
||||||
# Get the pid from the pidfile
|
# Get the pid from the pidfile
|
||||||
try:
|
try:
|
||||||
pf = file(self.pidfile, 'r')
|
pf = file(self.pidfile, 'r')
|
||||||
pid = int(pf.read().strip())
|
pid = int(pf.read().strip())
|
||||||
pf.close()
|
pf.close()
|
||||||
except IOError:
|
except IOError:
|
||||||
pid = None
|
pid = None
|
||||||
|
|
||||||
if not pid:
|
if not pid:
|
||||||
message = "pidfile %s does not exist. Daemon not running?\n"
|
message = "pidfile %s does not exist. Daemon not running?\n"
|
||||||
sys.stderr.write(message % self.pidfile)
|
sys.stderr.write(message % self.pidfile)
|
||||||
return # not an error in a restart
|
return # not an error in a restart
|
||||||
|
|
||||||
# Try killing the daemon process
|
# Try killing the daemon process
|
||||||
try:
|
try:
|
||||||
while 1:
|
while 1:
|
||||||
os.kill(pid, SIGTERM)
|
os.kill(pid, SIGTERM)
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
except OSError, err:
|
except OSError, err:
|
||||||
err = str(err)
|
err = str(err)
|
||||||
if err.find("No such process") > 0:
|
if err.find("No such process") > 0:
|
||||||
if os.path.exists(self.pidfile):
|
if os.path.exists(self.pidfile):
|
||||||
os.remove(self.pidfile)
|
os.remove(self.pidfile)
|
||||||
else:
|
else:
|
||||||
print(str(err))
|
print(str(err))
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
def restart(self):
|
def restart(self):
|
||||||
"""
|
"""
|
||||||
Restart the daemon
|
Restart the daemon
|
||||||
"""
|
"""
|
||||||
self.stop()
|
self.stop()
|
||||||
self.start()
|
self.start()
|
||||||
|
|
||||||
def run(self):
|
def run(self):
|
||||||
"""
|
"""
|
||||||
You should override this method when you subclass Daemon. It will be called after the process has been
|
You should override this method when you subclass Daemon. It will be called after the process has been
|
||||||
daemonized by start() or restart().
|
daemonized by start() or restart().
|
||||||
"""
|
"""
|
||||||
116
Code/MowingRobot/pibot_ros/pypibot/pypibot/err.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/err.py
Executable file → Normal file
116
Code/MowingRobot/pibot_ros/pypibot/pypibot/err.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/err.py
Executable file → Normal file
|
|
@ -1,58 +1,58 @@
|
||||||
#!/usr/bin/python
|
#!/usr/bin/python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
# 异常类
|
# 异常类
|
||||||
class PibotError(Exception):
|
class PibotError(Exception):
|
||||||
def __init__(self, errcode, errmsg):
|
def __init__(self, errcode, errmsg):
|
||||||
self.errcode = errcode
|
self.errcode = errcode
|
||||||
self.errmsg = errmsg
|
self.errmsg = errmsg
|
||||||
#Exception.__init__(self,self.__str__())
|
#Exception.__init__(self,self.__str__())
|
||||||
|
|
||||||
def msg(self, msg):
|
def msg(self, msg):
|
||||||
if not msg is None:return PibotError(self.errcode, msg)
|
if not msg is None:return PibotError(self.errcode, msg)
|
||||||
return PibotError(8,"unknow error")
|
return PibotError(8,"unknow error")
|
||||||
def __str__(self):
|
def __str__(self):
|
||||||
return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
|
return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
|
||||||
@property
|
@property
|
||||||
def message(self):
|
def message(self):
|
||||||
return str(self)
|
return str(self)
|
||||||
# 声明
|
# 声明
|
||||||
# 成功
|
# 成功
|
||||||
success=PibotError(0,"null")
|
success=PibotError(0,"null")
|
||||||
# 通用失败
|
# 通用失败
|
||||||
fail=PibotError(1,"fail")
|
fail=PibotError(1,"fail")
|
||||||
# 参数无效
|
# 参数无效
|
||||||
invalidParameter=PibotError(2,"invalid parameter")
|
invalidParameter=PibotError(2,"invalid parameter")
|
||||||
# 不支持
|
# 不支持
|
||||||
noSupport=PibotError(3,"no support")
|
noSupport=PibotError(3,"no support")
|
||||||
# 不存在
|
# 不存在
|
||||||
noExist=PibotError(4,"no exist")
|
noExist=PibotError(4,"no exist")
|
||||||
# 超时
|
# 超时
|
||||||
timeout=PibotError(5,"timeout")
|
timeout=PibotError(5,"timeout")
|
||||||
# 繁忙
|
# 繁忙
|
||||||
busy=PibotError(6,"busy")
|
busy=PibotError(6,"busy")
|
||||||
# 缺少参数
|
# 缺少参数
|
||||||
missParameter=PibotError(7,"miss parameter")
|
missParameter=PibotError(7,"miss parameter")
|
||||||
# 系统错误(通用错误)
|
# 系统错误(通用错误)
|
||||||
systemError=PibotError(8,"system error")
|
systemError=PibotError(8,"system error")
|
||||||
# 密码错误
|
# 密码错误
|
||||||
invalidPassword=PibotError(9,"invalid password")
|
invalidPassword=PibotError(9,"invalid password")
|
||||||
# 编码失败
|
# 编码失败
|
||||||
encodeFailed=PibotError(10,"encode failed")
|
encodeFailed=PibotError(10,"encode failed")
|
||||||
# 数据库操作失败
|
# 数据库操作失败
|
||||||
dbOpertationFailed=PibotError(11,"db error")
|
dbOpertationFailed=PibotError(11,"db error")
|
||||||
# 已占用
|
# 已占用
|
||||||
occupied=PibotError(12,"occupied")
|
occupied=PibotError(12,"occupied")
|
||||||
# session不存在
|
# session不存在
|
||||||
noSession = PibotError(13,'cannot find session')
|
noSession = PibotError(13,'cannot find session')
|
||||||
#没有找到
|
#没有找到
|
||||||
noFound = PibotError(14, "no found")
|
noFound = PibotError(14, "no found")
|
||||||
#已经存在
|
#已经存在
|
||||||
existed = PibotError(15, "existed")
|
existed = PibotError(15, "existed")
|
||||||
#已经锁定
|
#已经锁定
|
||||||
locked = PibotError(16, "locked")
|
locked = PibotError(16, "locked")
|
||||||
#已经过期
|
#已经过期
|
||||||
expired = PibotError(17, "is expired")
|
expired = PibotError(17, "is expired")
|
||||||
#无效的参数
|
#无效的参数
|
||||||
invalidParameter = PibotError(18, "invalid parameter")
|
invalidParameter = PibotError(18, "invalid parameter")
|
||||||
|
|
||||||
518
Code/MowingRobot/pibot_ros/pypibot/pypibot/log.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/log.py
Executable file → Normal file
518
Code/MowingRobot/pibot_ros/pypibot/pypibot/log.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/log.py
Executable file → Normal file
|
|
@ -1,259 +1,259 @@
|
||||||
#!/usr/bin/python
|
#!/usr/bin/python
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
import sys,os
|
import sys,os
|
||||||
import datetime
|
import datetime
|
||||||
import threading
|
import threading
|
||||||
import pypibot.assistant as assistant
|
import pypibot.assistant as assistant
|
||||||
import platform
|
import platform
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
import _thread
|
import _thread
|
||||||
else:
|
else:
|
||||||
import thread
|
import thread
|
||||||
import traceback
|
import traceback
|
||||||
"""
|
"""
|
||||||
%a Locale’s abbreviated weekday name.
|
%a Locale’s abbreviated weekday name.
|
||||||
%A Locale’s full weekday name.
|
%A Locale’s full weekday name.
|
||||||
%b Locale’s abbreviated month name.
|
%b Locale’s abbreviated month name.
|
||||||
%B Locale’s full month name.
|
%B Locale’s full month name.
|
||||||
%c Locale’s appropriate date and time representation.
|
%c Locale’s appropriate date and time representation.
|
||||||
%d Day of the month as a decimal number [01,31].
|
%d Day of the month as a decimal number [01,31].
|
||||||
%H Hour (24-hour clock) as a decimal number [00,23].
|
%H Hour (24-hour clock) as a decimal number [00,23].
|
||||||
%I Hour (12-hour clock) as a decimal number [01,12].
|
%I Hour (12-hour clock) as a decimal number [01,12].
|
||||||
%j Day of the year as a decimal number [001,366].
|
%j Day of the year as a decimal number [001,366].
|
||||||
%m Month as a decimal number [01,12].
|
%m Month as a decimal number [01,12].
|
||||||
%M Minute as a decimal number [00,59].
|
%M Minute as a decimal number [00,59].
|
||||||
%p Locale’s equivalent of either AM or PM. (1)
|
%p Locale’s equivalent of either AM or PM. (1)
|
||||||
%S Second as a decimal number [00,61]. (2)
|
%S Second as a decimal number [00,61]. (2)
|
||||||
%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3)
|
%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3)
|
||||||
%w Weekday as a decimal number [0(Sunday),6].
|
%w Weekday as a decimal number [0(Sunday),6].
|
||||||
%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3)
|
%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3)
|
||||||
%x Locale’s appropriate date representation.
|
%x Locale’s appropriate date representation.
|
||||||
%X Locale’s appropriate time representation.
|
%X Locale’s appropriate time representation.
|
||||||
%y Year without century as a decimal number [00,99].
|
%y Year without century as a decimal number [00,99].
|
||||||
%Y Year with century as a decimal number.
|
%Y Year with century as a decimal number.
|
||||||
%Z Time zone name (no characters if no time zone exists).
|
%Z Time zone name (no characters if no time zone exists).
|
||||||
%% A literal '%' character.
|
%% A literal '%' character.
|
||||||
|
|
||||||
"""
|
"""
|
||||||
isWindows=False
|
isWindows=False
|
||||||
if platform.system()=='Windows':
|
if platform.system()=='Windows':
|
||||||
isWindows=True
|
isWindows=True
|
||||||
defaultEncodeing="utf8"
|
defaultEncodeing="utf8"
|
||||||
if "-utf8" in sys.argv:
|
if "-utf8" in sys.argv:
|
||||||
defaultEncodeing="utf-8"
|
defaultEncodeing="utf-8"
|
||||||
if "-gbk" in sys.argv:
|
if "-gbk" in sys.argv:
|
||||||
defaultEncodeing="gbk"
|
defaultEncodeing="gbk"
|
||||||
|
|
||||||
TRACE=5
|
TRACE=5
|
||||||
DEBUG=4
|
DEBUG=4
|
||||||
INFORMATION=3
|
INFORMATION=3
|
||||||
WARNING=2
|
WARNING=2
|
||||||
ERROR=1
|
ERROR=1
|
||||||
NONE=0
|
NONE=0
|
||||||
|
|
||||||
MAX_MSG_SIZE = 4096
|
MAX_MSG_SIZE = 4096
|
||||||
|
|
||||||
def getLevelFromString(level):
|
def getLevelFromString(level):
|
||||||
level=level.lower()
|
level=level.lower()
|
||||||
if level=='t' or level=='trace':return 5
|
if level=='t' or level=='trace':return 5
|
||||||
elif level=='d' or level=='debug':return 4
|
elif level=='d' or level=='debug':return 4
|
||||||
elif level=='i' or level=='info':return 3
|
elif level=='i' or level=='info':return 3
|
||||||
elif level=='w' or level=='wran':return 2
|
elif level=='w' or level=='wran':return 2
|
||||||
elif level=='e' or level=='error':return 1
|
elif level=='e' or level=='error':return 1
|
||||||
else :return 0
|
else :return 0
|
||||||
def getLevelString(level):
|
def getLevelString(level):
|
||||||
if level==TRACE:return "T"
|
if level==TRACE:return "T"
|
||||||
elif level==DEBUG:return "D"
|
elif level==DEBUG:return "D"
|
||||||
elif level==INFORMATION:return "I"
|
elif level==INFORMATION:return "I"
|
||||||
elif level==WARNING:return "W"
|
elif level==WARNING:return "W"
|
||||||
elif level==ERROR:return "E"
|
elif level==ERROR:return "E"
|
||||||
else:return "N"
|
else:return "N"
|
||||||
class PibotLog:
|
class PibotLog:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.isEnableControlLog=True
|
self.isEnableControlLog=True
|
||||||
self.fileTemple=None
|
self.fileTemple=None
|
||||||
self.filePath=""
|
self.filePath=""
|
||||||
self.level=5
|
self.level=5
|
||||||
self._lock=threading.RLock()
|
self._lock=threading.RLock()
|
||||||
self.fd=None
|
self.fd=None
|
||||||
self.fd_day=-1
|
self.fd_day=-1
|
||||||
def setLevel(self,level):
|
def setLevel(self,level):
|
||||||
self.level=getLevelFromString(level)
|
self.level=getLevelFromString(level)
|
||||||
def enableControllog(self,enable):
|
def enableControllog(self,enable):
|
||||||
self.isEnableControlLog=enable
|
self.isEnableControlLog=enable
|
||||||
def enableFileLog(self,fileName):
|
def enableFileLog(self,fileName):
|
||||||
self.fileTemple=fileName
|
self.fileTemple=fileName
|
||||||
self.updateFilelog()
|
self.updateFilelog()
|
||||||
def updateFilelog(self):
|
def updateFilelog(self):
|
||||||
fn=assistant.SF(self.fileTemple)
|
fn=assistant.SF(self.fileTemple)
|
||||||
if fn!=self.filePath:
|
if fn!=self.filePath:
|
||||||
self.i("new log file:%s",fn)
|
self.i("new log file:%s",fn)
|
||||||
if self.fd:
|
if self.fd:
|
||||||
self.fd.close()
|
self.fd.close()
|
||||||
self.fd=None
|
self.fd=None
|
||||||
self.fd_day=-1
|
self.fd_day=-1
|
||||||
self.filePath=""
|
self.filePath=""
|
||||||
try:
|
try:
|
||||||
path=os.path.dirname(fn)
|
path=os.path.dirname(fn)
|
||||||
if path!="" and not os.path.isdir(path):os.makedirs(path)
|
if path!="" and not os.path.isdir(path):os.makedirs(path)
|
||||||
self.fd=open(fn,mode="w")
|
self.fd=open(fn,mode="w")
|
||||||
try:
|
try:
|
||||||
linkfn = fn.split(".log.", 1)[0] + ".INFO"
|
linkfn = fn.split(".log.", 1)[0] + ".INFO"
|
||||||
if os.path.exists(linkfn):
|
if os.path.exists(linkfn):
|
||||||
os.remove(linkfn)
|
os.remove(linkfn)
|
||||||
(filepath, tempfilename) = os.path.split(fn)
|
(filepath, tempfilename) = os.path.split(fn)
|
||||||
os.symlink(tempfilename, linkfn)
|
os.symlink(tempfilename, linkfn)
|
||||||
except:
|
except:
|
||||||
pass
|
pass
|
||||||
self.fd_day=datetime.datetime.now().day
|
self.fd_day=datetime.datetime.now().day
|
||||||
self.filePath=fn
|
self.filePath=fn
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print("open file fail!file=%s,err=%s"%(fn,e))
|
print("open file fail!file=%s,err=%s"%(fn,e))
|
||||||
def _output(self,level,msg,args):
|
def _output(self,level,msg,args):
|
||||||
if self.level<level:return
|
if self.level<level:return
|
||||||
try:
|
try:
|
||||||
if len(args)>0:
|
if len(args)>0:
|
||||||
t=[]
|
t=[]
|
||||||
for arg in args:
|
for arg in args:
|
||||||
if isinstance(arg,Exception):
|
if isinstance(arg,Exception):
|
||||||
t.append(traceback.format_exc(arg).decode('utf-8'))
|
t.append(traceback.format_exc(arg).decode('utf-8'))
|
||||||
elif isinstance(arg,bytes) :
|
elif isinstance(arg,bytes) :
|
||||||
t.append(arg.decode('utf-8'))
|
t.append(arg.decode('utf-8'))
|
||||||
else:
|
else:
|
||||||
t.append(arg)
|
t.append(arg)
|
||||||
args=tuple(t)
|
args=tuple(t)
|
||||||
try:
|
try:
|
||||||
msg=msg%args
|
msg=msg%args
|
||||||
except:
|
except:
|
||||||
try:
|
try:
|
||||||
for arg in args:
|
for arg in args:
|
||||||
msg=msg+str(arg)+" "
|
msg=msg+str(arg)+" "
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
msg=msg+"==LOG ERROR==>%s"%(e)
|
msg=msg+"==LOG ERROR==>%s"%(e)
|
||||||
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
|
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
|
||||||
now=datetime.datetime.now()
|
now=datetime.datetime.now()
|
||||||
msg=msg+"\r\n"
|
msg=msg+"\r\n"
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
id = _thread.get_ident()
|
id = _thread.get_ident()
|
||||||
else:
|
else:
|
||||||
id = thread.get_ident()
|
id = thread.get_ident()
|
||||||
s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg)
|
s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg)
|
||||||
prefix=''
|
prefix=''
|
||||||
suffix=''
|
suffix=''
|
||||||
if not isWindows:
|
if not isWindows:
|
||||||
suffix='\033[0m'
|
suffix='\033[0m'
|
||||||
if level==TRACE:
|
if level==TRACE:
|
||||||
prefix='\033[0;37m'
|
prefix='\033[0;37m'
|
||||||
elif level==DEBUG:
|
elif level==DEBUG:
|
||||||
prefix='\033[1m'
|
prefix='\033[1m'
|
||||||
elif level==INFORMATION:
|
elif level==INFORMATION:
|
||||||
prefix='\033[0;32m'
|
prefix='\033[0;32m'
|
||||||
elif level==WARNING:
|
elif level==WARNING:
|
||||||
prefix='\033[0;33m'
|
prefix='\033[0;33m'
|
||||||
elif level==ERROR:
|
elif level==ERROR:
|
||||||
prefix='\033[0;31m'
|
prefix='\033[0;31m'
|
||||||
else:
|
else:
|
||||||
pass
|
pass
|
||||||
self._lock.acquire()
|
self._lock.acquire()
|
||||||
try:
|
try:
|
||||||
if self.isEnableControlLog:
|
if self.isEnableControlLog:
|
||||||
sys.stdout.write((prefix+s+suffix))
|
sys.stdout.write((prefix+s+suffix))
|
||||||
if self.fd:
|
if self.fd:
|
||||||
if self.fd_day!=now.day:
|
if self.fd_day!=now.day:
|
||||||
self.updateFilelog()
|
self.updateFilelog()
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
self.fd.write(s)
|
self.fd.write(s)
|
||||||
else:
|
else:
|
||||||
self.fd.write(s.encode('utf-8'))
|
self.fd.write(s.encode('utf-8'))
|
||||||
self.fd.flush()
|
self.fd.flush()
|
||||||
finally:
|
finally:
|
||||||
self._lock.release()
|
self._lock.release()
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
print(e)
|
print(e)
|
||||||
else:
|
else:
|
||||||
print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
|
print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
|
||||||
|
|
||||||
def trace(self,msg,*args):
|
def trace(self,msg,*args):
|
||||||
self._output(TRACE,msg,args)
|
self._output(TRACE,msg,args)
|
||||||
def t(self,msg,*args):
|
def t(self,msg,*args):
|
||||||
self._output(TRACE,msg,args)
|
self._output(TRACE,msg,args)
|
||||||
def debug(self,msg,*args):
|
def debug(self,msg,*args):
|
||||||
self._output(DEBUG, msg,args)
|
self._output(DEBUG, msg,args)
|
||||||
def d(self,msg,*args):
|
def d(self,msg,*args):
|
||||||
self._output(DEBUG, msg,args)
|
self._output(DEBUG, msg,args)
|
||||||
def info(self,msg,*args):
|
def info(self,msg,*args):
|
||||||
self._output(INFORMATION, msg,args)
|
self._output(INFORMATION, msg,args)
|
||||||
def i(self,msg,*args):
|
def i(self,msg,*args):
|
||||||
self._output(INFORMATION, msg,args)
|
self._output(INFORMATION, msg,args)
|
||||||
def warn(self,msg,*args):
|
def warn(self,msg,*args):
|
||||||
self._output(WARNING, msg,args)
|
self._output(WARNING, msg,args)
|
||||||
def w(self,msg,*args):
|
def w(self,msg,*args):
|
||||||
self._output(WARNING, msg,args)
|
self._output(WARNING, msg,args)
|
||||||
def error(self,msg,*args):
|
def error(self,msg,*args):
|
||||||
self._output(ERROR, msg,args)
|
self._output(ERROR, msg,args)
|
||||||
def e(self,msg,*args):
|
def e(self,msg,*args):
|
||||||
self._output(ERROR, msg,args)
|
self._output(ERROR, msg,args)
|
||||||
def _log(self,level,msg,args):
|
def _log(self,level,msg,args):
|
||||||
self._output(level, msg,args)
|
self._output(level, msg,args)
|
||||||
def createNamedLog(self,name):
|
def createNamedLog(self,name):
|
||||||
return NamedLog(name)
|
return NamedLog(name)
|
||||||
log=PibotLog()
|
log=PibotLog()
|
||||||
class NamedLog:
|
class NamedLog:
|
||||||
def __init__(self,name=None):
|
def __init__(self,name=None):
|
||||||
global log
|
global log
|
||||||
self.name=''
|
self.name=''
|
||||||
if name:
|
if name:
|
||||||
self.name='['+name+']'
|
self.name='['+name+']'
|
||||||
self.log=log
|
self.log=log
|
||||||
def trace(self,msg,*args):
|
def trace(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(TRACE,msg,args)
|
self.log._log(TRACE,msg,args)
|
||||||
def t(self,msg,*args):
|
def t(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(TRACE,msg,args)
|
self.log._log(TRACE,msg,args)
|
||||||
def debug(self,msg,*args):
|
def debug(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(DEBUG, msg,args)
|
self.log._log(DEBUG, msg,args)
|
||||||
def d(self,msg,*args):
|
def d(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(DEBUG, msg,args)
|
self.log._log(DEBUG, msg,args)
|
||||||
def info(self,msg,*args):
|
def info(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(INFORMATION, msg,args)
|
self.log._log(INFORMATION, msg,args)
|
||||||
def i(self,msg,*args):
|
def i(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(INFORMATION, msg,args)
|
self.log._log(INFORMATION, msg,args)
|
||||||
def warn(self,msg,*args):
|
def warn(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(WARNING, msg,args)
|
self.log._log(WARNING, msg,args)
|
||||||
def w(self,msg,*args):
|
def w(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(WARNING, msg,args)
|
self.log._log(WARNING, msg,args)
|
||||||
def error(self,msg,*args):
|
def error(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(ERROR, msg,args)
|
self.log._log(ERROR, msg,args)
|
||||||
def e(self,msg,*args):
|
def e(self,msg,*args):
|
||||||
msg=self.name+msg
|
msg=self.name+msg
|
||||||
self.log._log(ERROR, msg,args)
|
self.log._log(ERROR, msg,args)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
log.trace("1%s","hello")
|
log.trace("1%s","hello")
|
||||||
log.debug("2%d",12)
|
log.debug("2%d",12)
|
||||||
try:
|
try:
|
||||||
raise Exception("EXC")
|
raise Exception("EXC")
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
log.info("3%s",e)
|
log.info("3%s",e)
|
||||||
log.warn("1")
|
log.warn("1")
|
||||||
log.error("1")
|
log.error("1")
|
||||||
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
|
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
|
||||||
log.trace("1")
|
log.trace("1")
|
||||||
log.debug("1")
|
log.debug("1")
|
||||||
log.info("1")
|
log.info("1")
|
||||||
log.warn("1")
|
log.warn("1")
|
||||||
log.error("1")
|
log.error("1")
|
||||||
log=NamedLog("test")
|
log=NamedLog("test")
|
||||||
log.d("1")
|
log.d("1")
|
||||||
log.i("1")
|
log.i("1")
|
||||||
log.warn("1")
|
log.warn("1")
|
||||||
log.error("1=%d,%s",100,e)
|
log.error("1=%d,%s",100,e)
|
||||||
0
Code/MowingRobot/pibot_ros/pypibot/pypibot/mapconvert.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/mapconvert.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/pypibot/pypibot/mapconvert.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/mapconvert.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/pypibot/pypibot/proxy.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/proxy.py
Executable file → Normal file
0
Code/MowingRobot/pibot_ros/pypibot/pypibot/proxy.py → Code/MowingRobot/PIBot_ROS/pypibot/pypibot/proxy.py
Executable file → Normal file
480
Code/MowingRobot/pibot_ros/pypibot/transport/dataholder.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/dataholder.py
Executable file → Normal file
480
Code/MowingRobot/pibot_ros/pypibot/transport/dataholder.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/dataholder.py
Executable file → Normal file
|
|
@ -1,240 +1,240 @@
|
||||||
import struct
|
import struct
|
||||||
|
|
||||||
params_size=29
|
params_size=29
|
||||||
|
|
||||||
# main board
|
# main board
|
||||||
class MessageID:
|
class MessageID:
|
||||||
ID_GET_VERSION = 0
|
ID_GET_VERSION = 0
|
||||||
ID_SET_ROBOT_PARAMETER = 1
|
ID_SET_ROBOT_PARAMETER = 1
|
||||||
ID_GET_ROBOT_PARAMETER = 2
|
ID_GET_ROBOT_PARAMETER = 2
|
||||||
ID_INIT_ODOM = 3
|
ID_INIT_ODOM = 3
|
||||||
ID_SET_VEL = 4
|
ID_SET_VEL = 4
|
||||||
ID_GET_ODOM = 5
|
ID_GET_ODOM = 5
|
||||||
ID_GET_PID_DEBUG = 6
|
ID_GET_PID_DEBUG = 6
|
||||||
ID_GET_IMU = 7
|
ID_GET_IMU = 7
|
||||||
ID_GET_ENCODER_COUNT = 8
|
ID_GET_ENCODER_COUNT = 8
|
||||||
ID_SET_MOTOR_PWM = 9
|
ID_SET_MOTOR_PWM = 9
|
||||||
|
|
||||||
class RobotMessage:
|
class RobotMessage:
|
||||||
def pack(self):
|
def pack(self):
|
||||||
return b''
|
return b''
|
||||||
|
|
||||||
def unpack(self):
|
def unpack(self):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotFirmwareInfo(RobotMessage):
|
class RobotFirmwareInfo(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.version = ''
|
self.version = ''
|
||||||
self.build_time = ''
|
self.build_time = ''
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
try:
|
try:
|
||||||
upk = struct.unpack('16s16s', bytes(data))
|
upk = struct.unpack('16s16s', bytes(data))
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
[self.version, self.build_time] = upk
|
[self.version, self.build_time] = upk
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotImuType:
|
class RobotImuType:
|
||||||
IMU_TYPE_GY65 = 49
|
IMU_TYPE_GY65 = 49
|
||||||
IMU_TYPE_GY85 = 69
|
IMU_TYPE_GY85 = 69
|
||||||
IMU_TYPE_GY87 = 71
|
IMU_TYPE_GY87 = 71
|
||||||
|
|
||||||
class RobotModelType:
|
class RobotModelType:
|
||||||
MODEL_TYPE_2WD_DIFF = 1
|
MODEL_TYPE_2WD_DIFF = 1
|
||||||
MODEL_TYPE_4WD_DIFF = 2
|
MODEL_TYPE_4WD_DIFF = 2
|
||||||
MODEL_TYPE_3WD_OMNI = 101
|
MODEL_TYPE_3WD_OMNI = 101
|
||||||
MODEL_TYPE_4WD_OMNI = 102
|
MODEL_TYPE_4WD_OMNI = 102
|
||||||
MODEL_TYPE_4WD_MECANUM = 201
|
MODEL_TYPE_4WD_MECANUM = 201
|
||||||
|
|
||||||
class RobotParameters():
|
class RobotParameters():
|
||||||
def __init__(self, wheel_diameter=0, \
|
def __init__(self, wheel_diameter=0, \
|
||||||
wheel_track=0, \
|
wheel_track=0, \
|
||||||
encoder_resolution=0, \
|
encoder_resolution=0, \
|
||||||
do_pid_interval=0, \
|
do_pid_interval=0, \
|
||||||
kp=0, \
|
kp=0, \
|
||||||
ki=0, \
|
ki=0, \
|
||||||
kd=0, \
|
kd=0, \
|
||||||
ko=0, \
|
ko=0, \
|
||||||
cmd_last_time=0, \
|
cmd_last_time=0, \
|
||||||
max_v_liner_x=0, \
|
max_v_liner_x=0, \
|
||||||
max_v_liner_y=0, \
|
max_v_liner_y=0, \
|
||||||
max_v_angular_z=0, \
|
max_v_angular_z=0, \
|
||||||
imu_type=0, \
|
imu_type=0, \
|
||||||
motor_ratio=0, \
|
motor_ratio=0, \
|
||||||
model_type=0, \
|
model_type=0, \
|
||||||
motor_nonexchange_flag=255, \
|
motor_nonexchange_flag=255, \
|
||||||
encoder_nonexchange_flag=255, \
|
encoder_nonexchange_flag=255, \
|
||||||
):
|
):
|
||||||
self.wheel_diameter = wheel_diameter
|
self.wheel_diameter = wheel_diameter
|
||||||
self.wheel_track = wheel_track
|
self.wheel_track = wheel_track
|
||||||
self.encoder_resolution = encoder_resolution
|
self.encoder_resolution = encoder_resolution
|
||||||
self.do_pid_interval = do_pid_interval
|
self.do_pid_interval = do_pid_interval
|
||||||
self.kp = kp
|
self.kp = kp
|
||||||
self.ki = ki
|
self.ki = ki
|
||||||
self.kd = kd
|
self.kd = kd
|
||||||
self.ko = ko
|
self.ko = ko
|
||||||
self.cmd_last_time = cmd_last_time
|
self.cmd_last_time = cmd_last_time
|
||||||
self.max_v_liner_x = max_v_liner_x
|
self.max_v_liner_x = max_v_liner_x
|
||||||
self.max_v_liner_y = max_v_liner_y
|
self.max_v_liner_y = max_v_liner_y
|
||||||
self.max_v_angular_z = max_v_angular_z
|
self.max_v_angular_z = max_v_angular_z
|
||||||
self.imu_type = imu_type
|
self.imu_type = imu_type
|
||||||
self.motor_ratio = motor_ratio
|
self.motor_ratio = motor_ratio
|
||||||
self.model_type = model_type
|
self.model_type = model_type
|
||||||
self.motor_nonexchange_flag = motor_nonexchange_flag
|
self.motor_nonexchange_flag = motor_nonexchange_flag
|
||||||
self.encoder_nonexchange_flag = encoder_nonexchange_flag
|
self.encoder_nonexchange_flag = encoder_nonexchange_flag
|
||||||
reserve=b'\xff'
|
reserve=b'\xff'
|
||||||
self.reserve=b''
|
self.reserve=b''
|
||||||
for i in range(64-params_size):
|
for i in range(64-params_size):
|
||||||
self.reserve+=reserve
|
self.reserve+=reserve
|
||||||
robotParam = RobotParameters()
|
robotParam = RobotParameters()
|
||||||
|
|
||||||
class GetRobotParameters(RobotMessage):
|
class GetRobotParameters(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.param = robotParam
|
self.param = robotParam
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
#print(bytes(data), len(bytes(data)))
|
#print(bytes(data), len(bytes(data)))
|
||||||
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
|
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
|
||||||
|
|
||||||
[self.param.wheel_diameter,
|
[self.param.wheel_diameter,
|
||||||
self.param.wheel_track,
|
self.param.wheel_track,
|
||||||
self.param.encoder_resolution,
|
self.param.encoder_resolution,
|
||||||
self.param.do_pid_interval,
|
self.param.do_pid_interval,
|
||||||
self.param.kp,
|
self.param.kp,
|
||||||
self.param.ki,
|
self.param.ki,
|
||||||
self.param.kd,
|
self.param.kd,
|
||||||
self.param.ko,
|
self.param.ko,
|
||||||
self.param.cmd_last_time,
|
self.param.cmd_last_time,
|
||||||
self.param.max_v_liner_x,
|
self.param.max_v_liner_x,
|
||||||
self.param.max_v_liner_y,
|
self.param.max_v_liner_y,
|
||||||
self.param.max_v_angular_z,
|
self.param.max_v_angular_z,
|
||||||
self.param.imu_type,
|
self.param.imu_type,
|
||||||
self.param.motor_ratio,
|
self.param.motor_ratio,
|
||||||
self.param.model_type,
|
self.param.model_type,
|
||||||
self.param.motor_nonexchange_flag,
|
self.param.motor_nonexchange_flag,
|
||||||
self.param.encoder_nonexchange_flag,
|
self.param.encoder_nonexchange_flag,
|
||||||
self.param.reserve] = upk
|
self.param.reserve] = upk
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class SetRobotParameters(RobotMessage):
|
class SetRobotParameters(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.param = robotParam
|
self.param = robotParam
|
||||||
|
|
||||||
def pack(self):
|
def pack(self):
|
||||||
data = [self.param.wheel_diameter,
|
data = [self.param.wheel_diameter,
|
||||||
self.param.wheel_track,
|
self.param.wheel_track,
|
||||||
self.param.encoder_resolution,
|
self.param.encoder_resolution,
|
||||||
self.param.do_pid_interval,
|
self.param.do_pid_interval,
|
||||||
self.param.kp,
|
self.param.kp,
|
||||||
self.param.ki,
|
self.param.ki,
|
||||||
self.param.kd,
|
self.param.kd,
|
||||||
self.param.ko,
|
self.param.ko,
|
||||||
self.param.cmd_last_time,
|
self.param.cmd_last_time,
|
||||||
self.param.max_v_liner_x,
|
self.param.max_v_liner_x,
|
||||||
self.param.max_v_liner_y,
|
self.param.max_v_liner_y,
|
||||||
self.param.max_v_angular_z,
|
self.param.max_v_angular_z,
|
||||||
self.param.imu_type,
|
self.param.imu_type,
|
||||||
self.param.motor_ratio,
|
self.param.motor_ratio,
|
||||||
self.param.model_type,
|
self.param.model_type,
|
||||||
self.param.motor_nonexchange_flag,
|
self.param.motor_nonexchange_flag,
|
||||||
self.param.encoder_nonexchange_flag,
|
self.param.encoder_nonexchange_flag,
|
||||||
self.param.reserve]
|
self.param.reserve]
|
||||||
|
|
||||||
print(data)
|
print(data)
|
||||||
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
|
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
|
||||||
return pk
|
return pk
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotVel(RobotMessage):
|
class RobotVel(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.v_liner_x = 0
|
self.v_liner_x = 0
|
||||||
self.v_liner_y = 0
|
self.v_liner_y = 0
|
||||||
self.v_angular_z = 0
|
self.v_angular_z = 0
|
||||||
|
|
||||||
def pack(self):
|
def pack(self):
|
||||||
data = [self.v_liner_x,
|
data = [self.v_liner_x,
|
||||||
self.v_liner_y,
|
self.v_liner_y,
|
||||||
self.v_angular_z]
|
self.v_angular_z]
|
||||||
pk = struct.pack('3h', *data)
|
pk = struct.pack('3h', *data)
|
||||||
return pk
|
return pk
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
#todo the rest of the message classes
|
#todo the rest of the message classes
|
||||||
class RobotOdom(RobotMessage):
|
class RobotOdom(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.v_liner_x = 0
|
self.v_liner_x = 0
|
||||||
self.v_liner_y = 0
|
self.v_liner_y = 0
|
||||||
self.v_angular_z = 0
|
self.v_angular_z = 0
|
||||||
self.x = 0
|
self.x = 0
|
||||||
self.y = 0
|
self.y = 0
|
||||||
self.yaw = 0
|
self.yaw = 0
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
try:
|
try:
|
||||||
upk = struct.unpack('<3H2l1H', bytes(data))
|
upk = struct.unpack('<3H2l1H', bytes(data))
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
[self.v_liner_x,
|
[self.v_liner_x,
|
||||||
self.v_liner_y,
|
self.v_liner_y,
|
||||||
self.v_angular_z,
|
self.v_angular_z,
|
||||||
self.x,
|
self.x,
|
||||||
self.y,
|
self.y,
|
||||||
self.yaw] = upk
|
self.yaw] = upk
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotPIDData(RobotMessage):
|
class RobotPIDData(RobotMessage):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
class RobotIMU(RobotMessage):
|
class RobotIMU(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.imu = [0]*9
|
self.imu = [0]*9
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
try:
|
try:
|
||||||
upk = struct.unpack('<9f', bytes(data))
|
upk = struct.unpack('<9f', bytes(data))
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
self.imu = upk
|
self.imu = upk
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotEncoderCount(RobotMessage):
|
class RobotEncoderCount(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.encoder = [0]*4
|
self.encoder = [0]*4
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
try:
|
try:
|
||||||
upk = struct.unpack('<4f', bytes(data))
|
upk = struct.unpack('<4f', bytes(data))
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
self.encoder = upk
|
self.encoder = upk
|
||||||
return True
|
return True
|
||||||
|
|
||||||
class RobotMotorPWM(RobotMessage):
|
class RobotMotorPWM(RobotMessage):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.pwm = [0]*4
|
self.pwm = [0]*4
|
||||||
|
|
||||||
def pack(self):
|
def pack(self):
|
||||||
pk = struct.pack('4h', *self.pwm)
|
pk = struct.pack('4h', *self.pwm)
|
||||||
return pk
|
return pk
|
||||||
|
|
||||||
def unpack(self, data):
|
def unpack(self, data):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
|
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
|
||||||
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
|
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
|
||||||
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
|
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
|
||||||
MessageID.ID_SET_VEL:RobotVel(),
|
MessageID.ID_SET_VEL:RobotVel(),
|
||||||
MessageID.ID_GET_ODOM:RobotOdom(),
|
MessageID.ID_GET_ODOM:RobotOdom(),
|
||||||
MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
|
MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
|
||||||
MessageID.ID_GET_IMU: RobotIMU(),
|
MessageID.ID_GET_IMU: RobotIMU(),
|
||||||
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
|
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
|
||||||
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
|
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
|
||||||
}
|
}
|
||||||
|
|
||||||
230
Code/MowingRobot/pibot_ros/pypibot/transport/main.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/main.py
Executable file → Normal file
230
Code/MowingRobot/pibot_ros/pypibot/transport/main.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/main.py
Executable file → Normal file
|
|
@ -1,115 +1,115 @@
|
||||||
import platform
|
import platform
|
||||||
import sys
|
import sys
|
||||||
sys.path.append("..")
|
sys.path.append("..")
|
||||||
import pypibot
|
import pypibot
|
||||||
from pypibot import log
|
from pypibot import log
|
||||||
from transport import Transport
|
from transport import Transport
|
||||||
from dataholder import MessageID
|
from dataholder import MessageID
|
||||||
import params
|
import params
|
||||||
import time
|
import time
|
||||||
import signal
|
import signal
|
||||||
|
|
||||||
#for linux
|
#for linux
|
||||||
port="/dev/pibot"
|
port="/dev/pibot"
|
||||||
|
|
||||||
#for windows
|
#for windows
|
||||||
#port="com3"
|
#port="com3"
|
||||||
|
|
||||||
pypibot.assistant.enableGlobalExcept()
|
pypibot.assistant.enableGlobalExcept()
|
||||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||||
log.setLevel("i")
|
log.setLevel("i")
|
||||||
|
|
||||||
run_flag = True
|
run_flag = True
|
||||||
|
|
||||||
def exit(signum, frame):
|
def exit(signum, frame):
|
||||||
global run_flag
|
global run_flag
|
||||||
run_flag = False
|
run_flag = False
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
signal.signal(signal.SIGINT, exit)
|
signal.signal(signal.SIGINT, exit)
|
||||||
|
|
||||||
mboard = Transport(port, params.pibotBaud)
|
mboard = Transport(port, params.pibotBaud)
|
||||||
if not mboard.start():
|
if not mboard.start():
|
||||||
log.error("can not open %s"%port)
|
log.error("can not open %s"%port)
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
DataHolder = mboard.getDataHolder()
|
DataHolder = mboard.getDataHolder()
|
||||||
|
|
||||||
for num in range(0,3):
|
for num in range(0,3):
|
||||||
log.info("****************get robot version*****************")
|
log.info("****************get robot version*****************")
|
||||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||||
if p:
|
if p:
|
||||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
log.error('read firmware version err\r\n')
|
log.error('read firmware version err\r\n')
|
||||||
import time
|
import time
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
if num == 2:
|
if num == 2:
|
||||||
log.error('please check connection or baudrate\r\n')
|
log.error('please check connection or baudrate\r\n')
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
# get robot parameter
|
# get robot parameter
|
||||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||||
if p:
|
if p:
|
||||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||||
%(robotParam.param.model_type, \
|
%(robotParam.param.model_type, \
|
||||||
robotParam.param.wheel_diameter, \
|
robotParam.param.wheel_diameter, \
|
||||||
robotParam.param.wheel_track, \
|
robotParam.param.wheel_track, \
|
||||||
robotParam.param.encoder_resolution
|
robotParam.param.encoder_resolution
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||||
%(robotParam.param.do_pid_interval, \
|
%(robotParam.param.do_pid_interval, \
|
||||||
robotParam.param.kp, \
|
robotParam.param.kp, \
|
||||||
robotParam.param.ki, \
|
robotParam.param.ki, \
|
||||||
robotParam.param.kd, \
|
robotParam.param.kd, \
|
||||||
robotParam.param.ko))
|
robotParam.param.ko))
|
||||||
|
|
||||||
log.info("cmd_last_time:%d imu_type:%d" \
|
log.info("cmd_last_time:%d imu_type:%d" \
|
||||||
%(robotParam.param.cmd_last_time,\
|
%(robotParam.param.cmd_last_time,\
|
||||||
robotParam.param.imu_type
|
robotParam.param.imu_type
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("max_v:%d %d %d\r\n" \
|
log.info("max_v:%d %d %d\r\n" \
|
||||||
%(robotParam.param.max_v_liner_x,\
|
%(robotParam.param.max_v_liner_x,\
|
||||||
robotParam.param.max_v_liner_y, \
|
robotParam.param.max_v_liner_y, \
|
||||||
robotParam.param.max_v_angular_z
|
robotParam.param.max_v_angular_z
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||||
%(robotParam.param.motor_nonexchange_flag,\
|
%(robotParam.param.motor_nonexchange_flag,\
|
||||||
robotParam.param.encoder_nonexchange_flag
|
robotParam.param.encoder_nonexchange_flag
|
||||||
))
|
))
|
||||||
else:
|
else:
|
||||||
log.error('get params err\r\n')
|
log.error('get params err\r\n')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
log.info("****************get odom&imu*****************")
|
log.info("****************get odom&imu*****************")
|
||||||
while run_flag:
|
while run_flag:
|
||||||
robotOdom = DataHolder[MessageID.ID_GET_ODOM]
|
robotOdom = DataHolder[MessageID.ID_GET_ODOM]
|
||||||
p = mboard.request(MessageID.ID_GET_ODOM)
|
p = mboard.request(MessageID.ID_GET_ODOM)
|
||||||
if p:
|
if p:
|
||||||
log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \
|
log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \
|
||||||
robotOdom.v_liner_y, \
|
robotOdom.v_liner_y, \
|
||||||
robotOdom.v_angular_z, \
|
robotOdom.v_angular_z, \
|
||||||
robotOdom.x, \
|
robotOdom.x, \
|
||||||
robotOdom.y, \
|
robotOdom.y, \
|
||||||
robotOdom.yaw))
|
robotOdom.yaw))
|
||||||
else:
|
else:
|
||||||
log.error('get odom err')
|
log.error('get odom err')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
|
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
|
||||||
p = mboard.request(MessageID.ID_GET_IMU)
|
p = mboard.request(MessageID.ID_GET_IMU)
|
||||||
if p:
|
if p:
|
||||||
log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \
|
log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \
|
||||||
robotIMU[3], robotIMU[4], robotIMU[5], \
|
robotIMU[3], robotIMU[4], robotIMU[5], \
|
||||||
robotIMU[6], robotIMU[7], robotIMU[8]))
|
robotIMU[6], robotIMU[7], robotIMU[8]))
|
||||||
else:
|
else:
|
||||||
log.error('get imu err')
|
log.error('get imu err')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
148
Code/MowingRobot/pibot_ros/pypibot/transport/params.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/params.py
Executable file → Normal file
148
Code/MowingRobot/pibot_ros/pypibot/transport/params.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/params.py
Executable file → Normal file
|
|
@ -1,75 +1,75 @@
|
||||||
import dataholder
|
import dataholder
|
||||||
import os
|
import os
|
||||||
from dataholder import RobotImuType
|
from dataholder import RobotImuType
|
||||||
from dataholder import RobotModelType
|
from dataholder import RobotModelType
|
||||||
|
|
||||||
pibotModel = os.environ['PIBOT_MODEL']
|
pibotModel = os.environ['PIBOT_MODEL']
|
||||||
boardType = os.environ['PIBOT_BOARD']
|
boardType = os.environ['PIBOT_BOARD']
|
||||||
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
|
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
|
||||||
|
|
||||||
print(pibotModel)
|
print(pibotModel)
|
||||||
print(boardType)
|
print(boardType)
|
||||||
print(pibotBaud)
|
print(pibotBaud)
|
||||||
|
|
||||||
pibotParam = dataholder.RobotParameters()
|
pibotParam = dataholder.RobotParameters()
|
||||||
|
|
||||||
if pibotModel == "apollo" and boardType == "arduino":
|
if pibotModel == "apollo" and boardType == "arduino":
|
||||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||||
75, 2500, 0, 10, \
|
75, 2500, 0, 10, \
|
||||||
250, 40, 0, 200, \
|
250, 40, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
elif pibotModel == "apollo" and boardType == "stm32f1":
|
elif pibotModel == "apollo" and boardType == "stm32f1":
|
||||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||||
320, 2700, 0, 10, \
|
320, 2700, 0, 10, \
|
||||||
250, 50, 0, 200, \
|
250, 50, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
elif pibotModel == "apollo" and boardType == "stm32f4":
|
elif pibotModel == "apollo" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
|
||||||
320, 2700, 0, 10, \
|
320, 2700, 0, 10, \
|
||||||
250, 40, 0, 200, \
|
250, 40, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
elif pibotModel == "zeus" and boardType == "stm32f4":
|
elif pibotModel == "zeus" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
|
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
|
||||||
320, 2700, 0, 10, \
|
320, 2700, 0, 10, \
|
||||||
250, 50, 50, 250, \
|
250, 50, 50, 250, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_3WD_OMNI)
|
RobotModelType.MODEL_TYPE_3WD_OMNI)
|
||||||
elif pibotModel == "hades" and boardType == "stm32f4":
|
elif pibotModel == "hades" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
|
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
|
||||||
320, 2700, 0, 10, \
|
320, 2700, 0, 10, \
|
||||||
250, 50, 50, 250, \
|
250, 50, 50, 250, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||||
elif pibotModel == "hadesX" and boardType == "stm32f4":
|
elif pibotModel == "hadesX" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
|
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
|
||||||
250, 2750, 0, 10, \
|
250, 2750, 0, 10, \
|
||||||
250, 50, 50, 250, \
|
250, 50, 50, 250, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 72, \
|
RobotImuType.IMU_TYPE_GY87, 72, \
|
||||||
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
RobotModelType.MODEL_TYPE_4WD_MECANUM)
|
||||||
elif pibotModel == "hera" and boardType == "stm32f4":
|
elif pibotModel == "hera" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
|
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
|
||||||
320, 2700, 0, 10, \
|
320, 2700, 0, 10, \
|
||||||
250, 50, 50, 250, \
|
250, 50, 50, 250, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_4WD_DIFF)
|
RobotModelType.MODEL_TYPE_4WD_DIFF)
|
||||||
elif pibotModel == "apolloX" and boardType == "arduino":
|
elif pibotModel == "apolloX" and boardType == "arduino":
|
||||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||||
75, 2500, 0, 10, \
|
75, 2500, 0, 10, \
|
||||||
250, 40, 0, 200, \
|
250, 40, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY85, 90, \
|
RobotImuType.IMU_TYPE_GY85, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
elif pibotModel == "apolloX" and boardType == "stm32f1":
|
elif pibotModel == "apolloX" and boardType == "stm32f1":
|
||||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||||
250, 1200, 0, 10, \
|
250, 1200, 0, 10, \
|
||||||
250, 50, 0, 200, \
|
250, 50, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
elif pibotModel == "apolloX" and boardType == "stm32f4":
|
elif pibotModel == "apolloX" and boardType == "stm32f4":
|
||||||
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
|
||||||
250, 1200, 0, 10, \
|
250, 1200, 0, 10, \
|
||||||
250, 50, 0, 200, \
|
250, 50, 0, 200, \
|
||||||
RobotImuType.IMU_TYPE_GY87, 90, \
|
RobotImuType.IMU_TYPE_GY87, 90, \
|
||||||
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
RobotModelType.MODEL_TYPE_2WD_DIFF)
|
||||||
178
Code/MowingRobot/pibot_ros/pypibot/transport/set_default_params.py → 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 → Code/MowingRobot/PIBot_ROS/pypibot/transport/set_default_params.py
Executable file → Normal file
|
|
@ -1,90 +1,90 @@
|
||||||
import platform
|
import platform
|
||||||
import sys
|
import sys
|
||||||
sys.path.append("..")
|
sys.path.append("..")
|
||||||
import pypibot
|
import pypibot
|
||||||
from pypibot import log
|
from pypibot import log
|
||||||
from transport import Transport
|
from transport import Transport
|
||||||
from dataholder import MessageID
|
from dataholder import MessageID
|
||||||
import params
|
import params
|
||||||
|
|
||||||
#for linux
|
#for linux
|
||||||
port="/dev/pibot"
|
port="/dev/pibot"
|
||||||
|
|
||||||
#for windows
|
#for windows
|
||||||
#port="com3"
|
#port="com3"
|
||||||
|
|
||||||
pypibot.assistant.enableGlobalExcept()
|
pypibot.assistant.enableGlobalExcept()
|
||||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||||
log.setLevel("i")
|
log.setLevel("i")
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
mboard = Transport(port, params.pibotBaud)
|
mboard = Transport(port, params.pibotBaud)
|
||||||
if not mboard.start():
|
if not mboard.start():
|
||||||
log.error("can not open %s"%port)
|
log.error("can not open %s"%port)
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
DataHolder = mboard.getDataHolder()
|
DataHolder = mboard.getDataHolder()
|
||||||
|
|
||||||
for num in range(0,3):
|
for num in range(0,3):
|
||||||
log.info("****************get robot version*****************")
|
log.info("****************get robot version*****************")
|
||||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||||
if p:
|
if p:
|
||||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
log.error('read firmware version err\r\n')
|
log.error('read firmware version err\r\n')
|
||||||
import time
|
import time
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
if num == 2:
|
if num == 2:
|
||||||
log.error('please check connection or baudrate\r\n')
|
log.error('please check connection or baudrate\r\n')
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
# set robot parameter
|
# set robot parameter
|
||||||
log.info("****************set robot parameter*****************")
|
log.info("****************set robot parameter*****************")
|
||||||
|
|
||||||
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
|
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
|
||||||
|
|
||||||
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
|
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
|
||||||
if p:
|
if p:
|
||||||
log.info('set parameter success')
|
log.info('set parameter success')
|
||||||
else:
|
else:
|
||||||
log.error('set parameter err')
|
log.error('set parameter err')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
# get robot parameter
|
# get robot parameter
|
||||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||||
if p:
|
if p:
|
||||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||||
%(robotParam.param.model_type, \
|
%(robotParam.param.model_type, \
|
||||||
robotParam.param.wheel_diameter, \
|
robotParam.param.wheel_diameter, \
|
||||||
robotParam.param.wheel_track, \
|
robotParam.param.wheel_track, \
|
||||||
robotParam.param.encoder_resolution
|
robotParam.param.encoder_resolution
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||||
%(robotParam.param.do_pid_interval, \
|
%(robotParam.param.do_pid_interval, \
|
||||||
robotParam.param.kp, \
|
robotParam.param.kp, \
|
||||||
robotParam.param.ki, \
|
robotParam.param.ki, \
|
||||||
robotParam.param.kd, \
|
robotParam.param.kd, \
|
||||||
robotParam.param.ko))
|
robotParam.param.ko))
|
||||||
|
|
||||||
log.info("cmd_last_time:%d imu_type:%d" \
|
log.info("cmd_last_time:%d imu_type:%d" \
|
||||||
%(robotParam.param.cmd_last_time,\
|
%(robotParam.param.cmd_last_time,\
|
||||||
robotParam.param.imu_type
|
robotParam.param.imu_type
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("max_v:%d %d %d\r\n" \
|
log.info("max_v:%d %d %d\r\n" \
|
||||||
%(robotParam.param.max_v_liner_x,\
|
%(robotParam.param.max_v_liner_x,\
|
||||||
robotParam.param.max_v_liner_y, \
|
robotParam.param.max_v_liner_y, \
|
||||||
robotParam.param.max_v_angular_z
|
robotParam.param.max_v_angular_z
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||||
%(robotParam.param.motor_nonexchange_flag,\
|
%(robotParam.param.motor_nonexchange_flag,\
|
||||||
robotParam.param.encoder_nonexchange_flag
|
robotParam.param.encoder_nonexchange_flag
|
||||||
))
|
))
|
||||||
else:
|
else:
|
||||||
log.error('get param err\r\n')
|
log.error('get param err\r\n')
|
||||||
quit(1)
|
quit(1)
|
||||||
242
Code/MowingRobot/pibot_ros/pypibot/transport/test_motors.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/test_motors.py
Executable file → Normal file
242
Code/MowingRobot/pibot_ros/pypibot/transport/test_motors.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/test_motors.py
Executable file → Normal file
|
|
@ -1,122 +1,122 @@
|
||||||
import platform
|
import platform
|
||||||
import sys
|
import sys
|
||||||
sys.path.append("..")
|
sys.path.append("..")
|
||||||
import pypibot
|
import pypibot
|
||||||
from pypibot import log
|
from pypibot import log
|
||||||
from transport import Transport
|
from transport import Transport
|
||||||
from dataholder import MessageID
|
from dataholder import MessageID
|
||||||
import params
|
import params
|
||||||
import signal
|
import signal
|
||||||
|
|
||||||
#for linux
|
#for linux
|
||||||
port="/dev/pibot"
|
port="/dev/pibot"
|
||||||
|
|
||||||
#for windows
|
#for windows
|
||||||
#port="com3"
|
#port="com3"
|
||||||
|
|
||||||
pypibot.assistant.enableGlobalExcept()
|
pypibot.assistant.enableGlobalExcept()
|
||||||
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
|
||||||
log.setLevel("i")
|
log.setLevel("i")
|
||||||
|
|
||||||
run_flag = True
|
run_flag = True
|
||||||
|
|
||||||
def exit(signum, frame):
|
def exit(signum, frame):
|
||||||
global run_flag
|
global run_flag
|
||||||
run_flag = False
|
run_flag = False
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
signal.signal(signal.SIGINT, exit)
|
signal.signal(signal.SIGINT, exit)
|
||||||
|
|
||||||
mboard = Transport(port, params.pibotBaud)
|
mboard = Transport(port, params.pibotBaud)
|
||||||
if not mboard.start():
|
if not mboard.start():
|
||||||
log.error("can not open %s"%port)
|
log.error("can not open %s"%port)
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
pwm=[0]*4
|
pwm=[0]*4
|
||||||
for i in range(len(sys.argv)-1):
|
for i in range(len(sys.argv)-1):
|
||||||
pwm[i] = int(sys.argv[i+1])
|
pwm[i] = int(sys.argv[i+1])
|
||||||
|
|
||||||
DataHolder = mboard.getDataHolder()
|
DataHolder = mboard.getDataHolder()
|
||||||
|
|
||||||
for num in range(0,3):
|
for num in range(0,3):
|
||||||
log.info("****************get robot version*****************")
|
log.info("****************get robot version*****************")
|
||||||
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
|
||||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||||
if p:
|
if p:
|
||||||
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
log.error('read firmware version err\r\n')
|
log.error('read firmware version err\r\n')
|
||||||
import time
|
import time
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
if num == 2:
|
if num == 2:
|
||||||
log.error('please check connection or baudrate\r\n')
|
log.error('please check connection or baudrate\r\n')
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
# get robot parameter
|
# get robot parameter
|
||||||
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
|
||||||
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
|
||||||
if p:
|
if p:
|
||||||
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
|
||||||
%(robotParam.param.model_type, \
|
%(robotParam.param.model_type, \
|
||||||
robotParam.param.wheel_diameter, \
|
robotParam.param.wheel_diameter, \
|
||||||
robotParam.param.wheel_track, \
|
robotParam.param.wheel_track, \
|
||||||
robotParam.param.encoder_resolution
|
robotParam.param.encoder_resolution
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
|
||||||
%(robotParam.param.do_pid_interval, \
|
%(robotParam.param.do_pid_interval, \
|
||||||
robotParam.param.kp, \
|
robotParam.param.kp, \
|
||||||
robotParam.param.ki, \
|
robotParam.param.ki, \
|
||||||
robotParam.param.kd, \
|
robotParam.param.kd, \
|
||||||
robotParam.param.ko))
|
robotParam.param.ko))
|
||||||
|
|
||||||
log.info("cmd_last_time:%d imu_type:%d" \
|
log.info("cmd_last_time:%d imu_type:%d" \
|
||||||
%(robotParam.param.cmd_last_time,\
|
%(robotParam.param.cmd_last_time,\
|
||||||
robotParam.param.imu_type
|
robotParam.param.imu_type
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("max_v:%d %d %d\r\n" \
|
log.info("max_v:%d %d %d\r\n" \
|
||||||
%(robotParam.param.max_v_liner_x,\
|
%(robotParam.param.max_v_liner_x,\
|
||||||
robotParam.param.max_v_liner_y, \
|
robotParam.param.max_v_liner_y, \
|
||||||
robotParam.param.max_v_angular_z
|
robotParam.param.max_v_angular_z
|
||||||
))
|
))
|
||||||
|
|
||||||
log.info("motor flag:%d encoder flag: %d\r\n" \
|
log.info("motor flag:%d encoder flag: %d\r\n" \
|
||||||
%(robotParam.param.motor_nonexchange_flag,\
|
%(robotParam.param.motor_nonexchange_flag,\
|
||||||
robotParam.param.encoder_nonexchange_flag
|
robotParam.param.encoder_nonexchange_flag
|
||||||
))
|
))
|
||||||
else:
|
else:
|
||||||
log.error('get params err\r\n')
|
log.error('get params err\r\n')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
|
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
|
||||||
|
|
||||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||||
if p:
|
if p:
|
||||||
log.info('set pwm success')
|
log.info('set pwm success')
|
||||||
else:
|
else:
|
||||||
log.error('set pwm err')
|
log.error('set pwm err')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
log.info("****************get encoder count*****************")
|
log.info("****************get encoder count*****************")
|
||||||
while run_flag:
|
while run_flag:
|
||||||
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
|
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
|
||||||
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
|
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
|
||||||
if p:
|
if p:
|
||||||
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
|
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
|
||||||
else:
|
else:
|
||||||
log.error('get encoder count err')
|
log.error('get encoder count err')
|
||||||
quit(1)
|
quit(1)
|
||||||
|
|
||||||
import time
|
import time
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
|
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
|
||||||
|
|
||||||
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
|
||||||
if p:
|
if p:
|
||||||
log.info('set pwm success')
|
log.info('set pwm success')
|
||||||
else:
|
else:
|
||||||
log.error('set pwm err')
|
log.error('set pwm err')
|
||||||
quit(1)
|
quit(1)
|
||||||
394
Code/MowingRobot/pibot_ros/pypibot/transport/transport.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/transport.py
Executable file → Normal file
394
Code/MowingRobot/pibot_ros/pypibot/transport/transport.py → Code/MowingRobot/PIBot_ROS/pypibot/transport/transport.py
Executable file → Normal file
|
|
@ -1,197 +1,197 @@
|
||||||
import sys
|
import sys
|
||||||
sys.path.append("..")
|
sys.path.append("..")
|
||||||
import pypibot
|
import pypibot
|
||||||
from pypibot import log
|
from pypibot import log
|
||||||
from pypibot import assistant
|
from pypibot import assistant
|
||||||
|
|
||||||
import serial
|
import serial
|
||||||
import threading
|
import threading
|
||||||
import struct
|
import struct
|
||||||
import time
|
import time
|
||||||
from dataholder import MessageID, BoardDataDict
|
from dataholder import MessageID, BoardDataDict
|
||||||
FIX_HEAD = 0x5a
|
FIX_HEAD = 0x5a
|
||||||
|
|
||||||
class Recstate():
|
class Recstate():
|
||||||
WAITING_HD = 0
|
WAITING_HD = 0
|
||||||
WAITING_MSG_ID = 1
|
WAITING_MSG_ID = 1
|
||||||
RECEIVE_LEN = 2
|
RECEIVE_LEN = 2
|
||||||
RECEIVE_PACKAGE = 3
|
RECEIVE_PACKAGE = 3
|
||||||
RECEIVE_CHECK = 4
|
RECEIVE_CHECK = 4
|
||||||
|
|
||||||
def checksum(d):
|
def checksum(d):
|
||||||
sum = 0
|
sum = 0
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
for i in d:
|
for i in d:
|
||||||
sum += i
|
sum += i
|
||||||
sum = sum&0xff
|
sum = sum&0xff
|
||||||
else:
|
else:
|
||||||
for i in d:
|
for i in d:
|
||||||
sum += ord(i)
|
sum += ord(i)
|
||||||
sum = sum&0xff
|
sum = sum&0xff
|
||||||
return sum
|
return sum
|
||||||
|
|
||||||
|
|
||||||
class Transport:
|
class Transport:
|
||||||
def __init__(self, port, baudrate=921600):
|
def __init__(self, port, baudrate=921600):
|
||||||
self._Port = port
|
self._Port = port
|
||||||
self._Baudrate = baudrate
|
self._Baudrate = baudrate
|
||||||
self._KeepRunning = False
|
self._KeepRunning = False
|
||||||
self.receive_state = Recstate.WAITING_HD
|
self.receive_state = Recstate.WAITING_HD
|
||||||
self.rev_msg = []
|
self.rev_msg = []
|
||||||
self.rev_data = []
|
self.rev_data = []
|
||||||
self.wait_event = threading.Event()
|
self.wait_event = threading.Event()
|
||||||
|
|
||||||
def getDataHolder(self):
|
def getDataHolder(self):
|
||||||
return BoardDataDict
|
return BoardDataDict
|
||||||
|
|
||||||
def start(self):
|
def start(self):
|
||||||
try:
|
try:
|
||||||
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
|
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
|
||||||
self._KeepRunning = True
|
self._KeepRunning = True
|
||||||
self._ReceiverThread = threading.Thread(target=self._Listen)
|
self._ReceiverThread = threading.Thread(target=self._Listen)
|
||||||
self._ReceiverThread.setDaemon(True)
|
self._ReceiverThread.setDaemon(True)
|
||||||
self._ReceiverThread.start()
|
self._ReceiverThread.start()
|
||||||
return True
|
return True
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
def Stop(self):
|
def Stop(self):
|
||||||
self._KeepRunning = False
|
self._KeepRunning = False
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
self._Serial.close()
|
self._Serial.close()
|
||||||
|
|
||||||
def _Listen(self):
|
def _Listen(self):
|
||||||
while self._KeepRunning:
|
while self._KeepRunning:
|
||||||
if self.receiveFiniteStates(self._Serial.read()):
|
if self.receiveFiniteStates(self._Serial.read()):
|
||||||
self.packageAnalysis()
|
self.packageAnalysis()
|
||||||
|
|
||||||
def receiveFiniteStates(self, s):
|
def receiveFiniteStates(self, s):
|
||||||
if len(s) == 0:
|
if len(s) == 0:
|
||||||
return False
|
return False
|
||||||
val = s[0]
|
val = s[0]
|
||||||
val_int = val
|
val_int = val
|
||||||
if not assistant.is_python3():
|
if not assistant.is_python3():
|
||||||
val_int = ord(val)
|
val_int = ord(val)
|
||||||
|
|
||||||
if self.receive_state == Recstate.WAITING_HD:
|
if self.receive_state == Recstate.WAITING_HD:
|
||||||
if val_int == FIX_HEAD:
|
if val_int == FIX_HEAD:
|
||||||
log.trace('got head')
|
log.trace('got head')
|
||||||
self.rev_msg = []
|
self.rev_msg = []
|
||||||
self.rev_data =[]
|
self.rev_data =[]
|
||||||
self.rev_msg.append(val)
|
self.rev_msg.append(val)
|
||||||
self.receive_state = Recstate.WAITING_MSG_ID
|
self.receive_state = Recstate.WAITING_MSG_ID
|
||||||
elif self.receive_state == Recstate.WAITING_MSG_ID:
|
elif self.receive_state == Recstate.WAITING_MSG_ID:
|
||||||
log.trace('got msg id')
|
log.trace('got msg id')
|
||||||
self.rev_msg.append(val)
|
self.rev_msg.append(val)
|
||||||
self.receive_state = Recstate.RECEIVE_LEN
|
self.receive_state = Recstate.RECEIVE_LEN
|
||||||
elif self.receive_state == Recstate.RECEIVE_LEN:
|
elif self.receive_state == Recstate.RECEIVE_LEN:
|
||||||
log.trace('got len:%d', val_int)
|
log.trace('got len:%d', val_int)
|
||||||
self.rev_msg.append(val)
|
self.rev_msg.append(val)
|
||||||
if val_int == 0:
|
if val_int == 0:
|
||||||
self.receive_state = Recstate.RECEIVE_CHECK
|
self.receive_state = Recstate.RECEIVE_CHECK
|
||||||
else:
|
else:
|
||||||
self.receive_state = Recstate.RECEIVE_PACKAGE
|
self.receive_state = Recstate.RECEIVE_PACKAGE
|
||||||
elif self.receive_state == Recstate.RECEIVE_PACKAGE:
|
elif self.receive_state == Recstate.RECEIVE_PACKAGE:
|
||||||
# if assistant.is_python3():
|
# if assistant.is_python3():
|
||||||
# self.rev_data.append((chr(val)).encode('latin1'))
|
# self.rev_data.append((chr(val)).encode('latin1'))
|
||||||
# else:
|
# else:
|
||||||
self.rev_data.append(val)
|
self.rev_data.append(val)
|
||||||
r = False
|
r = False
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
r = len(self.rev_data) == int(self.rev_msg[-1])
|
r = len(self.rev_data) == int(self.rev_msg[-1])
|
||||||
else:
|
else:
|
||||||
r = len(self.rev_data) == ord(self.rev_msg[-1])
|
r = len(self.rev_data) == ord(self.rev_msg[-1])
|
||||||
|
|
||||||
if r:
|
if r:
|
||||||
self.rev_msg.extend(self.rev_data)
|
self.rev_msg.extend(self.rev_data)
|
||||||
self.receive_state = Recstate.RECEIVE_CHECK
|
self.receive_state = Recstate.RECEIVE_CHECK
|
||||||
elif self.receive_state == Recstate.RECEIVE_CHECK:
|
elif self.receive_state == Recstate.RECEIVE_CHECK:
|
||||||
log.trace('got check')
|
log.trace('got check')
|
||||||
self.receive_state = Recstate.WAITING_HD
|
self.receive_state = Recstate.WAITING_HD
|
||||||
if val_int == checksum(self.rev_msg):
|
if val_int == checksum(self.rev_msg):
|
||||||
log.trace('got a complete message')
|
log.trace('got a complete message')
|
||||||
return True
|
return True
|
||||||
else:
|
else:
|
||||||
self.receive_state = Recstate.WAITING_HD
|
self.receive_state = Recstate.WAITING_HD
|
||||||
|
|
||||||
# continue receiving
|
# continue receiving
|
||||||
return False
|
return False
|
||||||
|
|
||||||
def packageAnalysis(self):
|
def packageAnalysis(self):
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
in_msg_id = int(self.rev_msg[1])
|
in_msg_id = int(self.rev_msg[1])
|
||||||
else:
|
else:
|
||||||
in_msg_id = ord(self.rev_msg[1])
|
in_msg_id = ord(self.rev_msg[1])
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
|
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
|
||||||
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
|
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
|
||||||
else:
|
else:
|
||||||
log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data))
|
log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data))
|
||||||
r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data))
|
r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data))
|
||||||
if r:
|
if r:
|
||||||
self.res_id = in_msg_id
|
self.res_id = in_msg_id
|
||||||
if in_msg_id<100:
|
if in_msg_id<100:
|
||||||
self.set_response()
|
self.set_response()
|
||||||
else:#notify
|
else:#notify
|
||||||
log.debug('msg %d'%self.rev_msg[4],'data incoming')
|
log.debug('msg %d'%self.rev_msg[4],'data incoming')
|
||||||
pass
|
pass
|
||||||
else:
|
else:
|
||||||
log.debug ('error unpacking pkg')
|
log.debug ('error unpacking pkg')
|
||||||
|
|
||||||
def request(self, id, timeout=0.5):
|
def request(self, id, timeout=0.5):
|
||||||
if not self.write(id):
|
if not self.write(id):
|
||||||
log.debug ('Serial send error!')
|
log.debug ('Serial send error!')
|
||||||
return False
|
return False
|
||||||
if self.wait_for_response(timeout):
|
if self.wait_for_response(timeout):
|
||||||
if id == self.res_id:
|
if id == self.res_id:
|
||||||
log.trace ('OK')
|
log.trace ('OK')
|
||||||
else:
|
else:
|
||||||
log.error ('Got unmatched response!')
|
log.error ('Got unmatched response!')
|
||||||
else:
|
else:
|
||||||
log.error ('Request got no response!')
|
log.error ('Request got no response!')
|
||||||
return False
|
return False
|
||||||
# clear response
|
# clear response
|
||||||
self.res_id = None
|
self.res_id = None
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def write(self, id):
|
def write(self, id):
|
||||||
cmd = self.make_command(id)
|
cmd = self.make_command(id)
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
|
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
|
||||||
else:
|
else:
|
||||||
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
|
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
|
||||||
self._Serial.write(cmd)
|
self._Serial.write(cmd)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
def wait_for_response(self, timeout):
|
def wait_for_response(self, timeout):
|
||||||
self.wait_event.clear()
|
self.wait_event.clear()
|
||||||
return self.wait_event.wait(timeout)
|
return self.wait_event.wait(timeout)
|
||||||
|
|
||||||
def set_response(self):
|
def set_response(self):
|
||||||
self.wait_event.set()
|
self.wait_event.set()
|
||||||
|
|
||||||
def make_command(self, id):
|
def make_command(self, id):
|
||||||
#print(DataDict[id])
|
#print(DataDict[id])
|
||||||
data = BoardDataDict[id].pack()
|
data = BoardDataDict[id].pack()
|
||||||
l = [FIX_HEAD, id, len(data)]
|
l = [FIX_HEAD, id, len(data)]
|
||||||
head = struct.pack("3B", *l)
|
head = struct.pack("3B", *l)
|
||||||
body = head + data
|
body = head + data
|
||||||
|
|
||||||
if assistant.is_python3():
|
if assistant.is_python3():
|
||||||
return body + chr(checksum(body)).encode('latin1')
|
return body + chr(checksum(body)).encode('latin1')
|
||||||
else:
|
else:
|
||||||
return body + chr(checksum(body))
|
return body + chr(checksum(body))
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
|
|
||||||
mboard = Transport('com10')
|
mboard = Transport('com10')
|
||||||
if not mboard.start():
|
if not mboard.start():
|
||||||
import sys
|
import sys
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
p = mboard.request(MessageID.ID_GET_VERSION)
|
p = mboard.request(MessageID.ID_GET_VERSION)
|
||||||
log.i("result=%s"%p)
|
log.i("result=%s"%p)
|
||||||
print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n")
|
print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n")
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
## ROS包功能说明:
|
||||||
|
|
||||||
|
> 更新时间：2024/02/04@詹力
|
||||||
|
|
||||||
|
`ipa_coverage_planning` ：新添加的全覆盖路径规划算法，这部分代码是原来`pibot`没有的。详细内容见包里面的`README.md`文档。
|
||||||
|
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue