Check and start ROS sim/real sketchup

needs tests

Author: MOJOliciousFTW

run_this_first.m

@@ -7,6 +7,8 @@
 UI = [];
 UI.simulation = 1;
 UI.port = 11311;
+UI.real_ros_master_ip = '172.31.1.21';
+UI.sim_ros_master_ip = 'localhost';
 
 UI.timestep = 0.001;
 UI.motion_duration = 20;
@@ -17,9 +19,9 @@
 
 % User eval
 if UI.simulation
-    UI.ros_master_ip = 'localhost';
+    UI.ros_master_ip = UI.sim_ros_master_ip;
 else
-    UI.ros_master_ip = '172.31.1.21';
+    UI.ros_master_ip = UI.real_ros_master_ip;
 end
 
 for i=1:7
@@ -133,6 +135,7 @@
     end
 
 end
+clear tmp
 disp('Inverse kinematics done.')
 
 %%
@@ -175,6 +178,7 @@
     Quat_path_filtered(i,:) = rotm2quat(tmp(1:3,1:3));
     Euler_path_filtered(i,:) = quat2eul(Quat_path_filtered(i,:));
 end
+clear tmp
 
 %% Null space 
 
@@ -185,7 +189,7 @@
 figure(1)
 
 %% ROS
-rosinit(UI.masterURI)
+run ./scripts/Check_start_ROS.m
 
 %% Simulink
 simulink_desired_joint_path = [];

scripts/Check_start_ROS.m

@@ -0,0 +1,54 @@
+ROS_STA = [];
+ROS_STA.ros_already_running = 0;
+ROS_STA.ros_started = 0;
+ROS_STA.ros_running = 0;
+ROS_STA.no_local_ros_core = 0;
+ROS_STA.current_port = NaN;
+ROS_STA.current_ros_master_ip = NaN;
+
+try 
+    rosnode list
+    ROS_STA.ros_already_running = 1;
+catch
+    if UI.simulation && (~exist('core','var') || isempty(core) || ~isvalid(core))
+        core = ros.Core(UI.port);
+    end
+    try
+        rosinit(UI.masterURI)
+    catch
+        warning('Oopsie, rosshutdown then rosinit again..')
+        rosshutdown
+        rosinit(UI.masterURI)
+    end
+    ROS_STA.ros_started = 1;
+end
+
+
+if ROS_STA.ros_already_running || ROS_STA.ros_started
+    ROS_STA.ros_running = 1;
+end
+
+if ~exist('core','var') || isempty(core)
+    ROS_STA.no_local_ros_core = 1;
+    if UI.simulation
+        error('Simulation selected, but no local core found.')
+    end
+    if ~ros.internal.NetworkIntrospection.isMasterReachable(UI.masterURI)
+        error('Will the real ros master please stand up?')
+    else
+        [ROS_STA.current_ros_master_ip, ROS_STA.current_port] = ...
+            ros.internal.NetworkIntrospection.getHostAndPortFromMasterURI(UI.masterURI);
+    end
+else
+    ROS_STA.no_local_ros_core = 0;
+    [ROS_STA.current_ros_master_ip, ROS_STA.current_port] = ...
+            ros.internal.NetworkIntrospection.getHostAndPortFromMasterURI(core.MasterURI);
+    if UI.simulation
+        if strcmp(ROS_STA.current_ros_master_ip, UI.real_ros_master_ip)
+            error('Simulation selected, but ROS master ip is same as core.')
+        end
+        clear tmp
+    end
+end
+
+ROS_STA