hector localization works but with no translation

[buckleytoby]

I'm using the hector localization ROS package to take the IMU data from my phone, streamed into a ROS Imu message and reconstructing the 6DOF pose. No other sensors are used.

I can get attitude just fine, but the translation of /sensor_pose (or any other pose or tf topic) remains 0 for all time... I was hoping you could help me figure out why this is. Maybe I'm not setting the Imu frame correctly?

I'm using ros-indigo and catkin_make'd the source from scratch. Below is the source for a python script publishing my phone's imu data

import rospy
from sensor_msgs.msg import Imu
import socket, traceback, string
import pdb
from sys import stderr

host = ''
port = 5555
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
s.bind((host, port))

message, address = s.recvfrom(8192)
t0 = float(message.split(",")[0]) #initial time
print(t0)

def talker():
    pub = rospy.Publisher('raw_imu', Imu, queue_size=10)
    rospy.init_node('android_imu')
    rate = rospy.Rate(300) #this number must be very large, larger than max hz so that the
                           #rospy node always gets most recent packet 

    while not rospy.is_shutdown():
        imu_msg = Imu() #init imu msg
        imu_msg.orientation_covariance = 9*[-1] #no orientation info, so must set all to -1
        imu_msg.angular_velocity_covariance = 9*[0]
        imu_msg.linear_acceleration_covariance = 9*[0]
        imu_msg.header.frame_id = 'base_frame'

        try:
            #get data from android phone
            message, address = s.recvfrom(8192)
            data = message.split( "," )
            # extract data
            t = float(data[0]) #time
            #pdb.set_trace()
            imu_msg.header.stamp.secs = t
            sensorID = int(data[1])
            if sensorID==3:     # sensor ID for the eccelerometer
                ax, ay, az = data[2], data[3], data[4]
            if len(data) > 5:
                sensorID = int(data[5])
                if sensorID == 4: #sensor ID for gyroscope
                    gx, gy, gz = data[6], data[7], data[8] #angular acceleration
                else:
                    gx, gy, gz = 0.0, 0.0, 0.0 #if no data, just set to zero
                    imu_msg.angular_velocity_covariance = 9*[-1]

        except:
            print('except')
            ax, ay, az, gx, gy, gz = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
            imu_msg.angular_velocity_covariance = 9*[-1]
            imu_msg.linear_acceleration_covariance = 9*[-1]
        #put into imu msg
        imu_msg.linear_acceleration.x = float(ax)
        imu_msg.linear_acceleration.y = float(ay)
        imu_msg.linear_acceleration.z = float(az)
        imu_msg.angular_velocity.x = float(gx)
        imu_msg.angular_velocity.y = float(gy)
        imu_msg.angular_velocity.z = float(gz)
        #publish data
        pub.publish(imu_msg)
        #sleep
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

Best, Toby

[meyerj]

What you describe is expected behavior. hector_localization locks the components of the full state vector (3D pose and twist) that are not observable from the given measurements. With a good IMU you might be able to predict the translational velocity and position for a few seconds, but the uncertainty and mean error would grow indefinitely over time. You need to provide absolute position measurements to the pose estimator, e.g. from GPS or a map-based localization or SLAM approach. If you have an Android phone you could use the Android Location API and the built-in GPS if operating outdoors.

hector_pose_estimation does not solve the full localization problem for you, it only fuses the available information into a single, hopefully consistent pose and twist estimate.

[buckleytoby]

Thanks for reply! We also have lidar data that wasn't used above, do you have a suggestion for which package to use to take lidar+IMU --> 3d reconstruction ?