Moveit perception pipeline octomap position is wrong, with Gazebo and Kinect

edit

Background

Hi, I was trying to develop a mobile manipulator in Gazebo simulation. My robot has a Kinect sensor config same as official tutorial on the mobile base. And the manipulator part is config with Moveit setup manager, although it keeps crashing at the sensor part so I added the below config by hand later. Configured after panda_moveit_config with /camera/depth_registered/image_raw -> /camera/depth/image_raw according to this post.

sensor_manager.launch.xml

<launch>
  <!-- This file makes it easy to include the settings for sensor managers -->
  <!-- Params for 3D sensors config: '' | pointcloud | depthmap -->
  <arg name="sensor_type" default="depthmap" />
  <rosparam if="$(eval arg('sensor_type')  != '')" command="load" file="$(find mobile_manipulator_moveit_config)/config/sensors_kinect_$(arg sensor_type).yaml" />
  <!-- Params for the octomap monitor -->
  <param name="octomap_frame" type="string" value="camera_link" />
  <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="mobile_manipulator" />
  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

sensors_kinect_depthmap.yaml

sensors:
  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    image_topic: /camera/depth/image_raw
    queue_size: 5
    near_clipping_plane_distance: 0.3
    far_clipping_plane_distance: 5.0
    shadow_threshold: 0.2
    padding_scale: 4.0
    padding_offset: 0.03
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

Problem:

The orientation of the octomap is completely off:

The tf of the robot with the last frame of the arm and the camera frame is shown.

And $rostopic echo /move_group/filtered_cloud/header gives me no clue, I initially thought that the octomap might be attached to the top_wrist frame by some default settings, but the echo says no.

$rostopic echo /move_group/filtered_cloud/header
seq: 0
stamp: 
  secs: 1398
  nsecs: 643000000
frame_id: "camera_link"
---
seq: 1
stamp: 
  secs: 1398
  nsecs: 643000000
frame_id: "camera_link"
---
...

The sensor is working, though, when people pass by the octomap updates.

So I think maybe the octomap looks for the Z-axis of the frame, so it ends up pointing upward. Is there any param I can set to let the Octomap looks for the X-axis?