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

Merge pull request 'main' (#9) from ray/RobotKernal-UESTC:main into main 

合并瑞瑞提交，将不同的功能包放在pibot下面
pull/1/head
詹力 2024-03-11 10:35:32 +08:00
parent afbfe9401f e644d1c97f
commit 7bb2ee4d5f
1140 changed files with 9210 additions and 26660 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Code/MowingRobot/PIBot_ROS/Image/ROS_Using (3).png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 108 KiB

93
Code/MowingRobot/PIBot_ROS/README.md
View File

@ -1,93 +0,0 @@
# `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、全覆盖路径规划的使用

6
Code/MowingRobot/PIBot_ROS/ros_ws/src/README.md
View File

@ -1,6 +0,0 @@
## ROS包功能说明:
> 更新时间：2024/02/04@詹力
`ipa_coverage_planning` ：新添加的全覆盖路径规划算法，这部分代码是原来`pibot`没有的。详细内容见包里面的`README.md`文档。

18
Code/MowingRobot/PIBot_ROS/ros_ws/src/pibot_imu/src/pibot_imu_node.cpp
View File

@ -1,18 +0,0 @@
/*--------------------------------------------------------------
* Node Desc ： 1.IMU的初始静态零偏校准
2.原始IMU的减零偏操作
3.接收IMU校准服务的校准请求
* Review : zhanli.2024.02.28
---------------------------------------------------------------*/
#include "pibot_imu/pibot_imu.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "pibot_imu");
ros::NodeHandle nh, pnh("~");
PibotIMU pibot_imu(nh, pnh);
ros::spin();
return 0;
}

20
Code/MowingRobot/README.md
View File

@ -24,7 +24,23 @@ PIBot官方资料：https://gitee.com/pibot/pibot_bringup/blob/master/doc/README
./runRoomExplorationServer.sh
# 启动全覆盖地图客户端
./runRoomExplorationClient.sh
#自己建图（画圈）、地图的优化、跑全覆盖
#建图
（UWB的）
#地图优化
cd test_map
./main input_map_name output_map_name
#全覆盖客户端路径
cd pibot_ros/ros_ws
source ./devel/setup.bash
roslaunch ipa_room_exploration room_exploration_client.launch robot_env:=map_name
#修改地图路径
#优化地图
test_map 的main.cpp
#全覆盖
room_exploration_action_client.cpp 下面77行
```

1
Code/MowingRobot/Robot_ROS_APP/.catkin_workspace
View File

@ -1 +0,0 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

2
Code/MowingRobot/Robot_ROS_APP/.gitignore vendored
View File

@ -1,2 +0,0 @@
/build/
/cmake-build-debug/

BIN
Code/MowingRobot/Robot_ROS_APP/Image/main.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 296 KiB

32
Code/MowingRobot/Robot_ROS_APP/README.md
View File

@ -1,32 +0,0 @@
# `UWB`建图指导
一、代码编译和运行
```shell
# 进入工程目录
cd Robot_ROS_Driver
# 代码的编译
catkin_make
# 环境变量引入
rosrun ros_merge_test ros_merge_test_node
```
​ 线程在`main.cpp`中开启：
<img src=".\image\main.png" alt="image-20231212192507303" style="zoom: 67%;" />
​ 主要功能是
uwb接收 uwb_trd
建图 map_trd
保存定位数据 align_trd
建图和保存数据需要哪个用哪个
建图：在rosrun之后 出现的图片上按q开始建图，再按一次q停止，可以再同一幅图上重复开始和暂停
保存数据：同样在rosrun之后出现的图片上按w开始保存 再按w停止

10468
Code/MowingRobot/Robot_ROS_APP/dataset/data1.txt
View File

File diff suppressed because it is too large Load Diff

9313
Code/MowingRobot/Robot_ROS_APP/dataset/data2.txt
View File

File diff suppressed because it is too large Load Diff

1
Code/MowingRobot/Robot_ROS_APP/devel/.built_by
View File

@ -1 +0,0 @@
catkin_make

1
Code/MowingRobot/Robot_ROS_APP/devel/.catkin
View File

@ -1 +0,0 @@
/home/luoruidi/Desktop/ros_learn/C++_Merge_Ros/merge_ros/src

2
Code/MowingRobot/Robot_ROS_APP/devel/.rosinstall
View File

@ -1,2 +0,0 @@
- setup-file:
local-name: /home/luoruidi/Desktop/ros_learn/C++_Merge_Ros/merge_ros/devel/setup.sh

304
Code/MowingRobot/Robot_ROS_APP/devel/_setup_util.py
View File

@ -1,304 +0,0 @@
#!/usr/bin/python3
# -*- coding: utf-8 -*-
# Software License Agreement (BSD License)
#
# Copyright (c) 2012, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
"""This file generates shell code for the setup.SHELL scripts to set environment variables."""
from __future__ import print_function
import argparse
import copy
import errno
import os
import platform
import sys
CATKIN_MARKER_FILE = '.catkin'
system = platform.system()
IS_DARWIN = (system == 'Darwin')
IS_WINDOWS = (system == 'Windows')
PATH_TO_ADD_SUFFIX = ['bin']
if IS_WINDOWS:
# while catkin recommends putting dll's into bin, 3rd party packages often put dll's into lib
# since Windows finds dll's via the PATH variable, prepend it with path to lib
PATH_TO_ADD_SUFFIX.extend([['lib', os.path.join('lib', 'x86_64-linux-gnu')]])
# subfolder of workspace prepended to CMAKE_PREFIX_PATH
ENV_VAR_SUBFOLDERS = {
'CMAKE_PREFIX_PATH': '',
'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'x86_64-linux-gnu')],
'PATH': PATH_TO_ADD_SUFFIX,
'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'x86_64-linux-gnu', 'pkgconfig')],
'PYTHONPATH': 'lib/python3/dist-packages',
}
def rollback_env_variables(environ, env_var_subfolders):
"""
Generate shell code to reset environment variables.
by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
This does not cover modifications performed by environment hooks.
"""
lines = []
unmodified_environ = copy.copy(environ)
for key in sorted(env_var_subfolders.keys()):
subfolders = env_var_subfolders[key]
if not isinstance(subfolders, list):
subfolders = [subfolders]
value = _rollback_env_variable(unmodified_environ, key, subfolders)
if value is not None:
environ[key] = value
lines.append(assignment(key, value))
if lines:
lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
return lines
def _rollback_env_variable(environ, name, subfolders):
"""
For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
:param subfolders: list of str '' or subfoldername that may start with '/'
:returns: the updated value of the environment variable.
"""
value = environ[name] if name in environ else ''
env_paths = [path for path in value.split(os.pathsep) if path]
value_modified = False
for subfolder in subfolders:
if subfolder:
if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
subfolder = subfolder[1:]
if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
subfolder = subfolder[:-1]
for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
path_to_remove = None
for env_path in env_paths:
env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
if env_path_clean == path_to_find:
path_to_remove = env_path
break
if path_to_remove:
env_paths.remove(path_to_remove)
value_modified = True
new_value = os.pathsep.join(env_paths)
return new_value if value_modified else None
def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
"""
Based on CMAKE_PREFIX_PATH return all catkin workspaces.
:param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
"""
# get all cmake prefix paths
env_name = 'CMAKE_PREFIX_PATH'
value = environ[env_name] if env_name in environ else ''
paths = [path for path in value.split(os.pathsep) if path]
# remove non-workspace paths
workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
return workspaces
def prepend_env_variables(environ, env_var_subfolders, workspaces):
"""Generate shell code to prepend environment variables for the all workspaces."""
lines = []
lines.append(comment('prepend folders of workspaces to environment variables'))
paths = [path for path in workspaces.split(os.pathsep) if path]
prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
for key in sorted(key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH'):
subfolder = env_var_subfolders[key]
prefix = _prefix_env_variable(environ, key, paths, subfolder)
lines.append(prepend(environ, key, prefix))
return lines
def _prefix_env_variable(environ, name, paths, subfolders):
"""
Return the prefix to prepend to the environment variable NAME.
Adding any path in NEW_PATHS_STR without creating duplicate or empty items.
"""
value = environ[name] if name in environ else ''
environ_paths = [path for path in value.split(os.pathsep) if path]
checked_paths = []
for path in paths:
if not isinstance(subfolders, list):
subfolders = [subfolders]
for subfolder in subfolders:
path_tmp = path
if subfolder:
path_tmp = os.path.join(path_tmp, subfolder)
# skip nonexistent paths
if not os.path.exists(path_tmp):
continue
# exclude any path already in env and any path we already added
if path_tmp not in environ_paths and path_tmp not in checked_paths:
checked_paths.append(path_tmp)
prefix_str = os.pathsep.join(checked_paths)
if prefix_str != '' and environ_paths:
prefix_str += os.pathsep
return prefix_str
def assignment(key, value):
if not IS_WINDOWS:
return 'export %s="%s"' % (key, value)
else:
return 'set %s=%s' % (key, value)
def comment(msg):
if not IS_WINDOWS:
return '# %s' % msg
else:
return 'REM %s' % msg
def prepend(environ, key, prefix):
if key not in environ or not environ[key]:
return assignment(key, prefix)
if not IS_WINDOWS:
return 'export %s="%s$%s"' % (key, prefix, key)
else:
return 'set %s=%s%%%s%%' % (key, prefix, key)
def find_env_hooks(environ, cmake_prefix_path):
"""Generate shell code with found environment hooks for the all workspaces."""
lines = []
lines.append(comment('found environment hooks in workspaces'))
generic_env_hooks = []
generic_env_hooks_workspace = []
specific_env_hooks = []
specific_env_hooks_workspace = []
generic_env_hooks_by_filename = {}
specific_env_hooks_by_filename = {}
generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
# remove non-workspace paths
workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
for workspace in reversed(workspaces):
env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
if os.path.isdir(env_hook_dir):
for filename in sorted(os.listdir(env_hook_dir)):
if filename.endswith('.%s' % generic_env_hook_ext):
# remove previous env hook with same name if present
if filename in generic_env_hooks_by_filename:
i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
generic_env_hooks.pop(i)
generic_env_hooks_workspace.pop(i)
# append env hook
generic_env_hooks.append(os.path.join(env_hook_dir, filename))
generic_env_hooks_workspace.append(workspace)
generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
# remove previous env hook with same name if present
if filename in specific_env_hooks_by_filename:
i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
specific_env_hooks.pop(i)
specific_env_hooks_workspace.pop(i)
# append env hook
specific_env_hooks.append(os.path.join(env_hook_dir, filename))
specific_env_hooks_workspace.append(workspace)
specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
env_hooks = generic_env_hooks + specific_env_hooks
env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
count = len(env_hooks)
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
for i in range(count):
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
return lines
def _parse_arguments(args=None):
parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
parser.add_argument('--local', action='store_true', help='Only consider this prefix path and ignore other prefix path in the environment')
return parser.parse_known_args(args=args)[0]
if __name__ == '__main__':
try:
try:
args = _parse_arguments()
except Exception as e:
print(e, file=sys.stderr)
sys.exit(1)
if not args.local:
# environment at generation time
CMAKE_PREFIX_PATH = r'/home/luoruidi/Desktop/ros_learn/communicat_01/src/commu_01/devel;/opt/ros/noetic'.split(';')
else:
# don't consider any other prefix path than this one
CMAKE_PREFIX_PATH = []
# prepend current workspace if not already part of CPP
base_path = os.path.dirname(__file__)
# CMAKE_PREFIX_PATH uses forward slash on all platforms, but __file__ is platform dependent
# base_path on Windows contains backward slashes, need to be converted to forward slashes before comparison
if os.path.sep != '/':
base_path = base_path.replace(os.path.sep, '/')
if base_path not in CMAKE_PREFIX_PATH:
CMAKE_PREFIX_PATH.insert(0, base_path)
CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
environ = dict(os.environ)
lines = []
if not args.extend:
lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
print('\n'.join(lines))
# need to explicitly flush the output
sys.stdout.flush()
except IOError as e:
# and catch potential "broken pipe" if stdout is not writable
# which can happen when piping the output to a file but the disk is full
if e.errno == errno.EPIPE:
print(e, file=sys.stderr)
sys.exit(2)
raise
sys.exit(0)

0
Code/MowingRobot/Robot_ROS_APP/devel/cmake.lock
View File

16
Code/MowingRobot/Robot_ROS_APP/devel/env.sh
View File

@ -1,16 +0,0 @@
#!/usr/bin/env sh
# generated from catkin/cmake/templates/env.sh.in
if [ $# -eq 0 ] ; then
/bin/echo "Usage: env.sh COMMANDS"
/bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
exit 1
fi
# ensure to not use different shell type which was set before
CATKIN_SHELL=sh
# source setup.sh from same directory as this file
_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
. "$_CATKIN_SETUP_DIR/setup.sh"
exec "$@"

8
Code/MowingRobot/Robot_ROS_APP/devel/local_setup.bash
View File

@ -1,8 +0,0 @@
#!/usr/bin/env bash
# generated from catkin/cmake/templates/local_setup.bash.in
CATKIN_SHELL=bash
# source setup.sh from same directory as this file
_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
. "$_CATKIN_SETUP_DIR/setup.sh" --extend --local

9
Code/MowingRobot/Robot_ROS_APP/devel/local_setup.sh
View File

@ -1,9 +0,0 @@
#!/usr/bin/env sh
# generated from catkin/cmake/template/local_setup.sh.in
# since this file is sourced either use the provided _CATKIN_SETUP_DIR
# or fall back to the destination set at configure time
: ${_CATKIN_SETUP_DIR:=/home/luoruidi/Desktop/ros_learn/C++_Merge_Ros/merge_ros/devel}
CATKIN_SETUP_UTIL_ARGS="--extend --local"
. "$_CATKIN_SETUP_DIR/setup.sh"
unset CATKIN_SETUP_UTIL_ARGS

8
Code/MowingRobot/Robot_ROS_APP/devel/local_setup.zsh
View File

@ -1,8 +0,0 @@
#!/usr/bin/env zsh
# generated from catkin/cmake/templates/local_setup.zsh.in
CATKIN_SHELL=zsh
# source setup.sh from same directory as this file
_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local'

8
Code/MowingRobot/Robot_ROS_APP/devel/setup.bash
View File

@ -1,8 +0,0 @@
#!/usr/bin/env bash
# generated from catkin/cmake/templates/setup.bash.in
CATKIN_SHELL=bash
# source setup.sh from same directory as this file
_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
. "$_CATKIN_SETUP_DIR/setup.sh"

96
Code/MowingRobot/Robot_ROS_APP/devel/setup.sh
View File

@ -1,96 +0,0 @@
#!/usr/bin/env sh
# generated from catkin/cmake/template/setup.sh.in
# Sets various environment variables and sources additional environment hooks.
# It tries it's best to undo changes from a previously sourced setup file before.
# Supported command line options:
# --extend: skips the undoing of changes from a previously sourced setup file
# --local: only considers this workspace but not the chained ones
# In plain sh shell which doesn't support arguments for sourced scripts you can
# set the environment variable `CATKIN_SETUP_UTIL_ARGS=--extend/--local` instead.
# since this file is sourced either use the provided _CATKIN_SETUP_DIR
# or fall back to the destination set at configure time
: ${_CATKIN_SETUP_DIR:=/home/luoruidi/Desktop/ros_learn/C++_Merge_Ros/merge_ros/devel}
_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
unset _CATKIN_SETUP_DIR
if [ ! -f "$_SETUP_UTIL" ]; then
echo "Missing Python script: $_SETUP_UTIL"
return 22
fi
# detect if running on Darwin platform
_UNAME=`uname -s`
_IS_DARWIN=0
if [ "$_UNAME" = "Darwin" ]; then
_IS_DARWIN=1
fi
unset _UNAME
# make sure to export all environment variables
export CMAKE_PREFIX_PATH
if [ $_IS_DARWIN -eq 0 ]; then
export LD_LIBRARY_PATH
else
export DYLD_LIBRARY_PATH
fi
unset _IS_DARWIN
export PATH
export PKG_CONFIG_PATH
export PYTHONPATH
# remember type of shell if not already set
if [ -z "$CATKIN_SHELL" ]; then
CATKIN_SHELL=sh
fi
# invoke Python script to generate necessary exports of environment variables
# use TMPDIR if it exists, otherwise fall back to /tmp
if [ -d "${TMPDIR:-}" ]; then
_TMPDIR="${TMPDIR}"
else
_TMPDIR=/tmp
fi
_SETUP_TMP=`mktemp "${_TMPDIR}/setup.sh.XXXXXXXXXX"`
unset _TMPDIR
if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
echo "Could not create temporary file: $_SETUP_TMP"
return 1
fi
CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ ${CATKIN_SETUP_UTIL_ARGS:-} >> "$_SETUP_TMP"
_RC=$?
if [ $_RC -ne 0 ]; then
if [ $_RC -eq 2 ]; then
echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
else
echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
fi
unset _RC
unset _SETUP_UTIL
rm -f "$_SETUP_TMP"
unset _SETUP_TMP
return 1
fi
unset _RC
unset _SETUP_UTIL
. "$_SETUP_TMP"
rm -f "$_SETUP_TMP"
unset _SETUP_TMP
# source all environment hooks
_i=0
while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
unset _CATKIN_ENVIRONMENT_HOOKS_$_i
eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
# set workspace for environment hook
CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
. "$_envfile"
unset CATKIN_ENV_HOOK_WORKSPACE
_i=$((_i + 1))
done
unset _i
unset _CATKIN_ENVIRONMENT_HOOKS_COUNT

8
Code/MowingRobot/Robot_ROS_APP/devel/setup.zsh
View File

@ -1,8 +0,0 @@
#!/usr/bin/env zsh
# generated from catkin/cmake/templates/setup.zsh.in
CATKIN_SHELL=zsh
# source setup.sh from same directory as this file
_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"'

8
Code/MowingRobot/Robot_ROS_APP/src/.idea/.gitignore vendored
View File

@ -1,8 +0,0 @@
# Default ignored files
/shelf/
/workspace.xml
# Editor-based HTTP Client requests
/httpRequests/
# Datasource local storage ignored files
/dataSources/
/dataSources.local.xml

14
Code/MowingRobot/Robot_ROS_APP/src/.idea/deployment.xml
View File

@ -1,14 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="PublishConfigData" remoteFilesAllowedToDisappearOnAutoupload="false">
<serverData>
<paths name="Remote Host (1dce3a15-503d-4545-b5bc-e482c493c313)">
<serverdata>
<mappings>
<mapping deploy="/tmp/src" local="$PROJECT_DIR$" />
</mappings>
</serverdata>
</paths>
</serverData>
</component>
</project>

4
Code/MowingRobot/Robot_ROS_APP/src/.idea/misc.xml
View File

@ -1,4 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
</project>

8
Code/MowingRobot/Robot_ROS_APP/src/.idea/modules.xml
View File

@ -1,8 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/src.iml" filepath="$PROJECT_DIR$/.idea/src.iml" />
</modules>
</component>
</project>

2
Code/MowingRobot/Robot_ROS_APP/src/.idea/src.iml
View File

@ -1,2 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />

6
Code/MowingRobot/Robot_ROS_APP/src/.idea/vcs.xml
View File

@ -1,6 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
</component>
</project>

1
Code/MowingRobot/Robot_ROS_APP/src/CMakeLists.txt
View File

@ -1 +0,0 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/01.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.4 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/02.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.6 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/03.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.8 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/04.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.6 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/05.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.5 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/06.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.9 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/08.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.8 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/09.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.8 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/10.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.1 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/11.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.5 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/12.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.9 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/13.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.2 KiB

BIN
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/Map/output_image.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 3.0 KiB

31
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/mapping.h
View File

@ -1,31 +0,0 @@
#include <queue>
#include <opencv2/opencv.hpp>
#include <mutex>
#include "uwb.h"
#ifndef MAPPING_H
#define MAPPING_H
namespace uwb_slam{
class Mapping
{
public:
const double PIXEL_SCALE = 5.0;
const int AREA_SIZE = 2000;
Mapping() {};
void Run();
bool check_uwb_point();
void feed_uwb_data(const cv::Point2d & data);
void process();
std::mutex mMutexMap;
std::shared_ptr<uwb_slam::Uwb> uwb_;
private:
std::queue<cv::Point2d> mv_uwb_point_;
bool read_uwb_ = false;
cv::Mat img;
cv::Point2d cur_point = {-1,-1};
};
}
#endif

29
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/senddata.h
View File

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

36
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/uwb.h
View File

@ -1,36 +0,0 @@
#include <ros/ros.h>
#include <mutex>
#include <boost/asio.hpp>
#include <iostream>
#include <string>
#include <cstdint>
#include "type.h"
#include <queue>
#include <chrono>
#ifndef __UWB_H__
#define __UWB_H__
namespace uwb_slam{
class Uwb
{
public:
Uwb();
void Run();
bool checknewdata();
void feed_imu_odom_pose_data();
void UartUSBRead();
public:
int pre_seq = -1;
int cur_seq = -1;
uint8_t tmpdata[13];
float x, y, theta, distance;
Uwb_data uwb_data_;
std::mutex mMutexUwb;
};
};
#endif

154
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/align.cpp
View File

@ -1,154 +0,0 @@
/******************** (C) COPYRIGHT 2023 UPBot **********************************
* File Name : align.cpp
* Current Version : V1.0
* Date of Issued : 2023.12.13 zhanli@review
* Comments : 传感器数据对齐
********************************************************************************/
#include "align.h"
namespace uwb_slam{
void Align::Run()
{
tmp = ros::Time::now();
ros::Time tmp1 = ros::Time::now();
ros::Time tmp2 = ros::Time::now();
// 这个地方分别订阅了原始IMU、轮式里程计、里程计(推测是UWB)
wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
imu_sub_ = nh_.subscribe("raw_imu",10,&Align::imuCB,this);
odom_sub_ = nh_.subscribe("odom",10,&Align::odomCB,this);
std::ofstream outfile("data.txt",std::ofstream::out);
if(outfile.is_open())
{
img1 = cv::Mat(200, 200, CV_8UC1, cv::Scalar(255,255,255));
cv::imshow("Image1", img1);
int key2 = cv::waitKey(0);
// if(key2 =='w'){
// bool write_data_ = true;
// }
// int count=0;
// while(write_data_){
while(1){
int key3 = cv::waitKey(1);
if(key3 == 'w'){
break;
}
if(tmp!=imu_odom_.imu_data_.imu_t_)
{
// outfile <<"imu_odom_: "<< "imu_timestamp "<<"imu_linear_acc_x_y_z "<<"imu_angular_x_y_z "<<
// "odom_vxy "<<"odom_angle_v_ "<<"\n";
// if(tmp1!=uwb_->uwb_data_.uwb_t_&& tmp2!=odom_tmp_){
outfile << std::left << std::setw(12)<<"imu_odom_: "<< std::setw(10)<< imu_odom_.imu_data_.imu_t_.sec << '.' <<std::setw(11)<< imu_odom_.imu_data_.imu_t_.nsec << std::setw(11)
<<imu_odom_.imu_data_.a_[0] << std::setw(11)<<imu_odom_.imu_data_.a_[1] << std::setw(11)<<imu_odom_.imu_data_.a_[2] << std::setw(12)
<<imu_odom_.imu_data_.w_[0] << std::setw(12)<<imu_odom_.imu_data_.w_[1] << std::setw(12)<<imu_odom_.imu_data_.w_[2] << std::setw(11)
<<imu_odom_.vxy_ << std::setw(11)<<imu_odom_.angle_v_ << std::setw(7)
<<" pose: " << std::setw(1)<<(odom_tmp_-tmp2).sec<<'.'<<std::setw(12)<<(odom_tmp_-tmp2).nsec<<std::setw(11)
<<imu_odom_.pose_[0] << std::setw(12)<<imu_odom_.pose_[1] << std::setw(2)<<imu_odom_.pose_[2] << std::setw(12)
<<imu_odom_.quat_[0] << std::setw(12)<<imu_odom_.quat_[1] << std::setw(12)<<imu_odom_.quat_[2] << std::setw(12)<<imu_odom_.quat_[3] << std::setw(6)
<<" uwb: " << std::setw(1)<<(uwb_->uwb_data_.uwb_t_-tmp1).sec<<'.'<<std::setw(13)<<(uwb_->uwb_data_.uwb_t_-tmp1).nsec << std::setw(9)
<<uwb_->uwb_data_.x_ << std::setw(9)<<uwb_->uwb_data_.y_<<"\n";
tmp1 = uwb_->uwb_data_.uwb_t_;
tmp2 = odom_tmp_;
// }
// else if(tmp1!=uwb_->uwb_data_.uwb_t_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<uwb_->uwb_data_.uwb_t_<<"*"<<uwb_->uwb_data_.x_<<"*"<<uwb_->uwb_data_.y_<<"\n";
// tmp1 = uwb_->uwb_data_.uwb_t_;
// }
// else if(tmp2!=odom_tmp_){
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "
// <<imu_odom_.pose_[0]<<"*"<<imu_odom_.pose_[1]<<"*"<<imu_odom_.pose_[2]<<"*"
// <<imu_odom_.quat_[0]<<"*"<<imu_odom_.quat_[1]<<"*"<<imu_odom_.quat_[2]<<"*"<<imu_odom_.quat_[3]<<"*"
// <<"uwb: "<<"****************************"<<"\n";
// tmp2 = odom_tmp_;
// }
// else {
// outfile <<"imu_odom_: "<< imu_odom_.imu_data_.imu_t_ <<"*"
// <<imu_odom_.imu_data_.a_[0]<<"*"<<imu_odom_.imu_data_.a_[1]<<"*"<<imu_odom_.imu_data_.a_[2]<<"*"
// <<imu_odom_.imu_data_.w_[0]<<"*"<<imu_odom_.imu_data_.w_[1]<<"*"<<imu_odom_.imu_data_.w_[2]<<"*"
// <<imu_odom_.vxy_<<"*"<<imu_odom_.angle_v_<<"*"
// <<"pose: "<<"****************************"
// <<"uwb: "<<"****************************"<<"\n";
// }
tmp = imu_odom_.imu_data_.imu_t_;
// tmp1 = uwb_->uwb_data_.uwb_t_;
// if(count>300)
// break;
}
}
outfile.close();
std::cout<< "Data written to file." << std::endl;
}
else{
std::cout<<"file can not open"<<std::endl;
}
}
void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
{
imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
// imu_odom_.pose_[0] = wheel_odom.pose.pose.position.x;
// imu_odom_.pose_[1] = wheel_odom.pose.pose.position.y;
// imu_odom_.pose_[2] = wheel_odom.pose.pose.position.z;
// imu_odom_.quat_[0] = wheel_odom.pose.pose.orientation.x;
// imu_odom_.quat_[1] = wheel_odom.pose.pose.orientation.y;
// imu_odom_.quat_[2] = wheel_odom.pose.pose.orientation.z;
// imu_odom_.quat_[3] = wheel_odom.pose.pose.orientation.w;
return;
}
void Align::imuCB(const ros_merge_test::RawImu& imu)
{
imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;
imu_odom_.imu_data_.w_[0] = imu.raw_angular_velocity.x;
imu_odom_.imu_data_.w_[1] = imu.raw_angular_velocity.y;
imu_odom_.imu_data_.w_[2] = imu.raw_angular_velocity.z;
return;
}
/**---------------------------------------------------------------------
* Function : odomCB
* Description : 里程计的回调函数, 定期会被ROS调用传参，这个函数不能做过于耗时
* 的操作
* Input : nav_msgs::Odometry& odom : 里程计输入结构体
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Align::odomCB(const nav_msgs::Odometry& odom)
{
odom_tmp_ = odom.header.stamp;
imu_odom_.pose_[0] = odom.pose.pose.position.x;
imu_odom_.pose_[1] = odom.pose.pose.position.y;
imu_odom_.pose_[2] = odom.pose.pose.position.z;
imu_odom_.quat_[0] = odom.pose.pose.orientation.x;
imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
imu_odom_.quat_[2] = odom.pose.pose.orientation.z;
imu_odom_.quat_[3] = odom.pose.pose.orientation.w;
}
};

53
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/main.cpp
View File

@ -1,53 +0,0 @@
/******************** (C) COPYRIGHT 2023 UPBot **********************************
* File Name : main.cpp
* Current Version : V1.0
* Date of Issued : 2023.12.13 zhanli@review
* Comments : UPbot割草机器人项目传感器融合定位入口函数
********************************************************************************/
#include "../include/system.h"
#include "../include/uwb.h"
#include <iostream>
#include <ros/ros.h>
#include <thread>
#include "senddata.h"
/**---------------------------------------------------------------------
* Function : main
* Description : 多传感器融合定位的入口函数
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
int main(int argc, char** argv)
{
// Initialize the ROS node
ros::init(argc, argv, "locate_info_pub_node");
std::shared_ptr<uwb_slam::Mapping> mp = std::make_shared<uwb_slam::Mapping>();
std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
std::shared_ptr<uwb_slam::Senddata> sender = std::make_shared<uwb_slam::Senddata>();
std::shared_ptr<uwb_slam::Align> align = std::make_shared<uwb_slam::Align>();
mp->uwb_ = uwb;
align->uwb_ = uwb;
// uwb serried read
std::thread uwb_trd([&uwb]() {
uwb->Run();
});
// 建图部分暂时没有使用到
/*std::thread map_trd([&mp]() {
mp->Run();
});*/
std::thread sender_trd([&sender, uwb]() {
sender->Run(uwb);
});
std::thread align_trd([&align]() {
align->Run();
});
// Start the ROS node's main loop
ros::spin();
}

89
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp
View File

@ -1,89 +0,0 @@
#include "mapping.h"
#include <mutex>
#include <unistd.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
namespace uwb_slam
{
bool Mapping::check_uwb_point()
{
//std::unique_lock<std::mutex> lock(mMutexMap);
return !mv_uwb_point_.empty();
}
void Mapping::feed_uwb_data(const cv::Point2d & data)
{
//std::unique_lock<std::mutex> lock(mMutexMap);
mv_uwb_point_.push(data);
}
void Mapping::process()
{
{
//std::unique_lock<std::mutex> lock(mMutexMap);
//std::cout << "SIZE: " <<mv_uwb_point_.size() << std::endl;
cur_point= mv_uwb_point_.front();
//std::cout << "x: " <<cur_point.x << " y:" << cur_point.y << std::endl;
mv_uwb_point_.pop();
}
/*生成图*/
int pix_x = cur_point.x / PIXEL_SCALE + ( fmod(cur_point.x ,PIXEL_SCALE) != 0);
int pix_y = cur_point.y / PIXEL_SCALE + ( fmod(cur_point.y ,PIXEL_SCALE) != 0);
img.at<unsigned char>(pix_y,pix_x)= 0;
//cv::imshow("Image",img);
}
void Mapping::Run()
{
int realWidth = AREA_SIZE / PIXEL_SCALE;
int realHeight = AREA_SIZE / PIXEL_SCALE;
img = cv::Mat(realHeight, realWidth, CV_8UC1, cv::Scalar(255,255,255));
for (int j=0;j<AREA_SIZE / PIXEL_SCALE;j+=16)
for (int i=0;i<AREA_SIZE / PIXEL_SCALE;i+=16)
img.at<unsigned char>(j,i)= 128;
for (int j=199+8;j<210;j+=1)
for (int i=199+8;i<210;i+=1)
img.at<unsigned char>(j,i)= 0;
cv::imshow("Image",img);
while(1)
{
// 这个地方会持续阻塞
int key = cv::waitKey(0);
if (key == 'q') {
read_uwb_ = true;
std::cout << "non" << key << std::endl;
//cv::destroyAllWindows();
}
while(read_uwb_ ) // 按下空格键
{
int key2 = cv::waitKey(1);
if (key2 == 'q' ) {
//TODO: save
std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径
cv::imwrite(pngimage, img);
read_uwb_ = false;
break;
}
this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y));
if(check_uwb_point()){
process();
}
}
}
}
} // namespace uwb_slam

83
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp
View File

@ -1,83 +0,0 @@
/******************** (C) COPYRIGHT 2023 UPBot **********************************
* File Name : senddata.cpp
* Current Version : V1.0
* Date of Issued : 2023.12.13 zhanli@review
* Comments : UWB数据的发送
********************************************************************************/
#include "senddata.h"
namespace uwb_slam{
/**---------------------------------------------------------------------
* Function : Senddata::Run
* Description : UWB位置发布以及odm数据的订阅
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
// 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
ros::Rate loop_rate(10);
// 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
// 主题被设置为"uwb_odom"，队列大小为50
position_pub_ = nh_.advertise<nav_msgs::Odometry>("uwb_odom", 50);
// 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
// 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
odom_sub_ = nh_.subscribe("odom", 10, &Senddata::odomCB,this);
while(ros::ok()){
// 按照10Hz频率发布uwb信息
publishOdometry(uwb);
ros::spinOnce();
// 用于控制循环速率
loop_rate.sleep();
}
}
void Senddata::odomCB(const nav_msgs::Odometry& odom){
// 这个地方接收的是轮速里程计的信息
// 包含位置和姿态
sub_odom_ = odom;
return;
}
/**---------------------------------------------------------------------
* Function : Senddata::publishOdometry
* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb> uwb)
{
std::mutex mMutexSend;
ros::Time current_time = ros::Time::now();
// 设置 Odometry 消息的头部信息
odom_.header.stamp = current_time; // 这个地方获取的时间是否会存在问题?
odom_.header.frame_id = "odom"; // 设置坐标系为 "map"
odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link"
// 填充 Odometry 消息的位置信息
odom_.pose.pose.position.x = uwb->x;
odom_.pose.pose.position.y = uwb->y;
odom_.pose.pose.position.z = 0.0;
// 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
// tf2::Quaternion quat;
// quat.setRPY(0, 0, uwb->theta);
// 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
// odom.pose.pose.orientation.x = quat.x();
// odom.pose.pose.orientation.y = quat.y();
// odom.pose.pose.orientation.z = quat.z();
// odom.pose.pose.orientation.w = quat.w();
// 从里程计拿到姿态信息
odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
// 发布 Odometry 消息
position_pub_.publish(odom_);
}
} // namespace uwb_slam

68
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp
View File

@ -1,68 +0,0 @@
/******************** (C) COPYRIGHT 2023 UPBot **********************************
* File Name : uwb.cpp
* Current Version : V1.0
* Date of Issued : 2023.12.13 zhanli@review
* Comments : UWB数据驱动, 负责从串口读取USB并发布出去，这个代码可能存在以下
改进：1) 创建固定的串口读取对象serial_port 2) 修改代码为异步
读取。3) 确认串口数据的长度
********************************************************************************/
#include "uwb.h"
#include <cmath>
#define PI acos(-1)
namespace uwb_slam{
//
Uwb::Uwb(){
}
void Uwb::Run() {
while(1){
// 这个地方不控制速率?
// UartUSBRead 这个地方本身就是同步读取串口，是阻塞的函数
this->UartUSBRead();
}
}
/**---------------------------------------------------------------------
* Function : UartUSBRead
* Description : 通过串口读取数据，目前这段代码存在部分问题：1) 每次都重复创建
* 串口读取对象，可能会影响性能。
* Date : 2023/12/13 zhanli@review
*---------------------------------------------------------------------**/
void Uwb::UartUSBRead()
{
try {
boost::asio::io_service io;
boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径
serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位
uint8_t tmpdata[12];
// 读取串口数据
size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12));
// UWB获取的数据是半径R和角度Theta
memcpy(&this->distance, &tmpdata[3], sizeof(distance));
memcpy(&this->theta, &tmpdata[7], sizeof(theta));
// 这个地方是为了把UWB的坐标移动到图像的中心位置 2023/12/13@李瑞瑞
this->uwb_data_.x_ = cosf(theta / 180*PI)*distance + 1000;
this->uwb_data_.y_ = sinf(theta / 180*PI)*distance + 1000;
// 获取此时的系统时间戳
this->uwb_data_.uwb_t_ = ros::Time::now();
std::cout << "theta: " << theta << " distance: " << distance << std::endl;
} catch (const std::exception& ex) {
std::cerr << "[ERR]: uwb.cpp::Uart USB read data exception: "
<< ex.what() << std::endl;
}
}
};

15
Code/MowingRobot/obj_dec/README.md
View File

@ -1,15 +0,0 @@
使用方法：
```shell
# 进入工作目录
cd ~/obj_dec
# 编译
catkin_make
# 引入环境变量
source ./devel/setup.bash
# 启动ros核心节点
roscore
# 运行
rosrun rknn_yolov5_demo main
```

80
Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/CMakeLists.txt
View File

@ -1,80 +0,0 @@
cmake_minimum_required(VERSION 3.0.2)
project(rknn_yolov5_demo)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)
find_package(OpenCV REQUIRED)
find_package(Threads REQUIRED)
add_message_files(
FILES
dis_info.msg
dis_info_array.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS roscpp std_msgs message_runtime
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
/home/firefly/obj_dec/src/runtime/RK3588/Linux/librknn_api/include
/home/firefly/obj_dec/src/3rdparty/rga/RK3588/include
)
add_library(
head
include/rknn_yolov5_demo/detection.h
include/rknn_yolov5_demo/postprocess.h
include/rknn_yolov5_demo/preprocess.h
include/rknn_yolov5_demo/ranging.h
include/rknn_yolov5_demo/Timer.h
# include/rknn_yolov5_demo/pub_info.h
src/detection.cc
src/postprocess.cc
src/preprocess.cc
src/ranging.cc
# src/pub_info.cc
)
add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(head
${catkin_LIBRARIES}
)
add_executable(main src/main.cc)
add_executable(sub_dis src/sub_dis.cc)
add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(sub_dis
${catkin_LIBRARIES}
)
target_link_libraries(main
head
${catkin_LIBRARIES}
${OpenCV_LIBS}
Threads::Threads
/home/firefly/obj_dec/src/librknnrt.so
/home/firefly/obj_dec/src/librga.so
/home/firefly/obj_dec/src/libopencv_highgui.so
)

32
Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/pub_info.h
View File

@ -1,32 +0,0 @@
#ifndef PUB_INFO_H
#define PUB_INFO_H
#include <ros/ros.h>
#include <thread>
#include "rknn_yolov5_demo/dis_info.h"
#include "rknn_yolov5_demo/dis_info_array.h"
#include "rknn_yolov5_demo/ranging.h"
class Pub_info{
public:
Pub_info(){
dis_pub_ = nh_.advertise<rknn_yolov5_demo::dis_info_array>("ceju_info",10);
thread_1 = std::thread(&Pub_info::pub_dis,this);
// thread_2 = std::thread(&Pub_info::)
};
~Pub_info(){
thread_1.join();
}
void pub_dis();
public:
ros::NodeHandle nh_;
ros::Publisher dis_pub_;
rknn_yolov5_demo::dis_info_array dis_array_;
rknn_yolov5_demo::dis_info data;
std::thread thread_1;
// std::thread thread_2;
};
#endif

1
Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/msg/dis_info_array.msg
View File

@ -1 +0,0 @@
rknn_yolov5_demo/dis_info[] dis

28
Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/src/pub_info.cc
View File

@ -1,28 +0,0 @@
#include "rknn_yolov5_demo/pub_info.h"
void Pub_info::pub_dis()
{
Ranging ranging;
ros::Rate loop_rate(10);
while(ros::ok()){
std::vector<cv::Mat> result = ranging.get_range();
cv::Mat info = result[2];
for(int i=0;i<info.rows;i++)
{
data.distance = info.at<float>(i,0);
data.width = info.at<float>(i,1);
data.height = info.at<float>(i,2);
data.angle = info.at<float>(i,3);
dis_array_.dis.push_back(data);
}
dis_pub_.publish(dis_array_);
loop_rate.sleep();
}
}

0
Code/MowingRobot/PIBot_ROS/.gitignore → Code/MowingRobot/pibot_ros/.gitignore vendored
View File

0
Code/MowingRobot/PIBot_ROS/.pibot_init_env.sh.swp → Code/MowingRobot/pibot_ros/.pibot_init_env.sh.swp
View File

2
Code/MowingRobot/PIBot_ROS/README_BAK.md → Code/MowingRobot/pibot_ros/README.md
View File

@ -1,6 +1,6 @@
# PIBOT ROS Workspace v2.0
## 安装ROS
## install ros
```shell
cd ~/pibot_ros/
./pibot_install_ros.sh

BIN
Code/MowingRobot/pibot_ros/a.out Executable file
View File

Binary file not shown.

0
Code/MowingRobot/PIBot_ROS/pibot_add_upstart.sh → Code/MowingRobot/pibot_ros/pibot_add_upstart.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_ap.sh → Code/MowingRobot/pibot_ros/pibot_ap.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_clear_rosproc.sh → Code/MowingRobot/pibot_ros/pibot_clear_rosproc.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_create_swap.sh → Code/MowingRobot/pibot_ros/pibot_create_swap.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_init_env.sh → Code/MowingRobot/pibot_ros/pibot_init_env.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_install_ros.sh → Code/MowingRobot/pibot_ros/pibot_install_ros.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_remove_upstart.sh → Code/MowingRobot/pibot_ros/pibot_remove_upstart.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_upstart/pibot.service → Code/MowingRobot/pibot_ros/pibot_upstart/pibot.service
View File

0
Code/MowingRobot/PIBot_ROS/pibot_upstart/pibot_restart.sh → Code/MowingRobot/pibot_ros/pibot_upstart/pibot_restart.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_upstart/pibot_start.sh → Code/MowingRobot/pibot_ros/pibot_upstart/pibot_start.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_upstart/pibot_stop.sh → Code/MowingRobot/pibot_ros/pibot_upstart/pibot_stop.sh
View File

0
Code/MowingRobot/PIBot_ROS/pibot_upstart/pibotenv → Code/MowingRobot/pibot_ros/pibot_upstart/pibotenv
View File

0
Code/MowingRobot/PIBot_ROS/pibot_view_env.sh → Code/MowingRobot/pibot_ros/pibot_view_env.sh
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

0
Code/MowingRobot/PIBot_ROS/ros_package/abseil-cpp.tar.gz → Code/MowingRobot/pibot_ros/ros_package/abseil-cpp.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/astra.tar.gz → Code/MowingRobot/pibot_ros/ros_package/astra.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/cartographer.tar.gz → Code/MowingRobot/pibot_ros/ros_package/cartographer.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/cartographer_ros.tar.gz → Code/MowingRobot/pibot_ros/ros_package/cartographer_ros.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/ceres-solver.tar.gz → Code/MowingRobot/pibot_ros/ros_package/ceres-solver.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/frontier_exploration.tar.gz → Code/MowingRobot/pibot_ros/ros_package/frontier_exploration.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_package/protobuf.tar.gz → Code/MowingRobot/pibot_ros/ros_package/protobuf.tar.gz
View File

0
Code/MowingRobot/PIBot_ROS/ros_ws/.catkin_workspace → Code/MowingRobot/pibot_ros/ros_ws/.catkin_workspace
View File

0
Code/MowingRobot/PIBot_ROS/ros_ws/src/.gitignore → Code/MowingRobot/pibot_ros/ros_ws/src/.gitignore vendored
View File

60
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt Normal file
View File

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

83
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h Normal file
View File

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

85
Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/align.h → Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h
View File

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

29
Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h Normal file
View File

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

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