From 135e906018e69a3968127853990dea6c13ee0314 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Mon, 4 Apr 2016 19:44:16 +0200
Subject: [PATCH] prepare obstacle_distance_monitor launch file

---
 cob_bringup/robots/cob4-1.xml                 |  4 +++
 cob_bringup/robots/cob4-2.xml                 |  5 +++-
 .../tools/obstacle_distance_monitor.launch    | 29 +++++++++++++++++++
 3 files changed, 37 insertions(+), 1 deletion(-)
 create mode 100644 cob_bringup/tools/obstacle_distance_monitor.launch

diff --git a/cob_bringup/robots/cob4-1.xml b/cob_bringup/robots/cob4-1.xml
index d165af223..e6b2e6b92 100644
--- a/cob_bringup/robots/cob4-1.xml
+++ b/cob_bringup/robots/cob4-1.xml
@@ -102,6 +102,10 @@
 			<arg name="robot" value="$(arg robot)" />
 			<arg name="pc" value="$(arg cob4-1-t1)" />
 		</include>
+		<include file="$(find cob_bringup)/tools/obstacle_distance_monitor.launch" >
+			<arg name="robot" value="$(arg robot)" />
+			<arg name="use_sensors" value="false" />
+		</include>
 
 		<!-- startup hardware -->
 		<include file="$(find cob_bringup)/drivers/canopen_402.launch" >
diff --git a/cob_bringup/robots/cob4-2.xml b/cob_bringup/robots/cob4-2.xml
index 86e473f44..03c4505d7 100644
--- a/cob_bringup/robots/cob4-2.xml
+++ b/cob_bringup/robots/cob4-2.xml
@@ -102,11 +102,14 @@
 		<include file="$(find cob_bringup)/tools/behavior.launch" >
 			<arg name="robot" value="$(arg robot)" />
 		</include>
-
 		<include file="$(find cob_bringup)/tools/pc_monitor.launch" >
 			<arg name="robot" value="$(arg robot)" />
 			<arg name="pc" value="$(arg cob4-2-t1)" />
 		</include>
+		<include file="$(find cob_bringup)/tools/obstacle_distance_monitor.launch" >
+			<arg name="robot" value="$(arg robot)" />
+			<arg name="use_sensors" value="false" />
+		</include>
 
 		<!-- startup hardware -->
 		<include file="$(find cob_bringup)/drivers/canopen_402.launch" >
diff --git a/cob_bringup/tools/obstacle_distance_monitor.launch b/cob_bringup/tools/obstacle_distance_monitor.launch
new file mode 100644
index 000000000..35dc0a622
--- /dev/null
+++ b/cob_bringup/tools/obstacle_distance_monitor.launch
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="robot" default="$(optenv ROBOT !!NO_ROBOT_SET!!)"/>
+	<arg name="pkg_hardware_config" default="$(find cob_hardware_config)"/>
+
+	<arg name="load_robot_description" default="false"/>
+	<arg name="pkg_moveit_config" default="$(find cob_moveit_config)"/>
+
+	<arg name="use_sensors" default="true"/>
+	<arg name="node_name" value="obstacle_distance_moveit"/>
+	
+	<!-- Load SRDF -->
+	<include file="$(arg pkg_moveit_config)/launch/planning_context.xml">
+		<arg name="load_robot_description" value="$(arg load_robot_description)" />
+		<arg name="robot" value="$(arg robot)" />
+		<arg name="config_path" value="$(arg pkg_moveit_config)/$(arg robot)/config" />
+	</include>
+
+	<!-- Load Sensor Input Information -->
+	<include ns="$(arg node_name)" file="$(find cob_moveit_config)/launch/sensor_manager.xml">
+		<arg name="config_path" value="$(arg config_path)"/>
+		<arg name="use_sensors" value="$(arg use_sensors)"/>
+	</include>
+
+	<node pkg="cob_obstacle_distance_moveit" type="obstacle_distance_node" name="$(arg node_name)" output="screen">
+		<remap from="/obstacle_distances" to="/obstacle_distances"/>
+	</node>
+
+</launch>