Remove unused methods and attributes from AbstractNavigationServer

mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h

@@ -271,13 +271,6 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
      */
     virtual void initializeServerComponents();
 
-    /**
-     * @brief Computes the current robot pose (robot_frame_) in the global frame (global_frame_).
-     * @param robot_pose Reference to the robot_pose message object to be filled.
-     * @return true, if the current robot pose could be computed, false otherwise.
-     */
-    bool getRobotPose(geometry_msgs::PoseStamped &robot_pose);
-
   protected:
 
     /**
@@ -348,24 +341,6 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     //! shared pointer to the common TransformListener
     const TFPtr tf_listener_ptr_;
 
-    //! current robot pose; moving controller is responsible to update it by calling getRobotPose
-    geometry_msgs::PoseStamped robot_pose_;
-
-    //! current goal pose; used to compute remaining distance and angle
-    geometry_msgs::PoseStamped goal_pose_;
-
-    //! timeout after a oscillation is detected
-    ros::Duration oscillation_timeout_;
-
-    //! minimal move distance to not detect an oscillation
-    double oscillation_distance_;
-
-    //! true, if recovery behavior for the MoveBase action is enabled.
-    bool recovery_enabled_;
-
-    //! true, if clearing rotate is allowed.
-    bool clearing_rotation_allowed_;
-
     //! cmd_vel publisher for all controller execution objects
     ros::Publisher vel_pub_;
 

mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h

@@ -108,8 +108,10 @@ class MoveBaseAction
   geometry_msgs::PoseStamped last_oscillation_pose_;
   ros::Time last_oscillation_reset_;
 
+  //! timeout after a oscillation is detected
   ros::Duration oscillation_timeout_;
 
+  //! minimal move distance to not detect an oscillation
   double oscillation_distance_;
 
   GoalHandle goal_handle_;
@@ -118,8 +120,12 @@ class MoveBaseAction
 
   RobotInformation robot_info_;
 
+  //! current robot pose; updated with exe_path action feedback
   geometry_msgs::PoseStamped robot_pose_;
 
+  //! current goal pose; used to compute remaining distance and angle
+  geometry_msgs::PoseStamped goal_pose_;
+
   ros::NodeHandle private_nh_;
 
   //! Action client used by the move_base action
@@ -135,6 +141,7 @@ class MoveBaseAction
   ros::Rate replanning_rate_;
   boost::mutex replanning_mtx_;
 
+  //! true, if recovery behavior for the MoveBase action is enabled.
   bool recovery_enabled_;
 
   mbf_msgs::MoveBaseFeedback move_base_feedback_;

mbf_abstract_nav/include/mbf_abstract_nav/robot_information.h

@@ -62,6 +62,11 @@ class RobotInformation{
       const std::string &robot_frame,
       const ros::Duration &tf_timeout);
 
+  /**
+   * @brief Computes the current robot pose (robot_frame_) in the global frame (global_frame_).
+   * @param robot_pose Reference to the robot_pose message object to be filled.
+   * @return true, if the current robot pose could be computed, false otherwise.
+   */
   bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const;
 
   bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const;

mbf_abstract_nav/src/abstract_navigation_server.cpp

@@ -67,12 +67,6 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
 {
   ros::NodeHandle nh;
 
-  // oscillation timeout and distance
-  double oscillation_timeout;
-  private_nh_.param("oscillation_timeout", oscillation_timeout, 0.0);
-  oscillation_timeout_ = ros::Duration(oscillation_timeout);
-  private_nh_.param("oscillation_distance", oscillation_distance_, 0.02);
-
   goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
 
   // init cmd_vel publisher for the robot velocity

mbf_abstract_nav/src/move_base_action.cpp

@@ -158,6 +158,7 @@ void MoveBaseAction::start(GoalHandle &goal_handle)
     goal_handle.setAborted(move_base_result, move_base_result.message);
     return;
   }
+  goal_pose_ = goal.target_pose;
 
   // wait for server connections
   if (!action_client_get_path_.waitForServer(connection_timeout) ||
@@ -250,7 +251,6 @@ void MoveBaseAction::actionGetPathDone(
   action_state_ =  FAILED;
 
   const mbf_msgs::GetPathResult &result = *(result_ptr.get());
-  const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
   mbf_msgs::MoveBaseResult move_base_result;
   switch (state.state_)
   {
@@ -294,8 +294,8 @@ void MoveBaseAction::actionGetPathDone(
         // copy result from get_path action
         move_base_result.outcome = result.outcome;
         move_base_result.message = result.message;
-        move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
-        move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
+        move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+        move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
         move_base_result.final_pose = robot_pose_;
 
         ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << result.message);
@@ -309,8 +309,8 @@ void MoveBaseAction::actionGetPathDone(
       // copy result from get_path action
       move_base_result.outcome = result.outcome;
       move_base_result.message = result.message;
-      move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
-      move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
+      move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+      move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
       move_base_result.final_pose = robot_pose_;
       goal_handle_.setCanceled(move_base_result, state.getText());
       break;
@@ -364,7 +364,6 @@ void MoveBaseAction::actionExePathDone(
   ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
 
   const mbf_msgs::ExePathResult& result = *(result_ptr.get());
-  const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
   mbf_msgs::MoveBaseResult move_base_result;
 
   // copy result from get_path action
@@ -490,14 +489,13 @@ void MoveBaseAction::actionRecoveryDone(
   action_state_ =  FAILED;  // unless recovery succeeds or gets canceled...
 
   const mbf_msgs::RecoveryResult& result = *(result_ptr.get());
-  const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
   mbf_msgs::MoveBaseResult move_base_result;
 
   // copy result from get_path action
   move_base_result.outcome = result.outcome;
   move_base_result.message = result.message;
-  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
-  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
+  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
+  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
   move_base_result.final_pose = robot_pose_;
 
   switch (state.state_)

mbf_abstract_nav/src/robot_information.cpp

@@ -52,7 +52,6 @@ RobotInformation::RobotInformation(TF &tf_listener,
 }
 
 
-
 bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
 {
   bool tf_success = mbf_utility::getRobotPose(tf_listener_, robot_frame_, global_frame_,