Merging ayrton04's branches

CHANGELOG.rst

@@ -2,6 +2,33 @@
 Changelog for package robot_localization
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2015-05-22)
+------------------
+* Added tf2-friendly tf_prefix appending
+* Corrected for IMU orientation in navsat_transform
+* Fixed issue with out-of-order measurements and pose resets
+* Nodes now assume ENU standard for yaw data
+* Removed gps_common dependency
+* Adding option to navsat_transform_node that enables the use of the heading from the odometry message instead of an IMU.
+* Changed frame_id used in setPoseCallback to be the world_frame
+* Optimized Eigen arithmetic for signficiant performance boost
+* Migrated to tf2
+* Code refactoring and reorganization
+* Removed roll and pitch from navsat_transform calculations
+* Fixed transform for IMU data to better support mounting IMUs in non-standard orientations
+* Added feature to navsat_transform_node whereby filtered odometry data can be coverted back into navsat data
+* Added a parameter to allow future dating the world_frame->base_link_frame transform.
+* Removed deprecated differential setting handler
+* Added relative mode
+* Updated and improved tests
+* Fixing source frame_id in pose data handling
+* Added initial covariance parameter
+* Fixed bug in covariance copyinh
+* Added parameters for topic queue sizes
+* Improved motion model's handling of angular velocities when robot has non-zero roll and pitch
+* Changed the way differential measurements are handled
+* Added diagnostics
+
 2.1.7 (2015-01-05)
 ------------------
 * Added some checks to eliminate unnecessary callbacks

CMakeLists.txt

@@ -88,7 +88,7 @@ target_link_libraries(ukf filter_base ${catkin_LIBRARIES})
 target_link_libraries(ros_filter ekf ukf ros_filter_utilities ${catkin_LIBRARIES})
 target_link_libraries(ekf_localization_node ros_filter ${catkin_LIBRARIES})
 target_link_libraries(ukf_localization_node ros_filter ${catkin_LIBRARIES})
-target_link_libraries(navsat_transform ${catkin_LIBRARIES})
+target_link_libraries(navsat_transform filter_utilities ros_filter_utilities ${catkin_LIBRARIES})
 target_link_libraries(navsat_transform_node navsat_transform ${catkin_LIBRARIES})
 
 #############

include/robot_localization/filter_utilities.h

@@ -56,6 +56,12 @@ namespace RobotLocalization
     //! @return the bounded value
     //!
     double clampRotation(double rotation);
+
+    //! @brief Utility method for appending tf2 prefixes cleanly
+    //! @param[in] tfPrefix - the tf2 prefix to append
+    //! @paramp[in, out] frameId - the resulting frame_id value
+    //!
+    void appendPrefix(std::string tfPrefix, std::string &frameId);
   }
 }
 

include/robot_localization/navsat_transform.h

@@ -40,6 +40,8 @@
 
 #include <tf2/LinearMath/Transform.h>
 #include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <Eigen/Dense>
 
@@ -148,6 +150,13 @@ namespace RobotLocalization
       //!
       bool useManualDatum_;
 
+      //! @brief Frame ID of the robot's body frame
+      //!
+      //! This is needed for obtaining transforms from the robot's body
+      //! frame to the frames of sensors (IMU and GPS)
+      //!
+      std::string baseLinkFrameId_;
+
       //! @brief Frame ID of the GPS odometry output
       //!
       //! This will just match whatever your odometry message has
@@ -186,6 +195,14 @@ namespace RobotLocalization
       //!
       Eigen::MatrixXd latestOdomCovariance_;
 
+      //! @brief Transform buffer for managing coordinate transforms
+      //!
+      tf2_ros::Buffer tfBuffer_;
+
+      //! @brief Transform listener for receiving transforms
+      //!
+      tf2_ros::TransformListener tfListener_;
+
       //! @brief Used for publishing the static world_frame->utm transform
       //!
       tf2_ros::StaticTransformBroadcaster utmBroadcaster_;

include/robot_localization/ros_filter.h

@@ -303,24 +303,6 @@ namespace RobotLocalization
       //!
       std::vector<int> loadUpdateConfig(const std::string &topicName);
 
-      //! @brief Method for safely obtaining transforms.
-      //! @param[in] targetFrame - The target frame of the desired transform
-      //! @param[in] sourceFrame - The source frame of the desired transform
-      //! @param[in] time - The time at which we want the transform
-      //! @param[out] targetFrameTrans - The resulting transform object
-      //! @return Sets the value of @p targetFrameTrans and returns true if successful,
-      //! false otherwise.
-      //!
-      //! This method attempts to obtain a transform from the @p sourceFrame to the @p
-      //! targetFrame at the specific @p time. If no transform is available at that time,
-      //! it attempts to simply obtain the latest transform. If that still fails, then the
-      //! method checks to see if the transform is going from a given frame_id to itself.
-      //! If any of these checks succeed, the method sets the value of @p targetFrameTrans
-      //! and returns true, otherwise it returns false.
-      //!
-      bool lookupTransformSafe(const std::string &targetFrame, const std::string &sourceFrame,
-                               const ros::Time &time, tf2::Transform &targetFrameTrans);
-
       //! @brief Prepares an IMU message's linear acceleration for integration into the filter
       //! @param[in] msg - The IMU message to prepare
       //! @param[in] topicName - The name of the topic over which this message was received
@@ -532,10 +514,6 @@ namespace RobotLocalization
       //!
       tf2_ros::TransformListener tfListener_;
 
-      //! @brief tf prefix
-      //!
-      std::string tfPrefix_;
-
       //! @brief For future (or past) dating the world_frame->base_link_frame transform
       //!
       ros::Duration tfTimeOffset_;

include/robot_localization/ros_filter_utilities.h

@@ -35,6 +35,7 @@
 
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
+#include <tf2_ros/buffer.h>
 
 #include <Eigen/Dense>
 
@@ -52,6 +53,28 @@ namespace RobotLocalization
 {
   namespace RosFilterUtilities
   {
+    //! @brief Method for safely obtaining transforms.
+    //! @param[in] buffer - tf buffer object to use for looking up the transform
+    //! @param[in] targetFrame - The target frame of the desired transform
+    //! @param[in] sourceFrame - The source frame of the desired transform
+    //! @param[in] time - The time at which we want the transform
+    //! @param[out] targetFrameTrans - The resulting transform object
+    //! @return Sets the value of @p targetFrameTrans and returns true if successful,
+    //! false otherwise.
+    //!
+    //! This method attempts to obtain a transform from the @p sourceFrame to the @p
+    //! targetFrame at the specific @p time. If no transform is available at that time,
+    //! it attempts to simply obtain the latest transform. If that still fails, then the
+    //! method checks to see if the transform is going from a given frame_id to itself.
+    //! If any of these checks succeed, the method sets the value of @p targetFrameTrans
+    //! and returns true, otherwise it returns false.
+    //!
+    bool lookupTransformSafe(const tf2_ros::Buffer &buffer,
+                             const std::string &targetFrame,
+                             const std::string &sourceFrame,
+                             const ros::Time &time,
+                             tf2::Transform &targetFrameTrans);
+
     //! @brief Utility method for converting quaternion to RPY
     //! @param[in] quat - The quaternion to convert
     //! @param[out] roll - The converted roll

src/filter_utilities.cpp

@@ -116,5 +116,26 @@ namespace RobotLocalization
 
       return rotation;
     }
+
+    void appendPrefix(std::string tfPrefix, std::string &frameId)
+    {
+      // Strip all leading slashes for tf2 compliance
+      if(!frameId.empty() && frameId.at(0) == '/')
+      {
+        frameId = frameId.substr(1);
+      }
+
+      if(!tfPrefix.empty() && tfPrefix.at(0) == '/')
+      {
+        tfPrefix = tfPrefix.substr(1);
+      }
+
+      // If we do have a tf prefix, then put a slash in between
+      if(!tfPrefix.empty())
+      {
+        frameId = tfPrefix + "/" + frameId;
+      }
+    }
+
   }
 }

src/navsat_transform.cpp

@@ -32,7 +32,9 @@
 
 #include "robot_localization/navsat_transform.h"
 #include "robot_localization/filter_common.h"
+#include "robot_localization/filter_utilities.h"
 #include "robot_localization/navsat_conversions.h"
+#include "robot_localization/ros_filter_utilities.h"
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
@@ -56,7 +58,9 @@ namespace RobotLocalization
     useManualDatum_(false),
     zeroAltitude_(false),
     worldFrameId_("odom"),
-    utmZone_("")
+    baseLinkFrameId_("base_link"),
+    utmZone_(""),
+    tfListener_(tfBuffer_)
  {
     latestUtmCovariance_.resize(POSE_SIZE, POSE_SIZE);
   }
@@ -171,6 +175,7 @@ namespace RobotLocalization
   {
     tf2::fromMsg(msg->pose.pose, transformWorldPose_);
     worldFrameId_ = msg->header.frame_id;
+    baseLinkFrameId_ = msg->child_frame_id;
     hasTransformOdom_ = true;
 
     ROS_INFO_STREAM("Initial odometry position is (" << std::fixed << 
@@ -184,29 +189,64 @@ namespace RobotLocalization
     // UTM->world_frame transform.
     if(!transformGood_ && useOdometryYaw_ && !useManualDatum_)
     {
-      sensor_msgs::Imu imu;
-      imu.orientation = msg->pose.pose.orientation;
-      imu.header.frame_id = msg->child_frame_id;
-      sensor_msgs::ImuConstPtr imuPtr(&imu);
+      sensor_msgs::Imu *imu = new sensor_msgs::Imu();
+      imu->orientation = msg->pose.pose.orientation;
+      imu->header.frame_id = msg->child_frame_id;
+      sensor_msgs::ImuConstPtr imuPtr(imu);
       imuCallback(imuPtr);
     }
   }
 
   void NavSatTransform::imuCallback(const sensor_msgs::ImuConstPtr& msg)
   {
-    /* This method only gets called if we don't yet have the
-     * IMU data (the subscriber gets shut down once we compute
-     * the transform), so we can assumed that every IMU message
-     * that comes here is meant to be used for that purpose. */
-    tf2::fromMsg(msg->orientation, transformOrientation_);
-    hasTransformImu_ = true;
-
-    double roll, pitch, yaw;
-    tf2::Matrix3x3 mat;
-    mat.setRotation(transformOrientation_);
-    mat.getRPY(roll, pitch, yaw);
-    ROS_INFO_STREAM("Initial orientation roll, pitch, yaw is (" << 
-                    roll << ", " << pitch << ", " << yaw << ")");
+    // We need the baseLinkFrameId_ from the odometry message, so
+    // we need to wait until we receive it.
+    if(hasTransformOdom_)
+    {
+      /* This method only gets called if we don't yet have the
+       * IMU data (the subscriber gets shut down once we compute
+       * the transform), so we can assumed that every IMU message
+       * that comes here is meant to be used for that purpose. */
+      tf2::fromMsg(msg->orientation, transformOrientation_);
+
+      // Correct for the IMU's orientation w.r.t. base_link
+      tf2::Transform targetFrameTrans;
+      RosFilterUtilities::lookupTransformSafe(tfBuffer_,
+                                              baseLinkFrameId_,
+                                              msg->header.frame_id,
+                                              msg->header.stamp,
+                                              targetFrameTrans);
+
+      double rollOffset = 0;
+      double pitchOffset = 0;
+      double yawOffset = 0;
+      double roll = 0;
+      double pitch = 0;
+      double yaw = 0;
+      RosFilterUtilities::quatToRPY(targetFrameTrans.getRotation(), rollOffset, pitchOffset, yawOffset);
+      RosFilterUtilities::quatToRPY(transformOrientation_, roll, pitch, yaw);
+
+      ROS_DEBUG_STREAM("Initial orientation roll, pitch, yaw is (" <<
+                       roll << ", " << pitch << ", " << yaw << ")");
+
+      // Apply the offset (making sure to bound them), and throw them in a vector
+      tf2::Vector3 rpyAngles(FilterUtilities::clampRotation(roll - rollOffset),
+                             FilterUtilities::clampRotation(pitch - pitchOffset),
+                             FilterUtilities::clampRotation(yaw - yawOffset));
+
+      // Now we need to rotate the roll and pitch by the yaw offset value.
+      // Imagine a case where an IMU is mounted facing sideways. In that case
+      // pitch for the IMU's world frame is roll for the robot.
+      tf2::Matrix3x3 mat;
+      mat.setRPY(0.0, 0.0, yawOffset);
+      rpyAngles = mat * rpyAngles;
+      transformOrientation_.setRPY(rpyAngles.getX(), rpyAngles.getY(), rpyAngles.getZ());
+
+      ROS_DEBUG_STREAM("Initial corrected orientation roll, pitch, yaw is (" <<
+                       rpyAngles.getX() << ", " << rpyAngles.getY() << ", " << rpyAngles.getZ() << ")");
+
+      hasTransformImu_ = true;
+    }
   }
 
   void NavSatTransform::computeTransform()