Merge pull request #119 from yujinrobot/odom_helper_check

[navi_toolkit] OdometryHelper checks / info if odom is available

yocs_navi_toolkit/include/yocs_navi_toolkit/odometry_helper.hpp

@@ -35,6 +35,11 @@ class OdometryHelper
   OdometryHelper(const std::string& odometry_topic_name);
   virtual ~OdometryHelper();
 
+  /**
+   * @brief Returns if initialised (odometry available)
+   */
+  bool initialized();
+
   /********************
   ** Setters
   ********************/
@@ -46,29 +51,33 @@ class OdometryHelper
   // convenience getters in various formats.
 
   /**
-   * @brief 3d position of the robot in eigen format.
+   * @brief 3d position of the robot in Eigen format.
+   * @return true if successful (helper got odometry)
    */
-  void position(Eigen::Vector3f& position);
+  bool position(Eigen::Vector3f& position);
 
   /**
    * @brief Heading angle for mobile robot 2d use cases.
    *
    * @param angle : in radians
+   * @return true if successful (helper got odometry)
    */
-  void yaw(float& angle);
+  bool yaw(float& angle);
 
   /**
    * @brief Mobile robot velocities in a 2d use case.
    *
    * @param mobile_robot_velocities : linear translational velocity, v and angular rate, w
+   * @return true if successful (helper got odometry)
    */
-  void velocities(std::pair<float, float>& mobile_robot_velocities);
-  nav_msgs::Odometry odometry();
+  bool velocities(std::pair<float, float>& mobile_robot_velocities);
+
+  std::shared_ptr<nav_msgs::Odometry> odometry();
 
 protected:
   ros::Subscriber odometry_subscriber_;
   std::mutex data_mutex_;
-  nav_msgs::Odometry odometry_;
+  std::unique_ptr<nav_msgs::Odometry> odometry_;
 };
 
 /*****************************************************************************

yocs_navi_toolkit/src/lib/odometry_helper.cpp

@@ -29,31 +29,74 @@ OdometryHelper::~OdometryHelper() {}
 void OdometryHelper::odometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
-  odometry_ = *msg;
+
+  if(!odometry_)
+  {
+    odometry_ = std::unique_ptr<nav_msgs::Odometry>(new nav_msgs::Odometry());
+  }
+
+  *odometry_ = *msg;
+}
+
+bool OdometryHelper::initialized()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  if(odometry_)
+  {
+    return true;
+  }
+
+  return false;
 }
 
-nav_msgs::Odometry OdometryHelper::odometry()
+std::shared_ptr<nav_msgs::Odometry> OdometryHelper::odometry()
 {
+  if(!initialized())
+  {
+    return std::shared_ptr<nav_msgs::Odometry>();
+  }
+
   std::lock_guard<std::mutex> lock(data_mutex_);
-  nav_msgs::Odometry msg = odometry_;
+  std::shared_ptr<nav_msgs::Odometry> msg = std::make_shared<nav_msgs::Odometry>(*odometry_);
   return msg;
 }
 
-void OdometryHelper::position(Eigen::Vector3f& position) {
+bool OdometryHelper::position(Eigen::Vector3f& position) {
+  if(!initialized())
+  {
+    return false;
+  }
+
   std::lock_guard<std::mutex> lock(data_mutex_);
-  position << odometry_.pose.pose.position.x, odometry_.pose.pose.position.y, odometry_.pose.pose.position.z;
+  position << odometry_->pose.pose.position.x, odometry_->pose.pose.position.y, odometry_->pose.pose.position.z;
+
+  return true;
 }
 
-void OdometryHelper::yaw(float& angle) {
+bool OdometryHelper::yaw(float& angle) {
+  if(!initialized())
+  {
+    return false;
+  }
+
   std::lock_guard<std::mutex> lock(data_mutex_);
-  angle = tf::getYaw(odometry_.pose.pose.orientation);
+  angle = tf::getYaw(odometry_->pose.pose.orientation);
+
+  return true;
 }
 
-void OdometryHelper::velocities(std::pair<float, float>& mobile_twist_velocities)
+bool OdometryHelper::velocities(std::pair<float, float>& mobile_twist_velocities)
 {
+  if(!initialized())
+  {
+    return false;
+  }
+
   std::lock_guard<std::mutex> lock(data_mutex_);
-  mobile_twist_velocities.first = odometry_.twist.twist.linear.x;
-  mobile_twist_velocities.second = odometry_.twist.twist.angular.z;
+  mobile_twist_velocities.first = odometry_->twist.twist.linear.x;
+  mobile_twist_velocities.second = odometry_->twist.twist.angular.z;
+
+  return true;
 }
 
 /*****************************************************************************