Merge pull request #1 from RyuYamamoto/add/checkerboard

enable octomap

nextage_moveit_config/config/kinect_sensor.yaml

@@ -0,0 +1,8 @@
+sensors:
+- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+point_cloud_topic: /camera/depth/points
+max_range: 5.0
+point_subsample: 1
+padding_offset: 0.1
+padding_scale: 1.0
+filtered_cloud_topic: filtered_cloud

nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch

@@ -1,3 +1,43 @@
 <launch>
+     <arg name="camera_link_pose" default="0.15 0.075 0.5 0.0 0.7854 0.0"/>
+     <arg name="kinect" default="false"/>
+     <group if="$(arg kinect)" >
+         <!-- launch openni to talk to kinect -->
+         <include file="$(find freenect_launch)/launch/freenect.launch">
+             <!-- These args are workarounds for tf_prefix issues in freenect.launch -->
+             <arg name="rgb_frame_id" value="camera_rgb_optical_frame"/>
+             <arg name="depth_frame_id" value="camera_depth_optical_frame"/>
+         </include>
+         <!-- Users update this to set transform between camera and robot -->
+         <node pkg="tf" type="static_transform_publisher" name="camera_link_broadcaster"
+               args="$(arg camera_link_pose) /torso /camera_link 100" />
 
+         <!-- octomap parameters for moveit -->
+         <group ns="move_group" >
+             <rosparam command="load" file="$(find nextage_moveit_config)/config/kinect_sensor.yaml" />
+			 <param name="octomap_frame" type="string" value="camera_link" />
+			 <param name="octomap_resolution" type="double" value="0.02" />
+
+         </group>
+     </group>
+     <arg name="xtion" default="false"/>
+     <group if="$(arg xtion)" >
+         <!-- launch openni to talk to xtion -->
+         <include file="$(find openni_launch)/launch/openni.launch">
+             <!-- These args are workarounds for tf_prefix issues in openni.launch -->
+             <arg name="rgb_frame_id" value="camera_rgb_optical_frame" />
+             <arg name="depth_frame_id" value="camera_depth_optical_frame" />
+         </include>
+         <!-- Users update this to set transform between camera and robot -->
+         <!-- This example has the Xtion mounted to the chest of the robot -->
+         <node pkg="tf" type="static_transform_publisher" name="camera_link_broadcaster"
+               args="$(arg camera_link_pose) /torso /camera_link 100" />
+
+         <!-- octomap parameters for moveit -->
+         <group ns="move_group" >
+             <param name="octomap_frame" type="string" value="camera_link" />
+             <param name="octomap_resolution" type="double" value="0.02" />
+             <rosparam command="load" file="$(find nextage_moveit_config)/config/xtion_sensor.yaml" />
+         </group>
+     </group>
 </launch>

nextage_moveit_config/launch/sensor_manager.launch

@@ -1,9 +1,11 @@
 <launch>
-
-  <!-- This file makes it easy to include the settings for sensor managers -->  
-
-  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
-  <arg name="moveit_sensor_manager" default="NextageOpen" />
-  <include file="$(find nextage_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch" />
-
+	<!-- This file makes it easy to include the settings for sensor managers -->
+	<!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
+	<arg name="moveit_sensor_manager" default="NextageOpen" />
+	<arg name="kinect" default="true" />
+	<arg name="xtion" default="false" />
+	<include file="$(find nextage_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch" >
+		<arg name="kinect" value="$(arg kinect)" />
+		<arg name="xtion" value="$(arg xtion)" />
+	</include>
 </launch>