Skip to content
Snippets Groups Projects
Commit aaa5d3c9 authored by Emil Harlan's avatar Emil Harlan
Browse files

a

parent 97357134
No related branches found
No related tags found
No related merge requests found
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import yaml
class MapFileUpdater(Node):
def __init__(self):
super().__init__('map_file_updater')
self.subscription = self.create_subscription(
String,
'/current_map',
self.map_file_callback,
10
)
self.subscription
def map_file_callback(self, msg):
# Pfad zur YAML-Datei
file_path = '/home/pi/robot_ws/src/robot_loc/config/localisation_params.yaml'
# Neuen Wert für map_file_name aus der empfangenen Nachricht erhalten
new_value = msg.data
# YAML-Datei lesen
with open(file_path, 'r') as file:
yaml_data = yaml.safe_load(file)
# Wert von map_file_name ändern
yaml_data['slam_toolbox']['ros__parameters']['map_file_name'] = new_value
# YAML-Datei mit dem neuen Wert aktualisieren
with open(file_path, 'w') as file:
yaml.safe_dump(yaml_data, file)
self.get_logger().info('Der Wert von map_file_name wurde erfolgreich aktualisiert.')
def main(args=None):
rclpy.init(args=args)
map_file_updater = MapFileUpdater()
rclpy.spin(map_file_updater)
map_file_updater.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
\ No newline at end of file
......@@ -20,6 +20,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
"map_update = robomaster_pi.map_update:main",
"command_executer = robomaster_pi.command_executer:main",
"IMU = robomaster_pi.IMU:main",
"Odom = robomaster_pi.Odom:main",
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment