Code Monkey home page Code Monkey logo

hector_localization's Introduction

hector_localization

The hector_localization stack is a collection of packages, that provide the full 6DOF pose of a robot or platform.

Team Hector Darmstadt uses the stack to estimate the full 6D pose of the robot within the real-time loop of the Hector quadrotor and even for estimating the position, velocity and attitude of a small airplane as part of our flight mechanics lab.

The design goals have been similar to what Chad Rockey described in his answer.

The core package currently provides a system model for generic 6DOF rigid body pose estimation based on pure IMU inputs (rates and accelerations), which can be specialized depending on the robot and for additional input values. As measurement models the package currently provides direct and barometric height measurements, GPS postion and velocity, magnetic field sensors as heading reference and a generic pose and twist measurement to fuse nav_msgs/Odometry messages from arbitrary sources (e.g. wheel odometry or SLAM).

The stack consists of a core library without dependencies to ROS beside message types and additional packages providing a node, nodelet or Orocos Real-Time Toolkit TaskContext interface. The system and measurement models could also be implemented and loaded as plugins with some minor modifications. In the background an Extended Kalman Filter based on the Bayesian Filter Library is responsible for fusing all information into a consistent state estimate and additionally keeps track which state variables are observable and which are not.

How to run the code

cd catkin_ws
git clone [email protected]:tu-darmstadt-ros-pkg/hector_localization.git
catkin_make --source hector_localization
roslaunch hector_pose_estimation hector_pose_estimation.launch 

#rosbag for test

  1. download the rosbag

    https://drive.google.com/folderview?id=0B4hFvojO5r3scWJRVWdhSmdLd0k&usp=sharing

  2. replay the rosbag

    rosbag play 2016-03-09-22-11-07.bag

hector_pose_estimation

hector_localization's People

Contributors

cehberlin avatar cottsay avatar libing64 avatar meyerj avatar mikaelarguedas avatar nolanholden avatar skohlbr avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

hector_localization's Issues

Potential vulnerability: topic names from ROS parameters

Hi,

We notice that you are using topic names from ROS parameters at the following locations:

sub1 = node.subscribe(g_odometry_topic, 10, &odomCallback);

sub2 = node.subscribe(g_pose_topic, 10, &poseCallback);

sub3 = node.subscribe(g_imu_topic, 10, &imuCallback);

sub4 = node.subscribe(g_topic, 10, &multiCallback);

g_pose_publisher = node.advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);

g_euler_publisher = node.advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);

For security reasons detailed below, we strongly suggest avoiding the usage of strings from parameters as topic names.

Although parameters are usually set in parameter files, they can also be changed by nodes. Specifically, other nodes in the same ROS application can also change the parameters listed above before it’s used, either by accident or intentionally (i.e., by potential attackers). Such changes can lead to failures to receive data of odometry, IMU, etc., which are used as the source of transformation messages, and therefore the robot can not receive the transformation. In addition, if an attacker exists, she can even fool the message_to_tf node to subscribe to wrong topics first, e.g. to /imu_topic_fake, and then forward the IMU data from the correct topic to /imu_topic_fake after modifying the contents, which will generate wrong transformations and lead to dangerous robot behaviors. Similarly, due to the use of parameters on the pose and eular topics, the publishing of pose messages can also be intercepted and even modified. Because ROS is an OSS (open-source software) community, third-party nodes are widely used in ROS applications, usually without complete vetting of their behavior, which gives the opportunity to potentially malicious actors to inject malicious code (e.g, by submitting hypocrite commits like in other OSS systems [1]) to infiltrate the ROS applications that use it (or software supply chain attacks, one of the primary means for real-world attackers today [2]).

We understand that using parameters to set topic names brings flexibility. Still, for the purpose of security, we strongly suggest that you avoid such vulnerable programming patterns if possible. For example, to avoid the exposure of this specific vulnerability, you may consider alternatives like remapping, which is designed for configuring names when launching the nodes.

[1] Q. Wu and K. Lu, “On the feasibility of stealthily introducing vulnerabilities in open-source software via hypocrite commits,” 2021, https://linuxreviews.org/images/d/d9/OpenSourceInsecurity.pdf.
[2] Supply chain attacks are the hacker’s new favourite weapon. and the threat is getting bigger. https://www.zdnet.com/article/supply-chain-attacks-are-the-hackers-new-favourite-weapon-and-the-threat-is-getting-bigger/.

Documentation

I found this library via your post on ROS answers [1]. At first glance it seems like a really nice and powerful framework. As you mentioned in your answer one of the current issues is the lack of documentation. I believe this library could be very valuable to a number of people including myself if there was at least a little bit of help to get started. In my opinion the following documentation would be helpful with in order of decreasing immediate usefulness.

  1. General overview over architecture of framework.
  2. Tutorial how to use the current components.
  3. Tutorial how to extend various parts of the system with new/altered components.
  4. Code documentation of classes/functions/etc...

I know time is most always scarce, but if you get round to creating some initial documentation, maybe you can consider this order of priority (to the extend you agree with it of course).

Cheers,
Nikolaus

[1] http://answers.ros.org/question/39790/robot_pose_ekf-and-gps/

Building on OS X Mavericks...

Hi,

I've successfully (with some pain and minor modifications, but...) built desktop_full on OS X Mavericks. However, after adding the hector_quadrotor package and it dependencies, I started getting problems. Here is the command that I ran:

raspberry:catkin_ws rasit$ VERBOSE=1 ./src/catkin/bin/catkin_make_isolated --pkg hector_pose_estimation_core -j1

[ 4%] Building CXX object CMakeFiles/hector_pose_estimation.dir/src/global_reference.cpp.o /usr/bin/c++ -DROSCONSOLE_BACKEND_LOG4CXX -DROS_PACKAGE_NAME=\"hector_pose_estimation_core\" -Dhector_pose_estimation_EXPORTS -fPIC -I/usr/local/Cellar/eigen/3.2.1/include/eigen3 -I/Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include -I/Volumes/Data/ZProjects/ROS/catkin_ws/install_isolated/include -I/usr/local/include -o CMakeFiles/hector_pose_estimation.dir/src/global_reference.cpp.o -c /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/src/global_reference.cpp In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/src/global_reference.cpp:29: In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include/hector_pose_estimation/global_reference.h:32: In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include/hector_pose_estimation/types.h:32: /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include/hector_pose_estimation/matrix.h:80:5: error: no template named 'DenseBase'; did you mean 'Eigen::DenseBase'? EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) ^ /usr/local/Cellar/eigen/3.2.1/include/eigen3/Eigen/src/Core/util/Macros.h:316:48: note: expanded from macro 'EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR' EIGEN_STRONG_INLINE Derived& operator=(const DenseBase& ... ^ /usr/local/Cellar/eigen/3.2.1/include/eigen3/Eigen/src/Core/DenseBase.h:41:34: note: 'Eigen::DenseBase' declared here template class DenseBase ^ In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/src/global_reference.cpp:29: In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include/hector_pose_estimation/global_reference.h:32: In file included from /Volumes/Data/ZProjects/ROS/catkin_ws/src/hector_localization/hector_localization/hector_pose_estimation_core/include/hector_pose_estimation/types.h:32:

Any help is greatly appreciated.

--Rasit

When I fuse IMU with Pressure sensor I got Error

Hi Im fusing IMU and Pressure Sensor . But I got the following error

[ERROR] [1632986676.837052390, 150.592000000]: Client [/hector_pose_estimation] wants topic /rexrov2/pressure to have datatype/md5sum [geometry_msgs/PointStamped/c63aecb41bfdfd6b7e1fac37c7cbe7bf], but our version has [sensor_msgs/FluidPressure/804dc5cea1c5306d6a2eb80b9833befe]. Dropping connection.

Any help how to transform sensor_msgs/FluidPressure to geometry_msgs/PointStamped so hector_pose_estimation can use it?

Nan tf in message_to_tf

I'm using the catkin branch of this repo, on a ROS indigo/Ubuntu trusty system. I'm sending IMU and Odometry information to message_to_tf using the libhector_gazebo_ros_imu and libgazebo_ros_p3d plugins. The base_link -> base_stabilized tf published by message_to_tf switches intermittently between

--
  - 
    header: 
      seq: 0
      stamp: 
        secs: 8
        nsecs: 130000000
      frame_id: base_stabilized
    child_frame_id: base_link
    transform: 
      translation: 
        x: nan
        y: 1.57918518582e-316
        z: 6.95335086244e-310
      rotation: 
        x: -9.66511399003e-05
        y: 0.000187435422708
        z: 1.81158476653e-08
        w: 0.999999977763

---

and

--
  - 
    header: 
      seq: 0
      stamp: 
        secs: 8
        nsecs: 95000000
      frame_id: base_stabilized
    child_frame_id: base_link
    transform: 
      translation: 
        x: 0.0
        y: 0.0
        z: 0.0
      rotation: 
        x: -7.33904804506e-08
        y: 2.31638185891e-08
        z: 1.70000377533e-15
        w: 1.0

---

After some investigation, in the part of imuCallback that creates this transform, the translation is never set. Adding a setOrigin(tf::Vector3(0.0, 0.0, 0.0)) resolves the issue. Does this make sense as a fix?

Roll Pitch problem (huge linear acceleration bias)

Hello all
I am trying to fuse imu, poseupdate which published from hector_slam using 2D Hokuyo lidar and 1D lidar for heigt measuerement using hector_localization.
At first I got a error meassge "Invalid state, resetting..." and I solved it with edit poseupdate.cpp (I think covariance and H matrix at updating Yaw was problem).
After that, it looks algorith works fine, but when robot start to move, roll and ptich of base_link and /pose are become tilted. And vector x, y of /linear acceleration bias goes larger and larger.
Attitude of /sensor_pose looks fine that I don`t think subscribing IMU problem or wrong frame problem..
And when I check the debug messages, "updating with system model gyro or accelerometer" results shows just 0 (dt * f(x) ] [ 0 0 0 0 0 0 0 0 ...]. Is it relavant?

If someone knows the answer, please let me know.. I really want make it move...

Best regards,

Sensor fusion of IMU and poseupdate topic

We have robotic platform working properly with robot_pose_ekf and robot_localization. We want to compare the performance of hector_localization package with these SW packages. But when I execute the hector_localization with IMU topic and poseupdate topic from odometry data, I got the error message " Invalid state, resetting". Is there any example working bag file(IMU + odometry data via poseupdate) for hector_localization? Could you please provide me any suggestion or hint about the reason of this error message?
Thanks for your support.

Best Regards
Min Latt

how to use hector_localication on quadrotor

Our quadrotor has localication problem when we use only hector_slam without odometry. We want to apply hector_localication but we are confused how to do it.

Our first question is how to use ahrs topic and rollpitch topic. Their data look same. Whats the differencess?

And also i want to know how to entegrate gps topic in indoor environment. Because no gps signal in indoor.

Thank you .

Could not build through instructions given, used an alternate command

I'm running ROS Noetic on Ubuntu 20.04, and I couldn't build with the commands in the README. Also, my source directory is catkin_ws/src, and also i couldn't clone through the command in the README (maybe since I hadn't logged in to git, but copy pasting the link to the repo instead of the link they put, is the simple fix). So to anyone reading this with a similar problem, just replace the --source flag with --only-pkg-with-deps, invoked from your catkin_ws folder is your workspace is structured like mine

Odometry+slam fusion?

Hello,

New to hector_localization, I have used hector_mapping for the last couple of months and I was wondering if it is possible to fuse slam_out_pose and odometry as input to hector_localization to obtain a fused pose. I am having issues with localization on corridors, which fusing odometry into the mix may help somewhat.

Any pointers into this issue will be greatly appreciated.

hector localization works but with no translation

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

Building on OSX Yosemite

Hi, I am unable to build Hector localization on Yosemite. One of the errors faced was the sincos use which was easily bypassed by the __sincos function in OSX.

Another issue faced is an error in a Macro which seems fine, any clues on how to make it work with OSX? http://pastebin.com/Qxcv2QNg

Any pointers would be great.

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.