0
$\begingroup$

Rosanswers logo

Hello, I am trying to set up an Extended Kalman Filter (EKF). I have the following types of sensors mounted on board of the ROV system:

UPDATES WITH SENSORS OUTPUT SAMPLES:

  1. IMU - Primarily used for Orientation

Sensor Output Sample:

From: rostopic list echo/

header: 
  seq: 1900
  stamp: 
    secs: 1517940907
    nsecs: 527472019
  frame_id: "base_link"
orientation: 
  x: -0.447962131618
  y: 0.530470375983
  z: -0.480721599498
  w: -0.535572453192
orientation_covariance: [0.010000000000000002, 0.0, 0.0, 0.0, 0.010000000000000002, 0.0, 0.0, 0.0, 0.010000000000000002]
angular_velocity: 
  x: 0.00829399318143
  y: -0.00832041039286
  z: -0.0137973047577
angular_velocity_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
linear_acceleration: 
  x: 9.94429087323
  y: -0.333990296188
  z: 0.402224859015
linear_acceleration_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
---
  1. USBL- Sonar sensor that provides a global position (X,Y)

Sensor Output Sample:

From: rostopic list echo /

header: 
  seq: 19
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 5.53904389031
      y: 28.6836443911
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  covariance: [5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
  1. Depth sensor (Z)

Sensor Output Sample:

From: rostopic list echo /

header: 
  seq: 1963
  stamp: 
    secs: 1517941110
    nsecs: 334011078
  frame_id: "base_link"
fluid_pressure: 54.99332428
variance: 6.93760519813e-310
---
  1. Camera odometry velocity (X,Y)

Sensor Output Sample:

From: rostopic list echo /

header: 
  seq: 0
  stamp: 
    secs: 1517941154
    nsecs: 288109779
  frame_id: "camera"
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0384064087279
      y: 0.00654432723456
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.000137899317246916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0039129564840004e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Last but not least I am also attaching the /tf which is bouncing from odom->base_link to map->odom and I don't understand if this is correct:

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1517941485
        nsecs: 531367064
      frame_id: "map"
    child_frame_id: "odom"
    transform: 
      translation: 
        x: 0.0
        y: 0.0
        z: 0.0
      rotation: 
        x: -0.000328594829182
        y: -0.000809341203755
        z: -0.000655427597093
        w: 0.999999403703
---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1517941486
        nsecs:  31367064
      frame_id: "odom"
    child_frame_id: "base_link"
    transform: 
      translation: 
        x: 0.0
        y: 0.0
        z: 0.0
      rotation: 
        x: 0.402161523997
        y: -0.56568206831
        z: 0.438444191176
        w: 0.570996144849
---

In my case odom0 is considered my moving frame and I am trying to understand why my ROV is not updating its position with position messages. odom0 is published confimed with rqt_topic; world_frame instance of robot_localization has subscribed to odom0 and I confirmed that with rqt_graph, additionally imu0 is subscribed and working properly.

I also used the debug option offered from robot_localization:

   <param name="debug" value="true"/>
   <param name="debug_out_file" value="/home/to/Desktop/ekf_debug.txt"/>

And the output of this file clearly states that odom0 is never passed to the world frame, as a consequence of that the ROV is not moving. Below an image of what I am trying to achieve:

odom0

I don't understand what is happening. Below part of the debug file

......
.....
dynamic_process_noise_covariance is false
print_diagnostics is true
Subscribed to /usbl/pose_projected (**odom0**)
    odom0_differential is false
    odom0_pose_rejection_threshold is 1.7977e+308
    odom0_twist_rejection_threshold is 1.7977e+308
    odom0_queue_size is 1
    odom0 pose update vector is [t  t  f  f  f  f  f  f  f  f  f  f  f  f  f  ]
    odom0 twist update vector is [f  f  f  f  f  f  f  f  f  f  f  f  f  f  f  ]
Subscribed to /imu/data_w_orientation (imu0)
    imu0_differential is false
    imu0_pose_rejection_threshold is 1.7977e+308
    imu0_twist_rejection_threshold is 1.7977e+308
    imu0_linear_acceleration_rejection_threshold is 1.7977e+308
    imu0_remove_gravitational_acceleration is false
    imu0_queue_size is 1
    imu0 pose update vector is [f  f  f  t  t  t  f  f  f  f  f  f  f  f  f  ]
    imu0 twist update vector is [f  f  f  f  f  f  f  f  f  f  f  f  f  f  f  ]
    imu0 acceleration update vector is [f  f  f  f  f  f  f  f  f  f  f  f  f  f  f  ]
------ RosFilter::integrateMeasurements ------

......
.....

I am publishing the transformation in the following way:

import pyproj as proj
import rospy
import tf
import datetime
from geometry_msgs.msg import PoseWithCovarianceStamped
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Joy

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="odom"
                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...")

    def joyCallback(self,joyMsg):
        if(joyMsg.buttons[0]==1):
            self.pressed = True
            print("saving current location")
        return
def main():
    rospy.init_node('nav_projection_node')
    transformer = TransformPose()
    rospy.loginfo("starting")
    rospy.spin()
main()

My launch file is the following:

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

<node pkg="robot_localization" type="ekf_localization_node"
    name="ekf_map" 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="smooth_lagged_data" value="true"/>
   <param name="debug" value="true"/>
   <param name="debug_out_file" value="/home/emanuele/Desktop/ekf_debug.txt"/>
   <param name="print_diagnostics" value ="true"/>

   <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="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>

<node pkg="rosbag" type="play" name="player" output="screen" args="--clock -q -s 2340 /home/test.bag"/>
<node pkg="ros_float" type="transform_pose.py" name="tras_pose" output="screen"/>

<!--Adding URDF model-->
<arg name="floatModel" default="$(find ros_float)/urdf/floatModel.urdf"/>
<arg name="gui" default="true" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg floatModel)" />
<param name="publish_frequency" value="1.0" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

</launch>

Originally posted by RayROS on ROS Answers with karma: 108 on 2019-06-28

Post score: 0

Original comments

Comment by Tom Moore on 2019-07-03:
Please provide a sample message from all sensor inputs.

Comment by RayROS on 2019-07-03:
Hello Tom and thanks for taking the time to read the question. I provided a print-screen of all the sensor involved both from rostopic echo / and from rqt_topic I hope that could be helpuf to figure out what the problem might be

Comment by gvdhoorn on 2019-07-03:
@RayROS: I'm sorry to have to do this, but I've closed your question as with your latest edit you're violating the support guidelines. Specifically: don't post screenshots of terminals / code / messages.

It's all text, so copy-paste whatever is needed into your question and use the Preformatted Text button (the one with 101010 on it) to format it properly. Use the edit button/link to edit your question.

After you've fixed this, we can re-open.

Comment by RayROS on 2019-07-03:
@gvdhoorn, thanks for point that out. I just updated the sensor sample according to the guidelines. It should be correct now and sorry for adding the print-screen I thought it would have been easier to see it form there.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

I found the error to this question thanks to @Tom Moore for asking sample message from all sensor inputs and thanks to @gvdhoorn. The fact that I had to reformat the updated output of the sensors made me realize that the error was that the messages of USBL where never read by rosbag. After running rostopic echo / the following is the reason of my urdf for not moving:

header: 
  seq: 19
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 5.53904389031
      y: 28.6836443911
      z: 0.0
    orientation:

After reading the timestamp everything was fine and the urdf moved properly. I hope this could be useful for many other users! :)

Originally posted by RayROS with karma: 108 on 2019-07-05

This answer was ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

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