Add getRobotVelocity function and mbf_msgs/ExePathFeedback/actual_vel

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

@@ -49,6 +49,7 @@
 #include <tf/transform_listener.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
 
 #include <mbf_utility/navigation_utility.h>
 #include <mbf_abstract_core/abstract_controller.h>
@@ -274,6 +275,11 @@ namespace mbf_abstract_nav
      */
     bool computeRobotPose();
 
+    /**
+     * @brief Update the robot actual velocity;
+     */
+    inline void updateActualRobotVelocity();
+
     /**
      * @brief Sets the controller state. This method makes the communication of the state thread safe.
      * @param state The current controller state.
@@ -358,6 +364,9 @@ namespace mbf_abstract_nav
     //! current robot pose;
     geometry_msgs::PoseStamped robot_pose_;
 
+    //! current robot actual velocity;
+    geometry_msgs::TwistStamped robot_velocity_;
+
     //! whether check for action specific tolerance
     bool tolerance_from_action_;
 

mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h

@@ -347,6 +347,9 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     //! current_goal publisher for all controller execution objects
     ros::Publisher goal_pub_;
 
+    //! subscribe for the actual velocity feedback from odometry(wheel encoder)
+    ros::Subscriber odom_sub_;
+
     RobotInformation robot_info_;
 
     ControllerAction controller_action_;

mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h

@@ -91,6 +91,7 @@ class ControllerAction :
 
   boost::mutex goal_mtx_; ///< lock goal handle for updating it while running
   geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
+  geometry_msgs::TwistStamped robot_actual_vel_; //Actual robot velocity feedback
   geometry_msgs::PoseStamped goal_pose_;  ///< Current goal pose
 
 };

mbf_abstract_nav/include/mbf_abstract_nav/robot_information.h

@@ -69,7 +69,7 @@ class RobotInformation{
    */
   bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const;
 
-  bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const;
+  bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity) const;
 
   const std::string& getGlobalFrame() const;
 

mbf_abstract_nav/src/abstract_controller_execution.cpp

@@ -183,13 +183,18 @@ bool AbstractControllerExecution::computeRobotPose()
   return true;
 }
 
+inline void AbstractControllerExecution::updateActualRobotVelocity()
+{
+  mbf_utility::getRobotActualVelocity(robot_velocity_);
+}
+
 
 uint32_t AbstractControllerExecution::computeVelocityCmd(const geometry_msgs::PoseStamped& robot_pose,
                                                          const geometry_msgs::TwistStamped& robot_velocity,
                                                          geometry_msgs::TwistStamped& vel_cmd,
                                                          std::string& message)
 {
-  return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
+  return controller_->computeVelocityCommands(robot_pose, robot_velocity_, vel_cmd, message);
 }
 
 
@@ -343,6 +348,9 @@ void AbstractControllerExecution::run()
       // compute robot pose and store it in robot_pose_
       computeRobotPose();
 
+      // update robot actual velocity and store it in robot_velocity
+      updateActualRobotVelocity();
+
       // ask planner if the goal is reached
       if (reachedGoalCheck())
       {
@@ -367,8 +375,7 @@ void AbstractControllerExecution::run()
 
         // call plugin to compute the next velocity command
         geometry_msgs::TwistStamped cmd_vel_stamped;
-        geometry_msgs::TwistStamped robot_velocity;   // TODO pass current velocity to the plugin!
-        outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
+        outcome_ = computeVelocityCmd(robot_pose_, robot_velocity_, cmd_vel_stamped, message_ = "");
 
         if (outcome_ < 10)
         {

mbf_abstract_nav/src/abstract_navigation_server.cpp

@@ -72,6 +72,9 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
   // init cmd_vel publisher for the robot velocity
   vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 
+  // init odom_sub_ subscribe for the actual robot velocity
+  odom_sub_ = nh.subscribe<nav_msgs::Odometry>("odom", 1, &mbf_utility::actualVelCallback);
+
   action_server_get_path_ptr_ = ActionServerGetPathPtr(
     new ActionServerGetPath(
       private_nh_,

mbf_abstract_nav/src/controller_action.cpp

@@ -335,13 +335,18 @@ void ControllerAction::publishExePathFeedback(
         const geometry_msgs::TwistStamped& current_twist)
 {
   mbf_msgs::ExePathFeedback feedback;
+
   feedback.outcome = outcome;
   feedback.message = message;
 
   feedback.last_cmd_vel = current_twist;
   if (feedback.last_cmd_vel.header.stamp.isZero())
     feedback.last_cmd_vel.header.stamp = ros::Time::now();
 
+  //TODO: maybe can consider the feedback error
+  robot_info_.getRobotVelocity(robot_actual_vel_);
+  feedback.actual_vel = robot_actual_vel_;
+
   feedback.current_pose = robot_pose_;
   feedback.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
   feedback.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));

mbf_abstract_nav/src/robot_information.cpp

@@ -66,10 +66,11 @@ bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) cons
   return true;
 }
 
-bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const
+bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity) const
 {
-  // TODO implement and filter tf data to compute velocity.
-  return false;
+  //TODO: maybe can consider the feedback error
+  mbf_utility::getRobotActualVelocity(robot_velocity);
+  return true;
 }
 
 const std::string& RobotInformation::getGlobalFrame() const {return global_frame_;};

mbf_msgs/action/ExePath.action

@@ -57,3 +57,4 @@ float32 dist_to_goal
 float32 angle_to_goal
 geometry_msgs/PoseStamped  current_pose
 geometry_msgs/TwistStamped last_cmd_vel  # last command calculated by the controller
+geometry_msgs/TwistStamped actual_vel  # robot actual velocity feedback from wheel encoder

mbf_utility/include/mbf_utility/navigation_utility.h

@@ -42,6 +42,8 @@
 #define MBF_UTILITY__NAVIGATION_UTILITY_H_
 
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
 #include <ros/duration.h>
 #include <ros/time.h>
 #include <string>
@@ -107,6 +109,20 @@ bool getRobotPose(const TF &tf,
                   const std::string &global_frame,
                   const ros::Duration &timeout,
                   geometry_msgs::PoseStamped &robot_pose);
+
+/**
+ * @brief subscribe odom_topic callback
+ * @param sub_odometry subscribe data from wheel odometry
+ */           
+void actualVelCallback(const nav_msgs::Odometry::ConstPtr& sub_odometry);
+
+/**
+ * @brief update the robot actual velocity.
+ * @param tf_listener TransformListener.
+ * @return true, if succeeded, false otherwise.
+ */
+void getRobotActualVelocity(geometry_msgs::TwistStamped &robot_velocity);
+
 /**
  * @brief Computes the Euclidean-distance between two poses.
  * @param pose1 pose 1

mbf_utility/src/navigation_utility.cpp

@@ -42,6 +42,8 @@
 
 namespace mbf_utility
 {
+  geometry_msgs::TwistStamped actual_vel;
+  boost::mutex vel_mutex;
 
 bool getRobotPose(const TF &tf,
                   const std::string &robot_frame,
@@ -169,6 +171,19 @@ bool transformPoint(const TF &tf,
   return true;
 }
 
+void actualVelCallback(const nav_msgs::Odometry::ConstPtr& sub_odometry)
+{
+  boost::lock_guard<boost::mutex> lock(vel_mutex);
+  actual_vel.header = sub_odometry->header;
+  actual_vel.twist = sub_odometry->twist.twist;
+}
+
+void getRobotActualVelocity(geometry_msgs::TwistStamped &robot_velocity)
+{
+  boost::lock_guard<boost::mutex> lock(vel_mutex);
+  robot_velocity = actual_vel;
+}
+
 double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
 {
   const geometry_msgs::Point &p1 = pose1.pose.position;