depth camera doesn't move in Rviz
Hello guys, I'm trying to do hetor_slam with simulated drone with Gazebo.
but there is problem with modeled kinect depth camera.
when i start move the drone, drone and LRF sensor are moving nicely.
but only the Depth camera pointcloud2 data doesn't move at all.
I guess there should be tf problem with it. so i tried various of case.
but it's still not working.
is there any other problem?
here's my code.
kinect.urdf.xacro
<robot name="sensor_kinect" xmlns:xacro="http://ros.org/wiki/xacro">
<property name="cam_px" value="0" /> <!-- Original value is 1.63[m] -->
<xacro:property name="kinect_cam_py" value="0"/> <!-- Original value is -0.0125[m] -->
<property name="cam_pz" value="0.2" /> <!-- Original value is 0.68[m] -->
<property name="cam_or" value="0" />
<property name="cam_op" value="0" />
<property name="cam_oy" value="0" />
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
<xacro:macro name="sensor_kinect" params="parent">
<joint name="camera_rgb_joint" type="fixed">
<origin xyz="${cam_px} ${kinect_cam_py} ${cam_pz}" rpy="${cam_or} ${cam_op} ${cam_oy}"/>
<parent link="${parent}"/>
<child link="camera_rgb_frame" />
</joint>
<link name="camera_rgb_frame"/>
<joint name="camera_rgb_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_rgb_frame" />
<child link="camera_rgb_optical_frame" />
</joint>
<link name="camera_rgb_optical_frame"/>
<joint name="camera_joint" type="fixed">
<origin xyz="-0.031 ${-kinect_cam_py} -0.016" rpy="0 0 0"/>
<parent link="camera_rgb_frame"/>
<child link="camera_link"/>
</joint>
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<mesh filename="package://ardupilot_sitl_gazebo_plugin/meshes/meshes_sensors/kinect/kinect.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.07271 0.27794 0.073"/>
</geometry>
</collision>
<inertial>
<mass value="0.564" />
<origin xyz="0 0 0" />
<inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
iyy="0.000498940" iyz="0.0"
izz="0.003879257" />
</inertial>
</link>
<!-- The fixed joints & links below are usually published by static_transformers launched by the OpenNi launch
files. However, for Gazebo simulation we need them, so we add them here.
(Hence, don't publish them additionally!) -->
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 ${2 * -kinect_cam_py} 0" rpy="${-M_PI/2} 0 0" />
<parent link="camera_rgb_frame" />
<child link="camera_depth_frame" />
</joint>
<link name="camera_depth_frame"/>
<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>
<link name="camera_depth_optical_frame"/>
<!-- Microsoft Kinect / ASUS Xtion PRO Live for simulation -->
<gazebo reference="camera_link">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${120.0*M_PI/180.0}</horizontal_fov> ...