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>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>100000.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>0</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName> <!-- original value was camera_link -->
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
erlecopter.xacro
<robot name="erlecopter" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Properties -->
<xacro:property name="namespace" value="erlecopter" />
<xacro:property name="rotor_velocity_slowdown_sim" value="10" />
<xacro:property name="mesh_file" value="erlecopter.dae" />
<xacro:property name="mesh_scale" value="1 1 1"/> <!-- 1 1 1 -->
<xacro:property name="mesh_scale_prop" value="1 1 1"/>
<xacro:property name="mass" value="1.1" /> <!-- [kg] -->
<xacro:property name="body_length" value="0.18" /> <!-- [m] 0.10 -->
<xacro:property name="body_width" value="0.12" /> <!-- [m] 0.10 -->
<xacro:property name="body_height" value="0.10" /> <!-- [m] -->
<xacro:property name="mass_rotor" value="0.005" /> <!-- [kg] -->
<xacro:property name="arm_length_front_x" value="0.141" /> <!-- [m] 0.1425 0.22 -->
<xacro:property name="arm_length_back_x" value="0.141" /> <!-- [m] 0.154 0.22 -->
<xacro:property name="arm_length_front_y" value="0.141" /> <!-- [m] 0.251 0.22 -->
<xacro:property name="arm_length_back_y" value="0.141" /> <!-- [m] 0.234 0.22 -->
<xacro:property name="rotor_offset_top" value="0.030" /> <!-- [m] 0.023-->
<xacro:property name="radius_rotor" value="0.12" /> <!-- [m] -->
<xacro:property name="motor_constant" value="8.54858e-06" /> <!-- [kg.m/s^2] -->
<xacro:property name="moment_constant" value="0.016" /> <!-- [m] -->
<xacro:property name="time_constant_up" value="0.0125" /> <!-- [s] -->
<xacro:property name="time_constant_down" value="0.025" /> <!-- [s] -->
<xacro:property name="max_rot_velocity" value="838" /> <!-- [rad/s] -->
<xacro:property name="sin30" value="0.5" />
<xacro:property name="cos30" value="0.866025403784" />
<xacro:property name="sqrt2" value="1.4142135623730951" />
<xacro:property name="rotor_drag_coefficient" value="8.06428e-05" />
<xacro:property name="rolling_moment_coefficient" value="0.000001" />
<xacro:property name="color" value="DarkGrey" />
<!-- Property Blocks -->
<xacro:property name="body_inertia">
<inertia ixx="0.0347563" ixy="0.0" ixz="0.0" iyy="0.0458929" iyz="0.0" izz="0.0977" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
</xacro:property>
<!-- inertia of a single rotor, assuming it is a cuboid. Height=3mm, width=15mm -->
<xacro:property name="rotor_inertia">
<inertia
ixx="${1/12 * mass_rotor * (0.015 * 0.015 + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
iyy="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
izz="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.015 * 0.015) * rotor_velocity_slowdown_sim}"
ixy="0.0" ixz="0.0" iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
</xacro:property>
<!-- Included URDF Files -->
<!-- <xacro:include filename="$(find rotors_description)/urdf/multirotor_base.xacro" /> -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/multirotor_base.xacro" />
<!-- Instantiate multirotor_base_macro once -->
<xacro:multirotor_base_macro
robot_namespace="${namespace}"
mass="${mass}"
body_length="${body_length}"
body_width="${body_width}"
body_height="${body_height}"
mesh_file="${mesh_file}"
mesh_scale="${mesh_scale}"
color="${color}"
>
<xacro:insert_block name="body_inertia" />
</xacro:multirotor_base_macro>
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- Recorder cameras -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/recorder_camera.xacro" />
<!--
<xacro:recorder_camera
name="recorder_cam_front_with_uav"
parent="base_link"
ros_topic="recorder_cam_front_with_uav"
update_rate="90"
res_x="1680"
res_y="1050"
image_format="R8G8B8"
hfov="81"
video_dir="/home/aurelien/Videos/shots/"
video_filename="cam_front_with_uav"
video_fileext="avi">
<origin xyz="-0.8 0.0 0.12" rpy="0 0 0"/>
</xacro:recorder_camera>
-->
<!--
<xacro:recorder_camera
name="recorder_cam_front_with_uav2"
parent="base_link"
ros_topic="recorder_cam_front_with_uav2"
update_rate="30"
res_x="1680"
res_y="1050"
image_format="R8G8B8"
hfov="81"
video_dir="/home/aurelien/Videos/shots/"
video_filename="cam_front_with_uav2"
video_fileext="avi">
<origin xyz="-0.3 0.0 0.15" rpy="0 0.2 0"/>
</xacro:recorder_camera>
-->
<!--
<xacro:recorder_camera
name="recorder_cam_rear_with_uav"
parent="base_link"
ros_topic="recorder_cam_rear_with_uav"
update_rate="30"
res_x="1680"
res_y="1050"
image_format="R8G8B8"
hfov="81"
video_dir="/home/aurelien/Videos/shots/"
video_filename="cam_rear_with_uav"
video_fileext="avi">
<origin xyz="0.6 0.0 -0.12" rpy="0 ${-20*M_PI/180} ${180*M_PI/180}"/>
</xacro:recorder_camera>
-->
<!--
<xacro:recorder_camera
name="recorder_cam_rear_with_uav2"
parent="base_link"
ros_topic="recorder_cam_rear_with_uav2"
update_rate="90"
res_x="1680"
res_y="1050"
image_format="R8G8B8"
hfov="81"
video_dir="/home/aurelien/Videos/shots/"
video_filename="cam_rear_with_uav2"
video_fileext="avi">
<origin xyz="0.4 0.0 0.12" rpy="0 ${20*M_PI/180} ${180*M_PI/180}"/>
</xacro:recorder_camera>
-->
<!-- Recorder cameras END -->
<!-- Sonar height sensor -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/sonar_sensor.urdf.xacro" />
<xacro:sonar_sensor
name="sonar"
parent="base_link"
ros_topic="sonar_down"
update_rate="10"
min_range="0.01"
max_range="50.0"
field_of_view="${1*M_PI/180}"
ray_count="1"
sensor_mesh="lidar_lite_v2/meshes/lidar_lite_v2.dae">
<origin xyz="0.12 0.0 0.0" rpy="0 ${90*M_PI/180} 0"/>
</xacro:sonar_sensor>
<!-- Frontal range sensor -->
<!-- <xacro:sonar_sensor
name="sonar2"
parent="base_link"
ros_topic="sonar_front"
update_rate="10"
min_range="0.01"
max_range="50.0"
field_of_view="${1*M_PI/180}"
ray_count="1"
sensor_mesh="lidar_lite_v2_withRay/meshes/lidar_lite_v2_withRay.dae">
<origin xyz="0.13 0.0 0.02" rpy="0 0 0"/>
</xacro:sonar_sensor>
-->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/lidar_sensor.urdf.xacro" />
<xacro:lidar_sensor
name="sonar2"
parent="base_link"
ros_topic="sonar_front"
update_rate="10"
min_range="0.06"
max_range="5.0"
field_of_view_horizontal="${240*M_PI/180}"
field_of_view_vertical="${1*M_PI/180}"
ray_count_horizontal="420"
ray_count_vertical="1"
sensor_mesh="lidar_lite_v2_withRay/meshes/lidar_lite_v2_withRay.dae">
<origin xyz="0.13 0.0 0.02" rpy="0 0 0"/>
</xacro:lidar_sensor>
<!-- [Edited] Kinect sensor -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/kinect.urdf.xacro" />
<xacro:sensor_kinect
parent="base_link">
</xacro:sensor_kinect>
<!-- Downward facing camera -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/generic_camera.urdf.xacro" />
<xacro:generic_camera
name="erlecopter/bottom"
parent="base_link"
ros_topic="image_raw"
cam_info_topic="camera_info"
update_rate="60"
res_x="640"
res_y="360"
image_format="R8G8B8"
hfov="81"
framename="erlecopter_bottomcam">
<origin xyz="0.14 0.0 -0.02" rpy="0 ${M_PI/2} 0"/>
</xacro:generic_camera>
<!-- Front facing camera -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/generic_camera.urdf.xacro" />
<xacro:generic_camera
name="erlecopter/front"
parent="base_link"
ros_topic="image_front_raw"
cam_info_topic="camera_front_info"
update_rate="25"
res_x="640"
res_y="360"
image_format="R8G8B8"
hfov="81"
framename="erlecopter_frontcam">
<origin xyz="0.17 0.0 0.0" rpy="0 0 0"/>
</xacro:generic_camera>
<!-- Instantiate rotors -->
<xacro:vertical_rotor
robot_namespace="${namespace}"
suffix="front_right"
direction="ccw"
motor_constant="${motor_constant}"
moment_constant="${moment_constant}"
parent="base_link"
mass_rotor="${mass_rotor}"
radius_rotor="${radius_rotor}"
time_constant_up="${time_constant_up}"
time_constant_down="${time_constant_down}"
max_rot_velocity="${max_rot_velocity}"
motor_number="0"
rotor_drag_coefficient="${rotor_drag_coefficient}"
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="erlecopter_prop"
mesh_scale="${mesh_scale_prop}"
color="Yellow">
<origin xyz="${arm_length_front_x} -${arm_length_front_y} ${rotor_offset_top}" rpy="0 0 0" />
<xacro:insert_block name="rotor_inertia" />
</xacro:vertical_rotor>
<xacro:vertical_rotor
robot_namespace="${namespace}"
suffix="back_left"
direction="ccw"
motor_constant="${motor_constant}"
moment_constant="${moment_constant}"
parent="base_link"
mass_rotor="${mass_rotor}"
radius_rotor="${radius_rotor}"
time_constant_up="${time_constant_up}"
time_constant_down="${time_constant_down}"
max_rot_velocity="${max_rot_velocity}"
motor_number="1"
rotor_drag_coefficient="${rotor_drag_coefficient}"
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="erlecopter_prop"
mesh_scale="${mesh_scale_prop}"
color="Black">
<origin xyz="-${arm_length_back_x} ${arm_length_back_y} ${rotor_offset_top}" rpy="0 0 0" />
<xacro:insert_block name="rotor_inertia" />
</xacro:vertical_rotor>
<xacro:vertical_rotor robot_namespace="${namespace}"
suffix="front_left"
direction="cw"
motor_constant="${motor_constant}"
moment_constant="${moment_constant}"
parent="base_link"
mass_rotor="${mass_rotor}"
radius_rotor="${radius_rotor}"
time_constant_up="${time_constant_up}"
time_constant_down="${time_constant_down}"
max_rot_velocity="${max_rot_velocity}"
motor_number="2"
rotor_drag_coefficient="${rotor_drag_coefficient}"
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="erlecopter_prop"
mesh_scale="${mesh_scale_prop}"
color="Yellow">
<origin xyz="${arm_length_front_x} ${arm_length_front_y} ${rotor_offset_top}" rpy="0 0 0" />
<xacro:insert_block name="rotor_inertia" />
</xacro:vertical_rotor>
<xacro:vertical_rotor robot_namespace="${namespace}"
suffix="back_right"
direction="cw"
motor_constant="${motor_constant}"
moment_constant="${moment_constant}"
parent="base_link"
mass_rotor="${mass_rotor}"
radius_rotor="${radius_rotor}"
time_constant_up="${time_constant_up}"
time_constant_down="${time_constant_down}"
max_rot_velocity="${max_rot_velocity}"
motor_number="3"
rotor_drag_coefficient="${rotor_drag_coefficient}"
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="erlecopter_prop"
mesh_scale="${mesh_scale_prop}"
color="Black">
<origin xyz="-${arm_length_back_x} -${arm_length_back_y} ${rotor_offset_top}" rpy="0 0 0" />
<xacro:insert_block name="rotor_inertia" />
</xacro:vertical_rotor>
</robot>
erlecopter_spawn.launch
<launch>
<node pkg="tf" type="static_transform_publisher" name="camera_to_map_tf" args="0 0 0 1.58 -1.58 0 /camera_depth_optical_frame /map 100"/>
<arg name="simRate" default="nan"/>
<!-- Enable simulation clock -->
<param name="use_sim_time" type="bool" value="true" />
<include file="$(find mavros)/launch/apm_sitl.launch"></include>
<arg name="enable_logging" default="true"/>
<arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="erlecopter"/>
<arg name="headless" default="false"/>
<arg name="gui" default="true"/>
<arg name="world_name" default="$(find ardupilot_sitl_gazebo_plugin)/worlds/disaster_world/disaster_01.world"/>
<env name="GAZEBO_MODEL_PATH" value="$(find drcsim_model_resources)/gazebo_models/environments:$(find ardupilot_sitl_gazebo_plugin)/meshes/meshes_sensors:$(find ardupilot_sitl_gazebo_plugin)/meshes/meshes_outdoor:$(find ardupilot_sitl_gazebo_plugin)/meshes/meshes_warehouse"/>
<arg name="name" default="erlecopter"/>
<arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/erlecopter_base.xacro"/>
<arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
<arg name="debug" default="true"/>
<arg name="verbose" default="true"/>
<!-- Initial pose for the drone -->
<arg name="x" default="-2.5"/> <!-- [m], positive to the North / default value was 0.0 -->
<arg name="y" default="-10.0"/> <!-- [m], negative to the East / default value was 0.0 -->
<arg name="z" default="0.08"/> <!-- [m], positive Up -->
<arg name="roll" default="0"/> <!-- [rad] -->
<arg name="pitch" default="0"/> <!-- [rad] -->
<arg name="yaw" default="1.57"/> <!-- [rad], negative clockwise -->
<!-- send the robot XML to param server -->
<param name="robot_description" command="
$(find xacro)/xacro.py '$(arg model)'
enable_logging:=$(arg enable_logging)
enable_ground_truth:=$(arg enable_ground_truth)
log_file:=$(arg log_file)"
/>
<param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_erlecopter" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description
-urdf
-x $(arg x)
-y $(arg y)
-z $(arg z)
-R $(arg roll)
-P $(arg pitch)
-Y $(arg yaw)
-model $(arg name)"
respawn="false" output="screen">
</node>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="true"/> <!-- value unrelevant due to Arducopter plugin steps -->
<arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world_name)"/>
</include>
<!-- <node name="tag1" pkg="gazebo_ros" type="spawn_model" args="-file $(find rotors_description)/urdf/ARtag.urdf -urdf -x 0.0 -y 0.0 -z 0.01 -model ARtag1" respawn="false" />-->
</launch>
Asked by suho0515 on 2019-09-26 11:04:21 UTC
Comments