mavros/local_position/pose values are totally different than the position generated from gazebo model and odometry sensors

asked 2017-08-13 23:55:23 -0600

RSA_kustar gravatar image

I am trying to use mavros with px4 firmware and sitl. I have a launch file that opens a gazebo environment, spawns the drone, runs mavros, and runs px4. The robot model I used is iris. I created the following xacro with the imu, odometry and gps plugins as you can see. Then I convert it to sdf to be used. Also, I created some static transformations to the launch file as shown as well.

The problem is that the position of the iris when using mavros/local_position/pose is different from ground_truth/pose generated from gazebo simulator. I do not know the source of the problem. So,

1- Is there any errors in the model used ?

2- What is the difference between parent_link and parent_frame_id link in the odometry plugin ??

3- Are the provided static transformation right or wrong ?

4- Any other suggestions to solve this problem

if more information required let me know and I will add it

Thanks in Advance

The xacro file for the iris as follows:

<?xml version="1.0"?>
<robot name="iris" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name='name' default='iris' />
<xacro:arg name='mavlink_addr' default='INADDR_ANY' />
<xacro:arg name='mavlink_udp_port' default='14560' />
<xacro:arg name='visual_material' default='DarkGrey' />
<xacro:arg name='enable_mavlink_interface' default='true' />
<xacro:arg name='enable_wind' default='false' />
<xacro:arg name='enable_ground_truth' default='true' />
<xacro:arg name='enable_logging' default='false' />
<xacro:arg name='log_file' default='iris' />
<!-- macros for gazebo plugins, sensors -->
<xacro:include filename="$(find pkg)/urdf/iris_accessories/component_snippets.xacro" />
<!-- Instantiate iris "mechanics" -->
 <xacro:include filename="$(find pkg)/urdf/iris_accessories/iris.xacro" />
 <xacro:if value="$(arg enable_wind)">
 <xacro:wind_plugin_macro
    namespace="${namespace}"
    wind_direction="0 0 1"
    wind_force_mean="0.7"
    xyz_offset="1 0 0"
    wind_gust_direction="0 0 0"
    wind_gust_duration="0"
    wind_gust_start="0"
    wind_gust_force_mean="0"
    />
   </xacro:if>
   <xacro:if value="$(arg enable_mavlink_interface)">
   <!-- Instantiate mavlink telemetry interface. -->
   <xacro:mavlink_interface_macro
    namespace="${namespace}"
    imu_sub_topic="/imu"
    mavlink_addr="INADDR_ANY"
    mavlink_udp_port="14560"
    >
   </xacro:mavlink_interface_macro>
    </xacro:if>
  <!-- Mount an ADIS16448 IMU. -->
  <xacro:imu_plugin_macro
    namespace="${namespace}"
    imu_suffix=""
    parent_link="base_link"
    imu_topic="/imu"
   mass_imu_sensor="0.015"
   gyroscope_noise_density="0.0003394"
   gyroscopoe_random_walk="0.000038785"
   gyroscope_bias_correlation_time="1000.0"
   gyroscope_turn_on_bias_sigma="0.0087"
   accelerometer_noise_density="0.004"
   accelerometer_random_walk="0.006"
   accelerometer_bias_correlation_time="300.0"
   accelerometer_turn_on_bias_sigma="0.1960"
   >
   <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
   <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:imu_plugin_macro>
   <xacro:if value="$(arg enable_ground_truth)">
    <!-- Mount an IMU providing ground truth. -->
    <xacro:imu_plugin_macro
      namespace="${namespace}"
      imu_suffix="gt"
      parent_link="base_link"
     imu_topic="ground_truth/imu"
     mass_imu_sensor="0.00001"
     gyroscope_noise_density="0.0"
     gyroscopoe_random_walk="0.0"
     gyroscope_bias_correlation_time="1000.0"
     gyroscope_turn_on_bias_sigma="0.0"
     accelerometer_noise_density="0.0"
     accelerometer_random_walk="0.0"
     accelerometer_bias_correlation_time="300.0"
     accelerometer_turn_on_bias_sigma="0.0"
    >
    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0"  izz="0.00001" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    </xacro:imu_plugin_macro>
   <!-- Mount a generic odometry sensor providing ground truth. -->
   <xacro:odometry_plugin_macro
     namespace="${namespace}/ground_truth"
     odometry_sensor_suffix="1"
     parent_link="/base_link"
     pose_topic="pose"
     pose_with_covariance_topic="pose_with_covariance"
     position_topic="position"
     transform_topic="transform"
     odometry_topic="odometry"
     parent_frame_id="world"
     mass_odometry_sensor="0 ...
(more)
edit retag flag offensive close merge delete