started adding odometry message

MarcGyongyosi · Jan 15, 2015 · 7806da6 · 7806da6
1 parent 1f508e4
commit 7806da6
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 2 deletions.
diff --git a/msf_core/CMakeLists.txt b/msf_core/CMakeLists.txt
@@ -40,7 +40,7 @@ generate_dynamic_reconfigure_options(cfg/MSF_Core.cfg)
 
 catkin_package(
     DEPENDS eigen glog_catkin
-    CATKIN_DEPENDS roscpp sensor_msgs dynamic_reconfigure msf_timing tf glog_catkin
+    CATKIN_DEPENDS roscpp sensor_msgs nav_msgs dynamic_reconfigure msf_timing tf glog_catkin
     INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
     LIBRARIES msf_core similaritytransform
     CFG_EXTRAS export_flags.cmake

diff --git a/msf_core/include/msf_core/implementation/msf_state_inl.h b/msf_core/include/msf_core/implementation/msf_state_inl.h
@@ -24,6 +24,7 @@
 #include <Eigen/Geometry>
 #include <vector>
 #include <msf_core/eigen_conversions.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_fusion_comm/ExtState.h>
 #include <sensor_fusion_comm/DoubleArrayStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
@@ -118,6 +119,23 @@ void GenericState_T<stateVector_T, StateDefinition_T>::ToPoseMsg(
   GetPoseCovariance(pose.pose.covariance);
 }
 
+
+/// Assembles an Odometry message from the state.
+/** it does not set the header */
+template<typename stateVector_T, typename StateDefinition_T>
+void GenericState_T<stateVector_T, StateDefinition_T>::ToOdometryMsg(
+    nav_msgs::Odometry& odometry) {
+  eigen_conversions::Vector3dToPoint(Get<StateDefinition_T::p>(),
+                                     odometry.pose.pose.position);
+  eigen_conversions::QuaternionToMsg(Get<StateDefinition_T::q>(),
+                                     odometry.pose.pose.orientation);
+  GetPoseCovariance(odometry.pose.covariance);
+  eigen_conversions::Vector3dToPoint(Get<StateDefinition_T::v>(),
+                                     odometry.twist.twist.linear);
+  eigen_conversions::Vector3dToPoint(w_m,
+                                     odometry.twist.twist.angular);
+}
+
 /// Assembles an ExtState message from the state
 /** it does not set the header */
 template<typename stateVector_T, typename StateDefinition_T>

diff --git a/msf_core/include/msf_core/msf_state.h b/msf_core/include/msf_core/msf_state.h
@@ -25,6 +25,7 @@
 #include <vector>
 #include <utility>
 #include <msf_core/eigen_conversions.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_fusion_comm/ExtState.h>
 #include <sensor_fusion_comm/DoubleArrayStamped.h>
 #include <sensor_fusion_comm/DoubleMatrixStamped.h>
@@ -206,6 +207,12 @@ struct GenericState_T {
    */
   void ToPoseMsg(geometry_msgs::PoseWithCovarianceStamped& pose);
 
+  /**
+   * \brief Assembles a Odometry message from the state.
+   * \note It does not set the header.
+   */
+  void ToOdometryMsg(nav_msgs::Odometry& odometry);
+
   /**
    * \brief Assemble an ExtState message from the state.
    * \note It does not set the header.

diff --git a/msf_core/package.xml b/msf_core/package.xml
@@ -20,12 +20,13 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>msf_timing</build_depend>
+  <build_depend>nav_msgs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>glog_catkin</build_depend>
   <build_depend>cmake_modules</build_depend>
-
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>msf_timing</run_depend>
   <run_depend>tf</run_depend>