Skip to content

Commit d676ee5

Browse files
jfkellerjfkeller
committed
merging
2 parents c7ff242 + c2c3d42 commit d676ee5

File tree

86 files changed

+1972
-93
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

86 files changed

+1972
-93
lines changed

‎.env

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
# This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
22
PROJECT_NAME="airstack"
3-
PROJECT_VERSION="0.12.0"
3+
PROJECT_VERSION="0.13.0"
44
# can replace with your docker hub username
55
PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
66
DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd"

‎common/inputrc

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
# /etc/inputrc - global inputrc for libreadline
2+
# See readline(3readline) and `info rluserman' for more information.
3+
4+
# Be 8 bit clean.
5+
set input-meta on
6+
set output-meta on
7+
8+
# To allow the use of 8bit-characters like the german umlauts, uncomment
9+
# the line below. However this makes the meta key not work as a meta key,
10+
# which is annoying to those which don't need to type in 8-bit characters.
11+
12+
# set convert-meta off
13+
14+
# try to enable the application keypad when it is called. Some systems
15+
# need this to enable the arrow keys.
16+
# set enable-keypad on
17+
18+
# see /usr/share/doc/bash/inputrc.arrows for other codes of arrow keys
19+
20+
# do not bell on tab-completion
21+
# set bell-style none
22+
# set bell-style visible
23+
24+
# some defaults / modifications for the emacs mode
25+
$if mode=emacs
26+
27+
# allow the use of the Home/End keys
28+
"\e[1~": beginning-of-line
29+
"\e[4~": end-of-line
30+
31+
# allow the use of the Delete/Insert keys
32+
"\e[3~": delete-char
33+
"\e[2~": quoted-insert
34+
35+
# mappings for "page up" and "page down" to step to the beginning/end
36+
# of the history
37+
# "\e[5~": beginning-of-history
38+
# "\e[6~": end-of-history
39+
40+
# alternate mappings for "page up" and "page down" to search the history
41+
"\e[5~": history-search-backward
42+
"\e[6~": history-search-forward
43+
44+
# mappings for Ctrl-left-arrow and Ctrl-right-arrow for word moving
45+
"\e[1;5C": forward-word
46+
"\e[1;5D": backward-word
47+
"\e[5C": forward-word
48+
"\e[5D": backward-word
49+
"\e\e[C": forward-word
50+
"\e\e[D": backward-word
51+
52+
$if term=rxvt
53+
"\e[7~": beginning-of-line
54+
"\e[8~": end-of-line
55+
"\eOc": forward-word
56+
"\eOd": backward-word
57+
$endif
58+
59+
# for non RH/Debian xterm, can't hurt for RH/Debian xterm
60+
# "\eOH": beginning-of-line
61+
# "\eOF": end-of-line
62+
63+
# for freebsd console
64+
# "\e[H": beginning-of-line
65+
# "\e[F": end-of-line
66+
67+
$endif

‎common/ros_packages/bag_recorder_pid/.gitignore

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
__pycache__/
2+
health_monitor/health_monitor/__pycache__
3+
health_monitor/__pycache__/*
4+
*.pyc
5+
.vscode/
6+
logging/

‎robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py renamed to ‎common/ros_packages/bag_recorder_pid/bag_record_pid/__init__.py

File renamed without changes.

‎common/ros_packages/bag_recorder_pid/bag_record_pid/bag_record_node.py

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
2+
import copy
3+
from rclpy.node import Node
4+
from pathlib import Path
5+
from std_msgs.msg import Bool
6+
import yaml
7+
import subprocess
8+
import signal
9+
import time
10+
import os
11+
import rclpy
12+
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
13+
import datetime
14+
15+
class BagRecorderNode(Node):
16+
def __init__(self):
17+
super().__init__("bag_record_node")
18+
19+
self.node_name = self.get_name() # Get the full node name, including namespace if any
20+
21+
self.declare_parameter(
22+
"cfg_path", str(Path(__file__).parents[3] / "config/cfg.yaml")
23+
)
24+
25+
self.declare_parameter(
26+
"output_dir", str("/logging/")
27+
)
28+
29+
self.declare_parameter(
30+
"mcap_qos_dir", ""
31+
)
32+
33+
self.cfg_path = self.get_parameter("cfg_path").get_parameter_value().string_value
34+
self.output_dir = self.get_parameter("output_dir").get_parameter_value().string_value
35+
self.mcap_qos_dir = self.get_parameter("mcap_qos_dir").get_parameter_value().string_value
36+
37+
self.active = False
38+
self.cfg = yaml.safe_load(open(self.cfg_path))
39+
40+
# TODO: check if the output directory exists.
41+
# Exit if it does not exist.
42+
os.chdir(self.output_dir)
43+
44+
self.command_prefix = ["ros2", "bag", "record", "-s", "mcap"]
45+
self.commands = dict()
46+
self.add_topics()
47+
48+
self.process = dict()
49+
50+
# Create QoS profile for reliable communication
51+
reliable_qos = QoSProfile(
52+
reliability=QoSReliabilityPolicy.RELIABLE,
53+
depth=10
54+
)
55+
56+
# Use reliable QoS for the status publisher
57+
self.status_pub = self.create_publisher(
58+
Bool,
59+
f"{self.node_name}/bag_recording_status",
60+
reliable_qos
61+
)
62+
63+
self.toggle_status = self.create_subscription(
64+
Bool,
65+
f"{self.node_name}/set_recording_status",
66+
self.set_status_callback,
67+
10
68+
)
69+
70+
self.timer = self.create_timer(0.5, self.pub_status_callback)
71+
72+
def add_topics(self):
73+
'''The configuration file looks like
74+
75+
sections:
76+
gps_lidar_spot_depth_status:
77+
mcap_qos: mcap_qos.yaml
78+
args:
79+
- -b
80+
- 4000000000 # ~4GB
81+
- --max-cache-size
82+
- 1073741824 # 1GB
83+
topics:
84+
- /tf
85+
- gq7/ekf/llh_position
86+
87+
The -o or --output argument should not be specified here.
88+
The "mcap_qos" field here will be interpreted as the filename of the MCAP QoS profile.
89+
90+
self.commands[section_name] = {
91+
'prefix': [],
92+
'suffix': [],
93+
}
94+
'''
95+
namespace = self.get_namespace()
96+
97+
for section_name, section_config in self.cfg['sections'].items():
98+
self.commands[section_name] = dict()
99+
100+
# Command lists.
101+
self.commands[section_name]['prefix'] = []
102+
self.commands[section_name]['suffix'] = []
103+
104+
# Populate the initial command line.
105+
self.commands[section_name]['prefix'].extend(self.command_prefix)
106+
107+
# Add the args to the command line.
108+
str_args = [ str(c) for c in section_config['args'] ]
109+
self.commands[section_name]['prefix'].extend(str_args)
110+
111+
# Set the mcap qos profile.
112+
if section_config['mcap_qos'] != "":
113+
mcap_qos_path = os.path.join(self.mcap_qos_dir, str(section_config['mcap_qos']))
114+
self.commands[section_name]['prefix'].append('--storage-config-file')
115+
self.commands[section_name]['prefix'].append(mcap_qos_path)
116+
117+
self.get_logger().warn(
118+
f'CMD for section {section_name}: '
119+
f'{" ".join(self.commands[section_name]["prefix"])}' )
120+
121+
# Add the topics to the command at the end.
122+
self.get_logger().warn(f"Recording section {section_name} topics:")
123+
if 'exclude' in section_config.keys():
124+
if 'topics' in section_config.keys():
125+
self.get_logger().error('Cannot mix exclude with topics.')
126+
exit()
127+
128+
self.commands[section_name]['suffix'].append('--all')
129+
for topic in section_config['exclude']:
130+
self.commands[section_name]['suffix'].append('--exclude')
131+
self.commands[section_name]['suffix'].append(topic)
132+
self.get_logger().info(str(self.commands[section_name]))
133+
else:
134+
for topic in section_config['topics']:
135+
if topic.startswith('/'):
136+
full_topic_name = topic
137+
else:
138+
full_topic_name = f"{namespace}/{topic}"
139+
140+
self.commands[section_name]['suffix'].append(full_topic_name)
141+
self.get_logger().warn(f"{full_topic_name}")
142+
143+
def pub_status_callback(self):
144+
msg = Bool()
145+
msg.data = self.active
146+
self.status_pub.publish(msg)
147+
148+
def set_status_callback(self, msg):
149+
if msg.data:
150+
self.run()
151+
else:
152+
self.interrupt()
153+
154+
def run(self):
155+
if not self.active:
156+
self.active = True
157+
158+
time_suffix = f"{datetime.datetime.now().strftime('%Y%m%d_%H%M%S')}"
159+
160+
for section_name, command_dict in self.commands.items():
161+
cmd = copy.deepcopy(command_dict['prefix'])
162+
163+
# Set the output filename.
164+
output_filename = f"{section_name}_{time_suffix}"
165+
cmd.append('-o')
166+
cmd.append(output_filename)
167+
168+
# Appending an empty string will cause the ros2 bag record to consider the space as a topic
169+
# and introduce an error.
170+
if len(command_dict['suffix']) > 0:
171+
cmd.extend(command_dict['suffix'])
172+
173+
self.get_logger().warn(f"Running command: {' '.join(cmd)}")
174+
#self.get_logger().warn(f"Running command: {cmd}")
175+
176+
self.process[section_name] = dict()
177+
self.process[section_name]['process'] = subprocess.Popen(cmd)
178+
self.process[section_name]['pid'] = self.process[section_name]['process'].pid
179+
self.process[section_name]['output_filename'] = output_filename
180+
self.get_logger().warn(f"Started Recording Section {section_name} with PID {self.process[section_name]['pid']} to {output_filename}")
181+
182+
def interrupt(self):
183+
if self.active:
184+
for section_name, process in self.process.items():
185+
process['process'].send_signal(signal.SIGINT)
186+
process['process'].wait()
187+
self.get_logger().info(f"Ending Recording of Section {section_name} with PID {process['pid']}")
188+
self.get_logger().warn(f"Output filename: {process['output_filename']}")
189+
self.active = False
190+
191+
192+
def main(args=None):
193+
rclpy.init(args=args)
194+
node = BagRecorderNode()
195+
rclpy.spin(node)
196+
node.interrupt()
197+
198+
node.destroy_node()
199+
rclpy.try_shutdown()
200+
201+
202+
if __name__ == "__main__":
203+
main()

‎common/ros_packages/bag_recorder_pid/config/cfg.yaml

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
topics:
2+
["/a", "/b"]

‎common/ros_packages/bag_recorder_pid/config/example-exclude.yaml

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
# Any line that does not start with a / will be automatically prefixed using the
2+
# namespace of the bag_record_pid node.
3+
# mcap_qos is the filename of the MCAP QoS profile. The actual directory will be prefixed by the
4+
# bag_record_pid node via a user specified argument. All MCAP QoS files must be in the same directory.
5+
# The -o or --output argument should not be specified here.
6+
# The -s mcap option will be added automatically.
7+
sections:
8+
spot_rgb:
9+
mcap_qos: mcap_qos.yaml
10+
args:
11+
- -b
12+
- 4000000000 # ~4GB
13+
- --max-cache-size
14+
- 1073741824 # 1GB
15+
exclude:
16+
- /tf
17+
- /tf_static
18+
19+
# Extra or skipped topics:
20+
# - spot/depth_registered/back/points
21+
# - spot/depth_registered/frontleft/points
22+
# - spot/depth_registered/frontright/points
23+
# - spot/depth_registered/left/points
24+
# - spot/depth_registered/right/point
25+
# - spot/depth_registered/back/camera_info
26+
# - spot/depth_registered/back/image
27+
# - spot/depth_registered/frontleft/camera_info
28+
# - spot/depth_registered/frontleft/image
29+
# - spot/depth_registered/frontright/camera_info
30+
# - spot/depth_registered/frontright/image
31+
# - spot/depth_registered/hand/points
32+
# - spot/depth_registered/hand/camera_info
33+
# - spot/depth_registered/hand/image
34+
# - spot/depth_registered/left/camera_info
35+
# - spot/depth_registered/left/image
36+
# - spot/depth_registered/right/camera_info
37+
# - spot/depth_registered/right/image
38+
# ouster/scan
39+
# spot/camera/back/compressed
40+
# spot/camera/frontleft/compressed
41+
# spot/camera/frontright/compressed
42+
# spot/camera/hand/compressed
43+
# spot/camera/left/compressed
44+
# spot/camera/right/compressed
45+
# spot/depth_registered/image_rect/compressed
46+
# spot/depth_registered/image_rect/compressedDepth
47+
# spot/depth_registered/image_rect/theora

0 commit comments

Comments
 (0)