From 57f38b87a89961c4f8820877a715c00791101f84 Mon Sep 17 00:00:00 2001
From: Koen de Vos <k.d.vos@tue.nl>
Date: Thu, 29 Sep 2022 15:23:33 +0200
Subject: [PATCH 1/6] First try overall restructure

---
 launch/start.launch                    | 181 +++++--------------------
 launch/startSingle.launch              |  79 +++++++++++
 launch/subfiles/laser.launch           |  19 +++
 launch/subfiles/launchsim.launch       |  35 +++++
 launch/subfiles/lowlevelcontrol.launch |  23 ++++
 launch/subfiles/spawn_robot.launch     |  24 ++++
 launch/subfiles/transforms.launch      |  48 +++++++
 7 files changed, 260 insertions(+), 149 deletions(-)
 create mode 100644 launch/startSingle.launch
 create mode 100644 launch/subfiles/laser.launch
 create mode 100644 launch/subfiles/launchsim.launch
 create mode 100644 launch/subfiles/lowlevelcontrol.launch
 create mode 100644 launch/subfiles/spawn_robot.launch
 create mode 100644 launch/subfiles/transforms.launch

diff --git a/launch/start.launch b/launch/start.launch
index 686ea94..7cd2f16 100644
--- a/launch/start.launch
+++ b/launch/start.launch
@@ -1,176 +1,59 @@
 <?xml version="1.0"?>
 <launch>
 	<!-- Configurable Parameters -->
-
+	<!-- Robot and Load Naming -->
 	<arg name="robotName" default="ropod"/>
-	<!--arg name="robotName" default="ropod_tue_1"/-->
 	<arg name="robotCommonName" default="ropod"/>
 	<arg name="loadName" default="mobidik"/>
 	<arg name="LOAD_ATTACHED" default="false" />
+	<!-- Laser parameters -->
 	<arg name="laser" default="true"/>
 	<arg name="laser1_name" value ="laser"/>
 	<arg name="laser2_name" value ="laser_back"/>
 	<arg name="add_laser_back" value="false"/>
-	<!--    <arg name="laser1_name" value ="hokuyo_laser_ip"/>-->
+	<!-- Camera parameters -->
 	<arg name="asus_cam" default="false"/>
 	<arg name="asus_cam_name" value="asusxtion"/>
 	<arg name="asus_cam_calibration" default="false"/>
+	<!-- Application parameters -->
 	<arg name="gmapping" default="false"/>
-	<!--<arg name="floorplan" default="$(find ropod_demo_dec_2017)/config/shape/floorplan_ropod_demo"/>-->
-	<!-- <arg name="floorplan" default="$(find ed_object_models)/models/hospital_test_amcl/walls/shape/Floorplan_demo_ROPOD_amcl"/> -->
 	<arg name="floorplan" default="$(find ropod_maps)/onsite_generic_demo/map"/>
 	<arg name="ROBOT_REAL" default="$(optenv ROBOT_REAL)" />
+	<!-- Path variables -->
 	<arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
-
-	<!--	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotName)"/>-->
 	<arg name="robotPackage" default="ropod_tue_2_bringup"/>
 	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
 
-	<group ns="$(arg robotName)">
-		<!-- Start gmapping if required-->
-		<group if="$(arg gmapping)">
-			<include file="$(find robot_common)/launch/localization/gmapping.launch">
-				<arg name="robot" value="$(arg robotCommonName)" />
-				<arg name="sensor" value="$(arg laser1_name)"/>
-			</include>
-		</group>
-
-		<group if="$(arg ROBOT_REAL)">
-			<!-- Do for both: Simulator and Real Robot -->
-
-			<node name="ropod_low_level_control" pkg="ropod_low_level_control" type="ropod_low_level_control_ros" output="screen">
-
-				<remap from="/$(arg robotName)/ropod_low_level_control/odom" to="/$(arg robotName)/odom_incomplete"/>
-                <param name="platform_dxdy_cntr/Kgain" value="350.0"/>
-				<param name="platform_dxdy_cntr/I_fhz" value="0.25"/>
-				<param name="platform_dxdy_cntr/LPF_fhz" value="50.0"/>
-				<param name="platform_dxdy_cntr/LL_wz_fhz" value="20.0"/>
-				<param name="platform_dxdy_cntr/LL_wp_fhz" value="20.0"/>
-				<param name="platform_dtheta_cntr/Kgain" value="200.0"/>
-				<param name="platform_dtheta_cntr/I_fhz" value="0.3"/>
-				<param name="platform_dtheta_cntr/LPF_fhz" value="50.0"/>
-				<param name="platform_dtheta_cntr/LL_wz_fhz" value="20.0"/>
-				<param name="platform_dtheta_cntr/LL_wp_fhz" value="20.0"/>
-				<param name="platform_dvarphi_cntr/Kgain" value="0.10"/>
-				<param name="platform_dvarphi_cntr/I_fhz" value="0.0"/>
-				<param name="platform_dvarphi_cntr/LPF_fhz" value="50.0"/>
-				<param name="platform_dvarphi_cntr/LL_wz_fhz" value="2.0"/>
-				<param name="platform_dvarphi_cntr/LL_wp_fhz" value="10.0"/>
-			</node>
-
-			<!-- dynamic tf broadcaster -->
-			<group unless="$(arg LOAD_ATTACHED)">
-				<!-- <node pkg="$(arg robotName)_bringup" type="ropod_mobidik_tf_broadcaster_only_ropod" name="ropod_mobidik_tf_broadcaster_only_ropod">-->
-				<node pkg="$(arg robotPackage)" type="ropod_mobidik_tf_broadcaster_only_ropod" name="ropod_mobidik_tf_broadcaster_only_ropod">
-					<remap from="/ropod/cmd_vel" to="/$(arg robotName)/cmd_vel"/>
-					<remap from="/ropod/odom" to="/$(arg robotName)/odom"/>
-					<!--				<remap from="/ropod/odom_incomplete" to="/$(arg robotName)/odom_incomplete"/>-->
-				</node>
-			</group>
-
-			<group if="$(arg LOAD_ATTACHED)">
-				<node pkg="$(arg robotName)_bringup" type="ropod_mobidik_tf_broadcaster" name="ropod_mobidik_tf_broadcaster" />
-			</group>
-
-			<!-- Specify static transforms -->
-			<include file="$(arg robot_bringup_path)/parameters/hardware/static_tf.launch">
-				<!--				<arg name="robot" value="$(arg robotCommonName)" /> -->
-				<arg name="robot" value="$(arg robotName)" />
-				<arg name="laser1" value="$(arg laser1_name)"/>
-			</include>
-
-			<!-- Laser publishing -->
-			<group if="$(arg laser)">
-				<!-- <include file="$(find robot_common)/launch/hardware/hokuyo_laser.launch">
-					<arg name="bringup_path" value="$(arg robot_bringup_path)"/>
-					<arg name="name" value="$(arg laser1_name)"/>
-					<arg name="robot" value="$(arg robotCommonName)"/>
-				</include> -->
-
-
-
-
-				<group ns="$(arg laser1_name)">
-					<node pkg="urg_node" type="urg_node" name="urg_node" output="screen">
-						<rosparam command="load" file="$(arg robot_bringup_path)/parameters/hardware/hokuyo_laser_front.yaml"/>
-						<remap from="/diagnostics" to="diagnostics_back"/>
-					</node>
-
-<!--
-					<node name="urg_node" pkg="urg_node" type="urg_node" output="screen">
-					    <param name="ip_address" value="192.168.44.127"/>
-					    <param name="serial_port" value="/dev/ttyACM0"/>
-					    <param name="serial_baud" value="115200"/>
-					    <param name="frame_id" value="laser"/>
-					    <param name="calibrate_time" value="true"/>
-					    <param name="publish_intensity" value="false"/>
-					    <param name="publish_multiecho" value="false"/>
-					    <param name="angle_min" value="-1.5707963"/>
-					    <param name="angle_max" value="1.5707963"/>
-					    <remap from="scan" to="/ropod/laser/scan" />
-					  </node>
--->
-
-
-					<node name="laserBack_NoiseReduction" pkg="ropod_tue_2_bringup" type="laserBack_NoiseReduction" output="screen">
-
-					</node>
-
-				</group>
-
-			</group>
-		</group>
-	</group>
-
-	<group unless="$(arg ROBOT_REAL)">
-		<!-- SIMULATION -->
-		<param name="/use_sim_time" value="true" />
-		<group unless="$(arg LOAD_ATTACHED)">
-			<include file="$(find ropod_sim_model)/launch/simulator/gazebo_simulator.launch">
-				<arg name="robot" value="$(arg robotCommonName)" />
-				<arg name="model_name" value="$(arg robotCommonName)_$(arg loadName)" />
-				<arg name="model_path" value="$(arg robot_path)/$(arg robotCommonName)_sim_model" />
-				<arg name="movingObjects" value="false" />
-				<!--  <arg name="doubleLaser" value="false"/>-->
-			</include>
-
-			<!-- Launch tf broadcaster for ropod robot -->
-			<node pkg="$(arg robotPackage)" type="ropod_mobidik_tf_broadcaster_only_ropod" name="ropod_mobidik_tf_broadcaster_only_ropod" />
-
-		</group>
-
-
-		<group if="$(arg LOAD_ATTACHED)">
-
-			<include file="$(find ropod_sim_model)/launch/simulator/gazebo_simulator.launch">
-				<arg name="robot" value="$(arg robotCommonName)" />
-				<arg name="model_name" value="$(arg robotCommonName)_$(arg loadName)" />
-				<arg name="model_path" value="$(arg robot_path)/$(arg robotCommonName)_model" />
-				<!--  <arg name="doubleLaser" value="false"/>-->
-			</include>
-
-			<!-- Launch tf broadcaster for ropod robot -->
-			<node pkg="$(arg robotPackage)" type="ropod_mobidik_tf_broadcaster_simulator" name="ropod_mobidik_tf_broadcaster_simulator" />
-
-		</group>
-
-		<!-- Specify static transforms -->
-		<include file="$(arg robot_bringup_path)/parameters/hardware/static_tf.launch">
-			<!-- arg name="robot" value="$(arg robotCommonName)" /> -->
-			<arg name="robot" value="$(arg robotName)" />
-			<arg name="laser1" value="$(arg laser1_name)"/>
-		</include>
-		<node pkg="tf" type="static_transform_publisher" name="base2LRFBack" args="-0.285 0.0 0.27 3.14 0 0 /$(arg robotName)/base_link /$(arg robotName)/$(arg laser2_name)/scan 100" />
 
+	<!-- Include Launch files -->
+	<!-- Launch Simulator -->
+	<include unless="$(arg ROBOT_REAL)" file="$(arg robot_bringup_path)/launch/subfiles/launchsim.launch"
+		/>
+	<!-- Include instance of robot -->
+	<include file="$(arg robot_bringup_path)/launch/startSingle.launch">
+		<!-- Robot and Load Naming -->
+		<arg name="robotName" value="$(arg robotName)"/>
+		<arg name="robotCommonName" value="$(arg robotCommonName)"/>
+		<arg name="LOAD_ATTACHED" value="$(arg LOAD_ATTACHED)"/>
+		<!-- Laser parameters -->
+		<arg name="laser" value="$(arg laser)"/>
+		<arg name="laser1_name" value="$(arg laser1_name)"/>
+		<arg name="laser2_name" value="$(arg laser2_name)"/>
+		<arg name="add_laser_back" value="$(arg add_laser_back)"/>
+		<!-- Camera parameters -->
+		<arg name="asus_cam" value="$(arg asus_cam)"/>
+		<arg name="asus_cam_name" value="$(arg asus_cam_name)"/>
+		<arg name="asus_cam_calibration" value="$(arg asus_cam_calibration)"/>
+		<!-- Application parameters -->
+		<arg name="gmapping" value="$(arg gmapping)"/>
+		<arg name="ROBOT_REAL" value="$(arg ROBOT_REAL)"/>	
+		<!-- Path variables -->
+		<arg name="robot_path" value="$(arg robot_path)"/>	
+		<arg name="robotPackage" value="$(arg robotPackage)"/>	
+		<arg name="robot_bringup_path" value="$(arg robot_bringup_path)"/>	
+	</include>
 
-		<!-- Add floorplan. -->
-		<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(arg floorplan).urdf -urdf -z 1 -model floorplan -x -4.5 -y 2.0 -z 0.0 -Y 0.0" />
 
-		<!-- Launch RVIZ -->
-		<include file="$(arg robot_bringup_path)/launch/tools/rviz.launch">
-			<arg name="robot_bringup" value="$(arg robot_bringup_path)"/>
-		</include>
 
-	</group>
 
 </launch>
diff --git a/launch/startSingle.launch b/launch/startSingle.launch
new file mode 100644
index 0000000..94e7fd7
--- /dev/null
+++ b/launch/startSingle.launch
@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<launch>
+	<!-- Configurable Parameters -->
+	<!-- Robot and Load Naming -->
+	<arg name="robotName" default="ropod2"/>
+	<arg name="robotCommonName" default="ropod"/>
+	<arg name="loadName" default="mobidik"/>
+	<arg name="LOAD_ATTACHED" default="false" />
+	<!-- Laser parameters -->
+	<arg name="laser" default="true"/>
+	<arg name="laser1_name" default ="laser"/>
+	<arg name="laser2_name" default ="laser_back"/>
+	<arg name="add_laser_back" default="false"/>
+	<!-- Camera parameters -->
+	<arg name="asus_cam" default="false"/>
+	<arg name="asus_cam_name" default="asusxtion"/>
+	<arg name="asus_cam_calibration" default="false"/>
+	<!-- Application parameters -->
+	<arg name="gmapping" default="false"/>
+	<arg name="floorplan" default="$(find ropod_maps)/onsite_generic_demo/map"/>
+	<arg name="ROBOT_REAL" default="$(optenv ROBOT_REAL)" />
+	<!-- Path variables -->
+	<arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
+	<arg name="robotPackage" default="ropod_tue_2_bringup"/>
+	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
+	<!-- Launch these nodes in the robot namespace -->
+	<group ns="$(arg robotName)">
+		<!-- Launch these nodes if we are working on the real robot hardware -->
+		<group if="$(arg ROBOT_REAL)">
+			<!-- #TODO check if we need to include gmapping, and which files.
+					Currently removed due to reliance on outdated files -->
+			<!-- Launch the lowlevelcontrol node -->
+			<include file="$(arg robot_bringup_path)/launch/subfiles/lowlevelcontrol.launch">
+				<arg name="robotName" value="$(arg robotName)"/>
+			</include>
+			<!-- Launch the transformation publishing nodes -->
+			<include file="$(arg robot_bringup_path)/launch/subfiles/transforms.launch">
+				<arg name="robotName" value="$(arg robotName)"/>
+				<arg name="robotPackage" value="$(arg robotPackage)"/>
+				<arg name="robot_path" value="$(arg robot_path)"/>
+		  		<arg name="robot_bringup_path" value="$(arg robot_bringup_path)"/> 
+		  		<arg name="LOAD_ATTACHED" value="$(arg LOAD_ATTACHED)"/>
+				<arg name="ROBOT_REAL" value="$(arg ROBOT_REAL)"/>
+				<arg name="laser1_name" value="$(arg laser1_name)"/>
+				<arg name="add_laser_back" value="$(arg add_laser_back)"/>
+				<arg name="laser2_name" value ="$(arg laser2_name)"/>
+			</include>
+			<!-- Launch the laser nodes -->
+			<include file="$(arg robot_bringup_path)/launch/subfiles/laser.launch">
+				<arg name="laser" value="$(arg laser)"/>
+    			<arg name="laser1_name" value="$(arg laser1_name)"/>
+				<arg name="robot_path" value="$(arg robot_path)"/>
+    			<arg name="robotPackage" value="$(arg robotPackage)"/>
+    			<arg name="robot_bringup_path" value="$(arg robot_bringup_path)"/>
+			</include>
+		</group>
+		<!-- Launch these nodes if we are working in simulation -->
+		<group unless="$(arg ROBOT_REAL)">
+			<!-- Launch the transformation publishing nodes -->
+			<include file="$(arg robot_bringup_path)/launch/subfiles/transforms.launch">
+				<arg name="robotName" value="$(arg robotName)"/>
+   		   	    <arg name="robotPackage" value="$(arg robotPackage)"/>
+				<arg name="robot_path" value="$(arg robot_path)"/>
+    			<arg name="robot_bringup_path" value="$(arg robot_bringup_path)"/> 
+    			<arg name="LOAD_ATTACHED" value="$(arg LOAD_ATTACHED)"/>
+    			<arg name="ROBOT_REAL" value="$(arg ROBOT_REAL)"/>
+    			<arg name="laser1_name" value="$(arg laser1_name)"/>
+    			<arg name="add_laser_back" value="$(arg add_laser_back)"/>
+    			<arg name="laser2_name" value ="$(arg laser2_name)"/>
+			</include>
+			<!-- Spawn the Robot -->
+			<include file="$(arg robot_bringup_path)/launch/subfiles/spawn_robot.launch">
+				
+			</include>
+		</group>
+	</group>
+
+
+</launch>
\ No newline at end of file
diff --git a/launch/subfiles/laser.launch b/launch/subfiles/laser.launch
new file mode 100644
index 0000000..7061a55
--- /dev/null
+++ b/launch/subfiles/laser.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="laser" default="true"/>
+    <arg name="laser1_name" default="laser"/>
+    <arg name="robotPackage" default="ropod_tue_2_bringup"/>
+    <arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
+    <arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
+
+    <group if="$(arg laser)">
+        <group ns="$(arg laser1_name)">
+            <node pkg="urg_node" type="urg_node" name="urg_node" output="screen">
+                <rosparam command="load" file="$(arg robot_bringup_path)/parameters/hardware/hokuyo_laser_front.yaml"/>
+                <remap from="/diagnostics" to="diagnostics_back"/>
+            </node>
+
+            <node name="laserBack_NoiseReduction" pkg="ropod_tue_2_bringup" type="laserBack_NoiseReduction" output="screen"/>
+        </group>
+    </group>
+</launch>
\ No newline at end of file
diff --git a/launch/subfiles/launchsim.launch b/launch/subfiles/launchsim.launch
new file mode 100644
index 0000000..dfccfce
--- /dev/null
+++ b/launch/subfiles/launchsim.launch
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<launch>
+	
+	<!-- Gazebo enviroment variables -->
+    <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
+    <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
+    <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
+
+	<!-- Configurable Parameters -->
+    <arg name="robot" default="ropod"/>
+	<arg name="floorplan" default="$(find ropod_maps)/onsite_generic_demo/map"/>
+    <arg name="gui" default="true" />
+
+	<arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
+	<arg name="robotPackage" default="ropod_tue_2_bringup"/>
+	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
+
+	<include file="$(find gazebo_ros)/launch/empty_world.launch"> 	
+		<arg name="gui" value ="true"/>
+	</include>
+
+    <node name="throttle_laser_front" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_sim 20.0 /$(arg robot)/laser/scan" />
+    <node name="throttle_laser_back" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_back_sim 20.0 /$(arg robot)/laser/scan_back" />
+
+    <param name="/use_sim_time" value="true" />
+	<!-- Add floorplan. -->
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(arg floorplan).urdf -urdf -z 1 -model floorplan -x -4.5 -y 2.0 -z 0.0 -Y 0.0" />
+	<!-- Launch RVIZ -->
+	<include file="$(arg robot_bringup_path)/launch/tools/rviz.launch">
+		<arg name="robot_bringup" value="$(arg robot_bringup_path)"/>
+	</include>
+
+
+
+</launch>
\ No newline at end of file
diff --git a/launch/subfiles/lowlevelcontrol.launch b/launch/subfiles/lowlevelcontrol.launch
new file mode 100644
index 0000000..e7517c9
--- /dev/null
+++ b/launch/subfiles/lowlevelcontrol.launch
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robotName" default="ropod"/>
+    
+    <node name="ropod_low_level_control" pkg="ropod_low_level_control" type="ropod_low_level_control_ros" output="screen">
+        <remap from="/$(arg robotName)/ropod_low_level_control/odom" to="/$(arg robotName)/odom_incomplete"/>
+        <param name="platform_dxdy_cntr/Kgain" value="350.0"/>
+        <param name="platform_dxdy_cntr/I_fhz" value="0.25"/>
+        <param name="platform_dxdy_cntr/LPF_fhz" value="50.0"/>
+        <param name="platform_dxdy_cntr/LL_wz_fhz" value="20.0"/>
+        <param name="platform_dxdy_cntr/LL_wp_fhz" value="20.0"/>
+        <param name="platform_dtheta_cntr/Kgain" value="200.0"/>
+        <param name="platform_dtheta_cntr/I_fhz" value="0.3"/>
+        <param name="platform_dtheta_cntr/LPF_fhz" value="50.0"/>
+        <param name="platform_dtheta_cntr/LL_wz_fhz" value="20.0"/>
+        <param name="platform_dtheta_cntr/LL_wp_fhz" value="20.0"/>
+        <param name="platform_dvarphi_cntr/Kgain" value="0.10"/>
+        <param name="platform_dvarphi_cntr/I_fhz" value="0.0"/>
+        <param name="platform_dvarphi_cntr/LPF_fhz" value="50.0"/>
+        <param name="platform_dvarphi_cntr/LL_wz_fhz" value="2.0"/>
+        <param name="platform_dvarphi_cntr/LL_wp_fhz" value="10.0"/>
+    </node>
+</launch>
\ No newline at end of file
diff --git a/launch/subfiles/spawn_robot.launch b/launch/subfiles/spawn_robot.launch
new file mode 100644
index 0000000..57be95a
--- /dev/null
+++ b/launch/subfiles/spawn_robot.launch
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch>
+    
+    <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
+    <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
+    <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
+
+    <arg name="robot" default="ropod"/>
+    <arg name="model_name" default="ropod"/>
+    <arg name="model_path" default="$(find ropod_sim_model)" />
+    <arg name="gui" default="true" />
+    <arg name="movingObjects" default="false" />
+
+    <node name="ropod_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf 
+                                                                                        -file $(find ropod_sim_model)simulator/models/ropod/$(arg model_name).sdf 
+                                                                                        -model $(arg robot)
+                                                                                        -x 0
+                                                                                        -y 0
+                                                                                        -z 0
+                                                                                        -R 0 -P 0 -Y 0"/>
+
+    <node name="throttle_laser_front" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_sim 20.0 /$(arg robot)/laser/scan" />
+    <node name="throttle_laser_back" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_back_sim 20.0 /$(arg robot)/laser/scan_back"/>
+</launch>
\ No newline at end of file
diff --git a/launch/subfiles/transforms.launch b/launch/subfiles/transforms.launch
new file mode 100644
index 0000000..c27cce2
--- /dev/null
+++ b/launch/subfiles/transforms.launch
@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Robot and package names and paths -->
+    <arg name="robotName" default="ropod"/>
+    <arg name="robotPackage" default="ropod_tue_2_bringup"/>
+    <arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
+    <arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/> <!-- #TODO findpackage -->
+    <!-- Parameters for load and real hardware -->
+    <arg name="LOAD_ATTACHED" default="false"/>
+    <arg name="ROBOT_REAL" default="$(optenv ROBOT_REAL)"/>
+    <!-- Laser names and exsistence -->
+    <arg name="laser1_name" default="laser"/>
+    <arg name="add_laser_back" default="false"/>
+    <arg name="laser2_name" default ="laser_back"/>
+
+
+    <!-- dynamic tf broadcasters -->
+    <!-- If load attached-->
+    <group if="$(arg LOAD_ATTACHED)">   
+        <!-- On real hardware -->
+		<node if="$(arg ROBOT_REAL)"    
+            pkg="$(arg robotName)_bringup" 
+            type="ropod_mobidik_tf_broadcaster"
+            name="ropod_mobidik_tf_broadcaster" />
+         <!-- In simulation-->
+            <node unless="$(arg ROBOT_REAL)"
+            pkg="$(arg robotPackage)" 
+            type="ropod_mobidik_tf_broadcaster_simulator"
+            name="ropod_mobidik_tf_broadcaster_simulator" />
+	</group>
+    <!-- When no load attached-->
+	<group unless="$(arg LOAD_ATTACHED)">
+		<node pkg="$(arg robotPackage)" type="ropod_mobidik_tf_broadcaster_only_ropod" name="ropod_mobidik_tf_broadcaster_only_ropod">
+			<remap from="/ropod/cmd_vel" to="/$(arg robotName)/cmd_vel"/>
+			<remap from="/ropod/odom" to="/$(arg robotName)/odom"/>
+			<remap from="/ropod/odom_incomplete" to="/$(arg robotName)/odom_incomplete"/>
+		</node>
+	</group>
+	<!-- Specify static transforms -->
+	<include file="$(arg robot_bringup_path)/parameters/hardware/static_tf.launch">
+		<arg name="robot" value="$(arg robotName)" />
+		<arg name="laser1" value="$(arg laser1_name)"/>
+	</include>
+
+    <group if="$(arg add_laser_back)">
+        <node pkg="tf" type="static_transform_publisher" name="base2LRFBack" args="-0.285 0.0 0.27 3.14 0 0 /$(arg robotName)/base_link /$(arg robotName)/$(arg laser2_name)/scan 100" />
+    </group>
+</launch>

From 6b412c3d7236b0f229d30fca3aba08b107203137 Mon Sep 17 00:00:00 2001
From: Koen de Vos <k.d.vos@tue.nl>
Date: Thu, 29 Sep 2022 15:30:44 +0200
Subject: [PATCH 2/6] add sim input arguments

---
 launch/start.launch | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/launch/start.launch b/launch/start.launch
index 7cd2f16..bef7fa9 100644
--- a/launch/start.launch
+++ b/launch/start.launch
@@ -27,8 +27,11 @@
 
 	<!-- Include Launch files -->
 	<!-- Launch Simulator -->
-	<include unless="$(arg ROBOT_REAL)" file="$(arg robot_bringup_path)/launch/subfiles/launchsim.launch"
-		/>
+	<include unless="$(arg ROBOT_REAL)" file="$(arg robot_bringup_path)/launch/subfiles/launchsim.launch">
+		<arg name="robot" value="$(arg robot)"/>
+		<arg name="floorplan" default="$(arg floorplan)"/>
+   		<arg name="gui" default="$(arg gui)" />
+	</include>
 	<!-- Include instance of robot -->
 	<include file="$(arg robot_bringup_path)/launch/startSingle.launch">
 		<!-- Robot and Load Naming -->

From 6d711ca0c7225677353d380e0248668e3f7879a8 Mon Sep 17 00:00:00 2001
From: Koen de Vos <k.d.vos@tue.nl>
Date: Thu, 29 Sep 2022 15:37:39 +0200
Subject: [PATCH 3/6] Improve further, not fully working yet

---
 launch/start.launch                | 20 +++++---------------
 launch/startSingle.launch          |  5 +++--
 launch/subfiles/spawn_robot.launch | 13 +++++--------
 3 files changed, 13 insertions(+), 25 deletions(-)

diff --git a/launch/start.launch b/launch/start.launch
index bef7fa9..e2ca48b 100644
--- a/launch/start.launch
+++ b/launch/start.launch
@@ -8,31 +8,28 @@
 	<arg name="LOAD_ATTACHED" default="false" />
 	<!-- Laser parameters -->
 	<arg name="laser" default="true"/>
-	<arg name="laser1_name" value ="laser"/>
-	<arg name="laser2_name" value ="laser_back"/>
 	<arg name="add_laser_back" value="false"/>
 	<!-- Camera parameters -->
 	<arg name="asus_cam" default="false"/>
-	<arg name="asus_cam_name" value="asusxtion"/>
 	<arg name="asus_cam_calibration" default="false"/>
 	<!-- Application parameters -->
 	<arg name="gmapping" default="false"/>
 	<arg name="floorplan" default="$(find ropod_maps)/onsite_generic_demo/map"/>
 	<arg name="ROBOT_REAL" default="$(optenv ROBOT_REAL)" />
+	<arg name="gui" default="true"/>
 	<!-- Path variables -->
 	<arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
 	<arg name="robotPackage" default="ropod_tue_2_bringup"/>
 	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
-
-
+	
 	<!-- Include Launch files -->
-	<!-- Launch Simulator -->
+		<!-- Launch Simulator -->
 	<include unless="$(arg ROBOT_REAL)" file="$(arg robot_bringup_path)/launch/subfiles/launchsim.launch">
-		<arg name="robot" value="$(arg robot)"/>
+		<arg name="robot" value="$(arg robotName)"/>
 		<arg name="floorplan" default="$(arg floorplan)"/>
    		<arg name="gui" default="$(arg gui)" />
 	</include>
-	<!-- Include instance of robot -->
+		<!-- Include instance of robot -->
 	<include file="$(arg robot_bringup_path)/launch/startSingle.launch">
 		<!-- Robot and Load Naming -->
 		<arg name="robotName" value="$(arg robotName)"/>
@@ -40,20 +37,13 @@
 		<arg name="LOAD_ATTACHED" value="$(arg LOAD_ATTACHED)"/>
 		<!-- Laser parameters -->
 		<arg name="laser" value="$(arg laser)"/>
-		<arg name="laser1_name" value="$(arg laser1_name)"/>
-		<arg name="laser2_name" value="$(arg laser2_name)"/>
 		<arg name="add_laser_back" value="$(arg add_laser_back)"/>
 		<!-- Camera parameters -->
 		<arg name="asus_cam" value="$(arg asus_cam)"/>
-		<arg name="asus_cam_name" value="$(arg asus_cam_name)"/>
 		<arg name="asus_cam_calibration" value="$(arg asus_cam_calibration)"/>
 		<!-- Application parameters -->
 		<arg name="gmapping" value="$(arg gmapping)"/>
 		<arg name="ROBOT_REAL" value="$(arg ROBOT_REAL)"/>	
-		<!-- Path variables -->
-		<arg name="robot_path" value="$(arg robot_path)"/>	
-		<arg name="robotPackage" value="$(arg robotPackage)"/>	
-		<arg name="robot_bringup_path" value="$(arg robot_bringup_path)"/>	
 	</include>
 
 
diff --git a/launch/startSingle.launch b/launch/startSingle.launch
index 94e7fd7..708cb02 100644
--- a/launch/startSingle.launch
+++ b/launch/startSingle.launch
@@ -2,7 +2,7 @@
 <launch>
 	<!-- Configurable Parameters -->
 	<!-- Robot and Load Naming -->
-	<arg name="robotName" default="ropod2"/>
+	<arg name="robotName" default="ropod"/>
 	<arg name="robotCommonName" default="ropod"/>
 	<arg name="loadName" default="mobidik"/>
 	<arg name="LOAD_ATTACHED" default="false" />
@@ -70,7 +70,8 @@
 			</include>
 			<!-- Spawn the Robot -->
 			<include file="$(arg robot_bringup_path)/launch/subfiles/spawn_robot.launch">
-				
+				<arg name="robot" value="$(arg robotName)"/>
+				<arg name="modelname" value="$(arg modelname)"/>
 			</include>
 		</group>
 	</group>
diff --git a/launch/subfiles/spawn_robot.launch b/launch/subfiles/spawn_robot.launch
index 57be95a..822e100 100644
--- a/launch/subfiles/spawn_robot.launch
+++ b/launch/subfiles/spawn_robot.launch
@@ -1,18 +1,15 @@
 <?xml version="1.0"?>
 <launch>
-    
-    <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
-    <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
-    <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
 
     <arg name="robot" default="ropod"/>
     <arg name="model_name" default="ropod"/>
-    <arg name="model_path" default="$(find ropod_sim_model)" />
-    <arg name="gui" default="true" />
-    <arg name="movingObjects" default="false" />
+
+    <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
+    <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
+    <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
 
     <node name="ropod_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf 
-                                                                                        -file $(find ropod_sim_model)simulator/models/ropod/$(arg model_name).sdf 
+                                                                                        -file $(find ropod_sim_model)/simulator/models/ropod/$(arg model_name).sdf 
                                                                                         -model $(arg robot)
                                                                                         -x 0
                                                                                         -y 0

From cfbd5cc240bd09b024412a7a7bd799f0a41ce1aa Mon Sep 17 00:00:00 2001
From: Koen de Vos <k.d.vos@tue.nl>
Date: Thu, 29 Sep 2022 16:08:10 +0200
Subject: [PATCH 4/6] argument improvements

---
 .gitignore                         | 2 ++
 launch/start.launch                | 2 +-
 launch/startSingle.launch          | 3 ++-
 launch/subfiles/spawn_robot.launch | 2 +-
 4 files changed, 6 insertions(+), 3 deletions(-)
 create mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..cacb85a
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+*.vscode/
+*build/
\ No newline at end of file
diff --git a/launch/start.launch b/launch/start.launch
index e2ca48b..e390a85 100644
--- a/launch/start.launch
+++ b/launch/start.launch
@@ -25,7 +25,7 @@
 	<!-- Include Launch files -->
 		<!-- Launch Simulator -->
 	<include unless="$(arg ROBOT_REAL)" file="$(arg robot_bringup_path)/launch/subfiles/launchsim.launch">
-		<arg name="robot" value="$(arg robotName)"/>
+		<arg name="robot" value="$(arg robotCommonName)"/>
 		<arg name="floorplan" default="$(arg floorplan)"/>
    		<arg name="gui" default="$(arg gui)" />
 	</include>
diff --git a/launch/startSingle.launch b/launch/startSingle.launch
index 708cb02..5f695e0 100644
--- a/launch/startSingle.launch
+++ b/launch/startSingle.launch
@@ -71,7 +71,8 @@
 			<!-- Spawn the Robot -->
 			<include file="$(arg robot_bringup_path)/launch/subfiles/spawn_robot.launch">
 				<arg name="robot" value="$(arg robotName)"/>
-				<arg name="modelname" value="$(arg modelname)"/>
+				<arg name="model_name" if="$(arg LOAD_ATTACHED)" value="$(arg robotCommonName)_$(arg loadName)"/>
+				<arg name="model_name" unless="$(arg LOAD_ATTACHED)" value="$(arg robotCommonName)"/>
 			</include>
 		</group>
 	</group>
diff --git a/launch/subfiles/spawn_robot.launch b/launch/subfiles/spawn_robot.launch
index 822e100..50dbd5e 100644
--- a/launch/subfiles/spawn_robot.launch
+++ b/launch/subfiles/spawn_robot.launch
@@ -9,7 +9,7 @@
     <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
 
     <node name="ropod_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf 
-                                                                                        -file $(find ropod_sim_model)/simulator/models/ropod/$(arg model_name).sdf 
+                                                                                        -file $(find ropod_sim_model)/simulator/models/$(arg model_name)/$(arg model_name).sdf 
                                                                                         -model $(arg robot)
                                                                                         -x 0
                                                                                         -y 0

From ba0749b191a8e196565feedc37ec47c6cc023b9d Mon Sep 17 00:00:00 2001
From: Koen de Vos <k.d.vos@tue.nl>
Date: Thu, 29 Sep 2022 16:13:08 +0200
Subject: [PATCH 5/6] Structure improvements

---
 launch/subfiles/laser.launch       | 16 +++++++---------
 launch/subfiles/launchsim.launch   | 15 +++++----------
 launch/subfiles/spawn_robot.launch | 11 ++++-------
 launch/subfiles/transforms.launch  |  2 +-
 4 files changed, 17 insertions(+), 27 deletions(-)

diff --git a/launch/subfiles/laser.launch b/launch/subfiles/laser.launch
index 7061a55..bbadef1 100644
--- a/launch/subfiles/laser.launch
+++ b/launch/subfiles/laser.launch
@@ -6,14 +6,12 @@
     <arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
     <arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
 
-    <group if="$(arg laser)">
-        <group ns="$(arg laser1_name)">
-            <node pkg="urg_node" type="urg_node" name="urg_node" output="screen">
-                <rosparam command="load" file="$(arg robot_bringup_path)/parameters/hardware/hokuyo_laser_front.yaml"/>
-                <remap from="/diagnostics" to="diagnostics_back"/>
-            </node>
-
-            <node name="laserBack_NoiseReduction" pkg="ropod_tue_2_bringup" type="laserBack_NoiseReduction" output="screen"/>
-        </group>
+    <group if="$(arg laser)" ns="$(arg laser1_name)">
+        <node pkg="urg_node" type="urg_node" name="urg_node" output="screen">
+            <rosparam command="load" file="$(arg robot_bringup_path)/parameters/hardware/hokuyo_laser_front.yaml"/>
+            <remap from="/diagnostics" to="diagnostics_back"/>
+        </node>
+        <node name="laserBack_NoiseReduction" pkg="ropod_tue_2_bringup" type="laserBack_NoiseReduction" output="screen"/>
     </group>
+
 </launch>
\ No newline at end of file
diff --git a/launch/subfiles/launchsim.launch b/launch/subfiles/launchsim.launch
index dfccfce..2442470 100644
--- a/launch/subfiles/launchsim.launch
+++ b/launch/subfiles/launchsim.launch
@@ -1,6 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-	
 	<!-- Gazebo enviroment variables -->
     <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
     <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
@@ -10,26 +9,22 @@
     <arg name="robot" default="ropod"/>
 	<arg name="floorplan" default="$(find ropod_maps)/onsite_generic_demo/map"/>
     <arg name="gui" default="true" />
-
+	<param name="/use_sim_time" value="true" />
+	<!-- Path parameters -->
 	<arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
 	<arg name="robotPackage" default="ropod_tue_2_bringup"/>
 	<arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
-
+	
+	<!-- Launch Gazebo with an empty world -->
 	<include file="$(find gazebo_ros)/launch/empty_world.launch"> 	
 		<arg name="gui" value ="true"/>
 	</include>
 
-    <node name="throttle_laser_front" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_sim 20.0 /$(arg robot)/laser/scan" />
-    <node name="throttle_laser_back" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_back_sim 20.0 /$(arg robot)/laser/scan_back" />
-
-    <param name="/use_sim_time" value="true" />
 	<!-- Add floorplan. -->
     <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(arg floorplan).urdf -urdf -z 1 -model floorplan -x -4.5 -y 2.0 -z 0.0 -Y 0.0" />
+
 	<!-- Launch RVIZ -->
 	<include file="$(arg robot_bringup_path)/launch/tools/rviz.launch">
 		<arg name="robot_bringup" value="$(arg robot_bringup_path)"/>
 	</include>
-
-
-
 </launch>
\ No newline at end of file
diff --git a/launch/subfiles/spawn_robot.launch b/launch/subfiles/spawn_robot.launch
index 50dbd5e..3c65caf 100644
--- a/launch/subfiles/spawn_robot.launch
+++ b/launch/subfiles/spawn_robot.launch
@@ -1,13 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-
+    <!-- Configurable Parameters -->
     <arg name="robot" default="ropod"/>
     <arg name="model_name" default="ropod"/>
-
-    <env name="GAZEBO_MODEL_PATH" value="$(find ropod_sim_model)/simulator/models:$(optenv GAZEBO_MODEL_PATH)"/>
-    <env name="GAZEBO_RESOURCE_PATH" value="$(find ropod_sim_model)/simulator:$(optenv GAZEBO_RESOURCE_PATH)"/>
-    <env name="GAZEBO_PLUGIN_PATH" value="$(env CATKIN_WORKSPACE)/devel/lib:$(optenv GAZEBO_PLUGIN_PATH)"/>
-
+    
+    <!-- Spawn a robot entitiy -->
     <node name="ropod_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf 
                                                                                         -file $(find ropod_sim_model)/simulator/models/$(arg model_name)/$(arg model_name).sdf 
                                                                                         -model $(arg robot)
@@ -15,7 +12,7 @@
                                                                                         -y 0
                                                                                         -z 0
                                                                                         -R 0 -P 0 -Y 0"/>
-
+    <!-- Throttle laser output -->
     <node name="throttle_laser_front" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_sim 20.0 /$(arg robot)/laser/scan" />
     <node name="throttle_laser_back" pkg="topic_tools" type="throttle" args="messages /$(arg robot)/laser_back_sim 20.0 /$(arg robot)/laser/scan_back"/>
 </launch>
\ No newline at end of file
diff --git a/launch/subfiles/transforms.launch b/launch/subfiles/transforms.launch
index c27cce2..11b126a 100644
--- a/launch/subfiles/transforms.launch
+++ b/launch/subfiles/transforms.launch
@@ -4,7 +4,7 @@
     <arg name="robotName" default="ropod"/>
     <arg name="robotPackage" default="ropod_tue_2_bringup"/>
     <arg name="robot_path" default="$(env CATKIN_WORKSPACE)/src/platform"/>
-    <arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/> <!-- #TODO findpackage -->
+    <arg name="robot_bringup_path" default="$(arg robot_path)/$(arg robotPackage)"/>
     <!-- Parameters for load and real hardware -->
     <arg name="LOAD_ATTACHED" default="false"/>
     <arg name="ROBOT_REAL" default="$(optenv ROBOT_REAL)"/>

From 642c928f25f20f727aadd3d560018994fbc679b9 Mon Sep 17 00:00:00 2001
From: Koen de Vos <50488480+KdVos@users.noreply.github.com>
Date: Fri, 30 Sep 2022 10:33:27 +0200
Subject: [PATCH 6/6] Delete .gitignore

---
 .gitignore | 2 --
 1 file changed, 2 deletions(-)
 delete mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index cacb85a..0000000
--- a/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-*.vscode/
-*build/
\ No newline at end of file