|
@@ -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
|
|
|
+'''
|
|
|
|
|
|
|
|
|
|
|
@@ -12,7 +20,7 @@ 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 +32,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):
|
|
|
+
|
|
|
+ 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:02d}.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 +65,28 @@ class RosbagRecorder:
|
|
|
|
|
|
def start_recording(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)
|
|
|
self.recording = True
|
|
|
|
|
|
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.SIGINT)
|
|
|
-
|
|
|
try:
|
|
|
self.bag_process.wait(timeout=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
|
|
|
+ self.bag_count += 1
|
|
|
+
|
|
|
+
|
|
|
+ if self.bag_count > self.total_count:
|
|
|
+ rospy.loginfo("All recordings completed. Exiting...")
|
|
|
+ rospy.sleep(2)
|
|
|
+ self.shutdown_node()
|
|
|
|
|
|
def check_stop_condition(self):
|
|
|
if self.goal_position and self.pose_position and self.cmd_vel:
|
|
@@ -76,10 +97,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)
|
|
|
+ rospy.signal_shutdown("Completed all recordings.")
|
|
|
+ sys.exit(0)
|
|
|
+
|
|
|
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
|
|
|
+ pass
|