fix：修复随机起终点&录包&点云偏移

requirements.txt

@@ -0,0 +1,123 @@
+actionlib==1.14.0
+angles==1.9.13
+base_local_planner==1.17.3
+bondpy==1.8.6
+camera-calibration==1.17.0
+camera-calibration-parsers==1.12.0
+catkin==0.8.10
+catkin-pkg==1.0.0
+certifi==2021.5.30
+controller-manager==0.20.0
+controller-manager-msgs==0.20.0
+cv-bridge==1.16.2
+cycler==0.11.0
+decorator==4.4.2
+diagnostic-analysis==1.11.0
+diagnostic-common-diagnostics==1.11.0
+diagnostic-updater==1.11.0
+docutils==0.18.1
+dynamic-reconfigure==1.7.3
+empy==3.3.4
+gazebo_plugins==2.9.2
+gazebo_ros==2.9.2
+gencpp==0.7.0
+geneus==3.0.0
+genlisp==0.4.18
+genmsg==0.6.0
+gennodejs==2.0.2
+genpy==0.6.15
+gnupg==2.3.1
+image-geometry==1.16.2
+interactive-markers==1.12.0
+joint-state-publisher==1.15.1
+joint-state-publisher-gui==1.15.1
+kiwisolver==1.3.1
+laser_geometry==1.6.7
+matplotlib==3.3.4
+message-filters==1.16.0
+networkx==2.5.1
+numpy==1.19.5
+opencv-python==4.3.0.38
+Pillow==8.4.0
+psutil==6.0.0
+pycollada==0.8
+pycryptodomex==3.20.0
+pyparsing==3.1.2
+python-dateutil==2.9.0.post0
+python-qt-binding==0.4.4
+PyYAML==6.0.1
+qt-dotgraph==0.4.2
+qt-gui==0.4.2
+qt-gui-cpp==0.4.2
+qt-gui-py-common==0.4.2
+reportlab==3.6.8
+resource_retriever==1.12.7
+rosbag==1.16.0
+rosboost-cfg==1.15.8
+rosclean==1.15.8
+roscreate==1.15.8
+rosgraph==1.16.0
+roslaunch==1.16.0
+roslib==1.15.8
+roslint==0.12.0
+roslz4==1.16.0
+rosmake==1.15.8
+rosmaster==1.16.0
+rosmsg==1.16.0
+rosnode==1.16.0
+rosparam==1.16.0
+rospkg==1.5.1
+rospy==1.16.0
+rosservice==1.16.0
+rostest==1.16.0
+rostopic==1.16.0
+rosunit==1.15.8
+roswtf==1.16.0
+rqt-console==0.4.12
+rqt-image-view==0.4.17
+rqt-logger-level==0.4.12
+rqt-moveit==0.5.11
+rqt-reconfigure==0.5.5
+rqt-robot-dashboard==0.5.8
+rqt-robot-monitor==0.5.15
+rqt-runtime-monitor==0.5.10
+rqt-rviz==0.7.0
+rqt-tf-tree==0.6.4
+rqt_action==0.4.9
+rqt_bag==0.5.1
+rqt_bag_plugins==0.5.1
+rqt_dep==0.4.12
+rqt_graph==0.4.14
+rqt_gui==0.5.3
+rqt_gui_py==0.5.3
+rqt_launch==0.4.9
+rqt_msg==0.4.10
+rqt_nav_view==0.5.7
+rqt_plot==0.4.13
+rqt_pose_view==0.5.11
+rqt_publisher==0.4.10
+rqt_py_common==0.5.3
+rqt_py_console==0.4.10
+rqt_robot_steering==0.5.12
+rqt_service_caller==0.4.10
+rqt_shell==0.4.11
+rqt_srv==0.4.9
+rqt_top==0.4.10
+rqt_topic==0.4.13
+rqt_web==0.4.10
+rviz==1.14.20
+scipy==1.5.4
+sensor-msgs==1.13.1
+six==1.16.0
+smach==2.5.2
+smach-ros==2.5.2
+smclib==1.8.6
+tf==1.13.2
+tf-conversions==1.13.2
+tf2-geometry-msgs==0.7.7
+tf2-kdl==0.7.7
+tf2-py==0.7.7
+tf2-ros==0.7.7
+topic-tools==1.16.0
+trimesh==3.23.5
+xacro==1.14.16

simulation/custom_point/install/.rosinstall

@@ -0,0 +1,2 @@
+- setup-file:
+    local-name: /home/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install/setup.sh

simulation/custom_point/install/_setup_util.py

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

simulation/custom_point/install/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 "$@"

simulation/custom_point/install/lib/pkgconfig/goal_publish.pc

@@ -0,0 +1,8 @@
+prefix=/home/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install
+
+Name: goal_publish
+Description: Description of goal_publish
+Version: 0.0.0
+Cflags: 
+Libs: -L${prefix}/lib 
+Requires: 

simulation/custom_point/install/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

simulation/custom_point/install/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/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install}
+CATKIN_SETUP_UTIL_ARGS="--extend --local"
+. "$_CATKIN_SETUP_DIR/setup.sh"
+unset CATKIN_SETUP_UTIL_ARGS

simulation/custom_point/install/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'

simulation/custom_point/install/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"

simulation/custom_point/install/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/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install}
+_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

simulation/custom_point/install/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"'

simulation/custom_point/install/share/goal_publish/cmake/goal_publishConfig-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()

simulation/custom_point/install/share/goal_publish/cmake/goal_publishConfig.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(goal_publish_CONFIG_INCLUDED)
+  return()
+endif()
+set(goal_publish_CONFIG_INCLUDED TRUE)
+
+# set variables for source/devel/install prefixes
+if("FALSE" STREQUAL "TRUE")
+  set(goal_publish_SOURCE_PREFIX /home/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/src/goal_publish)
+  set(goal_publish_DEVEL_PREFIX /home/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/devel)
+  set(goal_publish_INSTALL_PREFIX "")
+  set(goal_publish_PREFIX ${goal_publish_DEVEL_PREFIX})
+else()
+  set(goal_publish_SOURCE_PREFIX "")
+  set(goal_publish_DEVEL_PREFIX "")
+  set(goal_publish_INSTALL_PREFIX /home/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install)
+  set(goal_publish_PREFIX ${goal_publish_INSTALL_PREFIX})
+endif()
+
+# warn when using a deprecated package
+if(NOT "" STREQUAL "")
+  set(_msg "WARNING: package 'goal_publish' 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(goal_publish_FOUND_CATKIN_PROJECT TRUE)
+
+if(NOT " " STREQUAL " ")
+  set(goal_publish_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 'xulin <xulin@todo.todo>' 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 "${goal_publish_DIR}/../../../include" ABSOLUTE)
+      if(NOT IS_DIRECTORY ${include})
+        message(FATAL_ERROR "Project 'goal_publish' specifies '${idir}' as an include dir, which is not found.  It does not exist in '${include}'.  ${_report}")
+      endif()
+    else()
+      message(FATAL_ERROR "Project 'goal_publish' specifies '${idir}' as an include dir, which is not found.  It does neither exist as an absolute directory nor in '\${prefix}/${idir}'.  ${_report}")
+    endif()
+    _list_append_unique(goal_publish_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 goal_publish_LIBRARIES ${library})
+  elseif(${library} MATCHES "^-l")
+    list(APPEND goal_publish_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 goal_publish_NUM_DUMMY_TARGETS)
+      set(goal_publish_NUM_DUMMY_TARGETS 0)
+    endif()
+    # Make sure the target name is unique
+    set(interface_target_name "catkin::goal_publish::wrapped-linker-option${goal_publish_NUM_DUMMY_TARGETS}")
+    while(TARGET "${interface_target_name}")
+      math(EXPR goal_publish_NUM_DUMMY_TARGETS "${goal_publish_NUM_DUMMY_TARGETS}+1")
+      set(interface_target_name "catkin::goal_publish::wrapped-linker-option${goal_publish_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 goal_publish_LIBRARIES "${interface_target_name}")
+  elseif(TARGET ${library})
+    list(APPEND goal_publish_LIBRARIES ${library})
+  elseif(IS_ABSOLUTE ${library})
+    list(APPEND goal_publish_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/cicv/work/26_pji/simulation/jiaofu_20241016/indoor/custom_point/install/lib;/opt/ros/noetic/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(goal_publish_LIBRARY_DIRS ${lib_path})
+      list(APPEND goal_publish_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 'goal_publish'?  Did you find_package() it before the subdirectory containing its code is included?")
+      endif()
+      list(APPEND goal_publish_LIBRARIES ${lib})
+    endif()
+  endif()
+endforeach()
+
+set(goal_publish_EXPORTED_TARGETS "")
+# create dummy targets for exported code generation targets to make life of users easier
+foreach(t ${goal_publish_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 goal_publish_dep)
+  list(LENGTH depend_list count)
+  if(${count} EQUAL 1)
+    # simple dependencies must only be find_package()-ed once
+    if(NOT ${goal_publish_dep}_FOUND)
+      find_package(${goal_publish_dep} REQUIRED NO_MODULE)
+    endif()
+  else()
+    # dependencies with components must be find_package()-ed again
+    list(REMOVE_AT depend_list 0)
+    find_package(${goal_publish_dep} REQUIRED NO_MODULE ${depend_list})
+  endif()
+  _list_append_unique(goal_publish_INCLUDE_DIRS ${${goal_publish_dep}_INCLUDE_DIRS})
+
+  # merge build configuration keywords with library names to correctly deduplicate
+  _pack_libraries_with_build_configuration(goal_publish_LIBRARIES ${goal_publish_LIBRARIES})
+  _pack_libraries_with_build_configuration(_libraries ${${goal_publish_dep}_LIBRARIES})
+  _list_append_deduplicate(goal_publish_LIBRARIES ${_libraries})
+  # undo build configuration keyword merging after deduplication
+  _unpack_libraries_with_build_configuration(goal_publish_LIBRARIES ${goal_publish_LIBRARIES})
+
+  _list_append_unique(goal_publish_LIBRARY_DIRS ${${goal_publish_dep}_LIBRARY_DIRS})
+  _list_append_deduplicate(goal_publish_EXPORTED_TARGETS ${${goal_publish_dep}_EXPORTED_TARGETS})
+endforeach()
+
+set(pkg_cfg_extras "")
+foreach(extra ${pkg_cfg_extras})
+  if(NOT IS_ABSOLUTE ${extra})
+    set(extra ${goal_publish_DIR}/${extra})
+  endif()
+  include(${extra})
+endforeach()

simulation/custom_point/install/share/goal_publish/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>goal_publish</name>
+  <version>0.0.0</version>
+  <description>The goal_publish package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="xulin@todo.todo">xulin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/goal_publish</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rosbag</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>gazebo_msgs</build_depend>
+  <build_export_depend>rosbag</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>gazebo_msgs</build_export_depend>
+  <exec_depend>rosbag</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>gazebo_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

simulation/random_install/install/_setup_util.py

@@ -268,7 +268,7 @@ if __name__ == '__main__':
 
         if not args.local:
             # environment at generation time
-            CMAKE_PREFIX_PATH = r'/home/cicv/work/26_pji/simulation/random_point/devel;/opt/ros/noetic'.split(';')
+            CMAKE_PREFIX_PATH = r'/opt/ros/noetic'.split(';')
         else:
             # don't consider any other prefix path than this one
             CMAKE_PREFIX_PATH = []

simulation/random_install/install/share/common_msgs/cmake/common_msgsConfig.cmake

@@ -154,7 +154,7 @@ foreach(library ${libraries})
     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/cicv/work/26_pji/simulation/random_point/install/lib;/home/cicv/work/26_pji/simulation/random_point/devel/lib;/opt/ros/noetic/lib)
+    foreach(path /home/cicv/work/26_pji/simulation/random_point/install/lib;/opt/ros/noetic/lib)
       find_library(lib ${library}
         PATHS ${path}
         NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)

simulation/random_install/install/share/random_point/cmake/random_pointConfig.cmake

@@ -154,7 +154,7 @@ foreach(library ${libraries})
     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/cicv/work/26_pji/simulation/random_point/install/lib;/home/cicv/work/26_pji/simulation/random_point/devel/lib;/opt/ros/noetic/lib)
+    foreach(path /home/cicv/work/26_pji/simulation/random_point/install/lib;/opt/ros/noetic/lib)
       find_library(lib ${library}
         PATHS ${path}
         NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)

simulation/rosbag_record.py

@@ -1,114 +1,187 @@
-'''
-Description: 
-Version: 1.0
-Autor: Sun Yalun
-Date: 2024-09-05 14:53:32
-LastEditors: Sun Yalun
-LastEditTime: 2024-09-05 15:46:54
-'''
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 
 import rospy
-import rosbag
-import sys
 import os
 import signal
 import subprocess
+import threading
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from move_base_msgs.msg import MoveBaseActionGoal
+from math import sqrt
+import sys
 from datetime import datetime
 
-
 class RosbagRecorder:
     def __init__(self, bag_dir, count):
+        # 初始化ROS节点
         rospy.init_node('rosbag_recorder', anonymous=True)
-        rospy.set_param('/use_sim_time', False)
-        self.goal_sub = rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_callback)
-        self.pose_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.pose_callback)
-        self.cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)
+        rospy.set_param('/use_sim_time', False)        
 
-        self.goal_position = None
-        self.bag_process = None
+        # 参数初始化
+        self.bag_dir = bag_dir
+        self.total_count = int(count)
+        self.bag_count = 0
         self.recording = False
+        self.goal_position = None
         self.pose_position = None
         self.cmd_vel = None
-        self.bag_dir = bag_dir
-        self.bag_count = 1
-        self.total_count = int(count)
+        self.bag_process = None
+
+        # 创建锁，确保线程安全
+        self.lock = threading.Lock()
+
+        # 订阅话题
+        self.goal_sub = rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_callback)
+        self.pose_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.pose_callback)
+        self.cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback)
+
+        rospy.loginfo("录制脚本初始化完成，等待终点...")
 
     def goal_callback(self, msg):
-        # 如果当前已录制的 bag 文件数量大于等于传入的 count 参数，就不再录制
-        if self.bag_count > self.total_count:
-            rospy.loginfo("Recording limit reached. Exiting...")
-            self.shutdown_node()
-            return
-        # 更新时间戳为当前时间
-        msg.header.stamp = rospy.Time.now()
-        self.goal_position = msg.goal.target_pose.pose.position
-        rospy.loginfo("Goal received, starting rosbag recording...")
-
-        bag_file_name = f"test-{self.bag_count}.bag"
-        bag_file_path = os.path.join(self.bag_dir, bag_file_name)
-        self.bag_path = bag_file_path  
-        self.start_recording()
+        with self.lock:
+            if self.bag_count >= self.total_count:
+                rospy.loginfo("已达到录制数量上限，退出程序。")
+                self.shutdown_node()
+                return
+
+            if self.recording:
+                rospy.logwarn("当前正在录制中，等待当前录制完成后再开始新的录制。")
+                return
+
+            # 记录目标位置
+            self.goal_position = msg.goal.target_pose.pose.position
+            rospy.loginfo(f"接收到第{self.bag_count + 1}/{self.total_count}个终点，开始录制第{self.bag_count + 1}/{self.total_count}个rosbag。")
+            # 设置rosbag文件名
+            msg.header.stamp = rospy.Time.now()
+            bag_file_name = f"test{self.bag_count + 1:02d}.bag"
+            self.bag_name = bag_file_name
+            bag_file_path = os.path.join(self.bag_dir, bag_file_name)   
+            self.bag_path = bag_file_path         
+            # 开始录制
+            self.start_recording()
+
     def pose_callback(self, msg):
-        # 更新时间戳为当前时间
-        msg.header.stamp = rospy.Time.now()
-        self.pose_position = msg.pose.pose.position
-        self.check_stop_condition()
+        with self.lock:
+            # msg.header.stamp = rospy.Time.now()
+            self.pose_position = msg.pose.pose.position
+            self.check_stop_condition()
 
     def cmd_vel_callback(self, msg):
-        # 更新时间戳为当前时间
-        # msg.header.stamp = rospy.Time.now()
-        self.cmd_vel = msg
-        self.check_stop_condition()
+        with self.lock:
+            # msg.header.stamp = rospy.Time.now()
+            self.cmd_vel = msg
+            self.check_stop_condition()
 
-    def start_recording(self):
+    def check_stop_condition(self):
         if not self.recording:
-            self.bag_process = subprocess.Popen(['rosbag', 'record', '-O', self.bag_path, '/amcl_pose', '/imu', '/obstacle_detection', '/odom', '/sys_info'], preexec_fn=os.setsid)
+            return
+
+        if not self.goal_position or not self.pose_position or not self.cmd_vel:
+            return
+
+        # 计算距离
+        distance = self.calculate_distance(self.pose_position, self.goal_position)
+
+        # 判断是否满足停止录制的条件
+        if distance < 0.3 and self.cmd_vel.linear.x == 0 and self.cmd_vel.linear.y == 0:
+            rospy.loginfo(f"满足停止录制条件，停止录制第{self.bag_count + 1}个rosbag。")
+            self.stop_recording()
+
+    def calculate_distance(self, pos1, pos2):
+        return sqrt((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2)
+
+    def start_recording(self):
+        if self.recording:
+            rospy.logwarn("已经在录制中，不启动新的录制。")
+            return
+
+        # 生成文件名，包含时间戳以避免重名
+        # timestamp = datetime.now().strftime("%Y-%m-%d-%H-%M-%S")
+        # bag_file_name = f"test{self.bag_count + 1:02d}_{timestamp}.bag"
+        # msg.header.stamp = rospy.Time.now()
+        # bag_file_name = f"test{self.bag_count + 1:02d}.bag"
+        # bag_file_path = os.path.join(self.bag_dir, bag_file_name)
+
+        # # 确保目录存在
+        # if not os.path.exists(self.bag_dir):
+        #     try:
+        #         os.makedirs(self.bag_dir)
+        #         rospy.loginfo(f"创建目录：{self.bag_dir}")
+        #     except Exception as e:
+        #         rospy.logerr(f"无法创建目录 {self.bag_dir}：{e}")
+        #         self.shutdown_node()
+        #         return
+
+        # 启动rosbag录制
+        try:
+            self.bag_process = subprocess.Popen(
+                ['rosbag', 'record', '-o', self.bag_path, '/cmd_vel', '/move_base/goal', '/amcl_pose', '/imu', '/obstacle_detection', '/odom', '/sys_info'],
+                preexec_fn=os.setsid,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.PIPE
+            )
             self.recording = True
+            rospy.loginfo(f"开始录制rosbag：{self.bag_name}")
+        except Exception as e:
+            rospy.logerr(f"启动rosbag录制失败：{e}")
+            self.recording = False
 
     def stop_recording(self):
-        if self.recording and self.bag_process:
-            rospy.loginfo("Stopping rosbag recording...")
-            # os.killpg(os.getpgid(self.bag_process.pid), signal.SIGTERM)  # 首先发送SIGTERM信号
-            os.killpg(os.getpgid(self.bag_process.pid), signal.SIGINT)  # 首先发送SIGINT信号
-            try:
-                self.bag_process.wait(timeout=10)  # 等待进程终止，如果超过10秒，进行强制终止
-            except subprocess.TimeoutExpired:
-                rospy.logwarn("Rosbag process did not terminate, sending SIGKILL...")
-                os.killpg(os.getpgid(self.bag_process.pid), signal.SIGKILL)  # 强制终止
-                self.bag_process.wait()  # 再次等待确保进程已经被终止                      
-            self.recording = False
-            self.bag_count += 1
+        if not self.recording or not self.bag_process:
+            rospy.logwarn("尝试停止录制，但当前没有录制进程。")
+            return
 
-            # 如果录制的包数量达到总数，停止整个节点
-            if self.bag_count > self.total_count:
-                rospy.loginfo("All recordings completed. Exiting...")
-                rospy.sleep(2)  # 等待一秒，确保rosbag_recorder节点已经退出
-                self.shutdown_node()
+        try:
+            # 发送SIGTERM信号终止rosbag进程
+            os.killpg(os.getpgid(self.bag_process.pid), signal.SIGTERM)
+            rospy.loginfo("发送SIGTERM信号，尝试终止rosbag录制进程。")
 
-    def check_stop_condition(self):
-        if self.goal_position and self.pose_position and self.cmd_vel:
-            distance = self.calculate_distance(self.pose_position, self.goal_position)
-            if distance < 0.3 and self.cmd_vel.linear.x == 0 and self.cmd_vel.linear.y == 0:
-                self.stop_recording()
+            # 等待进程终止
+            self.bag_process.wait(timeout=10)
+            rospy.loginfo(f"{self.bag_name}录制进程已终止。")
+        except subprocess.TimeoutExpired:
+            rospy.logwarn("rosbag录制进程未能在10秒内终止，发送SIGKILL信号强制终止。")
+            try:
+                os.killpg(os.getpgid(self.bag_process.pid), signal.SIGKILL)
+                self.bag_process.wait()
+                rospy.loginfo("rosbag录制进程已被SIGKILL信号强制终止。")
+            except Exception as e:
+                rospy.logerr(f"强制终止rosbag录制进程失败：{e}")
+        except Exception as e:
+            rospy.logerr(f"停止rosbag录制进程时出错：{e}")
 
-    def calculate_distance(self, pos1, pos2):
-        return ((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2) ** 0.5
+        # 更新状态
+        self.recording = False
+        self.bag_count += 1
+        rospy.loginfo(f"第{self.bag_count}/{self.total_count}个rosbag录制完成。")
+        # print("bag_count: ", self.bag_count)
+        # print("total_count: ", self.total_count)
+
+        # 检查是否达到录制数量
+        if self.bag_count >= self.total_count:
+            # rospy.loginfo("所有rosbag录制完成，退出程序。")
+            self.shutdown_node()
+            # rospy.loginfo("所有rosbag录制完成，退出程序。")
+        else:
+            rospy.loginfo("等待下一个目标点以开始新的rosbag录制。")
 
     def shutdown_node(self):
-        """停止ROS节点和当前进程"""
-        rospy.loginfo("Shutting down ROS node and killing the process.")
-        rospy.sleep(2)  # 等待2秒，确保rosbag_recorder节点已经退出
-        rospy.signal_shutdown("Completed all recordings.")
-        sys.exit(0)  # 终止Python脚本
+        rospy.loginfo("所有rosbag录制完成")
+        rospy.loginfo("正在关闭ROS节点并退出程序。")
+        rospy.signal_shutdown("完成所有rosbag录制。")
+        rospy.sleep(1)  # 确保所有日志输出
+        os._exit(0)  # 强制退出程序
 
 if __name__ == '__main__':
     try:
+        # if len(sys.argv) != 3:
+        #     print("用法: rosrun your_package your_script.py <bag_directory> <count>")
+        #     sys.exit(1)
+
         bag_folder_path = sys.argv[1]
         count = sys.argv[2]
+
         recorder = RosbagRecorder(bag_folder_path, count)
         rospy.spin()
     except rospy.ROSInterruptException:

simulation/rosbag_record_old.py

@@ -1,3 +1,11 @@
+'''
+Description: 
+Version: 1.0
+Autor: Sun Yalun
+Date: 2024-09-05 14:53:32
+LastEditors: Sun Yalun
+LastEditTime: 2024-09-05 15:46:54
+'''
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 
@@ -11,8 +19,9 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from move_base_msgs.msg import MoveBaseActionGoal
 from datetime import datetime
 
+
 class RosbagRecorder:
-    def __init__(self, path):
+    def __init__(self, bag_dir, count):
         rospy.init_node('rosbag_recorder', anonymous=True)
         rospy.set_param('/use_sim_time', False)
         self.goal_sub = rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.goal_callback)
@@ -24,15 +33,25 @@ class RosbagRecorder:
         self.recording = False
         self.pose_position = None
         self.cmd_vel = None
-        self.bag_path = path
+        self.bag_dir = bag_dir
+        self.bag_count = 1
+        self.total_count = int(count)
 
     def goal_callback(self, msg):
+        # 如果当前已录制的 bag 文件数量大于等于传入的 count 参数，就不再录制
+        if self.bag_count > self.total_count:
+            rospy.loginfo("Recording limit reached. Exiting...")
+            self.shutdown_node()
+            return
         # 更新时间戳为当前时间
         msg.header.stamp = rospy.Time.now()
         self.goal_position = msg.goal.target_pose.pose.position
         rospy.loginfo("Goal received, starting rosbag recording...")
-        self.start_recording()
 
+        bag_file_name = f"test-{self.bag_count}.bag"
+        bag_file_path = os.path.join(self.bag_dir, bag_file_name)
+        self.bag_path = bag_file_path  
+        self.start_recording()
     def pose_callback(self, msg):
         # 更新时间戳为当前时间
         msg.header.stamp = rospy.Time.now()
@@ -47,25 +66,28 @@ class RosbagRecorder:
 
     def start_recording(self):
         if not self.recording:
-            # self.bag_process = subprocess.Popen(['rosbag', 'record', '/amcl_pose', '/cmd_vel', '/some_topic1', '/some_topic2'])
             self.bag_process = subprocess.Popen(['rosbag', 'record', '-O', self.bag_path, '/amcl_pose', '/imu', '/obstacle_detection', '/odom', '/sys_info'], preexec_fn=os.setsid)
             self.recording = True
 
     def stop_recording(self):
         if self.recording and self.bag_process:
             rospy.loginfo("Stopping rosbag recording...")
-            # self.bag_process.terminate()
-            # self.bag_process.wait(timeout=10)
             # os.killpg(os.getpgid(self.bag_process.pid), signal.SIGTERM)  # 首先发送SIGTERM信号
             os.killpg(os.getpgid(self.bag_process.pid), signal.SIGINT)  # 首先发送SIGINT信号
-            # self.bag_process.wait(timeout=10)
             try:
-                self.bag_process.wait(timeout=5)  # 等待进程终止，如果超过5秒，进行强制终止
+                self.bag_process.wait(timeout=10)  # 等待进程终止，如果超过10秒，进行强制终止
             except subprocess.TimeoutExpired:
                 rospy.logwarn("Rosbag process did not terminate, sending SIGKILL...")
                 os.killpg(os.getpgid(self.bag_process.pid), signal.SIGKILL)  # 强制终止
-                self.bag_process.wait()  # 再次等待确保进程已经被终止
+                self.bag_process.wait()  # 再次等待确保进程已经被终止                      
             self.recording = False
+            self.bag_count += 1
+
+            # 如果录制的包数量达到总数，停止整个节点
+            if self.bag_count > self.total_count:
+                rospy.loginfo("All recordings completed. Exiting...")
+                rospy.sleep(2)  # 等待一秒，确保rosbag_recorder节点已经退出
+                self.shutdown_node()
 
     def check_stop_condition(self):
         if self.goal_position and self.pose_position and self.cmd_vel:
@@ -76,10 +98,18 @@ class RosbagRecorder:
     def calculate_distance(self, pos1, pos2):
         return ((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2) ** 0.5
 
+    def shutdown_node(self):
+        """停止ROS节点和当前进程"""
+        rospy.loginfo("Shutting down ROS node and killing the process.")
+        rospy.sleep(2)  # 等待2秒，确保rosbag_recorder节点已经退出
+        rospy.signal_shutdown("Completed all recordings.")
+        sys.exit(0)  # 终止Python脚本
+
 if __name__ == '__main__':
     try:
-        bag_path = sys.argv[1]
-        recorder = RosbagRecorder(bag_path)
+        bag_folder_path = sys.argv[1]
+        count = sys.argv[2]
+        recorder = RosbagRecorder(bag_folder_path, count)
         rospy.spin()
     except rospy.ROSInterruptException:
         pass