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

合并瑞瑞提交，将不同的功能包放在pibot下面

Code/MowingRobot/PIBot_ROS/README.md

@@ -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、全覆盖路径规划的使用

Code/MowingRobot/PIBot_ROS/ros_ws/src/README.md

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

Code/MowingRobot/PIBot_ROS/ros_ws/src/pibot_imu/src/pibot_imu_node.cpp

@@ -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;
-}

Code/MowingRobot/README.md

@@ -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行
+
 ```
 
-
-

Code/MowingRobot/Robot_ROS_APP/.catkin_workspace

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

Code/MowingRobot/Robot_ROS_APP/.gitignore

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

Code/MowingRobot/Robot_ROS_APP/README.md

@@ -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停止

Code/MowingRobot/Robot_ROS_APP/devel/.built_by

@@ -1 +0,0 @@
-catkin_make

Code/MowingRobot/Robot_ROS_APP/devel/.catkin

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

Code/MowingRobot/Robot_ROS_APP/devel/.rosinstall

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

Code/MowingRobot/Robot_ROS_APP/devel/_setup_util.py

@@ -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)

Code/MowingRobot/Robot_ROS_APP/devel/env.sh

@@ -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 "$@"

Code/MowingRobot/Robot_ROS_APP/devel/local_setup.bash

@@ -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

Code/MowingRobot/Robot_ROS_APP/devel/local_setup.sh

@@ -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

Code/MowingRobot/Robot_ROS_APP/devel/local_setup.zsh

@@ -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'

Code/MowingRobot/Robot_ROS_APP/devel/setup.bash

@@ -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"

Code/MowingRobot/Robot_ROS_APP/devel/setup.sh

@@ -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

Code/MowingRobot/Robot_ROS_APP/devel/setup.zsh

@@ -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"'

Code/MowingRobot/Robot_ROS_APP/src/.idea/.gitignore

@@ -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

Code/MowingRobot/Robot_ROS_APP/src/.idea/deployment.xml

@@ -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>

Code/MowingRobot/Robot_ROS_APP/src/.idea/misc.xml

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

Code/MowingRobot/Robot_ROS_APP/src/.idea/modules.xml

@@ -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>

Code/MowingRobot/Robot_ROS_APP/src/.idea/src.iml

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

Code/MowingRobot/Robot_ROS_APP/src/.idea/vcs.xml

@@ -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>

Code/MowingRobot/Robot_ROS_APP/src/CMakeLists.txt

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

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/mapping.h

@@ -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

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/senddata.h

@@ -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

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/include/uwb.h

@@ -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

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/align.cpp

@@ -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;
-    }
-};
-
-
-

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/main.cpp

@@ -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(); 
-}

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp

@@ -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
-

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp

@@ -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

Code/MowingRobot/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp

@@ -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;
-        }
-    }
-};
-
-

Code/MowingRobot/obj_dec/README.md

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

Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/CMakeLists.txt

@@ -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
-)
-
-
-
-

Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/include/rknn_yolov5_demo/pub_info.h

@@ -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

Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/msg/dis_info_array.msg

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

Code/MowingRobot/obj_dec/src/rknn_yolov5_demo/src/pub_info.cc

@@ -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();
-
-    }
- 
-    
-
-}
-

Code/MowingRobot/pibot_ros/README.md

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

Code/MowingRobot/pibot_ros/pypibot/pypibot/__init__.py

@@ -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

Code/MowingRobot/pibot_ros/pypibot/pypibot/assistant.py

@@ -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

Code/MowingRobot/pibot_ros/pypibot/pypibot/configer.py

@@ -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)

Code/MowingRobot/pibot_ros/pypibot/pypibot/daemon.py

@@ -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().
+        """

Code/MowingRobot/pibot_ros/pypibot/pypibot/err.py

@@ -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")
+

Code/MowingRobot/pibot_ros/pypibot/pypibot/log.py

@@ -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)

Code/MowingRobot/pibot_ros/pypibot/transport/dataholder.py

@@ -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(),
+            }
+

Code/MowingRobot/pibot_ros/pypibot/transport/main.py

@@ -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) 

Code/MowingRobot/pibot_ros/pypibot/transport/params.py

@@ -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)

Code/MowingRobot/pibot_ros/pypibot/transport/set_default_params.py

@@ -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)

Code/MowingRobot/pibot_ros/pypibot/transport/test_motors.py

@@ -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)

Code/MowingRobot/pibot_ros/pypibot/transport/transport.py

@@ -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")
+
+

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt

@@ -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})
+
+

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h

@@ -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
+
+
+
+
+
+

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h

@@ -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

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h

@@ -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