Robot_localization not accepting Twist and Pose message types [closed]

edit

Hello all,

I am using Robot_localization ros package for estimating the pose of a UAV. I have successfully integrated the the following data:

1. Optical flow data (x and y speeds given by the optical flow sensor)
2. Altitude data (given by the altitude sensor)
3. Controller odometry data (roll, pitch, dyaw and daltitude data given by position controller)
4. IMU data

**First Problem : The pose obtained after integrating all these sensors is very good. But I am facing problems when sending the optical flow data (twist data) in the msg "geometry_msgs/TwistWithCovarianceStamped" and altitude data (pose data) in the msg "geometry_msgs/PoseWithCovarianceStamped". Whenever I send data in these msg types the ekf filter wont initialize and wont publish odometry/filtered. I publish the current time stamp as well as the framre id when I publish.

Since the filter was not initializing with the twist and pose msg types I sent the optical flow data and the altitude data in the msg nav_msgs/Odometry, and the filter initialized and started publishing the output. I don't know what could be causing this problem here?

This is the launch file I am using to launch the package

   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <param name="map_frame" value=""/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>  
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0"  value="optical_flow/data"/>
  <param name="odom1"  value="altitude/data"/>
  <param name="odom2"  value="controller/odom/data"/>
  <param name="imu0"   value="imu/data"/>

  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true,  true,  false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="odom1_config">[false, false, true,
                                  false, false, false,
                                  false, false, true,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="odom2_config">[false, false, false,
                                  false, false, false,
                                  true,  true,  true,
                                  false, false, true,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 true,  true,  true,
                                 false, false, false,
                                 true,  true,  true,
                                true,  true,  true]</rosparam>

</node>

<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 base_link fcu 100" >
</node>
<node pkg="tf" type="static_transform_publisher" name="link2_broadcaster" args="0 0 0 0 0 0 odom speeds_odom 100" >
</node>

I publish all the twist data and the IMU data in "/fcu" frame of reference and the pose data in "/speeds_odom" and hence I publish 2 static transforms one for "/base_link" and "/fcu" and other for "/odom" and "/speeds_odom" frame.

**Second Problem : The second problem I am facing is when using the topic "/set_pose" to start the ekf with a particular pose. For example I want to set EKF to start at a pose of x=2 and y=2 but when I publish this topic it resets the ekf and the ekf starts publishing again from x=0 and y=0 instead of x=2, y=2. Any Ideas where I might be going wrong?

Thanks.