fix：修复录包错误

simulation/data/pjirobot/data/config/static_config.json

@@ -1,10 +1,6 @@
 {
    "modeTypes" : null,
    "netConfig" : {
-      "accessKey" : "sicEVjENPgTIoXLfpQLKrUfFxOazYg",
-      "capem" : "-----BEGIN CERTIFICATE-----\nMIIDcTCCAlmgAwIBAgIJAMN+Bt2mf59BMA0GCSqGSIb3DQEBCwUAME8xCzAJBgNV\nBAYTAkNOMRAwDgYDVQQIDAdKaWFuZ3N1MQ8wDQYDVQQHDAZTdXpob3UxDDAKBgNV\nBAoMA1hYWDEPMA0GA1UEAwwGU2VsZkNBMB4XDTIzMDIwNzAyMTIxOVoXDTMzMDIw\nNDAyMTIxOVowTzELMAkGA1UEBhMCQ04xEDAOBgNVBAgMB0ppYW5nc3UxDzANBgNV\nBAcMBlN1emhvdTEMMAoGA1UECgwDWFhYMQ8wDQYDVQQDDAZTZWxmQ0EwggEiMA0G\nCSqGSIb3DQEBAQUAA4IBDwAwggEKAoIBAQCvVr7SwVajd41tRkiSZI8KkCX6yuNa\np1btHCDnncQhlKOoTHgiV56EYAPw/P6w+7hOPDUcpUepfG67qcVgs+9pWvLx6ECv\n94UXtCOVsST/NQe8lbMAJSwLDr9J6kfeKqUtgX2arpc5QjIwJoT19tOv9ITHn3Hr\nr1LjT+PSghFgJuTJK40aLVr2dJAeL4ne/dQp5rbC1El1hk4vYfh1Ept6hptPuwMp\n/L6ah5zsHYdC+vxrT+YaxOPZc3kD0M+Gx4PyXCz2yBq/6TsgET1/DZDzdFG4nVgn\nFiqJ9Bdr/4aXXtsF1SApdcPxIRLxmowAz/og3dEBCju+4Mx7fwKvmk9RAgMBAAGj\nUDBOMB0GA1UdDgQWBBRt9nealGx2UWeLlOSC92NEdUThtzAfBgNVHSMEGDAWgBRt\n9nealGx2UWeLlOSC92NEdUThtzAMBgNVHRMEBTADAQH/MA0GCSqGSIb3DQEBCwUA\nA4IBAQBfWHkb3+aPzbD+Hanaht18PLWl6JQmFUZ3/X6VJ80Wo3Uo5uZo7SyrmcuG\n72NeTwLnWEymqguXhgD1wK78Fx1UAsM3y/emqL69O9Fecw5IVXlSzEmFrPQbT/j0\nDEt8iRmH/X0fU/sVUiZ+NpCWLWwibko8kSy6et0wqXwlU50zzMrCcQmIHSAYPhbt\nnTK8T92VFMHokTt5KT8QPKQ4YXkbwnhoDYOjz58hHltsWWNvHVroraAGQfvU7cPf\nTAG2E9vOtBDVv6HzLQRouG212VDoxPufOKTw+GHwkkxk1KDNbJFMRhFopolltcVQ\nX7HZTyfbxiN9luP/M+8CeqeJutOk\n-----END CERTIFICATE-----\n",
-      "clientkey" : "-----BEGIN RSA PRIVATE KEY-----\nMIIEpAIBAAKCAQEA3brod3HpHWgF1koV2mJpJaEWyf8MBu5igR6ebVACEP1K5xI8\nE3xP6f7TTNdJCK08PEnn3ytmP9DveSuK78qaUzq+HbJgxyJIHiaohcsKZ3MO48n4\nT+T8Sbs9eL1ovYYS2Yz10sgVwow03i/rv9YbUzz0MKT0Akpp3uHKZHK6NrwZskML\npMbq2UbVrv9SX0QhDDsqwFzIMpmvIy50fkLriSL/1v64GhCWk1uBPq0+3edlMdKb\neievG5sj6hn+wObawJLt1nlzvNEf0otgROt/tGPvO+An6ZqbNlwWKAXYq05HIuSL\nqGisGc2c6ZoP78Ih2xdlvVzNK22D10ysRoNduwIDAQABAoIBAQDBzfQRKySqCllH\nw3u0ZV4ogNQNfPtDN6wr1dp+C3ey5+JHdJ3EHl0vVwfT2zy/88N5otOkMifuUa5v\nR8rp6znW07qG9Ho4tFM632lfgGg4oc369ncvRiPqQlnppgzd00m4k8fuE+tJIAm5\nx0MVa4x4xgdeWV/afLZrFQo1oD35RfHJfCyDMkgFK7xkKylm7grYqQ4fRLteOiBJ\nZVs8NT+h3ZzD3BvTMUPetYZmCcXdk2vWqC7oqISnLD7VkPx5j5l+7ZCRb5xfAhfO\n3sX4FuexZsWTkOsTxsRNdmmb/6rwBUdfzf/sU5Bh0/1ZHzbCSLHyw/KaGchL2rTr\nuAmdf8W5AoGBAPPIsVJY9K5eTGL0EfTfiAH9ZcP4arEVLfS3KveD+2BSqvJYKHlP\nAMKxwBusrP7szK2mAKHIvCFdmnXKMYQ4SM8goaTg9ieGnmU+Bzw2cZjHT+19fNN3\nvF+KodH6QZCIhwffNYdxyMKBiWlNMDCWg2WeKdpbN8+C19TaRGKSDgAdAoGBAOjX\nTe/7bt2+YIJz46x+SO7Ndf9VsNt68TPlo9/u3itFWq8QAn6h0RBusoKZQyyP//IR\nYcAzFmB6B22rJFB+1HAWIEHBQ7jVW0FBO6dbiCw0KkNCIO0OD82mpcepGh+VzlMG\nrx7WpTfxudpOQIQ5AN1o3Q55N2fvFXMui4h+Fh23AoGAGQbZbWLEe8IihWYZEdKA\n2/NvpxaxnUxXU3AacR6Th+f0tLxoK0v5AAPPfQfVGrakrI8GMZWx+prpaH5BZxmA\nIANxlMOjgZtIV4xuobfVIaIUioT/c5YvOH+67RuL75KUijJYBHeh7JXvzWenMtXA\n14XNNiV3LU55adiefKAR3+ECgYA0Zof+syovMzILCcO6RS1quMiObx1/N3rjABJS\nSjKrsE8JSOQW1Zn0RcVcyNsqQzoJo0nJdHXbvnq/fp2MlA6BLymRQspX5YRE+kZP\nkrThpdfBDTfVwa1izddPSMo9Q3yMiNuOaCYEohz+eU/hseZ07a2aHUOP22mLY3U5\ngY6+nwKBgQCbCOYKtwvDFE4jHPTg6aOw84PnsVu4iwDvsCzIsiwG842UdIzzQG4S\nr9W/W/FwjjBquRnn+1ERz25aI9ONPH7l3tT0nsst4c4rqlWhPR19FE7LPK/HV2nj\ncetQsri/mIvh+7vevZfl033ZQLR9psy7RzI9vduVzL4fljSqJrNuWQ==\n-----END RSA PRIVATE KEY-----\n",
-      "clientpem" : "-----BEGIN CERTIFICATE-----\nMIIDGjCCAgICCQDyK58wtoNykzANBgkqhkiG9w0BAQsFADBPMQswCQYDVQQGEwJD\nTjEQMA4GA1UECAwHSmlhbmdzdTEPMA0GA1UEBwwGU3V6aG91MQwwCgYDVQQKDANY\nWFgxDzANBgNVBAMMBlNlbGZDQTAeFw0yMzAyMDcwMjEyMjBaFw0zMzAyMDQwMjEy\nMjBaME8xCzAJBgNVBAYTAkNOMRAwDgYDVQQIDAdKaWFuZ3N1MQ8wDQYDVQQHDAZT\ndXpob3UxDDAKBgNVBAoMA1hYWDEPMA0GA1UEAwwGY2xpZW50MIIBIjANBgkqhkiG\n9w0BAQEFAAOCAQ8AMIIBCgKCAQEA3brod3HpHWgF1koV2mJpJaEWyf8MBu5igR6e\nbVACEP1K5xI8E3xP6f7TTNdJCK08PEnn3ytmP9DveSuK78qaUzq+HbJgxyJIHiao\nhcsKZ3MO48n4T+T8Sbs9eL1ovYYS2Yz10sgVwow03i/rv9YbUzz0MKT0Akpp3uHK\nZHK6NrwZskMLpMbq2UbVrv9SX0QhDDsqwFzIMpmvIy50fkLriSL/1v64GhCWk1uB\nPq0+3edlMdKbeievG5sj6hn+wObawJLt1nlzvNEf0otgROt/tGPvO+An6ZqbNlwW\nKAXYq05HIuSLqGisGc2c6ZoP78Ih2xdlvVzNK22D10ysRoNduwIDAQABMA0GCSqG\nSIb3DQEBCwUAA4IBAQCtrsf7RGG4Lo7bVyKP48l2zKcaDqaAcumvze11TyjjYBuO\nQDp8WeDXoa4rQFC1ZIXoQpnFOEBJIpYRnJmCxGUJCbnNvYnidxejhZI7icbdfi3V\nYNPGzXfwDEzM0ZmVVcMFROR1aNMBAAN5UokGZoOuUjLMCwnd94IwWC7itsMsoWhu\n3U8kVMzITcIgAyZC7CsTc3a5Fz9s/9KE6Q3SIjcnFA911nfWBvcZQ+ULykWLQCUh\nxXsLd4SmUHSlvmeFugg2YhRar0xgsC866g4yzdRVIoIHRuxbvhv1h12sSa3gafxv\nujaQLbxYHll+IdLLfedAkDjaLAryiTY3B0R3LIJ6\n-----END CERTIFICATE-----\n",
       "mqttIp" : "mqtt.chinapji.com",
       "mqttPort" : 1883,
       "passward" : "1646326924873211905",

simulation/rosbag_record.py

@@ -9,11 +9,12 @@ import signal
 import subprocess
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from move_base_msgs.msg import MoveBaseActionGoal
+from datetime import datetime
 
 class RosbagRecorder:
     def __init__(self, path):
         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)
@@ -26,36 +27,44 @@ class RosbagRecorder:
         self.bag_path = path
 
     def goal_callback(self, msg):
+        # 更新时间戳为当前时间
+        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()
 
     def pose_callback(self, msg):
+        # 更新时间戳为当前时间
+        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()
 
     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, '/imu', '/obstacle_detection', '/odom', '/sys_info'], preexec_fn=os.setsid)
+            self.bag_process = subprocess.Popen(['rosbag', 'record', '-O', self.bag_path, '/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()
-            os.killpg(os.getpgid(self.bag_process.pid), signal.SIGTERM)  # 首先发送SIGTERM信号
+            # 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=2)  # 等待进程终止，如果超过5秒，进行强制终止
+                self.bag_process.wait(timeout=5)  # 等待进程终止，如果超过5秒，进行强制终止
             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
 
     def check_stop_condition(self):

simulation/rosbag_record_bak.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import rospy
+import rosbag
+import sys
+import os
+import signal
+import subprocess
+from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
+from move_base_msgs.msg import MoveBaseActionGoal
+
+class RosbagRecorder:
+    def __init__(self, path):
+        rospy.init_node('rosbag_recorder', anonymous=True)
+
+        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)
+
+        self.goal_position = None
+        self.bag_process = None
+        self.recording = False
+        self.pose_position = None
+        self.cmd_vel = None
+        self.bag_path = path
+
+    def goal_callback(self, msg):
+        self.goal_position = msg.goal.target_pose.pose.position
+        rospy.loginfo("Goal received, starting rosbag recording...")
+        self.start_recording()
+
+    def pose_callback(self, msg):
+        self.pose_position = msg.pose.pose.position
+        self.check_stop_condition()
+
+    def cmd_vel_callback(self, msg):
+        self.cmd_vel = msg
+        self.check_stop_condition()
+
+    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, '/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()
+            os.killpg(os.getpgid(self.bag_process.pid), signal.SIGTERM)  # 首先发送SIGTERM信号
+            try:
+                self.bag_process.wait(timeout=2)  # 等待进程终止，如果超过5秒，进行强制终止
+            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
+
+    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()
+
+    def calculate_distance(self, pos1, pos2):
+        return ((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2) ** 0.5
+
+if __name__ == '__main__':
+    try:
+        bag_path = sys.argv[1]
+        recorder = RosbagRecorder(bag_path)
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

simulation/run_simulation.sh

@@ -16,24 +16,23 @@ START_POINT=$(echo "$4" | tr ',' ' ')
 END_POINT=$(echo "$5" | tr ',' ' ')
 
 BUILD_MAP_NAME="build_map.bag"
+MAP_BAG_NAME="origin_map.bag"
 SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"
 
-# 是否加载障碍物
-if [ "$OBSTACLE_FLAG" == "true" ]; then # 加载
-  echo "Loading default obstacle..."
-  # 动态加载障碍物
-  command="cd $SIMULATION_PATH/pji_work && source devel/setup.bash && rosrun test demo $SIMULATION_PATH/merge_obstacles_data/merged_obstacles_new_0517_1.csv; exec bash"
-  nohup bash -c "$command" >/dev/null 2>&1 &
-#  gnome-terminal --tab -e 'bash -c '\""$command"\"
-  sleep 0.5
-  echo 'bash -c '"$command"
-  echo "Default obstacle loaded"
-fi
+# 激活conda环境
+source activate simulation
 
 # 起点设置
 if [ "$DEFAULT_START_FLAG" == "true" ]; then
   # 默认起点
   echo "Use default start point"
+  #   /home/cicv/work/pji_desktop/simulation/data/build_map_bag/build_map.bag \
+#  # 默认起点配置
+#  python "$SIMULATION_PATH"/catkin_ws/src/goal_publish/script/send_initial_pos_0521.py \
+#    "$SIMULATION_PATH"/data/map_bag/"$MAP_BAG_NAME" \
+#    "$SIMULATION_PATH"/catkin_ws/src/SimulationEnvs/launch/env_node.launch \
+#    "$SIMULATION_PATH"/catkin_ws/src/Navigation/launch/amcl.launch
+#  sleep 1
 elif [ "$DEFAULT_START_FLAG" == "false" ]; then
   # 自定义起点
   echo "Setting start point: $START_POINT"
@@ -44,7 +43,7 @@ elif [ "$DEFAULT_START_FLAG" == "false" ]; then
   sleep 1
   echo 'bash -c '\""$command"\"
   nohup bash -c "$command" >/dev/null 2>&1 &
-  sleep 5
+  sleep 1
 
   ## 发布自定义起点 - rviz
   command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose_rviz $START_POINT"
@@ -63,33 +62,50 @@ if [ "$DEFAULT_END_FLAG" == "true" ]; then
   # 默认终点
   echo "Use default end point"
   ## 发布默认终点
-  command="cd $SIMULATION_PATH/catkin_ws && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/build_map_bag/$BUILD_MAP_NAME $SIMULATION_PATH/evaluation_bag/example_0821_4.bag; exec bash"
+#  command="cd $SIMULATION_PATH/catkin_ws && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/build_map_bag/$BUILD_MAP_NAME $SIMULATION_PATH/evaluation_bag/example_0821_4.bag"
+  command="cd $SIMULATION_PATH/catkin_ws && rm -r build devel && catkin_make && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/map_bag/$MAP_BAG_NAME $SIMULATION_PATH/evaluation_bag/example_0821_4.bag"
   ## 录包
   ### 激活conda环境
-  source activate simulation
-  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
+#  source activate simulation
+#  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
   ## 清除环境
   pkill -f "goal_publish/goal" # 下发自定义/默认终点前必须先清除goal，不能只清除pub_node
   pkill -f "goal_publish/pub_node"
+  pkill -f "rosbag_record"
   sleep 1
   echo 'bash -c '\""$command"\"
+  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >record_bag.log 2>&1 &
   ## 设置终点并执行
   nohup bash -c "$command" >/dev/null 2>&1 &
 elif [ "$DEFAULT_END_FLAG" == "false" ]; then
   # 自定义终点
   echo "Setting end point: $END_POINT"
   ## 发布自定义终点
-  command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal $END_POINT"
+  command="cd $SIMULATION_PATH/catkin_ws && rm -r build devel && catkin_make && source devel/setup.bash && rosrun goal_publish goal $END_POINT"
   ## 录包
   ### 激活conda环境
-  source activate simulation
-  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
+#  source activate simulation
+
   ## 清除环境
   pkill -f "goal_publish/goal"
   pkill -f "goal_publish/pub_node"
+  pkill -f "rosbag_record"
   sleep 1
   echo 'bash -c '\""$command"\"
+  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
   nohup bash -c "$command" >/dev/null 2>&1 &
   ## 设置终点并执行
   echo "End point: $END_POINT has been set"
+fi
+
+# 是否加载障碍物
+if [ "$OBSTACLE_FLAG" == "true" ]; then # 加载
+  echo "Loading default obstacle..."
+  # 动态加载障碍物
+  command="cd $SIMULATION_PATH/pji_work && source devel/setup.bash && rosrun test demo $SIMULATION_PATH/merge_obstacles_data/merged_obstacles_new_0517_1.csv"
+  nohup bash -c "$command" >/dev/null 2>&1 &
+#  gnome-terminal --tab -e 'bash -c '\""$command"\"
+  sleep 0.5
+  echo 'bash -c '"$command"
+  echo "Default obstacle loaded"
 fi