diff --git a/sweeping_robot_3568/.catkin_workspace b/sweeping_robot_3568/.catkin_workspace new file mode 100644 index 0000000..52fd97e --- /dev/null +++ b/sweeping_robot_3568/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/sweeping_robot_3568/devel/.built_by b/sweeping_robot_3568/devel/.built_by new file mode 100644 index 0000000..2e212dd --- /dev/null +++ b/sweeping_robot_3568/devel/.built_by @@ -0,0 +1 @@ +catkin_make \ No newline at end of file diff --git a/sweeping_robot_3568/devel/.catkin b/sweeping_robot_3568/devel/.catkin new file mode 100644 index 0000000..f39dd55 --- /dev/null +++ b/sweeping_robot_3568/devel/.catkin @@ -0,0 +1 @@ +/home/firefly/sweeping_robot/src \ No newline at end of file diff --git a/sweeping_robot_3568/devel/.rosinstall b/sweeping_robot_3568/devel/.rosinstall new file mode 100644 index 0000000..9818254 --- /dev/null +++ b/sweeping_robot_3568/devel/.rosinstall @@ -0,0 +1,2 @@ +- setup-file: + local-name: /home/firefly/sweeping_robot/devel/setup.sh diff --git a/sweeping_robot_3568/devel/_setup_util.py b/sweeping_robot_3568/devel/_setup_util.py new file mode 100644 index 0000000..a3a0e19 --- /dev/null +++ b/sweeping_robot_3568/devel/_setup_util.py @@ -0,0 +1,304 @@ +#!/usr/bin/python2 +# -*- 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', 'aarch64-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', 'aarch64-linux-gnu')], + 'PATH': PATH_TO_ADD_SUFFIX, + 'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'aarch64-linux-gnu', 'pkgconfig')], + 'PYTHONPATH': 'lib/python2.7/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/firefly/pibot_ros/ros_ws/devel;/opt/ros/melodic'.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) diff --git a/sweeping_robot_3568/devel/cmake.lock b/sweeping_robot_3568/devel/cmake.lock new file mode 100644 index 0000000..e69de29 diff --git a/sweeping_robot_3568/devel/env.sh b/sweeping_robot_3568/devel/env.sh new file mode 100644 index 0000000..8aa9d24 --- /dev/null +++ b/sweeping_robot_3568/devel/env.sh @@ -0,0 +1,16 @@ +#!/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 "$@" diff --git a/sweeping_robot_3568/devel/lib/fusion_car/main b/sweeping_robot_3568/devel/lib/fusion_car/main new file mode 100644 index 0000000..f9d5320 Binary files /dev/null and b/sweeping_robot_3568/devel/lib/fusion_car/main differ diff --git a/sweeping_robot_3568/devel/lib/libhead.so b/sweeping_robot_3568/devel/lib/libhead.so new file mode 100644 index 0000000..e5637d2 Binary files /dev/null and b/sweeping_robot_3568/devel/lib/libhead.so differ diff --git a/sweeping_robot_3568/devel/lib/pkgconfig/fusion_car.pc b/sweeping_robot_3568/devel/lib/pkgconfig/fusion_car.pc new file mode 100644 index 0000000..3015765 --- /dev/null +++ b/sweeping_robot_3568/devel/lib/pkgconfig/fusion_car.pc @@ -0,0 +1,8 @@ +prefix=/home/firefly/sweeping_robot/devel + +Name: fusion_car +Description: Description of fusion_car +Version: 0.0.0 +Cflags: +Libs: -L${prefix}/lib +Requires: diff --git a/sweeping_robot_3568/devel/local_setup.bash b/sweeping_robot_3568/devel/local_setup.bash new file mode 100644 index 0000000..7da0d97 --- /dev/null +++ b/sweeping_robot_3568/devel/local_setup.bash @@ -0,0 +1,8 @@ +#!/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 diff --git a/sweeping_robot_3568/devel/local_setup.sh b/sweeping_robot_3568/devel/local_setup.sh new file mode 100644 index 0000000..789e21c --- /dev/null +++ b/sweeping_robot_3568/devel/local_setup.sh @@ -0,0 +1,9 @@ +#!/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/firefly/sweeping_robot/devel} +CATKIN_SETUP_UTIL_ARGS="--extend --local" +. "$_CATKIN_SETUP_DIR/setup.sh" +unset CATKIN_SETUP_UTIL_ARGS diff --git a/sweeping_robot_3568/devel/local_setup.zsh b/sweeping_robot_3568/devel/local_setup.zsh new file mode 100644 index 0000000..e692acc --- /dev/null +++ b/sweeping_robot_3568/devel/local_setup.zsh @@ -0,0 +1,8 @@ +#!/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' diff --git a/sweeping_robot_3568/devel/setup.bash b/sweeping_robot_3568/devel/setup.bash new file mode 100644 index 0000000..ff47af8 --- /dev/null +++ b/sweeping_robot_3568/devel/setup.bash @@ -0,0 +1,8 @@ +#!/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" diff --git a/sweeping_robot_3568/devel/setup.sh b/sweeping_robot_3568/devel/setup.sh new file mode 100644 index 0000000..597bbba --- /dev/null +++ b/sweeping_robot_3568/devel/setup.sh @@ -0,0 +1,96 @@ +#!/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/firefly/sweeping_robot/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 diff --git a/sweeping_robot_3568/devel/setup.zsh b/sweeping_robot_3568/devel/setup.zsh new file mode 100644 index 0000000..9f780b7 --- /dev/null +++ b/sweeping_robot_3568/devel/setup.zsh @@ -0,0 +1,8 @@ +#!/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"' diff --git a/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig-version.cmake b/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig-version.cmake new file mode 100644 index 0000000..7fd9f99 --- /dev/null +++ b/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig-version.cmake @@ -0,0 +1,14 @@ +# generated from catkin/cmake/template/pkgConfig-version.cmake.in +set(PACKAGE_VERSION "0.0.0") + +set(PACKAGE_VERSION_EXACT False) +set(PACKAGE_VERSION_COMPATIBLE False) + +if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") + set(PACKAGE_VERSION_EXACT True) + set(PACKAGE_VERSION_COMPATIBLE True) +endif() + +if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE True) +endif() diff --git a/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig.cmake b/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig.cmake new file mode 100644 index 0000000..bce34cc --- /dev/null +++ b/sweeping_robot_3568/devel/share/fusion_car/cmake/fusion_carConfig.cmake @@ -0,0 +1,223 @@ +# generated from catkin/cmake/template/pkgConfig.cmake.in + +# append elements to a list and remove existing duplicates from the list +# copied from catkin/cmake/list_append_deduplicate.cmake to keep pkgConfig +# self contained +macro(_list_append_deduplicate listname) + if(NOT "${ARGN}" STREQUAL "") + if(${listname}) + list(REMOVE_ITEM ${listname} ${ARGN}) + endif() + list(APPEND ${listname} ${ARGN}) + endif() +endmacro() + +# append elements to a list if they are not already in the list +# copied from catkin/cmake/list_append_unique.cmake to keep pkgConfig +# self contained +macro(_list_append_unique listname) + foreach(_item ${ARGN}) + list(FIND ${listname} ${_item} _index) + if(_index EQUAL -1) + list(APPEND ${listname} ${_item}) + endif() + endforeach() +endmacro() + +# pack a list of libraries with optional build configuration keywords +# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig +# self contained +macro(_pack_libraries_with_build_configuration VAR) + set(${VAR} "") + set(_argn ${ARGN}) + list(LENGTH _argn _count) + set(_index 0) + while(${_index} LESS ${_count}) + list(GET _argn ${_index} lib) + if("${lib}" MATCHES "^(debug|optimized|general)$") + math(EXPR _index "${_index} + 1") + if(${_index} EQUAL ${_count}) + message(FATAL_ERROR "_pack_libraries_with_build_configuration() the list of libraries '${ARGN}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") + endif() + list(GET _argn ${_index} library) + list(APPEND ${VAR} "${lib}${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}${library}") + else() + list(APPEND ${VAR} "${lib}") + endif() + math(EXPR _index "${_index} + 1") + endwhile() +endmacro() + +# unpack a list of libraries with optional build configuration keyword prefixes +# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig +# self contained +macro(_unpack_libraries_with_build_configuration VAR) + set(${VAR} "") + foreach(lib ${ARGN}) + string(REGEX REPLACE "^(debug|optimized|general)${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}(.+)$" "\\1;\\2" lib "${lib}") + list(APPEND ${VAR} "${lib}") + endforeach() +endmacro() + + +if(fusion_car_CONFIG_INCLUDED) + return() +endif() +set(fusion_car_CONFIG_INCLUDED TRUE) + +# set variables for source/devel/install prefixes +if("TRUE" STREQUAL "TRUE") + set(fusion_car_SOURCE_PREFIX /home/firefly/sweeping_robot/src/fusion_car) + set(fusion_car_DEVEL_PREFIX /home/firefly/sweeping_robot/devel) + set(fusion_car_INSTALL_PREFIX "") + set(fusion_car_PREFIX ${fusion_car_DEVEL_PREFIX}) +else() + set(fusion_car_SOURCE_PREFIX "") + set(fusion_car_DEVEL_PREFIX "") + set(fusion_car_INSTALL_PREFIX /home/firefly/sweeping_robot/install) + set(fusion_car_PREFIX ${fusion_car_INSTALL_PREFIX}) +endif() + +# warn when using a deprecated package +if(NOT "" STREQUAL "") + set(_msg "WARNING: package 'fusion_car' is deprecated") + # append custom deprecation text if available + if(NOT "" STREQUAL "TRUE") + set(_msg "${_msg} ()") + endif() + message("${_msg}") +endif() + +# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project +set(fusion_car_FOUND_CATKIN_PROJECT TRUE) + +if(NOT " " STREQUAL " ") + set(fusion_car_INCLUDE_DIRS "") + set(_include_dirs "") + if(NOT " " STREQUAL " ") + set(_report "Check the issue tracker '' and consider creating a ticket if the problem has not been reported yet.") + elseif(NOT " " STREQUAL " ") + set(_report "Check the website '' for information and consider reporting the problem.") + else() + set(_report "Report the problem to the maintainer 'firefly ' and request to fix the problem.") + endif() + foreach(idir ${_include_dirs}) + if(IS_ABSOLUTE ${idir} AND IS_DIRECTORY ${idir}) + set(include ${idir}) + elseif("${idir} " STREQUAL "include ") + get_filename_component(include "${fusion_car_DIR}/../../../include" ABSOLUTE) + if(NOT IS_DIRECTORY ${include}) + message(FATAL_ERROR "Project 'fusion_car' specifies '${idir}' as an include dir, which is not found. It does not exist in '${include}'. ${_report}") + endif() + else() + message(FATAL_ERROR "Project 'fusion_car' specifies '${idir}' as an include dir, which is not found. It does neither exist as an absolute directory nor in '/home/firefly/sweeping_robot/src/fusion_car/${idir}'. ${_report}") + endif() + _list_append_unique(fusion_car_INCLUDE_DIRS ${include}) + endforeach() +endif() + +set(libraries "") +foreach(library ${libraries}) + # keep build configuration keywords, target names and absolute libraries as-is + if("${library}" MATCHES "^(debug|optimized|general)$") + list(APPEND fusion_car_LIBRARIES ${library}) + elseif(${library} MATCHES "^-l") + list(APPEND fusion_car_LIBRARIES ${library}) + elseif(${library} MATCHES "^-") + # This is a linker flag/option (like -pthread) + # There's no standard variable for these, so create an interface library to hold it + if(NOT fusion_car_NUM_DUMMY_TARGETS) + set(fusion_car_NUM_DUMMY_TARGETS 0) + endif() + # Make sure the target name is unique + set(interface_target_name "catkin::fusion_car::wrapped-linker-option${fusion_car_NUM_DUMMY_TARGETS}") + while(TARGET "${interface_target_name}") + math(EXPR fusion_car_NUM_DUMMY_TARGETS "${fusion_car_NUM_DUMMY_TARGETS}+1") + set(interface_target_name "catkin::fusion_car::wrapped-linker-option${fusion_car_NUM_DUMMY_TARGETS}") + endwhile() + add_library("${interface_target_name}" INTERFACE IMPORTED) + if("${CMAKE_VERSION}" VERSION_LESS "3.13.0") + set_property( + TARGET + "${interface_target_name}" + APPEND PROPERTY + INTERFACE_LINK_LIBRARIES "${library}") + else() + target_link_options("${interface_target_name}" INTERFACE "${library}") + endif() + list(APPEND fusion_car_LIBRARIES "${interface_target_name}") + elseif(TARGET ${library}) + list(APPEND fusion_car_LIBRARIES ${library}) + elseif(IS_ABSOLUTE ${library}) + list(APPEND fusion_car_LIBRARIES ${library}) + else() + set(lib_path "") + set(lib "${library}-NOTFOUND") + # since the path where the library is found is returned we have to iterate over the paths manually + foreach(path /home/firefly/sweeping_robot/devel/lib;/home/firefly/pibot_ros/ros_ws/devel/lib;/opt/ros/melodic/lib) + find_library(lib ${library} + PATHS ${path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + if(lib) + set(lib_path ${path}) + break() + endif() + endforeach() + if(lib) + _list_append_unique(fusion_car_LIBRARY_DIRS ${lib_path}) + list(APPEND fusion_car_LIBRARIES ${lib}) + else() + # as a fall back for non-catkin libraries try to search globally + find_library(lib ${library}) + if(NOT lib) + message(FATAL_ERROR "Project '${PROJECT_NAME}' tried to find library '${library}'. The library is neither a target nor built/installed properly. Did you compile project 'fusion_car'? Did you find_package() it before the subdirectory containing its code is included?") + endif() + list(APPEND fusion_car_LIBRARIES ${lib}) + endif() + endif() +endforeach() + +set(fusion_car_EXPORTED_TARGETS "") +# create dummy targets for exported code generation targets to make life of users easier +foreach(t ${fusion_car_EXPORTED_TARGETS}) + if(NOT TARGET ${t}) + add_custom_target(${t}) + endif() +endforeach() + +set(depends "") +foreach(depend ${depends}) + string(REPLACE " " ";" depend_list ${depend}) + # the package name of the dependency must be kept in a unique variable so that it is not overwritten in recursive calls + list(GET depend_list 0 fusion_car_dep) + list(LENGTH depend_list count) + if(${count} EQUAL 1) + # simple dependencies must only be find_package()-ed once + if(NOT ${fusion_car_dep}_FOUND) + find_package(${fusion_car_dep} REQUIRED NO_MODULE) + endif() + else() + # dependencies with components must be find_package()-ed again + list(REMOVE_AT depend_list 0) + find_package(${fusion_car_dep} REQUIRED NO_MODULE ${depend_list}) + endif() + _list_append_unique(fusion_car_INCLUDE_DIRS ${${fusion_car_dep}_INCLUDE_DIRS}) + + # merge build configuration keywords with library names to correctly deduplicate + _pack_libraries_with_build_configuration(fusion_car_LIBRARIES ${fusion_car_LIBRARIES}) + _pack_libraries_with_build_configuration(_libraries ${${fusion_car_dep}_LIBRARIES}) + _list_append_deduplicate(fusion_car_LIBRARIES ${_libraries}) + # undo build configuration keyword merging after deduplication + _unpack_libraries_with_build_configuration(fusion_car_LIBRARIES ${fusion_car_LIBRARIES}) + + _list_append_unique(fusion_car_LIBRARY_DIRS ${${fusion_car_dep}_LIBRARY_DIRS}) + list(APPEND fusion_car_EXPORTED_TARGETS ${${fusion_car_dep}_EXPORTED_TARGETS}) +endforeach() + +set(pkg_cfg_extras "") +foreach(extra ${pkg_cfg_extras}) + if(NOT IS_ABSOLUTE ${extra}) + set(extra ${fusion_car_DIR}/${extra}) + endif() + include(${extra}) +endforeach() diff --git a/sweeping_robot_3568/src/fusion_car/CMakeLists.txt b/sweeping_robot_3568/src/fusion_car/CMakeLists.txt new file mode 100644 index 0000000..14acc19 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.0.2) +project(fusion_car) + + +find_package(catkin REQUIRED COMPONENTS + rosconsole + roscpp + roscpp_serialization + roslib + rospy + rostime +) +find_package(OpenCV REQUIRED) +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES dynamic_connect +# CATKIN_DEPENDS roscpp rosmsg rospy +# DEPENDS system_lib +) + + + +include_directories( + include + /usr/local/include + /usr/local/include/opencv4 + /usr/include + /opt/ros/melodic/include + /home/firefly/Desktop/object_detect_sdk_1804/install/include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +add_library(head +include/fusion_car/fusion.h +include/fusion_car/lidar.h +include/fusion_car/ranging.h +include/fusion_car/rknn_sdk.h +src/fusion.cpp +src/lidar.cpp +src/ranging.cpp +) +add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(head + ${catkin_LIBRARIES} +) +add_executable(main src/main.cpp) + +add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(main + head + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + boost_system + pthread + udev + /home/firefly/sweeping_robot/src/librknn_sdk.so + /home/firefly/sweeping_robot/src/librknnrt.so +) + + diff --git a/sweeping_robot_3568/src/fusion_car/include/fusion_car/fusion.h b/sweeping_robot_3568/src/fusion_car/include/fusion_car/fusion.h new file mode 100644 index 0000000..f3af461 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/include/fusion_car/fusion.h @@ -0,0 +1,100 @@ +#pragma once +#include +#include +#include +#include +#include "lidar.h" +#include "ranging.h" + +using namespace cv; +using namespace std; + +struct ImgScan +{ + float angleMin; //弧度 + float angleMax; + float angleInc; + std::vector distance; // 米 + std::vector intensities; + Mat pointCloud; + std::vector> clustedCloud; +}; +class Ranging; //友元类 +class LCFusion +{ +public: + LCFusion(Ranging* ranging) + { + r = ranging; + rotate = cameraExtrinsicMat.rowRange(0, 3).colRange(0, 3).t(); // 雷达 -> 相机坐标系旋转矩阵 + translation = cameraExtrinsicMat.col(3).rowRange(0, 3); + translation = -rotate * translation; // 雷达 -> 相机坐标系平移矩阵 + p2r = r_y * r_c; // 像素坐标系矫正矩阵（旋转） + }; + char fusion(std::vector &camInfo); // fusion函数 + uchar lidar2box(Mat uv, Mat boxes, uchar *boxFlag); // 无用 + void filter_laser(const FrameData &lidarInfo, float angle_min, float angle_inc); // 雷达点和相机视野同步 + void scan_to_pointcloud(); // 雷达极坐标到二维坐标转换 + std::vector clusters_bboxs(Mat &bboxs); // 所有的聚类的雷达簇和所有的检测框的匹配 + int cluster_bboxs(std::vector &one_cluster, Mat &bboxs); // 寻找一簇雷达簇匹配的检测框 + void optimize_clusterlable(vector & clusterlable, Mat & bboxs); // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇 + void choose_forceground(vector & cluster_num, Mat & bbox, vector & clusterlable); // 针对镂空物体选择对应的雷达簇，筛选过程 + void big_fusion(vector & clusterlable, Mat & bboxs, Mat &dwha); // 大物体赋语义 + void small_fusion(Mat & bboxs, Mat & dwha); // 小物体赋语义 + Mat pointcloud_pixel(Mat &pointcloud); // 二维雷达点到像素坐标系转换 + void cluster(); // 聚类 + float ratio_in_1box(Mat &begin, Mat &end, Mat &box); // 计算一簇雷达落入检测框的比例 + void draw_point(std::vector &camInfo); // 画出雷达在图像中的映射点 + void draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha); // 扩散物体生成圆弧状伪激光雷达 + vector find_circle_center(int cluster_num); // 寻找扩散物体圆弧的圆心 + float circle(float d, float alfa ,float theta, float data); // 圆弧角度和距离的数学关系 + vector width_ladar(const int* coordinates); + void small_box_without_dis(Mat & bboxs, Mat & dwha); // 检测框到雷达坐标系的映射关系 + void set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData); // 设置雷达发布数据（原始雷达） + void set_laser_data(sensor_msgs::LaserScan& scan); // 设置雷达发布数据（语义雷达） + + ImgScan upScan; + // 调试使用，将雷达点映射到图像中显示 + vector projectedPoints; // 雷达 -> 像素坐标系映射的点 + std::vector clusterIdx; + +private: + Ranging* r; + //float cameraAnglemin = 1.117, cameraAnglemax = 2.0071; + float cameraAnglemin = 1.345, cameraAnglemax = 2.0595; // 相机视野角度 + Mat rotate, translation, p2r; // 雷达点映射矫正 + Mat cameraExtrinsicMat = (Mat_(4, 4) << 9.9710325100937192e-01, 5.9677185357037893e-02, + -4.7156551765403301e-02, 2.6591864512557874e-02, + 4.7136795335845721e-02, 1.7395474156941537e-03, + 9.9888692878636431e-01, 7.4034983851991365e-02, + 5.9692791457662736e-02, -9.9821621281296091e-01, + -1.0784828889205400e-03, -9.6176066499866750e-02, 0., 0., 0., 1.); + + Mat cameraMat = (Mat_(3, 3) << 5.0280958247840368e+02, 0., 3.1925413622163182e+02, 0., + 5.0206199606708202e+02, 2.1999125419411467e+02, 0., 0., 1.); + + Mat distCoeff = (Mat_(1, 5) << 1.2903666948900971e-01, -9.2734018203156590e-02, + -7.0528508350750380e-03, 8.3183285124261049e-04, + 7.2727934388070986e-02); + + // Mat c2lR = (Mat_(2, 2) << 0.99664806, 0.0802792, -0.0802792, 0.99664806); + Mat c2lR = (Mat_(2, 2) << 0.98163, 0.19081, -0.19081, 0.98163); //11 + //Mat c2lR = (Mat_(2, 2) << 0.9703, 0.24192, -0.24192, 0.9703); //13 + Mat c2lT = (Mat_(2, 1) << 0.15, 0.01); + //Mat c2lT = (Mat_(2, 1) << 0.13693697, 0.05094143); + Mat r_c = (Mat_(3, 3) << 9.977e-1, 6.7744e-02, 0., -6.7744e-02, 9.977e-1, 0., 0., 0., 1.); + Mat t = (Mat_(3, 1) << 50., 0., 0.); + Mat r_y = (Mat_(3, 3) << 1., 0., 0., 0., 1.19444e+00, 0., 0., 0., 1.); + /*int class_intensity[24] = { + 0, 120, 130, 130, 130, 130, 130, 130, 146, 146, + 146, 162, 162, 178, 178, 196, 196, 212, 212, 228, + 228, 0, 0, 0}; */ + int class_intensity[25] = { + 0, 115, 116, 117, 118, 119, 120, 121, 150, 151, + 152, 153, 154, 155, 156, 200, 201, 202, 203, 204, + 205, 206, 207, 208, 209}; // 物体类别和强度映射关系 + + map class_index = {{-1, 23}, {0, 0}, {1, 13}, {2, 14}, {3, 7}, {4, 8}, {5, 9}, {6, 1}, + {7, 2}, {8, 19}, {9, 18}, {10, 6}, {11, 3}, {12, 15}, {13, 10}, {14, 4}, {15, 5}, {16, 11}, {17, 16}, + {18, 12}, {19, 17}, {20, 20}, {21, 21}, {22, 22}}; +}; // 类别映射，方便大小物体判断 diff --git a/sweeping_robot_3568/src/fusion_car/include/fusion_car/lidar.h b/sweeping_robot_3568/src/fusion_car/include/fusion_car/lidar.h new file mode 100644 index 0000000..49b1d25 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/include/fusion_car/lidar.h @@ -0,0 +1,174 @@ +/** +* @file lidar.h +* @author LD Robot +* @version V01 + +* @brief +* @note +* @attention COPYRIGHT LDROBOT +**/ + +#ifndef __LIPKG_H +#define __LIPKG_H +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct PointData +{ + //�������ʾ��ʽ + float angle; + uint16_t distance; + uint8_t confidence; + //ֱ�������ʾ��ʽ + double x; + double y; + PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0) + { + this->angle = angle; + this->distance = distance; + this->confidence = confidence; + this->x = x; + this->y = y; + } + PointData() {} + friend std::ostream& operator<<(std::ostream& os, const PointData& data) + { + os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y; + return os; + } +}; +typedef std::vector Points2D; + + +enum +{ + PKG_HEADER = 0x54, + PKG_VER_LEN = 0x2C, + POINT_PER_PACK = 12, +}; +typedef struct __attribute__((packed)) +{ + uint16_t distance; + uint8_t confidence; +}LidarPointStructDef; +typedef struct __attribute__((packed)) +{ + uint8_t header; + uint8_t ver_len; + uint16_t speed; + uint16_t start_angle; + LidarPointStructDef point[POINT_PER_PACK]; + uint16_t end_angle; + uint16_t timestamp; + uint8_t crc8; +}LiDARFrameTypeDef; +struct FrameData +{ + ros::Time timestamp; + float angle_min; + float angle_max; + uint32_t len; + std::vector distance; + std::vector intensities; +}; +class LiPkg +{ +public: + LiPkg(); + double GetSpeed(void); /*Lidar spin speed (Hz)*/ + uint16_t GetTimestamp(void) { return mTimestamp; } /*time stamp of the packet */ + bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */ + bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/ + long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/ + const std::array& GetPkgData(void);/*original data package*/ + bool Parse(const uint8_t* data, long len);/*parse single packet*/ + virtual void Transform(std::vector& tmp) = 0;/*transform raw data to stantard data */ + bool AssemblePacket();/*combine stantard data into data frames and calibrate*/ + const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; } + std::vector scan_to_pointcloud(FrameData mFrameData); +private: + uint16_t mTimestamp; + double mSpeed; + std::vector mDataTmp; + long mErrorTimes; + std::arraymOnePkg; + std::vector mFrameTemp; + FrameData mFrameData; + bool mIsPkgReady; + bool mIsFrameReady; +}; +class LD14_LiPkg : public LiPkg +{ +public: + virtual void Transform(std::vector& tmp); +}; + +enum class LDVersion +{ + LD_ZERO, /*Zero generation lidar*/ + LD_THREE, /*Third generation lidar*/ + LD_EIGHT, /*Eight generation radar*/ + LD_NINE, /*Nine generation radar*/ + LD_FOURTEENTH /*Fourteenth generation radar*/ +}; + +class SlTransform +{ +private: + bool to_right_hand = true; + double offset_x; + double offset_y; + +public: + SlTransform(LDVersion version, bool to_right_hand = false); + Points2D Transform(const Points2D& data); + ~SlTransform(); +}; + + + +class CmdInterfaceLinux +{ +public: + CmdInterfaceLinux(int32_t ver); + ~CmdInterfaceLinux(); + + bool Open(std::string& port_name); + bool Close(); + bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len); + bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len); + bool GetCmdDevices(std::vector >& device_list); + void SetReadCallback(std::function callback) { mReadCallback = callback; } + bool IsOpened() { return mIsCmdOpened.load(); }; + bool IsExited() { return mRxThreadExitFlag.load(); }; + +private: + std::thread* mRxThread; + static void mRxThreadProc(void* param); + long long mRxCount; + int32_t version; + int32_t mComHandle; + std::atomic mIsCmdOpened, mRxThreadExitFlag; + std::function mReadCallback; +}; + +#endif +/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ diff --git a/sweeping_robot_3568/src/fusion_car/include/fusion_car/ranging.h b/sweeping_robot_3568/src/fusion_car/include/fusion_car/ranging.h new file mode 100644 index 0000000..2046554 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/include/fusion_car/ranging.h @@ -0,0 +1,112 @@ +#pragma once + +#include +#include + +#include +#include +#include "rknn_sdk.h" +#include "fusion.h" + +using namespace cv; +class LCFusion; +class Ranging +{ +private: + friend class LCFusion; + VideoCapture vcapture; + rknn_handle hdx; + const char *modelPath = "/home/firefly/sweeping_robot/src/fusion_car/m28.rknn"; + std::vector className = { + "Unknown", //未知 + "MaoGouWan", //猫碗/狗碗 + "BTYDiZuo", //吧台怅底座 + "FSDiZuo", //风扇底座 + "XiYiJi", //掗衣机 + "BingXiang", //冰箱 + "MaTong", //马桶 + "TiZhongCheng", //䜓重秀 + "DianXian", //电线 + "DianShiJi", //电视机 + "CanZhuo", //逐桌 + "DiTan", //地毯 + "MaBu", //抹垃 + "ChaJi", //茶几 + "DianShiGu", //电视柜 + "XieZi", //拖鞋/鞋子 + "WaZi", //袜子 + "YiGui", //衣柜 + "Chuang", //床 + "ShaFa", //沙发 + "YiZi", //怅子 + "Men", //闹 + "DLS", //倧理石 + "DiBan", //地板 + }; + + // 双目参数 + Mat cam_matrix_left = (Mat_(3, 3) << + 4.700950170847520e+02, 0, 0, + 0, 4.709244004725060e+02, 0, + 3.220628601465352e+02, 2.265265667516499e+02, 1); + + Mat cam_matrix_right = (Mat_(3, 3) << + 4.696016526986375e+02, 0, 0, + 0, 4.702371464881610e+02, 0, + 3.216994731643572e+02, 2.267449827655198e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.088023418049400, -0.047968406803285, 0, + 0, 0); + + Mat distortion_r = (Mat_(1, 5) << 0.097018738959281, -0.081707209740704, 0, + 0, 0); + + Mat rotate = (Mat_(3, 3) << + 0.999895538560624, 1.514179660491009e-04, 0.014452994124399, + -1.935031776135467e-04, 0.999995745723263, 0.002910514026079, + -0.014452491933249, -0.002913006689884, 0.999891314028152); + Mat trans = (Mat_(3, 1) << + -59.978995009996730, 0.050886852679934, -0.088565378305175); +/* + + Mat cam_matrix_left = (Mat_(3, 3) << + 3.627346593660044e+02, 0, 0, + 0, 3.629168542317134e+02, 0, + 3.093231127433517e+02, 2.409353074183058e+02, 1); + + Mat cam_matrix_right = (Mat_(3, 3) << + 3.615909129180773e+02, 0, 0, + 0, 3.617772267770084e+02, 0, + 3.062363746645480e+02, 2.298948842159400e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.117219440226075, -0.148594691012994, 0, + 0, 0); + Mat distortion_r = (Mat_(1, 5) << 0.116412295415583, -0.146817143572705, 0, + 0, 0); + Mat rotate = (Mat_(3, 3) << + 0.999962353120719, 4.648106081008926e-04, 0.008664657660520, + -4.493098874759400e-04, 0.999998295540464, -0.001790820145135, + -0.008665475284162, 0.001786859609987, 0.999960857569352); + Mat trans = (Mat_(3, 1) << + -60.097178758944190, -0.033859553542635, -0.423965574285253); +*/ +/* + Mat cam_matrix_left = (Mat_(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1); + Mat distortion_l = (Mat_(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682); + Mat distortion_r = (Mat_(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122); + Mat rotate = (Mat_(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437); + Mat trans = (Mat_(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760); +*/ + + Mat mapX1, mapX2, mapY1, mapY2, q, Z; + + int imgw, imgh; + +public: + Ranging(int index, int imgw, int imgh); + void rectifyImage(Mat &oriImgL, Mat &oriImgR); + void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info); + std::vector pic2cam(int u, int v); + std::vector muban(Mat &left_image, Mat &right_image, const int *coordinates); + std::vector get_range(); + void horizon_estimate(Mat& img, Mat& bboxs,int k); +}; diff --git a/sweeping_robot_3568/src/fusion_car/include/fusion_car/rknn_sdk.h b/sweeping_robot_3568/src/fusion_car/include/fusion_car/rknn_sdk.h new file mode 100644 index 0000000..cda1b0f --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/include/fusion_car/rknn_sdk.h @@ -0,0 +1,110 @@ +#ifndef __RKNN_SDK_H__ +#define __RKNN_SDK_H__ + +#include + +// 返回错误码 +enum XErrorCode { + RKNN_SUCCEED = 0, + RKNN_NOT_INIT = -1, //SDK未初始化 + RKNN_INVAILD_IMGSIZE = -2, //输入图像尺寸不匹配模型输入要求 + RKNN_INVAILD_PARAM = -3, //参数无效 +}; + +enum ObjCls { + OBJ_Unknown = -1, //未知 + OBJ_MaoGouWan = 0, //猫碗/狗碗 + OBJ_BTYDiZuo = 1, //吧台椅底座 + OBJ_FSDiZuo = 2, //风扇底座 + OBJ_XiYiJi = 3, //洗衣机 + OBJ_BingXiang = 4, //冰箱 + OBJ_MaTong = 5, //马桶 + OBJ_TiZhongCheng = 6, //体重秤 + OBJ_DianXian = 7, //电线 + OBJ_DianShiJi = 8, //电视机 + OBJ_CanZhuo = 9, //餐桌 + OBJ_DiTan = 10, //地毯 + OBJ_MaBu = 11, //抹布 + OBJ_ChaJi = 12, //茶几 + OBJ_DianShiGui = 13, //电视柜 + OBJ_XieZi = 14, //拖鞋/鞋子 + OBJ_WaZi = 15, //袜子 + OBJ_YiGui = 16, //衣柜 + OBJ_Chuang = 17, //床 + OBJ_ShaFa = 18, //沙发 + OBJ_YiZi = 19, //椅子 + OBJ_Men = 20, //门 + OBJ_DLS = 21, //da li shi + OBJ_DiBan = 22 //di ban + //OBJ_Xiang = 21 //宠物粪便 +}; + + +// 图像信息与buffer +typedef struct _InputData { + int shape[4]; // Num, Channel, Height, Width + void* data; // img.data +} InputData; + +// 检测结果 +typedef struct _DetRet +{ + int nID; //图像ID + int nLabel; //物体类别 ObjCls nLabel; //物体类别 + float fConf; //置信度 + float fLeft; //检测框左上角比例坐标x_min -> int(x_min*width) 获取像素坐标 + float fTop; //检测框左上角比例坐标y_min -> int(y_min*height) 获取像素坐标 + float fRight; //检测框右下角比例坐标x_max -> int(x_max*width) 获取像素坐标 + float fBottom; //检测框右下角比例坐标y_max -> int(y_max*height) 获取像素坐标 +} DetRet; + + + +#define RKNNSDK_API(retType) retType + +/// +#ifdef __cplusplus +extern "C" { +#endif + +typedef uint64_t rknn_handle; + + +/* RKNN_Create + SDK加载模型，仅初始化一次; + input: + rknn_handle hd 句柄 + const char* model_path 模型路径 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_Create(rknn_handle* hd, const char* model_path); + +/* RKNN_ObjDet + 输入图像，获取检测结果和检测目标数量; + input: + rknn_handle hd 句柄 + const InputData* input_data 输入图像数据 + DetRet** det_ret 获取检测结果 + int* det_num 获取检测目标数量 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_ObjDet(rknn_handle hd, const InputData* input_data, DetRet** det_ret, int* det_num); + +/* RKNN_Release + SDK资源释放，仅在程序退出前调用; + input: + rknn_handle hd 句柄 + return: + int 错误码 +*/ +RKNNSDK_API(int) RKNN_Release(rknn_handle hd); + +#ifdef __cplusplus +} +#endif + + + +#endif //__RKNN_SDK_H__ diff --git a/sweeping_robot_3568/src/fusion_car/m28.rknn b/sweeping_robot_3568/src/fusion_car/m28.rknn new file mode 100644 index 0000000..fb3a6d4 Binary files /dev/null and b/sweeping_robot_3568/src/fusion_car/m28.rknn differ diff --git a/sweeping_robot_3568/src/fusion_car/package.xml b/sweeping_robot_3568/src/fusion_car/package.xml new file mode 100644 index 0000000..df77961 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/package.xml @@ -0,0 +1,74 @@ + + + fusion_car + 0.0.0 + The fusion_car package + + + + + firefly + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rosconsole + roscpp + roscpp_serialization + roslib + rospy + rosconsole + roscpp + roscpp_serialization + roslib + rospy + rosconsole + roscpp + roscpp_serialization + roslib + rospy + + + + + + + + diff --git a/sweeping_robot_3568/src/fusion_car/src/fusion.cpp b/sweeping_robot_3568/src/fusion_car/src/fusion.cpp new file mode 100644 index 0000000..ce3a3cd --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/src/fusion.cpp @@ -0,0 +1,685 @@ +#include +#include "fusion_car/fusion.h" +// 角度 -> 弧度转换 +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) +// fusion函数 +char LCFusion::fusion(std::vector &camInfo) +{ + projectedPoints.clear(); + if (camInfo.size() < 3) // 判断返回的双目测距信息是否为空，正常为3 + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + Mat boxes = camInfo[1], dwha = camInfo[2]; + projectedPoints.clear(); + + if (this->upScan.pointCloud.empty()) + { + std::cout << "There is no lidar data!" << std::endl; + return -2; + } + + if (boxes.empty()) + { + std::cout << "There is no detection box!" << std::endl; + return -1; + } + else + { + draw_point(camInfo); // 画出雷达映射到图像中的点 + std::cout << "Fusion start!" << std::endl; + // 大物体 + vector clusterlable = clusters_bboxs(boxes); // 得到每簇雷达映射的检测框标签（检测框索引值） + optimize_clusterlable(clusterlable, boxes); // 优化检测框（针对镂空物体） + big_fusion(clusterlable, boxes, dwha); // 大物体赋语义 + + //小物体 + small_box_without_dis(boxes, dwha); // 无距离信息的检测框映射 + small_fusion(boxes, dwha); // 小物体赋语义 + } + return 1; +} + +void LCFusion::big_fusion(vector &clusterlable, Mat &bboxs, Mat &dwha) // 大物体赋语义 +{ + std::cout << "big_fusion" << std::endl; + for (int i = 0; i < clusterlable.size(); i++) // 遍历每簇雷达 + { + int bbox_num = clusterlable[i]; + + if (bbox_num != -1) // -1代表无检测框与之对应 + { + int bbox_cls = bboxs.at(bbox_num, 5); + if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) // 排除扩散物体（风扇底座、吧台椅） + { + //std::cout << "bboxcls:" << bbox_cls << std::endl; + for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) // 遍历一簇雷达的所有点赋语义 + { + int laser_index = this->upScan.clustedCloud[i][j].at(0, 3); + this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1]; + } + } + else + { + if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) // 扩散物体雷达点应该很少 + { + //cout << "******start draw circle******" << endl; + double d = dwha.at(bbox_num, 0); + if (d > 0) + { + draw_circle(i, bbox_num, bboxs, dwha); + } // 扩散物体赋语义 + } + + } + } + } +} +void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) // 小物体u赋语义 +{ + std::cout << "small_fusion" << std::endl; + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + float x_mim = bboxs.at(i, 0); + float y_min = bboxs.at(i, 1); + float x_max = bboxs.at(i, 2); + float y_max = bboxs.at(i, 3); + std::cout << "cls:" << c << " xmin:" << x_mim << " ymin:" << y_min << " xmax:" << x_max << " ymax:" << y_max << std::endl; + if (class_index[c] == 0 || class_index[c] == 2 || (class_index[c] == 4 || class_index[c] == 5)) // 判断小物体u + { + + Mat temp(2, 1, CV_32F); + double d = dwha.at(i, 0); + double a = dwha.at(i, 3); // 取距离和角度 + if (d == -1) + continue; + //std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float((dwha.at(i, 0) / 100.0) * cos(dwha.at(i, 3))); + temp.at(1, 0) = float((dwha.at(i, 0) / 100.0) * sin(dwha.at(i, 3))); + temp = c2lR * temp + c2lT; // 相机坐标系-> 雷达坐标系转换 + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); // 转极坐标 + + float angle = 0.0; // 得到雷达点索引 + if (y >= 0) + angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + double data_angle = atan((dwha.at(i, 1)) / (200 * dis)); + data_angle /= this->upScan.angleInc; // 由测距宽度信息得到需更改的雷达点数的一半 + angle = (int)angle - (int)data_angle; // 起始雷达索引 + int num_point = 2 * (int)data_angle + 1; // 更改的雷达点数 + + int len = this->upScan.distance.size(); + for (int i = 0; i < num_point; i++) + { + if ((angle + i) < 0 || (angle + i) > (len - 1)) // 防止越界 + continue; + + this->upScan.distance[angle + i] = dis; + this->upScan.intensities[angle + i] = class_intensity[c + 1]; + } + } + else if (class_index[c] == 1 || class_index[c] == 3 ||class_index[c] == 6) + { + int len = this->upScan.distance.size(); + Mat z = r->Z.clone(); + if (countNonZero(z) < 1) + { + return; + } + if (z.cols >= 2) + { + int begin_index = 0; + for(int j = z.cols-1; j >=0; j-- ) + { + int d = z.at(0, j); + if (d != 0) + { + begin_index = j; + break; + } + } + cout << "tizhongcheng " << z.cols << endl; + float d_begin = z.at(0, begin_index)/1000.; + float a_begin = z.at(1, begin_index); + cout << "d_begin: " << d_begin << " a_begin: " << a_begin << endl; + Mat temp(2, 1, CV_32F); + temp.at(0, 0) = float(d_begin * cos(a_begin)); + temp.at(1, 0) = float(d_begin * sin(a_begin)); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + float angle_index = 0.0; // 得到雷达点索引 + if (y >= 0) + angle_index = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_index = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + angle_index = (int)angle_index; + if (angle_index >= 0 && angle_index upScan.distance[angle_index] = dis; + this->upScan.intensities[angle_index] = class_intensity[c + 1]; + } + cout << "dis: " << dis << " angle_index: " << angle_index << endl; + angle_index += 1; + + for (int i = begin_index-1; i >= 0; i--) + { + float temp_d = z.at(0, i)/1000.; + float temp_a = z.at(1, i); + temp.at(0, 0) = float(temp_d * cos(temp_a)); + temp.at(1, 0) = float(temp_d * sin(temp_a)); + temp = c2lR * temp + c2lT; + y = temp.at(0, 0); + x = temp.at(1, 0); + dis = sqrt(x * x + y * y); + float temp_angle = 0.0; // 得到雷达点索引 + if (y >= 0) + temp_angle = acos(x / dis); + else + temp_angle =2 * M_PI - acos(x / dis); + float angle_now = this->upScan.angleMin + this->upScan.angleInc*angle_index; + if (temp_angle >= angle_now) + { + if (angle_index >= 0 && angle_index < len) // 防止越界 + { + this->upScan.distance[angle_index] = dis; + // cout << "dis: " << dis << " angle_index: " << angle_index << endl; + this->upScan.intensities[angle_index] = class_intensity[c + 1]; + } + angle_index += 1; + } + } + r->Z = Mat::zeros(2, 1, CV_32FC1); + } + } + } +} + +void LCFusion::optimize_clusterlable(vector &clusterlable, Mat &bboxs) // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇 +{ + for (int i = 0; i < bboxs.rows; i++) // 寻找每个框对应的所有雷达簇 + { + vector temp_cluster_num; + Mat temp_bbox = bboxs.row(i).clone(); + for (int j = 0; j < clusterlable.size(); j++) + { + if (clusterlable[j] == i) + { + temp_cluster_num.push_back(j); + } + } + if (temp_cluster_num.size() == 0) + continue; + choose_forceground(temp_cluster_num, temp_bbox, clusterlable); // 筛选镂空物体的前景 + } +} + +void LCFusion::choose_forceground(vector &cluster_num, Mat &bbox, vector &clusterlable) // 筛选镂空物体的前景 +{ + int box_class = bbox.at(0, 5); + if ((class_index[box_class] >= 15 && class_index[box_class] <= 18 || class_index[box_class] == 12 || class_index[box_class] == 10) && cluster_num.size() > 1) + { + cout << "class " << box_class << " have " << cluster_num.size() << " clusters" << endl; + int total_point = 0; + for (int i = 0; i < cluster_num.size(); i++) // 计算框内雷达点总数 + { + total_point += (this->upScan.clustedCloud[cluster_num[i]].size()); + } + for (int j = 0; j < cluster_num.size(); j++) // 计算每个雷达簇占比，大于0.2认为是背景 + { + float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size(); + if ((num_cluster / total_point > 0.2) || num_cluster > 8) + { + clusterlable[cluster_num[j]] = -1; + } + } + } +} + +void LCFusion::draw_point(std::vector &camInfo) // 画出映射到图像的雷达点 +{ + projectedPoints.clear(); + for (int i = 0; i < this->upScan.pointCloud.rows; i++) + { + Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone(); + Mat uv = pointcloud_pixel(temp_pointcloud); + int x = uv.at(0, 0); + int y = uv.at(1, 0); + if (x > 0 && x < 640 && y > 0 && y < 480) + { + Point2d pt_uv(x, y); + projectedPoints.push_back(pt_uv); + } + } + +} + +// void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) +// { +// upScan.distance.clear(); +// upScan.intensities.clear(); +// float pointAngle; +// upScan.angleInc = angleInc; + +// for (int i = 0; i < laserData.len; i++) +// { +// pointAngle = angle_min + i * upScan.angleInc; +// if (pointAngle > cameraAnglemin) +// { +// if (pointAngle > cameraAnglemax) +// { +// upScan.angleMax = pointAngle - upScan.angleInc; +// break; +// } +// upScan.distance.push_back(laserData.distance[i] / 1000.f); +// upScan.intensities.push_back(0); +// if (i == (laserData.len - 1)) +// upScan.angleMax = pointAngle; +// // upScan.intensities.push_back(laserData.intensities[i]); +// } +// } +// upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc; +// this->scan_to_pointcloud(); +// this->cluster(); +// } +void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) // 雷达点和相机视野同步 +{ + upScan.distance.clear(); + upScan.intensities.clear(); + upScan.angleMin = angle_min; + upScan.angleInc = angleInc; + upScan.angleMax = ANGLE_TO_RADIAN(laserData.angle_max); + float pointAngle; + for (int i = 0; i < laserData.len; i++) + { + upScan.distance.push_back(laserData.distance[i] / 1000.f); + pointAngle = angle_min + i * upScan.angleInc; + if (pointAngle > cameraAnglemin && pointAngle < cameraAnglemax) + upScan.intensities.push_back(1); // 视野范围内强度赋1 + else + upScan.intensities.push_back(0); // 视野范围外强度赋0 + } + + this->scan_to_pointcloud(); // 雷达极坐标到二维坐标转换 + this->cluster(); // 雷达聚类 +} + +void LCFusion::scan_to_pointcloud() // 雷达极坐标到二维坐标转换 +{ + u_int32_t len = upScan.distance.size(); + Mat pointcloud(len, 4, CV_32F); + + for (int i = 0; i < len; i++) + { + float dis = this->upScan.distance[i]; + float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis; + float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis; + pointcloud.at(i, 0) = x_temp; + pointcloud.at(i, 1) = y_temp; + pointcloud.at(i, 2) = 0; + pointcloud.at(i, 3) = i; + } + this->upScan.pointCloud = pointcloud.clone(); +} + +uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) // 无用 +{ + int x = uv.at(0, 0); + int y = uv.at(1, 0); + // 实验测试，过滤类别 + std::set filterCls = {0, 7, 11, 14, 15, 21}; + for (uchar i = 0; i < boxes.rows; i++) + { + int clsIdx = boxes.at(i, 5); + if (filterCls.find(clsIdx) != filterCls.end()) + continue; + + float xmin = boxes.at(i, 0); + float xmax = boxes.at(i, 2); + float ymin = boxes.at(i, 1); + float ymax = boxes.at(i, 3); + + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + boxflag[i] += 1; + continue; + } + + // 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况 + if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) + { + boxflag[i] += 1; + return class_intensity[clsIdx + 1]; + } + } + return 0; +} + +std::vector LCFusion::clusters_bboxs(Mat &bboxs) // 所有的聚类的雷达簇和所有的检测框的匹配 +{ + std::vector clusterlable; + for (int i = 0; i < this->upScan.clustedCloud.size(); i++) // 遍历每簇雷达 + { + std::vector temp_cluster = this->upScan.clustedCloud[i]; + int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); // 为一簇雷达寻找匹配的检测框 + clusterlable.push_back(temp_clusters_bbox); + } + return clusterlable; +} + +int LCFusion::cluster_bboxs(vector &one_cluster, Mat &bboxs) // 为一簇雷达寻找匹配的检测框 +{ + vector temp_result; + Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]); + Mat end = pointcloud_pixel(one_cluster[0]); // 一簇雷达起始点和终止点在像素坐标系的坐标 + for (int i = 0; i < bboxs.rows; i++) + { + Mat bbox = bboxs.row(i).clone(); + int cls = bbox.at(0, 5); + float ratio = ratio_in_1box(begin, end, bbox); // 在框中占比 + if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) // 适用于大物体 + temp_result.push_back(i); + } + if (temp_result.size() < 1) // 若一簇雷达落入多个框里，选择宽度较小的框 + return -1; + else if (temp_result.size() < 2) + return temp_result[0]; + else + { + int min = temp_result[0]; + int width_min = bboxs.at(min, 2) - bboxs.at(min, 0); + for (int i = 0; i < temp_result.size(); i++) + { + int width_new = bboxs.at(temp_result[i], 2) - bboxs.at(temp_result[i], 0); + if (width_new < width_min) + { + min = temp_result[i]; + width_min = width_new; + } + } + return min; + } + return -1; +} + +Mat LCFusion::pointcloud_pixel(Mat &pointcloud) // 二维雷达点到像素坐标系转换 +{ + Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3)); + Mat camPoint = rotate * cutPointCloud.t() + translation; + // float ppp = camPoint.at(2, 0); + if (camPoint.at(2, 0) <= 0) + { + uv = (Mat_(3, 1) << -1, -1, -1); + return uv; + } + + float scaleX = camPoint.at(0, 0) / camPoint.at(2, 0); + float scaleY = camPoint.at(1, 0) / camPoint.at(2, 0); + float scaleD = scaleX * scaleX + scaleY * scaleY; + float tempD = 1 + distCoeff.at(0, 0) * scaleD + distCoeff.at(0, 1) * scaleD * scaleD + distCoeff.at(0, 4) * scaleD * scaleD * scaleD; + + camPoint.at(0, 0) = scaleX * tempD + 2 * distCoeff.at(0, 2) * scaleX * scaleY + + distCoeff.at(0, 3) * (scaleD + 2 * scaleX * scaleX); + + camPoint.at(1, 0) = scaleY * tempD + distCoeff.at(0, 2) * (scaleD + 2 * scaleY * scaleY) + + 2 * distCoeff.at(0, 3) * scaleX * scaleY; + + camPoint.at(2, 0) = 1.0; + uv = cameraMat * camPoint; // uv为像素坐标 + uv = p2r * uv + t; // 矫正 + return uv; +} + +float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) // 计算一簇雷达落入检测框的比例 +{ + int x_begin = begin.at(0, 0), x_end = end.at(0, 0), y_begin = begin.at(1, 0), y_end = end.at(1, 0); + int xmin = box.at(0, 0), ymin = box.at(0, 1), xmax = box.at(0, 2), ymax = box.at(0, 3); + if (y_begin < ymin || y_begin > ymax) + return 0; + if (x_begin < xmin) + { + if (x_end < xmin) + return 0; + else if (x_end < xmax) + return (float)(x_end - xmin) / (x_end - x_begin); + else + return (float)(xmax - xmin) / (x_end - x_begin); + } + else if (x_begin < xmax) + { + if (x_end < xmax) + return 1; + else + return (float)(xmax - x_begin) / (x_end - x_begin); + } + else + return 0; +} + +void LCFusion::cluster() // 聚类 +{ + this->upScan.clustedCloud.clear(); + int i = 0; + + for (; i < this->upScan.distance.size();) + { + std::vector temp; + int j = i; + temp.push_back(this->upScan.pointCloud.row(i)); + for (; j < this->upScan.distance.size() - 1; j++) + { + + // 距离的6%作为分类的阈值 + if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 ) + { + temp.push_back(this->upScan.pointCloud.row(j + 1)); + } + else + { + i = j + 1; + break; + } + } + this->upScan.clustedCloud.push_back(temp); + if (j == this->upScan.distance.size() - 1) + break; + if (i == upScan.distance.size() - 1) + { + temp.push_back(upScan.pointCloud.row(i)); + this->upScan.clustedCloud.push_back(temp); + break; + } + } + + this->clusterIdx.clear(); + int ii = 0, totoalNum = 0; + for (auto c : this->upScan.clustedCloud) + { + for (int jj = 0; jj < c.size(); jj++) + { + this->clusterIdx.push_back(ii); + } + totoalNum += c.size(); + ii++; + } + //std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl; +} +void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) // 扩散物体生成圆弧状伪激光雷达 +{ + vector center_point = find_circle_center(cluster_num); + float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at(bbox_num, 1)/200.f, + d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc); + int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at(bbox_num, 5); + float laser_index = center_point[1] - half_data_num; + //cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl; + for (int i = 0; i < num_laser_circle; i++) + { + float theta = upScan.angleMin + laser_index * upScan.angleInc; + if (laser_index < 0 || laser_index > upScan.distance.size()) + { + laser_index++; + continue; + } + float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2); + if (data >= 0) + { + //cout << "draw_one_point" << endl; + upScan.distance[laser_index] = circle(d, alfa ,theta, data); + upScan.intensities[laser_index] = class_intensity[cls + 1]; + } + laser_index++; + } +} +vector LCFusion::find_circle_center(int cluster_num) // 寻找扩散物体圆弧的圆心 +{ + vector center_point(2); + int index = 0; + float total_dis = 0; + int num = upScan.clustedCloud[cluster_num].size(); + int no_zero_dis = 0; // 非零距离和 + for (int i = 0 ; i < num; i++) + { + index = upScan.clustedCloud[cluster_num][i].at(0, 3); + if (upScan.distance[index] != 0) + no_zero_dis++; + total_dis += upScan.distance[index]; + } + center_point[0] = total_dis/ no_zero_dis; + index -= (num/2); + float center_laser_index = index; + center_point[1] = center_laser_index; + return center_point; +} +float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度 +{ + float ro = d * cos(theta - alfa) - sqrt(data); + return ro > 0 ? ro:0; +} +vector LCFusion::width_ladar(const int* coordinates) +{ + int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3]; + + std::vector mid = r-> pic2cam(640 / 2, 480); //�ҵ��������� + std::vector loc_tar_start = r-> pic2cam(x1, 480); // �ҵ�Ŀ�����ʼλ�� + std::vector loc_tar_end = r-> pic2cam(x2, 480); //�ҵ�Ŀ�����ֹλ�� + //ת���ɻ���ֵ + float alfa_start = atan((loc_tar_start[0] - mid[0]) / r-> q.at(2, 3)); + float alfa_end = atan((loc_tar_end[0] - mid[0]) / r-> q.at(2, 3)); + std::vector ladar_range; + ladar_range.push_back(alfa_start); + ladar_range.push_back(alfa_end); + return ladar_range; +} +void LCFusion::small_box_without_dis(Mat & bboxs, Mat & dwha) // 无距离检测框到像素坐标系映射，过程和small_fusion类似 +{ + std::cout << "small_box_without_dis" << std::endl; + double set_dis = 2.0; // 设置距离为2米 + for (int i = 0; i < bboxs.rows; i++) + { + int c = bboxs.at(i, 5); + int x_mim = bboxs.at(i, 0); + int y_min = bboxs.at(i, 1); + int x_max = bboxs.at(i, 2); + int y_max = bboxs.at(i, 3); + double d = dwha.at(i, 0); + int coordinates[4] = {x_mim, y_min, x_max, y_max}; + if ((class_index[c] >= 0 && class_index[c] <= 1) || (class_index[c] >= 3 && class_index[c] <= 6)) + { + + Mat temp(2, 1, CV_32F); + + + if (d == -1) + { + vector box_angle = width_ladar(coordinates); + //std::cout << "d:" << d << " a: " << a << std::endl; + temp.at(0, 0) = float(set_dis * cos(box_angle[0])); + temp.at(1, 0) = float(set_dis * sin(box_angle[0])); + temp = c2lR * temp + c2lT; + double y = temp.at(0, 0); + double x = temp.at(1, 0); + float dis = sqrt(x * x + y * y); + float angle_end = 0.0; + if (y >= 0) + angle_end = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_end = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + angle_end = int(angle_end); + // std:: cout << "angle end: " << angle_end << std::endl; + temp.at(0, 0) = float(set_dis * cos(box_angle[1])); + temp.at(1, 0) = float(set_dis * sin(box_angle[1])); + temp = c2lR * temp + c2lT; + y = temp.at(0, 0); + x = temp.at(1, 0); + dis = sqrt(x * x + y * y); + float angle_start = 0.0; + if (y >= 0) + angle_start = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc; + else + angle_start = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc; + + angle_start = int(angle_start); + // std:: cout << "angle start: " << angle_start << std::endl; + if (angle_end-angle_start <= 0) + continue; + int num_point = angle_end-angle_start+1; + for (int i = 0; i < num_point; i++) + { + if ((angle_start + i) < 0 || (angle_start + i) > (upScan.distance.size() - 1)) + continue; + this->upScan.intensities[angle_start + i] = 0; + } + } + } + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) // 设置雷达发布数据（原始雷达） +{ + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); + scan.header.frame_id = "down_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_min = angle_min; + scan.angle_max = angle_max; + scan.angle_increment = angle_inc; + scan.ranges.resize(len); + scan.intensities.resize(len); + scan.time_increment = 1. / 2400.; + for (int i = 0; i < len; i++) + { + scan.ranges[i] = laserData.distance[i] / 1000.f; + scan.intensities[i] = 0; + } +} +void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) // 设置雷达发布数据（语义雷达） +{ + scan.header.frame_id = "up_laser_link"; + scan.range_min = 0.15; + scan.range_max = 8.0; + scan.angle_increment = upScan.angleInc; + scan.ranges.resize(upScan.distance.size()); + scan.intensities.resize(upScan.distance.size()); + scan.angle_min = upScan.angleMin; + scan.angle_max = upScan.angleMax; + for (int i = 0; i < upScan.distance.size(); i++) + { + scan.ranges[i] = upScan.distance[i]; + scan.intensities[i] = upScan.intensities[i]; + } + scan.time_increment = 1. / 2400.; +} + + diff --git a/sweeping_robot_3568/src/fusion_car/src/lidar.cpp b/sweeping_robot_3568/src/fusion_car/src/lidar.cpp new file mode 100644 index 0000000..793689b --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/src/lidar.cpp @@ -0,0 +1,583 @@ +/** + * @file lipkg.cpp + * @author LD Robot + * @version V01 + * @brief + * @note + * @attention COPYRIGHT LDROBOT + **/ + +#include "fusion_car/lidar.h" +#include +#include + +// #ifdef USE_SLBI +// #include "slbi.h" +// #endif + +// #ifdef USE_SLBF +// #include "slbf.h" +// #endif +//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000) +#define MAX_ACK_BUF_LEN 2304000 + +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) + +static const uint8_t CrcTable[256] = + { + 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, + 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, + 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, + 0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, + 0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, + 0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, + 0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, + 0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, + 0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, + 0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, + 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, + 0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, + 0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, + 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, + 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, + 0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, + 0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, + 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, + 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, + 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, + 0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, + 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8}; + +CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr), + mRxCount(0), + mReadCallback(nullptr), + version(ver) +{ + mComHandle = -1; +} +CmdInterfaceLinux::~CmdInterfaceLinux() +{ + Close(); +} +bool CmdInterfaceLinux::Open(std::string &port_name) +{ + int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); + + mComHandle = open(port_name.c_str(), flags); + if (-1 == mComHandle) + { + std::cout << "CmdInterfaceLinux::Open " << port_name << " error !" << std::endl; + return false; + } + + // get port options + struct termios options; + if (-1 == tcgetattr(mComHandle, &options)) + { + Close(); + std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl; + return false; + } + + options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS); + options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD); + options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | + ISIG | IEXTEN); //|ECHOPRT + options.c_oflag &= (tcflag_t) ~(OPOST); + options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); + + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 0; + + switch (version) + { + case 0: + cfsetispeed(&options, B115200); + break; + case 3: + cfsetispeed(&options, B115200); + break; + case 6: + cfsetispeed(&options, B230400); + break; + case 8: + cfsetispeed(&options, B115200); + break; + case 14: + cfsetispeed(&options, B115200); + break; + default: + cfsetispeed(&options, B115200); + } + + if (tcsetattr(mComHandle, TCSANOW, &options) < 0) + { + std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl; + Close(); + return false; + } + + tcflush(mComHandle, TCIFLUSH); + + mRxThreadExitFlag = false; + mRxThread = new std::thread(mRxThreadProc, this); + mIsCmdOpened = true; + + return true; +} +bool CmdInterfaceLinux::Close() +{ + if (mIsCmdOpened == false) + { + return true; + } + + mRxThreadExitFlag = true; + + if (mComHandle != -1) + { + close(mComHandle); + mComHandle = -1; + } + + if ((mRxThread != nullptr) && mRxThread->joinable()) + { + mRxThread->join(); + delete mRxThread; + mRxThread = NULL; + } + + mIsCmdOpened = false; + + return true; +} +bool CmdInterfaceLinux::GetCmdDevices(std::vector> &device_list) +{ + struct udev *udev; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + struct udev_device *dev; + + udev = udev_new(); + if (!udev) + { + return false; + } + enumerate = udev_enumerate_new(udev); + udev_enumerate_add_match_subsystem(enumerate, "tty"); + udev_enumerate_scan_devices(enumerate); + devices = udev_enumerate_get_list_entry(enumerate); + udev_list_entry_foreach(dev_list_entry, devices) + { + const char *path; + + path = udev_list_entry_get_name(dev_list_entry); + dev = udev_device_new_from_syspath(udev, path); + std::string dev_path = std::string(udev_device_get_devnode(dev)); + dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device"); + if (dev) + { + std::pair p; + p.first = dev_path; + p.second = udev_device_get_sysattr_value(dev, "product"); + device_list.push_back(p); + udev_device_unref(dev); + } + else + { + continue; + } + } + udev_enumerate_unref(enumerate); + udev_unref(udev); + return true; +} +bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len) +{ + static timespec timeout = {0, (long)(100 * 1e6)}; + int32_t len = -1; + + if (IsOpened()) + { + fd_set read_fds; + FD_ZERO(&read_fds); + FD_SET(mComHandle, &read_fds); + int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL); + if (r < 0) + { + // Select was interrupted + if (errno == EINTR) + { + return false; + } + } + else if (r == 0) // timeout + { + return false; + } + + if (FD_ISSET(mComHandle, &read_fds)) + { + len = (int32_t)read(mComHandle, rx_buf, rx_buf_len); + if ((len != -1) && rx_len) + { + *rx_len = len; + } + } + } + return len == -1 ? false : true; +} +bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len) +{ + int32_t len = -1; + + if (IsOpened()) + { + len = (int32_t)write(mComHandle, tx_buf, tx_buf_len); + if ((len != -1) && tx_len) + { + *tx_len = len; + } + } + return len == -1 ? false : true; +} +void CmdInterfaceLinux::mRxThreadProc(void *param) +{ + CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param; + char *rx_buf = new char[MAX_ACK_BUF_LEN + 1]; + uchar failedNum = 0; + while (!cmd_if->mRxThreadExitFlag.load()) + { + uint32_t readed = 0; + bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); + if (res && readed) + { + cmd_if->mRxCount += readed; + if (cmd_if->mReadCallback != nullptr) + { + cmd_if->mReadCallback(rx_buf, readed); + } + failedNum = 0; + } + else + { + if(++failedNum == 255) + break; + } + } + cmd_if->mRxThreadExitFlag = true; + delete[] rx_buf; +} + +LiPkg::LiPkg() : mTimestamp(0), + mSpeed(0), + mErrorTimes(0), + mIsFrameReady(false), + mIsPkgReady(false) +{ +} +double LiPkg::GetSpeed(void) +{ + return mSpeed / 360.0; +} +bool LiPkg::Parse(const uint8_t *data, long len) +{ + for (int i = 0; i < len; i++) + { + mDataTmp.push_back(*(data + i)); + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + return false; + + if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100) + { + mErrorTimes++; + mDataTmp.clear(); + return false; + } + + uint16_t start = 0; + + while (start < mDataTmp.size() - 2) + { + start = 0; + while (start < mDataTmp.size() - 2) + { + if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN)) + { + break; + } + + if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5)))) + { + break; + } + start++; + } + + if (start != 0) + { + mErrorTimes++; + for (int i = 0; i < start; i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + return false; + + LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data(); + uint8_t crc = 0; + + for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++) + { + crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff]; + } + + if (crc == pkg->crc8) + { + double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360; + if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2) + { + mErrorTimes++; + } + else + { + mSpeed = pkg->speed; + mTimestamp = pkg->timestamp; + uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000; + float step = diff / (POINT_PER_PACK - 1) / 100.0; + float start = (double)pkg->start_angle / 100.0; + float end = (double)(pkg->end_angle % 36000) / 100.0; + PointData data; + for (int i = 0; i < POINT_PER_PACK; i++) + { + data.distance = pkg->point[i].distance; + data.angle = start + i * step; + if (data.angle >= 360.0) + { + data.angle -= 360.0; + } + data.confidence = pkg->point[i].confidence; + mOnePkg[i] = data; + mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence)); + } + // prevent angle invert + mOnePkg.back().angle = end; + + mIsPkgReady = true; + } + + for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + + if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) + { + break; + } + } + else + { + mErrorTimes++; + /*only remove header,not all frame,because lidar data may contain head*/ + for (int i = 0; i < 2; i++) + { + mDataTmp.erase(mDataTmp.begin()); + } + } + } + + return true; +} +bool LiPkg::AssemblePacket() +{ + float last_angle = 0; + std::vector tmp; + int count = 0; + for (auto n : mFrameTemp) + { + /*wait for enough data, need enough data to show a circle*/ + if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */ + { + mFrameData.len = tmp.size(); + Transform(tmp); /*transform raw data to stantard data */ + + if (tmp.size() == 0) + { + mFrameTemp.clear(); + mIsFrameReady = false; + return false; + } + +#ifdef USE_SLBI + Slbi sb(mSpeed); + sb.FindBarcode(tmp); +#endif + +#ifdef USE_SLBF + Slbf sb(mSpeed); + tmp = sb.NearFilter(tmp); +#endif + + std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) + { return a.angle < b.angle; }); + mFrameData.angle_min = tmp[0].angle; + mFrameData.angle_max = tmp.back().angle; + mFrameData.distance.clear(); + mFrameData.intensities.clear(); + mFrameData.distance.resize(mFrameData.len); + mFrameData.intensities.resize(mFrameData.len); + float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len; + float angle_acc = mFrameData.angle_min; + int tmp_count = 0; + /* interpolation method */ + for (uint32_t i = 0; i < mFrameData.len; i++) + { + if (angle_acc >= tmp[tmp_count].angle) + { + mFrameData.distance[i] = tmp[tmp_count].distance; + mFrameData.intensities[i] = tmp[tmp_count].confidence; + tmp_count++; + if (tmp_count == tmp.size()) + { + break; + } + } + else + { + mFrameData.distance[i] = 0; + mFrameData.intensities[i] = 0; + } + angle_acc += step; + } + std::vector tmp2; + for (uint32_t i = count; i < mFrameTemp.size(); i++) + { + tmp2.push_back(mFrameTemp[i]); + } + mFrameTemp.clear(); + mFrameTemp = tmp2; + mIsFrameReady = true; + return true; + } + else + { + tmp.push_back(n); /* getting data */ + } + + count++; + last_angle = n.angle; + } + + return false; +} +const std::array &LiPkg::GetPkgData(void) +{ + mIsPkgReady = false; + return mOnePkg; +} + +std::vector LiPkg::scan_to_pointcloud(FrameData mFrameData) +{ + u_int32_t len = mFrameData.len; + float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min); + float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max); + cv::Mat pointcloud2(len, 3, CV_32F); + cv::Mat intensities(1, len, CV_32F); + cv::Mat pointdis(1, len, CV_32F); + float a_increment = (angle_max - angle_min) / (float)(len - 1); + for (int i = 0; i < len; i++) + { + float dis = mFrameData.distance[i] / 1000.f; + intensities.at(0, i) = mFrameData.intensities[i]; + pointdis.at(0, i) = dis; + + float x_temp = cos(angle_min + i * a_increment) * dis; + float y_temp = sin(angle_min + i * a_increment) * dis; + pointcloud2.at(i, 0) = x_temp; + pointcloud2.at(i, 1) = y_temp; + pointcloud2.at(i, 2) = 0.0; + } + + return std::vector{pointcloud2, intensities, pointdis}; +} + +/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ + +SlTransform::SlTransform(LDVersion version, bool to_right_hand) +{ + switch (version) + { + case LDVersion::LD_ZERO: + case LDVersion::LD_NINE: + offset_x = 8.1; + offset_y = -22.5156; + break; + + case LDVersion::LD_THREE: + case LDVersion::LD_EIGHT: + offset_x = 5.9; + offset_y = -20.14; + break; + + case LDVersion::LD_FOURTEENTH: + offset_x = 5.9; + offset_y = -20.14; + break; + + default: + break; + } +} +Points2D SlTransform::Transform(const Points2D &data) +{ + Points2D tmp2; + for (auto n : data) + { + /*Filter out invalid data*/ + if (n.distance == 0) + { + continue; + } + /*transfer the origin to the center of lidar circle*/ + /*The default direction of radar rotation is clockwise*/ + /*transfer to the right-hand coordinate system*/ + float right_hand = (360.f - n.angle); + double x = n.distance + offset_x; + double y = n.distance * 0.11923 + offset_y; + double d = sqrt(x * x + y * y); + double shift = atan(y / x) * 180.f / 3.14159; + /*Choose whether to use the right-hand system according to the flag*/ + double angle; + if (to_right_hand) + angle = right_hand + shift; + else + angle = n.angle - shift; + + if (angle > 360) + { + angle -= 360; + } + if (angle < 0) + { + angle += 360; + } + + tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y)); + } + + return tmp2; +} +SlTransform::~SlTransform() +{ +} +void LD14_LiPkg::Transform(std::vector &tmp) +{ + SlTransform trans(LDVersion::LD_FOURTEENTH); + tmp = trans.Transform(tmp); + // std::cout << "Transform LD_FOURTEENTH !!" << std::endl; +} \ No newline at end of file diff --git a/sweeping_robot_3568/src/fusion_car/src/main.cpp b/sweeping_robot_3568/src/fusion_car/src/main.cpp new file mode 100644 index 0000000..c67a77c --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/src/main.cpp @@ -0,0 +1,158 @@ +#include +#include +#include "fusion_car/lidar.h" +#include +#include +#include +#include "fusion_car/ranging.h" +#include "fusion_car/fusion.h" +#include +#include +#include +#include +/*#include +#include */ +#include "opencv2/imgcodecs/imgcodecs.hpp" +#include + +#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) + +Ranging r(9, 640, 480); +std::queue> frameInfo; +std::queue laser_queue; +std::mutex g_mutex; + +void *ranging(void *args) // ranging线程 +{ + while (true) + { + std::cout<<" ************ enter ranging *********** "< result = r.get_range(); + g_mutex.lock(); + for (uchar i = 0; i < frameInfo.size(); i++) // 只保存当前最新的图片帧信息 + frameInfo.pop(); + frameInfo.push(result); + g_mutex.unlock(); + } +} + +int main(int argc, char **argv) +{ + uint8_t laser_queue_size = 2; // 雷达帧队列大小参数 + LD14_LiPkg *pkg = new LD14_LiPkg; + CmdInterfaceLinux cmd_port(9); + std::string port_name = "/dev/ttyUSB0"; + if (argc > 1) + port_name = argv[1]; +// 雷达读取回调函数，一直在执行雷达读取和处理数据 + cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) { + if (pkg->Parse((uint8_t*)byte, len)) + { + pkg->AssemblePacket(); + } }); + if (!cmd_port.Open(port_name)) + return -1; + + ros::init(argc, argv, "publish"); + ros::NodeHandle nh; /* create a ROS Node */ + ros::Publisher uplidar_pub = nh.advertise("up_scan", 1); + //ros::Publisher downlidar_pub = nh.advertise("down_scan", 1); + sensor_msgs::LaserScan upscan; + // sensor_msgs::LaserScan downscan; + + + /*image_transport::ImageTransport it(nh); + image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/ + pthread_t tids[1]; // 执行ranging线程 + int ret = pthread_create(&tids[0], NULL, ranging, NULL); + if (ret != 0) + { + std::cout << "Multithreading error !" << std::endl; + return -2; + } + + LCFusion lcFusion(&r); + namedWindow("camera"); + + while (ros::ok() && !cmd_port.IsExited()) + { + int64 t = getTickCount(); + if (pkg->IsFrameReady()) // 若一帧雷达处理好，执行后续操作 + { + FrameData laserData_queue = pkg->GetFrameData(); // 得到雷达数据 + laserData_queue.timestamp = ros::Time::now(); + laser_queue.push(laserData_queue); + while(laser_queue.size() > laser_queue_size) // 只保存两帧雷达数据 + { + laser_queue.pop(); + } + } + else + continue; + if (laser_queue.size() == laser_queue_size) // 若队列有两帧数据，执行后续操作 + { + FrameData laserData = laser_queue.front(); + laser_queue.pop(); + upscan.header.stamp = laserData.timestamp; + // upscan.header.stamp = ros::Time::now(); + // downscan.header.stamp = upscan.header.stamp; + + // uint16_t times_stamp = pkg->GetTimestamp(); + // FrameData laserData = pkg->GetFrameData(); + char state = -1; + + float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); // 角度弧度转换 + float angle_max = ANGLE_TO_RADIAN(laserData.angle_max); + uint32_t len = laserData.len; + float angle_inc = (angle_max - angle_min) / (len - 1.); // 角度增量 + lcFusion.filter_laser(laserData, angle_min, angle_inc); // 视角同步，处理视角内雷达数据 + if (!frameInfo.empty()) + { + g_mutex.lock(); + std::vector camInfo = frameInfo.front(); // 读取图像帧数据 + frameInfo.pop(); + g_mutex.unlock(); + double fusion_old,fusion_now; + fusion_old = ros::Time::now().toSec(); + state = lcFusion.fusion(camInfo); // 融合 + fusion_now = ros::Time::now().toSec(); + cout << "data_fusion: " << fusion_now - fusion_old << endl; + if (state != -2) + { + lcFusion.set_laser_data(upscan); // 设置雷达发布数据 + uplidar_pub.publish(upscan); // 发布 + } + // 调试使用，将雷达点映射到图像中显示 + if (!lcFusion.projectedPoints.empty()) + { + for (int i = 0; i < lcFusion.projectedPoints.size(); i++) + { + Point2f p = lcFusion.projectedPoints[i]; + circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8); + } + } + /* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg(); + img_pub.publish(msg);*/ + if (!camInfo[0].empty()) + { + cv::Mat pic; + cv::Size dsize= Size(int(640*0.75),int(480*0.75)); + cv::resize(camInfo[0],pic,dsize,0,0,INTER_AREA); + // cv::pyrDown(camInfo[0],pic); + imshow("camera", pic); + } + t = getTickCount() - t; + char fps[50]; + std::cout << "fps: " << int(1 / (t / getTickFrequency())) << std::endl; + if (waitKey(1) == 81) + break; + } + //lcFusion.set_laser_data(downscan, laserData); + //downlidar_pub.publish(downscan); + } + } + std::cout << "ROS or lidar error !" << std::endl; + delete pkg; + cmd_port.Close(); + return 0; +} diff --git a/sweeping_robot_3568/src/fusion_car/src/ranging.cpp b/sweeping_robot_3568/src/fusion_car/src/ranging.cpp new file mode 100644 index 0000000..f3088b2 --- /dev/null +++ b/sweeping_robot_3568/src/fusion_car/src/ranging.cpp @@ -0,0 +1,444 @@ +#include +#include +#include +#include +#include "fusion_car/ranging.h" +#include +#include + +#include "fusion_car/rknn_sdk.h" +#include +#include +#include +#include "opencv2/core.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/imgproc.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; + + +void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数 +{ + remap(oriImgL, oriImgL, mapX1, mapX2, INTER_LINEAR); + remap(oriImgR, oriImgR, mapY1, mapY2, INTER_LINEAR); +} + +std::vector Ranging::pic2cam(int u, int v) //像素坐标转相机坐标 +{ + //(u,v)->(x,y)"(loc[0],loc[1])" + std::vector loc; + loc.push_back((u - cam_matrix_right.at(0, 2)) * q.at(2, 3) / cam_matrix_right.at(0, 0)); + loc.push_back((v - cam_matrix_right.at(1, 2)) * q.at(2, 3) / cam_matrix_right.at(1, 1)); + return loc; +} + +std::vector Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配 +{ + int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3]; + // cv::Mat right_image_,left_image_; + // cv::pyrDown(right_image, right_image_); + // cv::pyrDown(left_image, left_image_); + Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框 + Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域 + // Mat tpl = right_image_.rowRange(max(int(0.5*y1 - 2), 0), min(int(0.5*y2 + 2), 479)).colRange(int(0.5*x1), int(0.5*x2)); + // Mat target = left_image_.rowRange(max(int(0.5*y1 - 10), 0), min(int(0.5*y2 + 10), 239)).colRange(0, 319); + int th = tpl.rows, tw = tpl.cols; + Mat result; + // methods = TM_CCOEFF_NORMED, TM_SQDIFF_NORMED, TM_CCORR_NORMED] + matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关 + double minVal, maxVal; + Point minLoc, maxLoc; + minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标 + Point tl = maxLoc, br; + + br.x = min(maxLoc.x + tw, 639); //转为像素坐标系 + br.y = min(maxLoc.y + th, 479); //转为像素坐标系 + ////展示匹配结果 + // br.x = min(maxLoc.x + tw, 319); + // br.y = min(maxLoc.y + th, 239); + //rectangle(target, tl, br, (0, 255, 0), 3); + //imshow("match-", target); + //waitKey(2); + std::vector maxloc; + maxloc.push_back(maxLoc.x); + maxloc.push_back(maxLoc.y); + return maxloc; +} + +void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k) +{ + //保证摄像头与地面平行 + int x1 = bboxs.at(k, 0); + int x2 = bboxs.at(k, 2); + int y1 = bboxs.at(k, 1); + int y2 = bboxs.at(k, 3); + float Conf = bboxs.at(k, 4); + int cls = bboxs.at(k, 5); + float Y_B, Y_H; + Mat edge, grayImage; + vector idx; + Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围 + //Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); + Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1); + cvtColor(tpl, grayImage, COLOR_BGR2GRAY); + GaussianBlur(grayImage,grayImage,Size(5,5),0); + Canny(grayImage, edge, 120, 180, 3); //提取边缘，获取与地面接触点 + //cv::imshow("1",edge); + //cv::waitKey(1); + float cluster[650]; + for (int i = 0;i<650;i++) + { + cluster[i] = 0; + } + int y_b, y_h; + int j = 0; + for (int i = 0; i < x2-x1; i++) + { + //Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2); + Mat temp = edge.col(i); //取第i列 + // std::cout << "temp: " < point_b = pic2cam(x1 + i, 240); //转为相机坐标系 + std::vector point_H = pic2cam(320, 240); + float alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + if (idx.size() < 1) + { + Z.at(0, i) = 0; + Z.at(1, i) = alfa; + continue; + } + int y_b = idx[idx.size() - 1].y + y1; // + y_b = int(y_b + y_b*0.03); + int y_h = 240; + point_b = pic2cam(x1 + i, y_b); //转为相机坐标系 + point_H = pic2cam(320, y_h); + Y_B = point_b[1]; + Y_H = point_H[1]; + // std::cout << "y_b: " << y_b << std::endl; + float H_c = 60; //摄像头离地高度，单位mm + float theta = 0; //摄像头与地面夹角，弧度 + float d = (1/cos(theta)* cos(theta)) * q.at(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta); + alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + //cout << "d: " << d << endl; + if (d > 700) + {d = 0;} + Z.at(0, i) = d/cos(alfa); + Z.at(1, i) = alfa; + } + /*if (i > 0) + { + if (abs(Z.at(0, i) - Z.at(0, i - 1)) < 40) //聚类 + { + cluster[j] = cluster[j] + 1; + } + else + { + j = j + 1; + cluster[j] = 1; + //std::cout<<"j : "<< j<< std::endl; + } + } + } + //int max_loc = distance(cluster,max_element(cluster,cluster + sizeof(cluster)/sizeof(cluster[0]))); //只保留数量最多的一类，其余置0 + //std::cout<<" max_loc : "<< max_loc<(0, t) = 0; + } + } + temp = temp + cluster[i]; + }*/ + // std::cout << "z : " <Z = Z.clone(); +} + +void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info) +{ + // ֱ��ͼ���⻯ + Mat imgGrayL, imgGrayR; + cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY); + cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY); + Mat imgR_weight = imgR.clone(); + Mat infoRow; + for (uchar i = 0; i < detBoxes.rows; i++) + { + int x1 = detBoxes.at(i, 0); + int y1 = detBoxes.at(i, 1); + int x2 = detBoxes.at(i, 2); + int y2 = detBoxes.at(i, 3); + //std::cout<<"x1: "<(i, 4); + int cls = detBoxes.at(i, 5); + + if (cls == 6 || cls == 10 || cls == 11) //体重秤，地毯和毛巾采用单目测距方法 + { + horizon_estimate(imgR_weight, detBoxes, i); + } + if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体 + { + + //char cm[15]; + //sprintf(cm, "cannot match !"); + + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); + continue; + } + if (cls!=0 && cls!=1 && cls!=2 && cls!= 6 && cls!= 7 && cls!= 10 && cls!=11 && cls!=14 && cls!=15) //大物体不进行测距 + { + if (x1 < 30 || x2 < 80 || x1>580 || x2 > 610) //删除边缘的目标框 + { + detBoxes.at(i, 5) = -1; + cls = detBoxes.at(i, 5); + } + //char cm[15]; + //sprintf(cm, "cannot match !"); + + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); + continue; + } + + rectangle(imgR, Point(int(x1), int(y1)), + Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框 + + int coordinates[4] = {x1, y1, x2, y2}; + std::vector disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配 + float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差 + float disp_pixel_y = disp_pixel[1] - y1; // + disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到 + + //Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz; + Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息 + Mat threed_pixel_xyz, threedImage; + reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d + threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方， + std::vector channels; + split(threed_pixel_xyz.clone(), channels); + threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离 + threed_pixel_xyz.forEach([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d + + int mid_pixel = int((x1 + x2) / 2); + std::vector mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标 + std::vector loc_tar = pic2cam(mid_pixel, imgGrayR.rows); + float alfa = atan((loc_tar[0] - mid[0]) / q.at(2, 3)); + + + if (disp_pixel_x > 240) // 距离太近，视差过大 + { + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + else + { + float median = threed_pixel_xyz.at((int)(y1 + y2) / 2, (int)(x1 + x2) / 2); + + std::vector ltPoint = pic2cam(x1, y1); + std::vector rbPoint = pic2cam(x2, y2); + float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高 + float f = q.at(2, 3); + float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出 + //float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1; + float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1; + float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f); + //float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2; + float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2; + float w1 = sqrt(pow((threedImage.at(y2, x1)[0] - threedImage.at(y2, x2)[0]), 2) + + pow((threedImage.at(y2, x1)[1] - threedImage.at(y2, x2)[1]), 2) + + pow((threedImage.at(y2, x1)[2] - threedImage.at(y2, x2)[2]), 2)); + + w1 = w1 / 10; + h1 = (h1 + h2) / 10; + median /= 10; + if (median > 120) //过远测距误差较大 + { + //char tf[9]; + //sprintf(tf, "Too far!"); + char cm[15]; + //sprintf(cm, "cannot match !"); + sprintf(cm, "%d , %.2f", cls,Conf); + putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2); + infoRow = (Mat_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + // ��ͼ���ϻ�����Ϣ + char dc[50], wh[50]; + std::string cname = className[cls + 1]; + sprintf(dc, "dis:%.2fcm %d", median, cls); + sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa); + putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2); + putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5); + + + //返回数据 + infoRow = (Mat_(1, 4) << median, w1, h1, alfa); + infoRow.copyTo(info.row(i)); + }; + } +} + +Ranging::Ranging(int index, int imgw, int imgh) : //初始化 + mapX1(imgh, imgw, CV_64F), //初始化矩阵 ，用于计算无畸变和修正转换映射。 + mapX2(imgh, imgw, CV_64F), + mapY1(imgh, imgw, CV_64F), + mapY2(imgh, imgw, CV_64F), + q(4, 4, CV_64F), + imgw(imgw), + imgh(imgh) +{ + // Z = Mat::zeros(2, 1, CV_32FC1); + if (!vcapture.open(index)) + { + std::cout << "Open camera failed !" << std::endl; + exit(EXIT_FAILURE); + } + else + { + //vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG); + vcapture.set(CAP_PROP_FPS, 30); + vcapture.set(CAP_PROP_FRAME_WIDTH, imgw * 2); + vcapture.set(CAP_PROP_FRAME_HEIGHT, imgh); + vcapture.set(CAP_PROP_BUFFERSIZE, 1); + + auto imgSize = Size(imgw, imgh); + Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F); + + stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r, + imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正 + + initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射 + initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射 + + RKNN_Create(&hdx, modelPath); // 初始化检测模型 + std::cout<< " ******************* CAMERA initialization ********************" << std::endl; + } +} + +// std::vector Ranging::pic2cam(int u, int v) //像素坐标转相机坐标 +// { +// std::vector loc; +// loc.push_back((u - cam_matrix_right.at(0, 2)) * q.at(2, 3) / cam_matrix_right.at(0, 0)); +// loc.push_back((v - cam_matrix_right.at(1, 2)) * q.at(2, 3) / cam_matrix_right.at(1, 1)); +// return loc; +// } + + + +std::vector Ranging::get_range() +{ + double rang_old, rang_now; + rang_old = ros::Time::now().toSec(); //测试运行时间 + Mat frame, lframe, rframe; + vcapture.read(frame); //获取视频帧 + + if (!frame.empty()) + { + int64 t = getTickCount(); + Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图 + Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图 + //imshow("lframe",lframe); + //waitKey(1); + rectifyImage(lframe, rframe); // + + DetRet *det_ret = NULL; // + InputData input_data; //定义输入数据 + int shape[4] = {1, 3, rframe.rows, rframe.cols}, det_num = 0; + memcpy(input_data.shape, shape, sizeof(shape)); + Mat Rframe = rframe.clone(); + + double detect_old, detect_now; + detect_old = ros::Time::now().toSec(); + input_data.data = Rframe.data; + //std::cout<<"Rframe.data.shape"<{rframe}; //没有检测框时，退出 + } + std::cout << "det_num: " << det_num << std::endl; + // detction box transfor to our format + Mat detBoxes(det_num, 6, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls) + for (int i = 0; i < det_num; i++) //存储目标检测内容 (x,y,x,y,conf,cls) + { + DetRet det_result = det_ret[i]; + // xyxy conf cls + float xmin = rframe.cols * det_result.fLeft; + float ymin = rframe.rows * det_result.fTop; + float xmax = rframe.cols * det_result.fRight; + float ymax = rframe.rows * det_result.fBottom; + + detBoxes.at(i, 0) = xmin; + detBoxes.at(i, 1) = ymin; + detBoxes.at(i, 2) = xmax; + detBoxes.at(i, 3) = ymax; + detBoxes.at(i, 4) = det_result.fConf; + // 实验测试，过滤过大的误检框 + float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + if (ratio > 0.7) + { + detBoxes.at(i, 5) = -1; + continue; + } + detBoxes.at(i, 5) = det_result.nLabel; + if (det_result.nLabel == 1 || det_result.nLabel == 2) //检测目标为体重秤和风扇底座时，将目标框拉高，使杆子的雷达点能与目标框对应 + { + detBoxes.at(i, 1) = 100; + } + } + Mat info(det_num, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α + if (det_num) + { + // double rang_old, rang_now; + // rang_old = ros::Time::now().toSec(); + getInfo(lframe, rframe, detBoxes, info); + // rang_now = ros::Time::now().toSec(); + // cout << "data_dis_time: " << rang_now-rang_old << endl; + } + t = getTickCount() - t; + char fps[50]; + sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency()))); + putText(rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5); + // rang_now = ros::Time::now().toSec(); + // cout << "data_rang_time" << rang_now - rang_old << endl; + return std::vector{rframe, detBoxes, info}; + } + return std::vector{rframe}; +} diff --git a/sweeping_robot_3568/src/librknn_sdk.so b/sweeping_robot_3568/src/librknn_sdk.so new file mode 100644 index 0000000..5f7bce1 Binary files /dev/null and b/sweeping_robot_3568/src/librknn_sdk.so differ diff --git a/sweeping_robot_3568/src/librknnrt.so b/sweeping_robot_3568/src/librknnrt.so new file mode 100644 index 0000000..8488198 Binary files /dev/null and b/sweeping_robot_3568/src/librknnrt.so differ diff --git a/sweeping_robot_3568/src/stereo.yaml b/sweeping_robot_3568/src/stereo.yaml new file mode 100644 index 0000000..ddfc469 --- /dev/null +++ b/sweeping_robot_3568/src/stereo.yaml @@ -0,0 +1,19 @@ +Mat cam_matrix_left = (Mat_(3, 3) << + 7.678687801628776e+02, 0, 0, + 0,7.679707836247087e+02, 0, + 6.566692968103456e+02, 3.390198663125851e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << + 7.824799867061021e+02, 0, 0, + 0,7.823093118883010e+02, 0, + 6.354310487450947e+02, 3.471699497317592e+02, 1); + Mat distortion_l = (Mat_(1, 5) <<0.124875722106329,-0.135334967786191, 0, + 0, 0); + + Mat distortion_r = (Mat_(1, 5) <.115700316133502, -0.115533076004258, 0, + 0, 0); + Mat rotate = (Mat_(3, 3) << + 0.999990145255059, 4.053889756923358e-04, 0.004420978686347, + -3.935924151610147e-04, 0.999996361134497, -0.002668857953253, + -0.004422044524592, 0.002667091588660,0.999986665983442); + Mat trans = (Mat_(3, 1) << + -60.619847880565940, -0.180746754134188, -0.343456473917128); \ No newline at end of file