Merge pull request ZebraDevs#8 from fetchrobotics/allow_active_sensing

add octomap configuration

fetch_moveit_config/config/sensors.yaml

@@ -0,0 +1,7 @@
+sensors:
+  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    point_cloud_topic: /head_camera/depth_downsample/points
+    max_range: 1.5
+    point_subsample: 1
+    padding_offset: 0.05
+    padding_scale: 1.0

fetch_moveit_config/launch/fetch_moveit_sensor_manager.launch.xml

@@ -1,3 +1,13 @@
 <launch>
 
+  <!-- Set the file describing which sensor(s) to use to build the octomap  -->
+  <arg name="moveit_octomap_sensor_params_file" default="$(find fetch_moveit_config)/config/sensors.yaml" />
+
+  <!-- Params for the octomap monitor -->
+  <param name="octomap_frame" type="string" value="base_link" />
+  <param name="octomap_resolution" type="double" value="0.05" />
+
+  <!-- sensors used to update the map -->
+  <rosparam command="load" file="$(arg moveit_octomap_sensor_params_file)" />
+
 </launch>

fetch_moveit_config/launch/move_group.launch

@@ -15,6 +15,7 @@
 
   <!-- move_group settings -->
   <arg name="allow_trajectory_execution" default="true"/>
+  <arg name="allow_active_sensing" default="false"/>
   <arg name="fake_execution" default="false"/>
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
@@ -34,7 +35,8 @@
   </include>
 
   <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(find fetch_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+  <rosparam command="delete" param="move_group/sensors" />
+  <include ns="move_group" file="$(find fetch_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_active_sensing)">
     <arg name="moveit_sensor_manager" value="fetch" />
   </include>
 

fetch_moveit_config/launch/sensor_manager.launch.xml

@@ -1,12 +1,5 @@
 <launch>
 
-  <!-- This file makes it easy to include the settings for sensor managers -->  
-
-  <!-- Params for the octomap monitor -->
-  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.025" />
-  <param name="max_range" type="double" value="5.0" />
-
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="fetch" />
   <include file="$(find fetch_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />