Frame problems/confusion

[aled96]

Hi, I am not so sure how to set the parameters in the launchfile.

I have a legged robot with a realsense D435i rigidly attached on the body.

I have strange behaviours, here there is an image of the gazebo scene and the voxel_cloud resulting in rviz

Here I have the launch file used for what concerns the mapping part

    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="_publish_tf" type="bool" value="false" />
        <param name="_world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/d435i_camera/accel/sample"/>
    </node>

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="frame_id" value="torso_2"/>
        <arg name="args" value="--delete_db_on_start"/>
        <arg name="rgb_topic" value="/d435i_camera/color/image_raw"/>
        <arg name="depth_topic" value="/d435i_camera/aligned_depth_to_color/image_raw2"/>
        <arg name="camera_info_topic" value="/d435i_camera/color/camera_info"/>
        <arg name="depth_camera_info_topic" value="/d435i_camera/depth/camera_info"/>
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>
    </include>

    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="torso_2"/>
    <param name="/ukf_se/odom0" value="rtabmap/odom"/>
    <rosparam param="/ukf_se/odom0_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom0_relative" value="true"/>
    <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu0" value="/d435i_camera/accel/sample"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu0_differential" value="true"/>
    <param name="/ukf_se/imu0_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->

Thank you for the help !

[TouchDeeper]

@aled96 Hi, I'm finding a simulation d435i work in gazebo. Is your camera plugin the d435i?

[aled96]

Hi, yes. The model was taken from pal-robotics.

I tried to change the settings to this:

<node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
        <param name="use_mag" type="bool" value="false" />
        <param name="_publish_tf" type="bool" value="false" />
        <param name="_world_frame" type="string" value="enu" />
        <remap from="/imu/data_raw" to="/d435i_camera/aceel/sample"/>
    </node>

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="frame_id" value="base_link"/>
        <arg name="args" value="--delete_db_on_start"/>
        <arg name="rgb_topic" value="/d435i_camera/color/image_raw"/>
        <arg name="depth_topic" value="/d435i_camera/depth/image_rect_raw"/>
        <arg name="camera_info_topic" value="/d435i_camera/color/camera_info"/>
        <arg name="depth_camera_info_topic" value="/d435i_camera/depth/camera_info"/>
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>
    </include>

    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="torso_2"/>
    <param name="/ukf_se/odom" value="rtabmap/odom"/>
    <rosparam param="/ukf_se/odom_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom_relative" value="true"/>
    <param name="/ukf_se/odom_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu" value="/d435i_camera/accel/sample"/>
    <rosparam param="/ukf_se/imu_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu_differential" value="true"/>
    <param name="/ukf_se/imu_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->

And I obtained this as "voxel_cloud":

Moreover, I noticed that the depth/image_rect_raw and aligned_depth_to_color/image_raw are not the same. For example, one has also the portion of the wall over the hole (door let's say), while the other one stops earlier.

[TouchDeeper]

@aled96 I don't find the d435i plugin you said, could you please post a link? Thank you a lot.

[aled96]

There is a pull request here: https://github.com/pal-robotics/realsense_gazebo_plugin/pulls

[TouchDeeper]

@aled96 Thank you very much.