refactor: 默认*自定义起终点 - 前端交互参数设置

simulation/rosbag_record.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

@@ -3,90 +3,93 @@
 set -u
 
 # 检查参数数量
-if [ "$#" -ne 1 ]; then
+if [ "$#" -ne 5 ]; then
   echo "Error: Incorrect number of arguments"
-  echo "Usage: $0 BUILD_MAP_NAME"
+  echo "Usage: $0 OBSTACLE_FLAG DEFAULT_START_FLAG DEFAULT_END_FLAG START_POINT END_POINT"
   exit 1
 fi
 
-BUILD_MAP_NAME=$1
-SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"
-
-# 发布自定义起点 - gazebo
-command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose  1 1 0 0 0 0"
-# 清除环境
-pkill -f "goal_publish/initialPose"
-sleep 1
-echo 'bash -c '\""$command"\"
-nohup bash -c "$command" >/dev/null 2>&1 &
-sleep 5
-
-# 发布自定义起点 - rviz
-command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose_rviz  1 1 0 0 0 0"
-# 清除环境
-pkill -f "goal_publish/initialPose_rviz"
-sleep 1
-echo 'bash -c '\""$command"\"
-nohup bash -c "$command" >/dev/null 2>&1 &
-# 这里必须设置不少于5s的延迟时间，否则rviz中的起点设置会出现问题
-sleep 5
+OBSTACLE_FLAG=$1
+DEFAULT_START_FLAG=$2
+DEFAULT_END_FLAG=$3
+START_POINT=$(echo "$4" | tr ',' ' ')
+END_POINT=$(echo "$5" | tr ',' ' ')
 
-# 动态加载障碍物
-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"
-gnome-terminal --tab -e 'bash -c '\""$command"\"
-sleep 0.5
-echo 'bash -c '"$command"
+BUILD_MAP_NAME="build_map.bag"
+SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"
 
-# 发布自定义终点 - 1
-command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 3.26 4.65 0.00537 0 0 0"
-# 清除环境
-pkill -f "goal_publish/goal"
-pkill -f "goal_publish/pub_node"
-sleep 1
-# 开始录包
-nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_1.bag  >/dev/null 2>&1 &
-echo 'bash -c '\""$command"\"
-nohup bash -c "$command" >/dev/null 2>&1 &
-sleep 10
-# 结束录包
-pkill -f "rosbag record"
+# 是否加载障碍物
+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
 
-# 发布自定义终点 - 2
-command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 2.47 -1.17 0.00304 0 0 0"
-# 清除环境
-pkill -f "goal_publish/goal"
-pkill -f "goal_publish/pub_node"
-sleep 1
-# 开始录包
-nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_2.bag  >/dev/null 2>&1 &
-echo 'bash -c '\""$command"\"
-nohup bash -c "$command" >/dev/null 2>&1 &
-sleep 15
-# 结束录包
-pkill -f "rosbag record"
+# 起点设置
+if [ "$DEFAULT_START_FLAG" == "true" ]; then
+  # 默认起点
+  echo "Use default start point"
+elif [ "$DEFAULT_START_FLAG" == "false" ]; then
+  # 自定义起点
+  echo "Setting start point: $START_POINT"
+  ## 发布自定义起点 - gazebo
+  command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose $START_POINT"
+  ### 清除环境
+  pkill -f "goal_publish/initialPose"
+  sleep 1
+  echo 'bash -c '\""$command"\"
+  nohup bash -c "$command" >/dev/null 2>&1 &
+  sleep 5
 
-# 发布自定义终点 - 3
-command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 8.51 3.32 0.00705 0 0 0"
-# 清除环境
-pkill -f "goal_publish/goal"
-pkill -f "goal_publish/pub_node"
-sleep 1
-echo 'bash -c '\""$command"\"
-# 开始录包
-nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_3.bag  >/dev/null 2>&1 &
-nohup bash -c "$command" >/dev/null 2>&1 &
-sleep 20
-# 结束录包
-pkill -f "rosbag record"
+  ## 发布自定义起点 - rviz
+  command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose_rviz $START_POINT"
+  ### 清除环境
+  pkill -f "goal_publish/initialPose_rviz"
+  sleep 1
+  echo 'bash -c '\""$command"\"
+  nohup bash -c "$command" >/dev/null 2>&1 &
+  ### 这里必须设置不少于5s的延迟时间，否则rviz中的起点设置会出现问题
+  sleep 5
+  echo "Start point: $START_POINT has been set"
+fi
 
-# 发布默认终点并录包
-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"
-# 清除环境
-pkill -f "goal_publish/goal" # 下发自定义/默认终点前必须先清除goal，不能只清除pubnode
-pkill -f "goal_publish/pub_node"
-sleep 1
-echo 'bash -c '\""$command"\"
-nohup bash -c "$command" >/dev/null 2>&1 &
-sleep 10
-# 结束录包
-pkill -f "goal_publish/pub_node"
+# 终点设置
+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"
+  ## 录包
+  ### 激活conda环境
+  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"
+  sleep 1
+  echo 'bash -c '\""$command"\"
+  ## 设置终点并执行
+  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"
+  ## 录包
+  ### 激活conda环境
+  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"
+  pkill -f "goal_publish/pub_node"
+  sleep 1
+  echo 'bash -c '\""$command"\"
+  nohup bash -c "$command" >/dev/null 2>&1 &
+  ## 设置终点并执行
+  echo "End point: $END_POINT has been set"
+fi