0
$\begingroup$

Rosanswers logo

I am using robot_localizationin my project. I'm fusing data from camera odometry, imu, depth sensor and usbl (XY position sensor) as odom0, , odom1, odom2 and imu0 respectively. I followed the instructions in configuring the parameters on official documentation and set the frames as a "fixed frame" and a "moving frame". In my case the moving frame is the odom frame and the fixed frame is the base_link.

rviz

The documentation tells me this should make the ekf_localization node publish the transform from map to odom if the publish_tf param is set to true. The launch file both for the "fixed frame" and the "moving frame" is below together with the tf tree:

<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="true" />

 <node pkg="robot_localization" type="ekf_localization_node"
     name="ekf_odom" clear_params="false">
    <param name="use_sim_time" value="true" />
    <param name="sensor_timeout" value="2.0"/>
    <param name="two_d_mode" value="false"/>
    <param name="map_frame" value="map"/>
    <param name="odom_frame" value="odom"/>
    <param name="base_link_frame" value="base_link"/>
    <param name="world_frame" value="odom"/>
    <param name="publish_tf" value="true"/>
    <param name="frequency" value="10"/>

    <param name="imu0" value="/imu/data_w_orientation"/>
    <rosparam param="imu0_config">[false, false, false,
                                   true,  true,  true,
                                   false, false, false,
                                   false, false, false,
                                   false, false, false]</rosparam>
    <param name="imu0_differential" value="false"/>

    <param name="smooth_lagged_data" value="true"/>


    <param name="odom0" value="/cam/odometry"/>
    <rosparam param="odom0_config">[false, false, false,
                                    true,  true,  false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>
    <param name="odom0_differential" value="false"/>
  </node>

<node pkg="robot_localization" type="ekf_localization_node"
    name="ekf_map" clear_params="false">
   <param name="use_sim_time" value="true" />
   <param name="transform_time_offset" value="0.2"/>
   <param name="sensor_timeout" value="2.0"/>
   <param name="two_d_mode" value="false"/>
   <param name="map_frame" value="map"/>
   <param name="odom_frame" value="odom"/>
   <param name="base_link_frame" value="base_link"/>
   <param name="world_frame" value="map"/>
   <param name="publish_tf" value="true"/>
   <param name="frequency" value="10"/>
   <param name="smooth_lagged_data" value="true"/>

<!-- upload config files here -->
   <rosparam file="$(find ros_float)/config/au_au.yaml" command="load" />
   <rosparam file="$(find ros_float)/config/cov_config.yaml" command="load" />

   <param name="odom0" value="/usbl/pose_projected"/>
   <rosparam param="odom0_config">[true, true,  false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>
   <param name="odom0_differential" value="false"/>
   <param name="odom0_nodelay" value="true"/>
   <param name="odom0_relative" value="false"/>

   <param name="imu0" value="/imu/data_w_orientation"/>
   <rosparam param="imu0_config">[false, false, false,
                                  true,  true,  true,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>
   <param name="imu0_differential" value="false"/>

   <param name="odom1" value="/filter/fluid_pressure/depth"/>
   <rosparam param="odom1_config">[false, false, true,
                                   false, false, false,
                                   false, false, false,
                                   false, false, false,
                                   false, false, false]</rosparam>
   <param name="odom1_differential" value="false"/>

   <param name="odom2" value="/cam/odometry"/>
   <rosparam param="odom2_config">[false, false, false,
                                   true,  true,  false,
                                   false, false, false,
                                   false, false, false,
                                   false, false, false]</rosparam>
   <param name="odom2_differential" value="false"/>

 </node>
</launch>

what I have tried to solve the problem so far:

Someone already had the same problem here but a clear answer was not provided. A small update was posted advising to add the following specification to the .launch file:

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

However it didn't work.

Following my previous posts here and here no answer was provided. Unfortunately I am still stuck with this problem and no urdf model is moving according to the moving frame. The odometry does not show any particular uncertainty and in fact the arrow is still pointed in the middle of the urdf model.

On official documentation there is the presence of a dual EKF example and in fact it is possible to launch it from here /opt/ros/melodic/share/robot_localization/launch/dual_ekf_navsat_example.launch and exactly here it was provided the duality "fixed frame" and "moving frame". I did it exactly as it said the example but still no luck

UPDATES on tf_tree

My tf_tree is the following

tree

Finally the small python script that does the transformation can be seen below:

from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry

class TransformPose:
    def __init__(self):
        self.pub = rospy.Publisher('/usbl/pose_projected', Odometry, queue_size=10)
        self.pressed = False
        rospy.Subscriber("/usbl/latitude_longitude", NavSatFix, self.callback)

    def paramsReady(self):
        return rospy.has_param('/utm_zone') and\
               rospy.has_param('/origin_lon') and\
               rospy.has_param('/origin_lat') and\
               rospy.has_param('/origin_z')

    def callback(self,data):
        rospy.loginfo_once("received data lat lon usbl")
        if(self.paramsReady()):
            rospy.loginfo_once("Valid survey params found:  transform_pose starting")
            crs_wgs = proj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic
            utm_zone = rospy.get_param("/utm_zone")
            crs_bng = proj.Proj(init=utm_zone) # use a locally appropriate projected CRS

            originLon = rospy.get_param("/origin_lon")
            originLat = rospy.get_param("/origin_lat")
            # then cast your geographic coordinate pair to the projected system
            xoff, yoff = proj.transform(crs_wgs, crs_bng, originLon, originLat)
            zoff = rospy.get_param("/origin_z",0)
            # input from NavSatFix
            lon = data.longitude;
            lat = data.latitude;
            x, y = proj.transform(crs_wgs, crs_bng, lon, lat)
            x = x-xoff
            y = y-yoff
            z = 0;

            # output message
            transformed_msg = Odometry()
            transformed_msg.child_frame_id = "base_link"

            if x== float('Inf') or y==float('Inf'):
                pass
            else:
                transformed_msg.pose.pose.position.x=x
                transformed_msg.pose.pose.position.y=y
                transformed_msg.pose.pose.position.z=z
                transformed_msg.header.frame_id="map"
                transformed_msg.pose.covariance = [5., 0., 0., 0., 0., 0.,
                                                   0., 5., 0., 0., 0., 0.,
                                                   0., 0., 5., 0., 0., 0.,
                                                   0., 0., 0., 0., 0., 0.,
                                                   0., 0., 0., 0., 0., 0.,
                                                   0., 0., 0., 0., 0., 0.]
                self.pub.publish(transformed_msg)
                if self.pressed:
                    self.button_pub.publish(transformed_msg)
                    self.pressed = False
        else:
            rospy.loginfo_once("Receiving odom message but urvey params not ready.  waiting...")

Please point in the right direction or advice on what the problem could be because as of now I don't have any additional idea on how to move on and make the urdf according to fixed-moving frame using robot localization.


Originally posted by RayROS on ROS Answers with karma: 108 on 2019-07-02

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You have a bad frame configuration in your map-frame EKF.

Here's your odom-frame EKF:

<node pkg="robot_localization" type="ekf_localization_node"
     name="ekf_odom" clear_params="false">
  <param name="use_sim_time" value="true" />
  <param name="sensor_timeout" value="2.0"/>
  <param name="two_d_mode" value="false"/>
  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>

Here's your map-frame EKF:

<node pkg="robot_localization" type="ekf_localization_node"
    name="ekf_map" clear_params="false">
  <param name="use_sim_time" value="true" />
  <param name="transform_time_offset" value="0.2"/>
  <param name="sensor_timeout" value="2.0"/>
  <param name="two_d_mode" value="false"/>
  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>          <!-- OFFENDING LINE -->

The world_frame in your map-frame EKF should be map, not odom. There's a template config in the package.

There may be other issues, but that's a big one.


Originally posted by Tom Moore with karma: 13689 on 2019-07-03

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.