prepare obstacle_distance_monitor launch file

ipa320 · Apr 4, 2016 · 135e906 · 135e906
1 parent 67992d2
commit 135e906
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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>