diff --git a/app/navigation2/scripts/Navigation_ROS2_R1_SIM.xml b/app/navigation2/scripts/Navigation_ROS2_R1_SIM.xml
index 24b19bd..9ee92dd 100644
--- a/app/navigation2/scripts/Navigation_ROS2_R1_SIM.xml
+++ b/app/navigation2/scripts/Navigation_ROS2_R1_SIM.xml
@@ -5,6 +5,13 @@
 
    <!-- modules -->
 
+   <module>
+      <name>ros2_launch.sh</name>
+      <parameters>/usr/local/src/hsp/tour-guide-robot/app/navigation2/launch/robot_state_publisher.launch.py use_sim_time:=true</parameters>
+      <workdir>/home/user1/tour-guide-robot/app/navigation2/scripts/</workdir>
+      <node>console</node>
+   </module>
+
    <module>
       <name>ros2</name>
       <parameters>launch robot_state_publisher.launch.py use_sim_time:=true</parameters>
diff --git a/app/navigation2/scripts/ros2_launch.sh b/app/navigation2/scripts/ros2_launch.sh
new file mode 100755
index 0000000..eb15817
--- /dev/null
+++ b/app/navigation2/scripts/ros2_launch.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# This scripts wraps ros2 launch handling the SIGTERM signal - currently bugged in ros2 (https://github.com/ros2/launch/issues/666)
+# In particular, the user can substitute the ros2 launch command in yarpmanager with this script.
+# The syntax is the same but the full path to the launch_file is needed.
+# Stopping the script via yarpmanager will correctly stop the nodes. Killing the script will leave the nodes dangling.
+
+
+_term() {
+	echo "Got SIGTERM"
+	kill -2 -${group}
+}
+
+if [ "$#" -lt 2 ]
+then
+	echo "Usage ros2_launch.sh <package> <launch_file> <args>"
+	exit -1
+fi
+
+trap _term SIGTERM
+
+setsid ros2 launch $@ --noninteractive &
+
+group=$!
+echo "Group pid: $group"
+child=$!
+wait "$child"