From b6ba5317864ad4214ebca6ff2b2074edcf80ac79 Mon Sep 17 00:00:00 2001
From: hiroyuki Okada <okdhryk@gmail.com>
Date: Sat, 5 Dec 2015 12:49:53 +0900
Subject: [PATCH 1/3] Add xtion head coordinate

---
 .../launch/moveit_viewnatto.rviz              | 32 ++++++++++++++++++-
 .../launch/hironx_ros_bridge.launch           |  7 ++++
 .../launch/hironxo_head_xtion.launch          |  9 ++++++
 3 files changed, 47 insertions(+), 1 deletion(-)
 create mode 100644 hironx_ros_bridge/launch/hironxo_head_xtion.launch

diff --git a/hironx_moveit_config/launch/moveit_viewnatto.rviz b/hironx_moveit_config/launch/moveit_viewnatto.rviz
index 54127857..6e68f268 100644
--- a/hironx_moveit_config/launch/moveit_viewnatto.rviz
+++ b/hironx_moveit_config/launch/moveit_viewnatto.rviz
@@ -9,6 +9,7 @@ Panels:
         - /MotionPlanning1
         - /MotionPlanning1/Planning Request1
         - /MarkerArray1
+        - /PointCloud21
       Splitter Ratio: 0.5
     Tree Height: 176
   - Class: rviz/Selection
@@ -29,7 +30,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
 Visualization Manager:
   Class: ""
   Displays:
@@ -361,6 +362,35 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.0108
+        Min Value: -1.76403
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /camera/depth/points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge.launch b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
index 2f3ea32a..fcecc786 100644
--- a/hironx_ros_bridge/launch/hironx_ros_bridge.launch
+++ b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
@@ -6,6 +6,7 @@
 
   <arg name="nameserver" default="hiro014" /> <!-- for real robot -->
   <arg name="MODEL_FILE" default="$(find hironx_ros_bridge)/models/kawada-hironx.dae" />
+  <arg name="CAMERA_XTION" default="false" />
   <arg name="COLLADA_FILE" default="$(find hironx_ros_bridge)/models/kawada-hironx.dae" />
   <arg name="SIMULATOR_NAME" default="RobotHardware0" />
   <arg name="corbaport" default="15005" />
@@ -61,4 +62,10 @@
     <node pkg="rqt_gui" type="rqt_gui" name="rqt_hironxo" respawn="false" output="screen"
           args="--perspective-file $(find hironx_ros_bridge)/resource/hironx_rqt_dashboard.perspective"/>
   </group>
+
+  <!-- add xtion head -->
+  <group if="$(arg CAMERA_XTION)">
+    <include file="$(find hironx_ros_bridge)/launch/hironxo_head_xtion.launch" />
+  </group>
+
 </launch>
diff --git a/hironx_ros_bridge/launch/hironxo_head_xtion.launch b/hironx_ros_bridge/launch/hironxo_head_xtion.launch
new file mode 100644
index 00000000..13d1b51c
--- /dev/null
+++ b/hironx_ros_bridge/launch/hironxo_head_xtion.launch
@@ -0,0 +1,9 @@
+<launch>
+  <!-- This launch is specifically for the Xtion head for Hironx / NEXTAGE Open
+       that TORk produces. See http://opensource-robotics.tokyo.jp/?p=1337&lang=en
+       for more detail. -->
+
+  <!-- coordinate values should be worked out; these are not yet accurate. -->
+  <node pkg="tf" type="static_transform_publisher" name="head_link_to_xtion_link"
+        args="0.07 0.06 0.09 0 0.0 0 /HEAD_JOINT1_Link /camera_link 100" />
+</launch>

From a1e5e2d140b87b7df09bfc5b0e50cc57e1ad6228 Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Wed, 3 Feb 2016 19:37:23 +0900
Subject: [PATCH 2/3] [moveit_config] Add vision sensor configs

---
 .../config/kinect_sensor.yaml                 |  9 ++++
 hironx_moveit_config/config/xtion_sensor.yaml |  9 ++++
 .../HiroNX_moveit_sensor_manager.launch       | 42 +++++++++++++++++++
 hironx_moveit_config/launch/move_group.launch |  5 +++
 .../launch/moveit_planning_execution.launch   |  5 +++
 .../launch/sensor_manager.launch              |  8 +++-
 6 files changed, 76 insertions(+), 2 deletions(-)
 create mode 100644 hironx_moveit_config/config/kinect_sensor.yaml
 create mode 100644 hironx_moveit_config/config/xtion_sensor.yaml

diff --git a/hironx_moveit_config/config/kinect_sensor.yaml b/hironx_moveit_config/config/kinect_sensor.yaml
new file mode 100644
index 00000000..b45b8164
--- /dev/null
+++ b/hironx_moveit_config/config/kinect_sensor.yaml
@@ -0,0 +1,9 @@
+sensors: 
+ - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+   point_cloud_topic: /camera/depth_registered/points
+   max_range: 5.0
+   padding_offset: 0
+   padding_scale: 3.0
+   frame_subsample: 1
+   point_subsample: 1
+
diff --git a/hironx_moveit_config/config/xtion_sensor.yaml b/hironx_moveit_config/config/xtion_sensor.yaml
new file mode 100644
index 00000000..9945fc9e
--- /dev/null
+++ b/hironx_moveit_config/config/xtion_sensor.yaml
@@ -0,0 +1,9 @@
+sensors:
+ - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+   point_cloud_topic: /camera/depth_registered/points
+   max_range: 4.0
+   padding_offset: 0.1
+   padding_scale: 3.0
+   frame_subsample: 1
+   point_subsample: 10
+
diff --git a/hironx_moveit_config/launch/HiroNX_moveit_sensor_manager.launch b/hironx_moveit_config/launch/HiroNX_moveit_sensor_manager.launch
index 5d02698d..7bc37ee9 100644
--- a/hironx_moveit_config/launch/HiroNX_moveit_sensor_manager.launch
+++ b/hironx_moveit_config/launch/HiroNX_moveit_sensor_manager.launch
@@ -1,3 +1,45 @@
 <launch>
+    <!-- Originally copied from
+    https://github.com/ros-planning/moveit_robots/blob/323d2c5b17260a59ae3645fe654ecf03945ab6fc/baxter/baxter_moveit_config/launch/baxter_moveit_sensor_manager.launch
+     -->
+     <arg name="camera_link_pose" default="0.07 0.06 0.09 0 0.0 0"/>
+     <arg name="kinect" default="false"/>
+     <arg name="xtion" 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="head_link_to_xtion_link"
+               args="$(arg camera_link_pose) /HEAD_JOINT1_Link /camera_link 100" />
+        
+         <!-- octomap parameters for moveit -->
+         <group ns="move_group" >
+             <param name="octomap_frame" type="string" value="WAIST" />
+             <param name="octomap_resolution" type="double" value="0.02" />
+             <rosparam command="load" file="$(find hironx_moveit_config)/config/kinect_sensor.yaml" />
+         </group>
+     </group>
+     <group if="$(arg xtion)" >
+         <!-- launch openni to talk to xtion -->
+         <include file="$(find openni2_launch)/launch/openni2.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="head_link_to_xtion_link"
+               args="$(arg camera_link_pose) /HEAD_JOINT1_Link /camera_link 100" />
 
+         <!-- octomap parameters for moveit -->
+         <group ns="move_group" >
+             <param name="octomap_frame" type="string" value="WAIST" />
+             <param name="octomap_resolution" type="double" value="0.02" />
+             <rosparam command="load" file="$(find hironx_moveit_config)/config/xtion_sensor.yaml" />
+         </group>
+     </group>
 </launch>
diff --git a/hironx_moveit_config/launch/move_group.launch b/hironx_moveit_config/launch/move_group.launch
index 5f30650a..33dd618d 100644
--- a/hironx_moveit_config/launch/move_group.launch
+++ b/hironx_moveit_config/launch/move_group.launch
@@ -15,6 +15,9 @@
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="false"/>
 
+  <arg name="kinect" default="false" />
+  <arg name="xtion" default="false" />
+  
   <include ns="move_group" file="$(find hironx_moveit_config)/launch/planning_pipeline.launch">
     <arg name="pipeline" value="ompl" />
   </include>
@@ -26,6 +29,8 @@
 
   <include ns="move_group" file="$(find hironx_moveit_config)/launch/sensor_manager.launch" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_sensor_manager" value="HiroNX" /> 
+    <arg name="kinect" value="$(arg kinect)" />
+    <arg name="xtion" value="$(arg xtion)" />    
   </include>
 
   <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
diff --git a/hironx_moveit_config/launch/moveit_planning_execution.launch b/hironx_moveit_config/launch/moveit_planning_execution.launch
index fece26f0..6f550e65 100644
--- a/hironx_moveit_config/launch/moveit_planning_execution.launch
+++ b/hironx_moveit_config/launch/moveit_planning_execution.launch
@@ -1,9 +1,14 @@
 <launch>
+ <arg name="kinect" default="false" />
+ <arg name="xtion" default="false" />
+  
  # The planning and execution components of MoveIt! configured to 
  # publish the current configuration of the robot (simulated or real)
  # and the current state of the world as seen by the planner
  <include file="$(find hironx_moveit_config)/launch/move_group.launch">
   <arg name="publish_monitored_planning_scene" value="true" />
+  <arg name="kinect" value="$(arg kinect)" />  
+  <arg name="xtion" value="$(arg xtion)" />    
  </include>
  # The visualization component of MoveIt!
  <include file="$(find hironx_moveit_config)/launch/moveit_rviz.launch"> 
diff --git a/hironx_moveit_config/launch/sensor_manager.launch b/hironx_moveit_config/launch/sensor_manager.launch
index 386e7601..9230458c 100644
--- a/hironx_moveit_config/launch/sensor_manager.launch
+++ b/hironx_moveit_config/launch/sensor_manager.launch
@@ -4,6 +4,10 @@
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
   <arg name="moveit_sensor_manager" default="HiroNX" />
-  <include file="$(find hironx_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch" />
-  
+  <arg name="kinect" default="false" />
+  <arg name="xtion" default="false" />  
+  <include file="$(find hironx_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>

From a7573755368b0929aa2f4fd49260167da32b97cb Mon Sep 17 00:00:00 2001
From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp>
Date: Mon, 16 May 2016 14:31:43 -0700
Subject: [PATCH 3/3] More sensor files