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/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,13 +1,15 @@
 #include <cmath>
 #include <ros/ros.h>
+#include <opencv2/opencv.hpp>
 #include <nav_msgs/Odometry.h>
 #include <utility>
 #include <queue>
 #include <fstream>
-#include "ros_merge_test/RawImu.h"
+#include "FollowingCar/RawImu.h"
 #include "type.h"
 #include "uwb.h"
-#include <opencv2/opencv.hpp>
+#include "lighthouse.h"
+#include "Mat.h"
 
 #ifndef ALIGN_H
 #define AlIGN_H
@@ -15,10 +17,12 @@ namespace uwb_slam{
     class Align
     {
     public:
-        Align(){};
+        Align(){
+
+        };
         void Run();
         void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
-        void imuCB(const ros_merge_test::RawImu& imu);
+        void imuCB(const FollowingCar::RawImu& imu);
         void odomCB(const nav_msgs::Odometry& odom);
 
     public:
@@ -26,16 +30,15 @@ namespace uwb_slam{
         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 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