From cb6ed045fde9ee52daadb852371c50ff254bf3ed Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 23 Jan 2020 14:29:32 +0900 Subject: [PATCH 01/30] initial commit --- .../mbf_abstract_core/abstract_controller.h | 14 + mbf_abstract_nav/CMakeLists.txt | 6 +- .../abstract_controller_execution.h | 16 +- .../abstract_navigation_server.h | 25 +- .../mbf_abstract_nav/controller_action.h | 3 +- .../mbf_abstract_nav/navigate_action.h | 160 ++++++++++ mbf_abstract_nav/package.xml | 4 + .../src/abstract_controller_execution.cpp | 31 +- .../src/abstract_navigation_server.cpp | 57 ++-- mbf_abstract_nav/src/controller_action.cpp | 69 ++--- mbf_abstract_nav/src/move_base_action.cpp | 27 +- mbf_abstract_nav/src/navigate_action.cpp | 273 ++++++++++++++++++ .../mbf_costmap_core/costmap_controller.h | 8 + mbf_msgs/CMakeLists.txt | 3 + mbf_msgs/action/ExePath.action | 35 +-- mbf_msgs/package.xml | 2 + 16 files changed, 621 insertions(+), 112 deletions(-) create mode 100644 mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h create mode 100644 mbf_abstract_nav/src/navigate_action.cpp diff --git a/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h b/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h index 2c5713f5..cddc452d 100644 --- a/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h +++ b/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h @@ -45,6 +45,7 @@ #include #include #include +#include namespace mbf_abstract_core{ @@ -106,6 +107,19 @@ namespace mbf_abstract_core{ */ virtual bool setPlan(const std::vector &plan) = 0; + /** + * @brief Set the plan that the local planner is following + * @param plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + virtual bool setPlan(const std::vector &plan) {}; + + /** + * @brief Requests the planner to give feedback for the current plan + * @return Returns the id of last checkpoint covered and the checkpoint targeting + */ + virtual std::pair getFeedback() { }; + /** * @brief Requests the planner to cancel, e.g. if it takes too much time. * @return True if a cancel has been successfully requested, false if not implemented. diff --git a/mbf_abstract_nav/CMakeLists.txt b/mbf_abstract_nav/CMakeLists.txt index d747b1e4..6952b86c 100644 --- a/mbf_abstract_nav/CMakeLists.txt +++ b/mbf_abstract_nav/CMakeLists.txt @@ -5,8 +5,10 @@ find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs + forklift_interfaces dynamic_reconfigure geometry_msgs + aifl_msg mbf_msgs mbf_abstract_core mbf_utility @@ -38,6 +40,8 @@ catkin_package( actionlib_msgs dynamic_reconfigure geometry_msgs + forklift_interfaces + aifl_msg mbf_msgs mbf_abstract_core mbf_utility @@ -62,7 +66,7 @@ add_library(${MBF_ABSTRACT_SERVER_LIB} src/controller_action.cpp src/planner_action.cpp src/recovery_action.cpp - src/move_base_action.cpp + src/navigate_action.cpp src/abstract_execution_base.cpp src/abstract_navigation_server.cpp src/abstract_planner_execution.cpp diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h index 0cda256c..6bcb854c 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -112,6 +113,17 @@ namespace mbf_abstract_nav */ void setNewPlan(const std::vector &plan); + /** + * @brief Sets a new plan to the controller execution + * @param plan A vector of stamped poses. + */ + void setNewPlan(const std::vector &plan); + + /** + * @brief Requests the planner to give feedback for the current plan + * @return Returns the id of last checkpoint covered and the checkpoint targeting + */ + virtual std::pair getFeedback(); /** * @brief Cancel the planner execution. This calls the cancel method of the planner plugin. This could be useful if the * computation takes too much time. @@ -294,13 +306,13 @@ namespace mbf_abstract_nav * @brief Gets the new available plan. This method is thread safe. * @return The plan */ - std::vector getNewPlan(); + std::vector getNewPlan(); //! the last calculated velocity command geometry_msgs::TwistStamped vel_cmd_stamped_; //! the last set plan which is currently processed by the controller - std::vector plan_; + std::vector plan_; //! the duration which corresponds with the controller frequency. boost::chrono::microseconds calling_duration_; diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h index 50e9dd6a..289fc420 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h @@ -62,7 +62,9 @@ #include "mbf_abstract_nav/planner_action.h" #include "mbf_abstract_nav/controller_action.h" #include "mbf_abstract_nav/recovery_action.h" -#include "mbf_abstract_nav/move_base_action.h" +#include "mbf_abstract_nav/navigate_action.h" + + #include "mbf_abstract_nav/MoveBaseFlexConfig.h" @@ -91,9 +93,10 @@ typedef boost::shared_ptr ActionServerExePathPtr; typedef actionlib::ActionServer ActionServerRecovery; typedef boost::shared_ptr ActionServerRecoveryPtr; -//! MoveBase action server -typedef actionlib::ActionServer ActionServerMoveBase; -typedef boost::shared_ptr ActionServerMoveBasePtr; +//! Navigate action server +typedef actionlib::ActionServer ActionServerNavigate; +typedef boost::shared_ptr ActionServerNavigatePtr; + //! ExePath action topic name const std::string name_action_exe_path = "exe_path"; @@ -101,8 +104,9 @@ const std::string name_action_exe_path = "exe_path"; const std::string name_action_get_path = "get_path"; //! Recovery action topic name const std::string name_action_recovery = "recovery"; -//! MoveBase action topic name -const std::string name_action_move_base = "move_base"; +//! Navigate action topic name +const std::string name_action_navigate = "navigate"; + typedef boost::shared_ptr > DynamicReconfigureServer; @@ -255,9 +259,9 @@ typedef boost::shared_ptr& checkpoint_feedback); /** * @brief Utility method to fill the ExePath action result in a single line diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h new file mode 100644 index 00000000..85001e1c --- /dev/null +++ b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h @@ -0,0 +1,160 @@ +/* + * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * move_base_action.h + * + * authors: + * Sebastian Pütz + * Jorge Santos Simón + * + */ +#ifndef MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_ +#define MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "mbf_abstract_nav/MoveBaseFlexConfig.h" +#include "mbf_abstract_nav/robot_information.h" + + +namespace mbf_abstract_nav +{ + +class NavigateAction +{ + public: + + //! Action clients for the Navigate action + typedef actionlib::SimpleActionClient ActionClientExePath; + typedef actionlib::SimpleActionClient ActionClientSpinTurn; + + + typedef actionlib::ActionServer::GoalHandle GoalHandle; + + NavigateAction(const std::string &name, const RobotInformation &robot_info); + + ~NavigateAction(); + + void start(GoalHandle &goal_handle); + + void cancel(); + + void reconfigure( + mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level); + + protected: + + void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback); + + void getSplitPath( + const forklift_interfaces::NavigatePath &plan, + const forklift_interfaces::NavigatePath &result); + + void actionExePathActive(); + + void actionExePathDone( + const actionlib::SimpleClientGoalState &state, + const mbf_msgs::ExePathResultConstPtr &result); + + void actionGetPathReplanningDone( + const actionlib::SimpleClientGoalState &state, + const mbf_msgs::GetPathResultConstPtr &result); + + void actionSpinTurnFeedback(const aifl_msg::SpinTurnFeedbackConstPtr &feedback); + + void actionSpinTurnDone( + const actionlib::SimpleClientGoalState &state, + const aifl_msg::SpinTurnResultConstPtr &result); + + + mbf_msgs::ExePathGoal exe_path_goal_; + mbf_msgs::GetPathGoal get_path_goal_; + aifl_msg::SpinTurnGoal spin_turn_goal_; + + + geometry_msgs::PoseStamped last_oscillation_pose_; + ros::Time last_oscillation_reset_; + + ros::Duration oscillation_timeout_; + + double oscillation_distance_; + + GoalHandle goal_handle_; + + std::string name_; + + RobotInformation robot_info_; + + geometry_msgs::PoseStamped robot_pose_; + + ros::NodeHandle private_nh_; + + //! Action client used by the navigate action + ActionClientExePath action_client_exe_path_; + + //! Action client used by the navigate action + ActionClientSpinTurn action_client_spin_turn_; + + bool replanning_; + ros::Rate replanning_rate_; + boost::mutex replanning_mtx_; + + + forklift_interfaces::NavigateFeedback navigate_feedback_; + + + enum NavigateActionState + { + NONE, + GET_PATH, + EXE_PATH, + SPIN_TURN, + OSCILLATING, + SUCCEEDED, + CANCELED, + FAILED + }; + + NavigateActionState action_state_; +}; + +} /* mbf_abstract_nav */ + +#endif //MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_ diff --git a/mbf_abstract_nav/package.xml b/mbf_abstract_nav/package.xml index 33d28df3..426e8d6a 100644 --- a/mbf_abstract_nav/package.xml +++ b/mbf_abstract_nav/package.xml @@ -17,6 +17,8 @@ actionlib actionlib_msgs dynamic_reconfigure + forklift_interfaces + aifl_msg std_msgs std_srvs nav_msgs @@ -31,6 +33,8 @@ actionlib actionlib_msgs dynamic_reconfigure + forklift_interfaces + aifl_msg std_msgs std_srvs nav_msgs diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp index 6c6b2bb9..26dd889a 100644 --- a/mbf_abstract_nav/src/abstract_controller_execution.cpp +++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp @@ -129,6 +129,23 @@ namespace mbf_abstract_nav } void AbstractControllerExecution::setNewPlan(const std::vector &plan) + { + + ROS_INFO("Setting plan"); + boost::lock_guard guard(plan_mtx_); + new_plan_ = true; + + plan_.clear(); + + for(const auto pose: plan) + { + forklift_interfaces::Checkpoint checkpoint; + checkpoint.pose = pose; + plan_.emplace_back(checkpoint); + } + } + + void AbstractControllerExecution::setNewPlan(const std::vector &plan) { if (moving_) { @@ -149,7 +166,7 @@ namespace mbf_abstract_nav } - std::vector AbstractControllerExecution::getNewPlan() + std::vector AbstractControllerExecution::getNewPlan() { boost::lock_guard guard(plan_mtx_); new_plan_ = false; @@ -174,6 +191,10 @@ namespace mbf_abstract_nav return true; } + std::pair AbstractControllerExecution::getFeedback() + { + return controller_->getFeedback(); + } uint32_t AbstractControllerExecution::computeVelocityCmd(const geometry_msgs::PoseStamped& robot_pose, const geometry_msgs::TwistStamped& robot_velocity, @@ -226,8 +247,8 @@ namespace mbf_abstract_nav { // check whether the controller plugin returns goal reached or if mbf should check for goal reached. return controller_->isGoalReached(dist_tolerance_, angle_tolerance_) || (mbf_tolerance_check_ - && mbf_utility::distance(robot_pose_, plan_.back()) < dist_tolerance_ - && mbf_utility::angle(robot_pose_, plan_.back()) < angle_tolerance_); + && mbf_utility::distance(robot_pose_, plan_.back().pose) < dist_tolerance_ + && mbf_utility::angle(robot_pose_, plan_.back().pose) < angle_tolerance_); } bool AbstractControllerExecution::cancel() @@ -249,7 +270,7 @@ namespace mbf_abstract_nav start_time_ = ros::Time::now(); // init plan - std::vector plan; + std::vector plan; if (!hasNewPlan()) { setState(NO_PLAN); @@ -306,7 +327,7 @@ namespace mbf_abstract_nav moving_ = false; return; } - current_goal_pub_.publish(plan.back()); + current_goal_pub_.publish(plan.back().pose); } // compute robot pose and store it in robot_pose_ diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index 51b69a2a..c02d4cb9 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -63,7 +63,7 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) controller_action_(name_action_exe_path, robot_info_), planner_action_(name_action_get_path, robot_info_), recovery_action_(name_action_recovery, robot_info_), - move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames()) + navigate_action_(name_action_navigate, robot_info_) { ros::NodeHandle nh; @@ -102,14 +102,14 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionRecovery, this, _1), false)); - action_server_move_base_ptr_ = ActionServerMoveBasePtr( - new ActionServerMoveBase( + action_server_navigate_ptr_ = ActionServerNavigatePtr( + new ActionServerNavigate( private_nh_, - name_action_move_base, - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionMoveBase, this, _1), - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1), + name_action_navigate, + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionNavigate, this, _1), + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionNavigate, this, _1), false)); - + // XXX note that we don't start a dynamic reconfigure server, to avoid colliding with the one possibly created by // the base class. If none, it should call startDynamicReconfigureServer method to start the one defined here for // providing just the abstract server parameters @@ -198,20 +198,20 @@ void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle else { mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN; - result.message = "No plugins loaded at all!"; - ROS_WARN_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + result.status = mbf_msgs::ExePathResult::INVALID_PLUGIN; + result.remarks = "No plugins loaded at all!"; + ROS_WARN_STREAM_NAMED("exe_path", result.remarks); + goal_handle.setRejected(result, result.remarks); return; } if(!controller_plugin_manager_.hasPlugin(controller_name)) { mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN; - result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!"; - ROS_WARN_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + result.status = mbf_msgs::ExePathResult::INVALID_PLUGIN; + result.remarks = "No plugin loaded with the given name \"" + goal.controller + "\"!"; + ROS_WARN_STREAM_NAMED("exe_path", result.remarks); + goal_handle.setRejected(result, result.remarks); return; } @@ -231,10 +231,10 @@ void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle else { mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR; - result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!"; - ROS_FATAL_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + result.status = mbf_msgs::ExePathResult::INTERNAL_ERROR; + result.remarks = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!"; + ROS_FATAL_STREAM_NAMED("exe_path", result.remarks); + goal_handle.setRejected(result, result.remarks); } } @@ -302,16 +302,16 @@ void AbstractNavigationServer::cancelActionRecovery(ActionServerRecovery::GoalHa recovery_action_.cancel(goal_handle); } -void AbstractNavigationServer::callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle) +void AbstractNavigationServer::callActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { - ROS_INFO_STREAM_NAMED("move_base", "Start action \"move_base\""); - move_base_action_.start(goal_handle); + ROS_INFO_STREAM_NAMED("navigate", "Start action \"navigate\""); + navigate_action_.start(goal_handle); } -void AbstractNavigationServer::cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle) +void AbstractNavigationServer::cancelActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { - ROS_INFO_STREAM_NAMED("move_base", "Cancel action \"move_base\""); - move_base_action_.cancel(); + ROS_INFO_STREAM_NAMED("navigate", "Cancel action \"navigate\""); + navigate_action_.cancel(); } mbf_abstract_nav::AbstractPlannerExecution::Ptr AbstractNavigationServer::newPlannerExecution( @@ -342,7 +342,7 @@ void AbstractNavigationServer::startActionServers() action_server_get_path_ptr_->start(); action_server_exe_path_ptr_->start(); action_server_recovery_ptr_->start(); - action_server_move_base_ptr_->start(); + action_server_navigate_ptr_->start(); } void AbstractNavigationServer::startDynamicReconfigureServer() @@ -373,8 +373,7 @@ void AbstractNavigationServer::reconfigure( planner_action_.reconfigureAll(config, level); controller_action_.reconfigureAll(config, level); recovery_action_.reconfigureAll(config, level); - move_base_action_.reconfigure(config, level); - + navigate_action_.reconfigure(config, level); last_config_ = config; } @@ -382,7 +381,7 @@ void AbstractNavigationServer::stop(){ planner_action_.cancelAll(); controller_action_.cancelAll(); recovery_action_.cancelAll(); - move_base_action_.cancel(); + navigate_action_.cancel(); } } /* namespace mbf_abstract_nav */ diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 8d2df808..b4b058bd 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -76,12 +76,12 @@ void ControllerAction::start( // Goal requests to run the same controller on the same concurrency slot: // we update the goal handle and pass the new plan to the execution without stopping it execution_ptr = slot_it->second.execution; - execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses); + execution_ptr->setNewPlan(goal_handle.getGoal()->path.checkpoints); // Update also goal pose, so the feedback remains consistent - goal_pose_ = goal_handle.getGoal()->path.poses.back(); + goal_pose_ = goal_handle.getGoal()->path.checkpoints.back().pose; mbf_msgs::ExePathResult result; fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result); - concurrency_slots_[slot].goal_handle.setCanceled(result, result.message); + concurrency_slots_[slot].goal_handle.setCanceled(result, result.remarks); concurrency_slots_[slot].goal_handle = goal_handle; concurrency_slots_[slot].goal_handle.setAccepted(); } @@ -125,21 +125,21 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution goal_mtx_.lock(); const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get()); - const std::vector &plan = goal.path.poses; + const std::vector &plan = goal.path.checkpoints; if (plan.empty()) { fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result); - goal_handle.setAborted(result, result.message); - ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call."); + goal_handle.setAborted(result, result.remarks); + ROS_ERROR_STREAM_NAMED(name_, result.remarks << " Canceling the action call."); controller_active = false; } - goal_pose_ = plan.back(); + goal_pose_ = plan.back().pose; ROS_DEBUG_STREAM_NAMED(name_, "Called action \"" << name_ << "\" with plan:" << std::endl << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl << "stamp: " << goal.path.header.stamp << std::endl - << "poses: " << goal.path.poses.size() << std::endl + << "poses: " << goal.path.checkpoints.size() << std::endl << "goal: (" << goal_pose_.pose.position.x << ", " << goal_pose_.pose.position.y << ", " << goal_pose_.pose.position.z << ")"); @@ -161,9 +161,9 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result); goal_mtx_.lock(); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); goal_mtx_.unlock(); - ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call."); + ROS_ERROR_STREAM_NAMED(name_, result.remarks << " Canceling the action call."); break; } @@ -191,7 +191,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution case AbstractControllerExecution::CANCELED: ROS_INFO_STREAM("Action \"exe_path\" canceled"); fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result); - goal_handle.setCanceled(result, result.message); + goal_handle.setCanceled(result, result.remarks); controller_active = false; break; @@ -218,41 +218,41 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!"); controller_active = false; fillExePathResult(execution.getOutcome(), execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::PAT_EXCEEDED: ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time"); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::NO_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!"); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::EMPTY_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan"); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::INVALID_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan"); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::NO_LOCAL_CMD: ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! " << execution.getMessage()); - publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); + publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd(), execution.getFeedback()); break; case AbstractControllerExecution::GOT_LOCAL_CMD: @@ -271,25 +271,25 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution execution.stop(); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; } } - publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); + publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd(), execution.getFeedback()); break; case AbstractControllerExecution::ARRIVED_GOAL: ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal"); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result); - goal_handle.setSucceeded(result, result.message); + goal_handle.setSucceeded(result, result.remarks); break; case AbstractControllerExecution::INTERNAL_ERROR: ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: " << execution.getMessage()); controller_active = false; fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; default: @@ -297,8 +297,8 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ss << "Internal error: Unknown state in a move base flex controller execution with the number: " << static_cast(state_moving_input); fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result); - ROS_FATAL_STREAM_NAMED(name_, result.message); - goal_handle.setAborted(result, result.message); + ROS_FATAL_STREAM_NAMED(name_, result.remarks); + goal_handle.setAborted(result, result.remarks); controller_active = false; } goal_mtx_.unlock(); @@ -327,16 +327,19 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution void ControllerAction::publishExePathFeedback( GoalHandle& goal_handle, - uint32_t outcome, const std::string &message, - const geometry_msgs::TwistStamped& current_twist) + uint32_t status, const std::string &remarks, + const geometry_msgs::TwistStamped& current_twist, + const std::pair& checkpoint_feedback) { mbf_msgs::ExePathFeedback feedback; - feedback.outcome = outcome; - feedback.message = message; + feedback.status = status; + feedback.remarks = remarks; + feedback.last_checkpoint = checkpoint_feedback.first; + feedback.target_checkpoint = checkpoint_feedback.second; - feedback.last_cmd_vel = current_twist; - if (feedback.last_cmd_vel.header.stamp.isZero()) - feedback.last_cmd_vel.header.stamp = ros::Time::now(); + feedback.velocity = current_twist; + if (feedback.velocity.header.stamp.isZero()) + feedback.velocity.header.stamp = ros::Time::now(); feedback.current_pose = robot_pose_; feedback.dist_to_goal = static_cast(mbf_utility::distance(robot_pose_, goal_pose_)); @@ -345,11 +348,11 @@ void ControllerAction::publishExePathFeedback( } void ControllerAction::fillExePathResult( - uint32_t outcome, const std::string &message, + uint32_t status, const std::string &remarks, mbf_msgs::ExePathResult &result) { - result.outcome = outcome; - result.message = message; + result.status = status; + result.remarks = remarks; result.final_pose = robot_pose_; result.dist_to_goal = static_cast(mbf_utility::distance(robot_pose_, goal_pose_)); result.angle_to_goal = static_cast(mbf_utility::angle(robot_pose_, goal_pose_)); diff --git a/mbf_abstract_nav/src/move_base_action.cpp b/mbf_abstract_nav/src/move_base_action.cpp index 26a7fab9..a639bc47 100644 --- a/mbf_abstract_nav/src/move_base_action.cpp +++ b/mbf_abstract_nav/src/move_base_action.cpp @@ -186,12 +186,12 @@ void MoveBaseAction::actionExePathActive() void MoveBaseAction::actionExePathFeedback( const mbf_msgs::ExePathFeedbackConstPtr &feedback) { - move_base_feedback_.outcome = feedback->outcome; - move_base_feedback_.message = feedback->message; + move_base_feedback_.outcome = feedback->status; + move_base_feedback_.message = feedback->remarks; move_base_feedback_.angle_to_goal = feedback->angle_to_goal; move_base_feedback_.dist_to_goal = feedback->dist_to_goal; move_base_feedback_.current_pose = feedback->current_pose; - move_base_feedback_.last_cmd_vel = feedback->last_cmd_vel; + move_base_feedback_.last_cmd_vel = feedback->velocity; robot_pose_ = feedback->current_pose; goal_handle_.publishFeedback(move_base_feedback_); @@ -259,8 +259,11 @@ void MoveBaseAction::actionGetPathDone( ROS_DEBUG_STREAM_NAMED("move_base", "Action \"" << "move_base\" received a path from \"" << "get_path\": " << state.getText()); - - exe_path_goal_.path = result.path; + exe_path_goal_.path.header = result.path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path + for(std::size_t it=0; itpath; + exe_path_goal_.path.header = result->path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path + for(std::size_t it=0; itpath.poses.size(); it++) + { + exe_path_goal_.path.checkpoints[it].pose = result->path.poses[it]; + } mbf_msgs::ExePathGoal goal(exe_path_goal_); action_client_exe_path_.sendGoal( goal, diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp new file mode 100644 index 00000000..18b80be8 --- /dev/null +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -0,0 +1,273 @@ +/* + * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * navigate_action.cpp + * + * authors: + * Sebastian Pütz + * Jorge Santos Simón + * + */ + +#include + +#include "mbf_abstract_nav/MoveBaseFlexConfig.h" +#include "mbf_abstract_nav/navigate_action.h" + +namespace mbf_abstract_nav +{ + +NavigateAction::NavigateAction(const std::string &name, + const RobotInformation &robot_info) + : name_(name), robot_info_(robot_info), private_nh_("~"), + action_client_exe_path_(private_nh_, "exe_path"), + action_client_spin_turn_(private_nh_, "spin_turn"), + oscillation_timeout_(0), + oscillation_distance_(0), + action_state_(NONE), + replanning_(false), + replanning_rate_(1.0) +{ +} + +NavigateAction::~NavigateAction() +{ +} + +void NavigateAction::reconfigure( + mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level) +{ + oscillation_timeout_ = ros::Duration(config.oscillation_timeout); + oscillation_distance_ = config.oscillation_distance; +} + +void NavigateAction::cancel() +{ + action_state_ = CANCELED; + + if(!action_client_exe_path_.getState().isDone()) + { + action_client_exe_path_.cancelGoal(); + } + + if(!action_client_spin_turn_.getState().isDone()) + { + action_client_spin_turn_.cancelGoal(); + } +} + +void NavigateAction::start(GoalHandle &goal_handle) +{ + action_state_ = GET_PATH; + + goal_handle.setAccepted(); + + goal_handle_ = goal_handle; + + ROS_DEBUG_STREAM_NAMED("navigate", "Start action " << "navigate"); + + const forklift_interfaces::NavigateGoal& goal = *(goal_handle.getGoal().get()); + const forklift_interfaces::NavigatePath &plan = goal.path; + + forklift_interfaces::NavigateResult navigate_result; + + exe_path_goal_.controller = goal.controller; + + ros::Duration connection_timeout(1.0); + + last_oscillation_reset_ = ros::Time::now(); + + + geometry_msgs::PoseStamped robot_pose; + // get the current robot pose only at the beginning, as exe_path will keep updating it as we move + if (!robot_info_.getRobotPose(robot_pose)) + { + ROS_ERROR_STREAM_NAMED("navigate", "Could not get the current robot pose!"); + navigate_result.remarks = "Could not get the current robot pose!"; + navigate_result.status = forklift_interfaces::NavigateResult::TF_ERROR; + goal_handle.setAborted(navigate_result, navigate_result.remarks); + return; + } + + // wait for server connections + if (!action_client_exe_path_.waitForServer(connection_timeout) || + !action_client_spin_turn_.waitForServer(connection_timeout)) + { + ROS_ERROR_STREAM_NAMED("navigate", "Could not connect to one or more of navigate actions:" + "\"get_path\" , \"exe_path\", \"spin_turn \"!"); + navigate_result.status = forklift_interfaces::NavigateResult::INTERNAL_ERROR; + navigate_result.remarks = "Could not connect to the navigate actions!"; + goal_handle.setAborted(navigate_result, navigate_result.remarks); + return; + } + + // call get_path action server to get a first plan + +} + +void NavigateAction::actionExePathActive() +{ + ROS_DEBUG_STREAM_NAMED("navigate", "The \"exe_path\" action is active."); +} + +void NavigateAction::actionExePathFeedback( + const mbf_msgs::ExePathFeedbackConstPtr &feedback) +{ + navigate_feedback_.status = feedback->status; + navigate_feedback_.remarks = feedback->remarks; + navigate_feedback_.angle_to_goal = feedback->angle_to_goal; + navigate_feedback_.dist_to_goal = feedback->dist_to_goal; + navigate_feedback_.current_pose = feedback->current_pose; + navigate_feedback_.velocity = feedback->velocity; + robot_pose_ = feedback->current_pose; + goal_handle_.publishFeedback(navigate_feedback_); + + // we create a navigation-level oscillation detection using exe_path action's feedback, + // as the later doesn't handle oscillations created by quickly failing repeated plans + + // if oscillation detection is enabled by osciallation_timeout != 0 + if (!oscillation_timeout_.isZero()) + { + // check if oscillating + // moved more than the minimum oscillation distance + if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_) + { + last_oscillation_reset_ = ros::Time::now(); + last_oscillation_pose_ = robot_pose_; + } + else if (last_oscillation_reset_ + oscillation_timeout_ < ros::Time::now()) + { + std::stringstream oscillation_msgs; + oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!"; + ROS_WARN_STREAM_NAMED("exe_path", oscillation_msgs.str()); + action_client_exe_path_.cancelGoal(); + + + forklift_interfaces::NavigateResult navigate_result; + navigate_result.status = forklift_interfaces::NavigateResult::OSCILLATION; + navigate_result.remarks = oscillation_msgs.str(); + navigate_result.final_pose = robot_pose_; + navigate_result.angle_to_goal = navigate_feedback_.angle_to_goal; + navigate_result.dist_to_goal = navigate_feedback_.dist_to_goal; + goal_handle_.setAborted(navigate_result, navigate_result.remarks); + + } + } +} + + +void NavigateAction::actionExePathDone( + const actionlib::SimpleClientGoalState &state, + const mbf_msgs::ExePathResultConstPtr &result_ptr) +{ + action_state_ = FAILED; + + ROS_DEBUG_STREAM_NAMED("navigate", "Action \"exe_path\" finished."); + + const mbf_msgs::ExePathResult& result = *(result_ptr.get()); + const forklift_interfaces::NavigateGoal& goal = *(goal_handle_.getGoal().get()); + forklift_interfaces::NavigateResult navigate_result; + + navigate_result.status = result.status; + navigate_result.remarks = result.remarks; + navigate_result.dist_to_goal = result.dist_to_goal; + navigate_result.angle_to_goal = result.angle_to_goal; + navigate_result.final_pose = result.final_pose; + + ROS_DEBUG_STREAM_NAMED("exe_path", "Current state:" << state.toString()); + + switch (state.state_) + { + case actionlib::SimpleClientGoalState::SUCCEEDED: + navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; + navigate_result.remarks = "Action \"navigate\" succeeded!"; + ROS_INFO_STREAM_NAMED("navigate", navigate_result.remarks); + goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); + action_state_ = SUCCEEDED; + break; + + case actionlib::SimpleClientGoalState::ABORTED: + switch (result.status) + { + case mbf_msgs::ExePathResult::INVALID_PATH: + case mbf_msgs::ExePathResult::TF_ERROR: + case mbf_msgs::ExePathResult::NOT_INITIALIZED: + case mbf_msgs::ExePathResult::INVALID_PLUGIN: + case mbf_msgs::ExePathResult::INTERNAL_ERROR: + // none of these errors is recoverable + goal_handle_.setAborted(navigate_result, state.getText()); + break; + + default: + // all the rest are, so we start calling the recovery behaviors in sequence + + ROS_WARN_STREAM_NAMED("navigate", "Abort the execution of the controller: " << result.remarks); + goal_handle_.setAborted(navigate_result, state.getText()); + + } + break; + + case actionlib::SimpleClientGoalState::PREEMPTED: + // action was preempted successfully! + ROS_DEBUG_STREAM_NAMED("navigate", "The action \"" + << "exe_path" << "\" was preempted successfully!"); + // TODO + break; + + case actionlib::SimpleClientGoalState::RECALLED: + ROS_DEBUG_STREAM_NAMED("navigate", "The action \"" + << "exe_path" << "\" was recalled!"); + // TODO + break; + + case actionlib::SimpleClientGoalState::REJECTED: + ROS_DEBUG_STREAM_NAMED("navigate", "The action \"" + << "exe_path" << "\" was rejected!"); + // TODO + break; + + case actionlib::SimpleClientGoalState::LOST: + // TODO + break; + + default: + ROS_FATAL_STREAM_NAMED("navigate", + "Reached unreachable case! Unknown SimpleActionServer state!"); + goal_handle_.setAborted(); + break; + } +} + + + +} /* namespace mbf_abstract_nav */ + diff --git a/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h b/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h index 84a48c6e..644c22f0 100644 --- a/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h +++ b/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace mbf_costmap_core { /** @@ -103,6 +104,13 @@ namespace mbf_costmap_core { */ virtual bool setPlan(const std::vector &plan) = 0; + /** + * @brief Set the plan that the local planner is following + * @param plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + virtual bool setPlan(const std::vector &plan) = 0; + /** * @brief Requests the planner to cancel, e.g. if it takes too much time * @remark New on MBF API diff --git a/mbf_msgs/CMakeLists.txt b/mbf_msgs/CMakeLists.txt index dd7659ea..959a92b0 100644 --- a/mbf_msgs/CMakeLists.txt +++ b/mbf_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED actionlib_msgs genmsg geometry_msgs + forklift_interfaces message_generation message_runtime nav_msgs @@ -35,6 +36,7 @@ generate_messages( DEPENDENCIES actionlib_msgs geometry_msgs + forklift_interfaces nav_msgs std_msgs ) @@ -43,6 +45,7 @@ catkin_package( CATKIN_DEPENDS actionlib_msgs geometry_msgs + forklift_interfaces message_runtime nav_msgs std_msgs diff --git a/mbf_msgs/action/ExePath.action b/mbf_msgs/action/ExePath.action index 3a225f23..1e98068d 100644 --- a/mbf_msgs/action/ExePath.action +++ b/mbf_msgs/action/ExePath.action @@ -1,15 +1,7 @@ -# Follow the given path until completion or failure - -nav_msgs/Path path - -# Controller to use; defaults to the first one specified on "controllers" parameter +forklift_interfaces/NavigatePath path string controller - -# use different slots for concurrency uint8 concurrency_slot - --- - # Predefined success codes: uint8 SUCCESS = 0 # 1..9 are reserved as plugin specific non-error results @@ -30,22 +22,23 @@ uint8 TF_ERROR = 111 uint8 NOT_INITIALIZED = 112 uint8 INVALID_PLUGIN = 113 uint8 INTERNAL_ERROR = 114 -# 121..149 are reserved as plugin specific errors -uint32 outcome -string message - -geometry_msgs/PoseStamped final_pose +uint8 status # Final navigation status +geometry_msgs/PoseStamped final_pose # Final pose of forklift +string remarks # Remarks for result float32 dist_to_goal float32 angle_to_goal - --- - -# Outcome of most recent controller cycle. Same values as in result -uint32 outcome -string message +uint8 IN_PROGRESS # Following path +uint8 PAUSED # Explicitly requested by LBC 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 + +uint8 status +uint32 target_checkpoint # Checkpoint it is trying to reach in the path +uint32 last_checkpoint # Checkpoint it has covered in the path +geometry_msgs/PoseStamped current_pose # Current pose of forklift +geometry_msgs/TwistStamped velocity # Current velocity +string remarks # Remarks for progress + diff --git a/mbf_msgs/package.xml b/mbf_msgs/package.xml index f4417271..9afe4168 100644 --- a/mbf_msgs/package.xml +++ b/mbf_msgs/package.xml @@ -15,6 +15,7 @@ std_msgs nav_msgs geometry_msgs + forklift_interfaces actionlib_msgs genmsg message_runtime @@ -23,6 +24,7 @@ std_msgs nav_msgs geometry_msgs + forklift_interfaces actionlib_msgs message_runtime From ce91b087e6bc6ce7ea1232f45ce3e122e18dd9c8 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 23 Jan 2020 18:26:33 +0900 Subject: [PATCH 02/30] Added structure for the state machine and a path splitter function --- .../abstract_execution_base.h | 7 ++ .../mbf_abstract_nav/navigate_action.h | 14 ++- .../src/abstract_navigation_server.cpp | 1 + mbf_abstract_nav/src/navigate_action.cpp | 108 +++++++++++++++++- mbf_costmap_nav/CMakeLists.txt | 4 + .../nav_core_wrapper/wrapper_local_planner.h | 15 ++- mbf_costmap_nav/package.xml | 4 + .../wrapper_local_planner.cpp | 32 +++++- 8 files changed, 177 insertions(+), 8 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h index c97ce1fe..94100790 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h @@ -43,6 +43,7 @@ #include #include #include +#include namespace mbf_abstract_nav { @@ -92,6 +93,12 @@ class AbstractExecutionBase */ virtual void postRun() { }; + /** + * @brief Requests the planner to give feedback for the current plan + * @return Returns the id of last checkpoint covered and the checkpoint targeting + */ + virtual std::pair getFeedback() { }; + protected: virtual void run() = 0; diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h index 85001e1c..85231398 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h @@ -83,12 +83,14 @@ class NavigateAction void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback); - void getSplitPath( + bool getSplitPath( const forklift_interfaces::NavigatePath &plan, - const forklift_interfaces::NavigatePath &result); + std::vector &result); void actionExePathActive(); + void startNavigate(); + void actionExePathDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result); @@ -102,10 +104,11 @@ class NavigateAction void actionSpinTurnDone( const actionlib::SimpleClientGoalState &state, const aifl_msg::SpinTurnResultConstPtr &result); + mbf_msgs::ExePathGoal exe_path_goal_; - mbf_msgs::GetPathGoal get_path_goal_; + forklift_interfaces::NavigatePath get_path_goal_; aifl_msg::SpinTurnGoal spin_turn_goal_; @@ -114,6 +117,8 @@ class NavigateAction ros::Duration oscillation_timeout_; + std::vector path_segments_; + double oscillation_distance_; GoalHandle goal_handle_; @@ -143,7 +148,8 @@ class NavigateAction enum NavigateActionState { NONE, - GET_PATH, + SPLIT_PATH, + NAVIGATE, EXE_PATH, SPIN_TURN, OSCILLATING, diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index c02d4cb9..178847a6 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -65,6 +65,7 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) recovery_action_(name_action_recovery, robot_info_), navigate_action_(name_action_navigate, robot_info_) { + ROS_INFO("Abstract navigation server"); ros::NodeHandle nh; // oscillation timeout and distance diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 18b80be8..31c514f8 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -50,7 +50,7 @@ NavigateAction::NavigateAction(const std::string &name, const RobotInformation &robot_info) : name_(name), robot_info_(robot_info), private_nh_("~"), action_client_exe_path_(private_nh_, "exe_path"), - action_client_spin_turn_(private_nh_, "spin_turn"), + action_client_spin_turn_("spin_turn"), oscillation_timeout_(0), oscillation_distance_(0), action_state_(NONE), @@ -83,11 +83,12 @@ void NavigateAction::cancel() { action_client_spin_turn_.cancelGoal(); } + } void NavigateAction::start(GoalHandle &goal_handle) { - action_state_ = GET_PATH; + action_state_ = SPLIT_PATH; goal_handle.setAccepted(); @@ -106,6 +107,8 @@ void NavigateAction::start(GoalHandle &goal_handle) last_oscillation_reset_ = ros::Time::now(); + path_segments_.clear(); + geometry_msgs::PoseStamped robot_pose; // get the current robot pose only at the beginning, as exe_path will keep updating it as we move @@ -131,9 +134,51 @@ void NavigateAction::start(GoalHandle &goal_handle) } // call get_path action server to get a first plan + + bool split_result; + split_result = getSplitPath(plan, path_segments_); + + if(!split_result) + { + ROS_ERROR_STREAM_NAMED("navigate", "Path provided was empty!"); + navigate_result.remarks = "Empty path provided!"; + navigate_result.status = forklift_interfaces::NavigateResult::INVALID_PATH; + goal_handle.setAborted(navigate_result, navigate_result.remarks); + return; + } + + startNavigate(); + + } +void NavigateAction::startNavigate() +{ + action_state_ = NAVIGATE; + while (action_state_ != SUCCEEDED || action_state_ != FAILED) + { + switch (action_state_) + { + case NAVIGATE: + /* code */ + break; + case EXE_PATH: + /* code */ + break; + case SPIN_TURN: + /* code */ + break; + case OSCILLATING: + /* code */ + break; + + default: + break; + } + } +} + void NavigateAction::actionExePathActive() { ROS_DEBUG_STREAM_NAMED("navigate", "The \"exe_path\" action is active."); @@ -148,6 +193,8 @@ void NavigateAction::actionExePathFeedback( navigate_feedback_.dist_to_goal = feedback->dist_to_goal; navigate_feedback_.current_pose = feedback->current_pose; navigate_feedback_.velocity = feedback->velocity; + navigate_feedback_.last_checkpoint = feedback->last_checkpoint; + navigate_feedback_.target_checkpoint = feedback->target_checkpoint; robot_pose_ = feedback->current_pose; goal_handle_.publishFeedback(navigate_feedback_); @@ -184,6 +231,63 @@ void NavigateAction::actionExePathFeedback( } } + bool NavigateAction::getSplitPath( + const forklift_interfaces::NavigatePath &plan, + std::vector &result) + { + ROS_INFO_STREAM_NAMED("navigate","Splitting the path!"); + if(plan.checkpoints.size()<1) + { + return false; + } + forklift_interfaces::NavigatePath segment; + + for (size_t i = 0 ; i < plan.checkpoints.size(); i++) + { + + segment.header = plan.header; + segment.xy_goal_tolerance = plan.xy_goal_tolerance; + segment.yaw_goal_tolerance = plan.xy_goal_tolerance; + + if (i<1) + { + segment.checkpoints.push_back(plan.checkpoints[i]); + } + else if (i(point.info.spin_turn)<< "]"); + } + } + + return true; + } void NavigateAction::actionExePathDone( const actionlib::SimpleClientGoalState &state, diff --git a/mbf_costmap_nav/CMakeLists.txt b/mbf_costmap_nav/CMakeLists.txt index 740d9142..c9895ac3 100644 --- a/mbf_costmap_nav/CMakeLists.txt +++ b/mbf_costmap_nav/CMakeLists.txt @@ -6,10 +6,12 @@ find_package(catkin REQUIRED base_local_planner dynamic_reconfigure geometry_msgs + forklift_interfaces mbf_abstract_nav mbf_costmap_core mbf_msgs mbf_utility + rr_path_follower nav_core nav_msgs roscpp @@ -38,10 +40,12 @@ catkin_package( base_local_planner dynamic_reconfigure geometry_msgs + forklift_interfaces mbf_abstract_nav mbf_costmap_core mbf_msgs mbf_utility + rr_path_follower nav_core nav_msgs pluginlib diff --git a/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h b/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h index 09ed123c..c001ba39 100644 --- a/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h +++ b/mbf_costmap_nav/include/nav_core_wrapper/wrapper_local_planner.h @@ -43,8 +43,8 @@ #include #include "mbf_costmap_core/costmap_controller.h" - #include +#include namespace mbf_nav_core_wrapper { /** @@ -93,6 +93,19 @@ namespace mbf_nav_core_wrapper { */ virtual bool setPlan(const std::vector &plan); + /** + * @brief Set the plan that the local planner is following + * @param plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + virtual bool setPlan(const std::vector &plan); + + /** + * @brief Requests the planner to give feedback for the current plan + * @return Returns the id of last checkpoint covered and the checkpoint targeting + */ + virtual std::pair getFeedback(); + /** * @brief Requests the planner to cancel, e.g. if it takes too much time * @remark New on MBF API diff --git a/mbf_costmap_nav/package.xml b/mbf_costmap_nav/package.xml index 1fd42b2e..d0038549 100644 --- a/mbf_costmap_nav/package.xml +++ b/mbf_costmap_nav/package.xml @@ -21,6 +21,8 @@ actionlib_msgs base_local_planner dynamic_reconfigure + forklift_interfaces + rr_path_follower std_msgs std_srvs mbf_abstract_nav @@ -38,6 +40,8 @@ actionlib_msgs base_local_planner dynamic_reconfigure + forklift_interfaces + rr_path_follower std_msgs std_srvs mbf_abstract_nav diff --git a/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp b/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp index cfb6e153..f924edb9 100644 --- a/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp +++ b/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp @@ -39,7 +39,7 @@ */ #include "nav_core_wrapper/wrapper_local_planner.h" - +#include namespace mbf_nav_core_wrapper { @@ -69,8 +69,38 @@ bool WrapperLocalPlanner::setPlan(const std::vector return nav_core_plugin_->setPlan(plan); } +bool WrapperLocalPlanner::setPlan(const std::vector &plan) +{ + rr_path_follower::PathFollowerROS* path_follower = dynamic_cast(nav_core_plugin_.get()); //TODO: use dynamic_pointer_cast + if (path_follower) + { + return path_follower->setPlan(plan); + } + std::vector path; + for (const auto checkpoint: plan) + { + path.push_back(checkpoint.pose); + } + return nav_core_plugin_->setPlan(path); +} + +std::pair WrapperLocalPlanner::getFeedback() +{ + rr_path_follower::PathFollowerROS* path_follower = dynamic_cast(nav_core_plugin_.get()); //TODO: use dynamic_pointer_cast + if (path_follower) + { + return path_follower->getFeedback(); + } + return std::make_pair(0, 0); +} + bool WrapperLocalPlanner::cancel() { + rr_path_follower::PathFollowerROS* path_follower = dynamic_cast(nav_core_plugin_.get()); //TODO: use dynamic_pointer_cast + if (path_follower) + { + return path_follower->cancel(); + } return false; } From 4c2415b23b6a0a37daf49476ee3e7b0f5fdaeb20 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 6 Feb 2020 21:51:25 +0900 Subject: [PATCH 03/30] Added a features to call spin turn and exe_path from navigation --- .../mbf_abstract_nav/navigate_action.h | 9 +- mbf_abstract_nav/src/navigate_action.cpp | 202 ++++++++++++++---- 2 files changed, 167 insertions(+), 44 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h index 85231398..9fcdef3b 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h @@ -89,8 +89,12 @@ class NavigateAction void actionExePathActive(); + void actionSpinTurnActive(); + void startNavigate(); + void runNavigate(); + void actionExePathDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result); @@ -105,6 +109,8 @@ class NavigateAction const actionlib::SimpleClientGoalState &state, const aifl_msg::SpinTurnResultConstPtr &result); + double getSpinAngle(geometry_msgs::Quaternion orientation); + mbf_msgs::ExePathGoal exe_path_goal_; @@ -147,7 +153,8 @@ class NavigateAction enum NavigateActionState { - NONE, + ACTIVE, + IDLE, SPLIT_PATH, NAVIGATE, EXE_PATH, diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 31c514f8..16916c02 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -53,7 +53,7 @@ NavigateAction::NavigateAction(const std::string &name, action_client_spin_turn_("spin_turn"), oscillation_timeout_(0), oscillation_distance_(0), - action_state_(NONE), + action_state_(IDLE), replanning_(false), replanning_rate_(1.0) { @@ -161,13 +161,24 @@ void NavigateAction::startNavigate() switch (action_state_) { case NAVIGATE: + runNavigate(); /* code */ break; case EXE_PATH: - /* code */ + action_client_exe_path_.sendGoal( + exe_path_goal_, + boost::bind(&NavigateAction::actionExePathDone, this, _1, _2), + boost::bind(&NavigateAction::actionExePathActive, this), + boost::bind(&NavigateAction::actionExePathFeedback, this, _1)); + action_state_ = ACTIVE; break; case SPIN_TURN: /* code */ + + action_client_spin_turn_.sendGoal(spin_turn_goal_, + boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), + boost::bind(&NavigateAction::actionSpinTurnActive, this)); + action_state_ = ACTIVE; break; case OSCILLATING: /* code */ @@ -176,12 +187,65 @@ void NavigateAction::startNavigate() default: break; } + ros::spinOnce(); } } +void NavigateAction::runNavigate() +{ + action_state_ = FAILED; + ROS_INFO_STREAM_NAMED("navigate", "Segments remaning: " << path_segments_.size()); + if(!path_segments_.empty()) + { + ROS_INFO_STREAM_NAMED("navigate","Spin turn: "<< static_cast(path_segments_.front().checkpoints.front().spin_turn)); + if(path_segments_.front().checkpoints.front().spin_turn) + { + const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; + double yaw = getSpinAngle(orientation); + + spin_turn_goal_.angle = yaw; + action_state_ = SPIN_TURN; + path_segments_.front().checkpoints.front().spin_turn = false; + return; + } + else + { + for (const auto& point : path_segments_.front().checkpoints) + { + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "," << static_cast(point.spin_turn)<< "]"); + } + exe_path_goal_.path = path_segments_.front(); + + action_state_ = EXE_PATH; + return; + } + } + else + { + forklift_interfaces::NavigateResult navigate_result; + navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; + navigate_result.remarks = "Navigate action completed successfully!"; + navigate_result.final_pose = robot_pose_; + ROS_INFO_STREAM_NAMED("navigate", "Navigate action completed successfully"); + goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); + + action_state_ = SUCCEEDED; + return; + } + +} + +void NavigateAction::actionSpinTurnActive() +{ + ROS_INFO_STREAM_NAMED("navigate", "The \"spin_turn\" action is active."); + action_state_ = ACTIVE; +} + + void NavigateAction::actionExePathActive() { ROS_DEBUG_STREAM_NAMED("navigate", "The \"exe_path\" action is active."); + action_state_= ACTIVE; } void NavigateAction::actionExePathFeedback( @@ -231,64 +295,95 @@ void NavigateAction::actionExePathFeedback( } } - bool NavigateAction::getSplitPath( +bool NavigateAction::getSplitPath( const forklift_interfaces::NavigatePath &plan, std::vector &result) +{ + ROS_INFO_STREAM_NAMED("navigate","Splitting the path!"); + if(plan.checkpoints.size()<1) { - ROS_INFO_STREAM_NAMED("navigate","Splitting the path!"); - if(plan.checkpoints.size()<1) + return false; + } + forklift_interfaces::NavigatePath segment; + + for (size_t i = 0 ; i < plan.checkpoints.size(); i++) + { + + segment.header = plan.header; + segment.xy_goal_tolerance = plan.xy_goal_tolerance; + segment.yaw_goal_tolerance = plan.xy_goal_tolerance; + + if (i<1) { - return false; + segment.checkpoints.push_back(plan.checkpoints[i]); } - forklift_interfaces::NavigatePath segment; - - for (size_t i = 0 ; i < plan.checkpoints.size(); i++) + else if (i(point.info.spin_turn)<< "]"); - } + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "," << static_cast(point.spin_turn)<< "]"); } + } + + return true; +} + +void NavigateAction::actionSpinTurnDone( + const actionlib::SimpleClientGoalState &state, + const aifl_msg::SpinTurnResultConstPtr &result_ptr) +{ + action_state_ = FAILED; + + ROS_INFO_STREAM_NAMED("navigate", "Action \"spin_turn\" finished."); + + const aifl_msg::SpinTurnResult& result = *(result_ptr.get()); - return true; + if(!(state == actionlib::SimpleClientGoalState::SUCCEEDED)) + { + ROS_INFO_STREAM_NAMED("navigate", "Action \"spin_turn\" did not succeed, retrying..."); + action_state_ = SPIN_TURN; + } + else + { + if(result.status != 2) + { + ROS_ERROR_STREAM_NAMED("navigate", "Action \"spin_turn\" failed :" << result); + } + else + { + ROS_INFO_STREAM_NAMED("navigate", "Action \"spin_turn\" completed successfully"); + action_state_ = NAVIGATE; + } + } +} + void NavigateAction::actionExePathDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result_ptr) @@ -312,11 +407,20 @@ void NavigateAction::actionExePathDone( switch (state.state_) { case actionlib::SimpleClientGoalState::SUCCEEDED: - navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; - navigate_result.remarks = "Action \"navigate\" succeeded!"; - ROS_INFO_STREAM_NAMED("navigate", navigate_result.remarks); - goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); - action_state_ = SUCCEEDED; + if(path_segments_.front().checkpoints.back().spin_turn) + { + const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; + double yaw = getSpinAngle(orientation); + spin_turn_goal_.angle = yaw; + action_state_ = SPIN_TURN; + path_segments_.erase(path_segments_.begin()); + } + else + { + path_segments_.erase(path_segments_.begin()); + action_state_ = NAVIGATE; + } + break; case actionlib::SimpleClientGoalState::ABORTED: @@ -371,6 +475,18 @@ void NavigateAction::actionExePathDone( } } +double NavigateAction::getSpinAngle(geometry_msgs::Quaternion orientation) +{ + tf2::Quaternion qp(orientation.x, orientation.y, orientation.z, orientation.w); + tf2::Matrix3x3 mp(qp); + double roll, pitch, yaw; + mp.getRPY(roll, pitch, yaw); + + yaw = yaw * 180 / M_PI; + + return yaw; +} + } /* namespace mbf_abstract_nav */ From 0d176a60f46dd30376a8bda6a07e6c3f7db9c5bc Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Fri, 7 Feb 2020 17:29:31 +0900 Subject: [PATCH 04/30] Added check for calling spin only if the robot is not already in the same orientation --- mbf_abstract_nav/src/navigate_action.cpp | 39 +++++++++++++++--------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 16916c02..68384101 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -74,6 +74,9 @@ void NavigateAction::cancel() { action_state_ = CANCELED; + ROS_INFO_STREAM_NAMED ("navigate", "Cancel called for navigate"); + goal_handle_.setCanceled(); + if(!action_client_exe_path_.getState().isDone()) { action_client_exe_path_.cancelGoal(); @@ -83,6 +86,7 @@ void NavigateAction::cancel() { action_client_spin_turn_.cancelGoal(); } + } @@ -125,8 +129,7 @@ void NavigateAction::start(GoalHandle &goal_handle) if (!action_client_exe_path_.waitForServer(connection_timeout) || !action_client_spin_turn_.waitForServer(connection_timeout)) { - ROS_ERROR_STREAM_NAMED("navigate", "Could not connect to one or more of navigate actions:" - "\"get_path\" , \"exe_path\", \"spin_turn \"!"); + ROS_ERROR_STREAM_NAMED("navigate", "Could not connect to one or more of navigate actions: exe_path, spin_turn!"); navigate_result.status = forklift_interfaces::NavigateResult::INTERNAL_ERROR; navigate_result.remarks = "Could not connect to the navigate actions!"; goal_handle.setAborted(navigate_result, navigate_result.remarks); @@ -162,7 +165,6 @@ void NavigateAction::startNavigate() { case NAVIGATE: runNavigate(); - /* code */ break; case EXE_PATH: action_client_exe_path_.sendGoal( @@ -173,11 +175,10 @@ void NavigateAction::startNavigate() action_state_ = ACTIVE; break; case SPIN_TURN: - /* code */ - - action_client_spin_turn_.sendGoal(spin_turn_goal_, - boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), - boost::bind(&NavigateAction::actionSpinTurnActive, this)); + action_client_spin_turn_.sendGoal( + spin_turn_goal_, + boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), + boost::bind(&NavigateAction::actionSpinTurnActive, this)); action_state_ = ACTIVE; break; case OSCILLATING: @@ -201,9 +202,19 @@ void NavigateAction::runNavigate() if(path_segments_.front().checkpoints.front().spin_turn) { const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; - double yaw = getSpinAngle(orientation); + geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + double min_angle = mbf_utility::angle(robot_pose , path_segments_.front().checkpoints.front().pose); + double yaw_goal = getSpinAngle(orientation); + ROS_INFO_STREAM("min_angle: " << min_angle); + if (fabs(min_angle)<10.0) + { + path_segments_.front().checkpoints.front().spin_turn = false; + action_state_ = NAVIGATE; + return; + } - spin_turn_goal_.angle = yaw; + spin_turn_goal_.angle = yaw_goal; action_state_ = SPIN_TURN; path_segments_.front().checkpoints.front().spin_turn = false; return; @@ -212,7 +223,7 @@ void NavigateAction::runNavigate() { for (const auto& point : path_segments_.front().checkpoints) { - ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "," << static_cast(point.spin_turn)<< "]"); + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]"); } exe_path_goal_.path = path_segments_.front(); @@ -299,7 +310,7 @@ bool NavigateAction::getSplitPath( const forklift_interfaces::NavigatePath &plan, std::vector &result) { - ROS_INFO_STREAM_NAMED("navigate","Splitting the path!"); + ROS_INFO_STREAM_NAMED("navigate","Splitting the path"); if(plan.checkpoints.size()<1) { return false; @@ -343,10 +354,10 @@ bool NavigateAction::getSplitPath( for(const auto& segment : result) { - ROS_INFO_STREAM_NAMED("navigate","Segment:"); + ROS_INFO_STREAM_NAMED("navigate","Split segments:"); for (const auto& point : segment.checkpoints) { - ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "," << static_cast(point.spin_turn)<< "]"); + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.spin_turn)); } } From 0d5349b9296949575f30cec844be95ef51effdec Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Mon, 17 Feb 2020 17:22:53 +0900 Subject: [PATCH 05/30] Added support for single checkpoint and fixed spin action behaviour for preempted calls --- mbf_abstract_nav/src/navigate_action.cpp | 60 +++++++++++++++++++++--- 1 file changed, 53 insertions(+), 7 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 68384101..4e9d80e5 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -175,6 +175,7 @@ void NavigateAction::startNavigate() action_state_ = ACTIVE; break; case SPIN_TURN: + action_client_spin_turn_.sendGoal( spin_turn_goal_, boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), @@ -205,7 +206,10 @@ void NavigateAction::runNavigate() geometry_msgs::PoseStamped robot_pose; robot_info_.getRobotPose(robot_pose); double min_angle = mbf_utility::angle(robot_pose , path_segments_.front().checkpoints.front().pose); + min_angle = min_angle * 180 / M_PI ; double yaw_goal = getSpinAngle(orientation); + double curr_yaw = getSpinAngle(robot_pose.pose.orientation); + ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", " << curr_yaw); ROS_INFO_STREAM("min_angle: " << min_angle); if (fabs(min_angle)<10.0) { @@ -235,7 +239,7 @@ void NavigateAction::runNavigate() { forklift_interfaces::NavigateResult navigate_result; navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; - navigate_result.remarks = "Navigate action completed successfully!"; + navigate_result.remarks = "Action navigate completed successfully!"; navigate_result.final_pose = robot_pose_; ROS_INFO_STREAM_NAMED("navigate", "Navigate action completed successfully"); goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); @@ -317,6 +321,30 @@ bool NavigateAction::getSplitPath( } forklift_interfaces::NavigatePath segment; + if (plan.checkpoints.size()==1) + { + segment.header = plan.header; + segment.xy_goal_tolerance = plan.xy_goal_tolerance; + segment.yaw_goal_tolerance = plan.xy_goal_tolerance; + geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + forklift_interfaces::Checkpoint start; + start.pose = robot_pose; + start.spin_turn = false; + segment.checkpoints.push_back(start); + segment.checkpoints.push_back(plan.checkpoints[0]); + result.push_back(segment); + segment.checkpoints.clear(); + ROS_INFO_STREAM_NAMED("navigate","Single checkpoint"); + for (const auto& point : segment.checkpoints) + { + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.spin_turn)); + } + + + return true; + } + for (size_t i = 0 ; i < plan.checkpoints.size(); i++) { @@ -420,18 +448,36 @@ void NavigateAction::actionExePathDone( case actionlib::SimpleClientGoalState::SUCCEEDED: if(path_segments_.front().checkpoints.back().spin_turn) { - const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; - double yaw = getSpinAngle(orientation); - spin_turn_goal_.angle = yaw; - action_state_ = SPIN_TURN; - path_segments_.erase(path_segments_.begin()); + const auto orientation = path_segments_.front().checkpoints.back().pose.pose.orientation; + geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + double min_angle = mbf_utility::angle(robot_pose , path_segments_.front().checkpoints.back().pose); + min_angle = min_angle * 180 / M_PI ; + double yaw_goal = getSpinAngle(orientation); + double curr_yaw = getSpinAngle(robot_pose.pose.orientation); + ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", " << curr_yaw); + ROS_INFO_STREAM("min_angle: " << min_angle); + action_state_ = SPIN_TURN; + spin_turn_goal_.angle = yaw_goal; + if (fabs(min_angle)<10.0) + { + action_state_ = NAVIGATE; + } + } else { - path_segments_.erase(path_segments_.begin()); action_state_ = NAVIGATE; } + if(path_segments_.empty()) + { + navigate_result.remarks = "Action navigate completed successfully!"; + ROS_INFO_STREAM_NAMED("navigate", "Navigate action completed successfully"); + goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); + action_state_ = SUCCEEDED; + } + path_segments_.erase(path_segments_.begin()); break; case actionlib::SimpleClientGoalState::ABORTED: From 0b3eb55a399069b9095c927aec75f4fe30d00b35 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Tue, 18 Feb 2020 17:01:58 +0900 Subject: [PATCH 06/30] Minor debug message printing fixes --- mbf_abstract_nav/src/navigate_action.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 4e9d80e5..e51e6e90 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -209,7 +209,7 @@ void NavigateAction::runNavigate() min_angle = min_angle * 180 / M_PI ; double yaw_goal = getSpinAngle(orientation); double curr_yaw = getSpinAngle(robot_pose.pose.orientation); - ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", " << curr_yaw); + ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", Current yaw: " << curr_yaw); ROS_INFO_STREAM("min_angle: " << min_angle); if (fabs(min_angle)<10.0) { @@ -334,13 +334,13 @@ bool NavigateAction::getSplitPath( segment.checkpoints.push_back(start); segment.checkpoints.push_back(plan.checkpoints[0]); result.push_back(segment); - segment.checkpoints.clear(); + ROS_INFO_STREAM_NAMED("navigate","Single checkpoint"); for (const auto& point : segment.checkpoints) { ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.spin_turn)); } - + segment.checkpoints.clear(); return true; } From 6c260550e89e98645772a45dc76f7809f32ea60a Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Wed, 19 Feb 2020 19:27:24 +0900 Subject: [PATCH 07/30] added some comments and improved readability --- mbf_abstract_nav/src/navigate_action.cpp | 52 ++++++++++++++++++++---- 1 file changed, 45 insertions(+), 7 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index e51e6e90..1813d297 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -136,9 +136,10 @@ void NavigateAction::start(GoalHandle &goal_handle) return; } - // call get_path action server to get a first plan bool split_result; + + // call function to split path between spin turns split_result = getSplitPath(plan, path_segments_); if(!split_result) @@ -150,6 +151,7 @@ void NavigateAction::start(GoalHandle &goal_handle) return; } + // start navigating with the split path startNavigate(); @@ -164,9 +166,11 @@ void NavigateAction::startNavigate() switch (action_state_) { case NAVIGATE: + // state for executing next segment in the plan runNavigate(); break; case EXE_PATH: + // state for executing current segment action_client_exe_path_.sendGoal( exe_path_goal_, boost::bind(&NavigateAction::actionExePathDone, this, _1, _2), @@ -175,7 +179,7 @@ void NavigateAction::startNavigate() action_state_ = ACTIVE; break; case SPIN_TURN: - + // state for executing spin turn action_client_spin_turn_.sendGoal( spin_turn_goal_, boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), @@ -195,22 +199,32 @@ void NavigateAction::startNavigate() void NavigateAction::runNavigate() { + action_state_ = FAILED; ROS_INFO_STREAM_NAMED("navigate", "Segments remaning: " << path_segments_.size()); + if(!path_segments_.empty()) { + ROS_INFO_STREAM_NAMED("navigate","Spin turn: "<< static_cast(path_segments_.front().checkpoints.front().spin_turn)); + //check if the first checkpoint needs spin turn if(path_segments_.front().checkpoints.front().spin_turn) { + const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + double min_angle = mbf_utility::angle(robot_pose , path_segments_.front().checkpoints.front().pose); min_angle = min_angle * 180 / M_PI ; + double yaw_goal = getSpinAngle(orientation); double curr_yaw = getSpinAngle(robot_pose.pose.orientation); + ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", Current yaw: " << curr_yaw); ROS_INFO_STREAM("min_angle: " << min_angle); + if (fabs(min_angle)<10.0) { path_segments_.front().checkpoints.front().spin_turn = false; @@ -219,8 +233,11 @@ void NavigateAction::runNavigate() } spin_turn_goal_.angle = yaw_goal; + action_state_ = SPIN_TURN; - path_segments_.front().checkpoints.front().spin_turn = false; + + path_segments_.front().checkpoints.front().spin_turn = false; + return; } else @@ -237,6 +254,7 @@ void NavigateAction::runNavigate() } else { + forklift_interfaces::NavigateResult navigate_result; navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; navigate_result.remarks = "Action navigate completed successfully!"; @@ -314,28 +332,39 @@ bool NavigateAction::getSplitPath( const forklift_interfaces::NavigatePath &plan, std::vector &result) { + ROS_INFO_STREAM_NAMED("navigate","Splitting the path"); + //check if incoming path is empty if(plan.checkpoints.size()<1) { return false; } + forklift_interfaces::NavigatePath segment; - + + //single checkpoint case if (plan.checkpoints.size()==1) { + segment.header = plan.header; segment.xy_goal_tolerance = plan.xy_goal_tolerance; segment.yaw_goal_tolerance = plan.xy_goal_tolerance; + geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + forklift_interfaces::Checkpoint start; + start.pose = robot_pose; start.spin_turn = false; segment.checkpoints.push_back(start); segment.checkpoints.push_back(plan.checkpoints[0]); + result.push_back(segment); ROS_INFO_STREAM_NAMED("navigate","Single checkpoint"); + for (const auto& point : segment.checkpoints) { ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.spin_turn)); @@ -367,8 +396,10 @@ bool NavigateAction::getSplitPath( segment.checkpoints.push_back(plan.checkpoints[i]); result.push_back(segment); segment.checkpoints.clear(); + forklift_interfaces::Checkpoint checkpoint = plan.checkpoints[i]; checkpoint.spin_turn = false; + segment.checkpoints.push_back(checkpoint); } } @@ -446,22 +477,29 @@ void NavigateAction::actionExePathDone( switch (state.state_) { case actionlib::SimpleClientGoalState::SUCCEEDED: + // check if we need a spin turn at the last checkpoint if(path_segments_.front().checkpoints.back().spin_turn) { const auto orientation = path_segments_.front().checkpoints.back().pose.pose.orientation; geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + double min_angle = mbf_utility::angle(robot_pose , path_segments_.front().checkpoints.back().pose); min_angle = min_angle * 180 / M_PI ; + double yaw_goal = getSpinAngle(orientation); double curr_yaw = getSpinAngle(robot_pose.pose.orientation); + ROS_INFO_STREAM_NAMED("navigate", "Spin goal: " << yaw_goal << ", " << curr_yaw); ROS_INFO_STREAM("min_angle: " << min_angle); - action_state_ = SPIN_TURN; + + action_state_ = SPIN_TURN; //set state to execute spin spin_turn_goal_.angle = yaw_goal; + if (fabs(min_angle)<10.0) { - action_state_ = NAVIGATE; + action_state_ = NAVIGATE; //set state to navigate because angle below spin threshold } } @@ -477,7 +515,7 @@ void NavigateAction::actionExePathDone( goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); action_state_ = SUCCEEDED; } - path_segments_.erase(path_segments_.begin()); + path_segments_.erase(path_segments_.begin()); //erase the done segment break; case actionlib::SimpleClientGoalState::ABORTED: From 49ffc039686c393ba6c86bd4d2f780218b809c57 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 20 Feb 2020 00:27:04 +0900 Subject: [PATCH 08/30] Fixed a small error in virtual function declaration --- .../include/mbf_costmap_core/costmap_controller.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h b/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h index 644c22f0..e4de617a 100644 --- a/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h +++ b/mbf_costmap_core/include/mbf_costmap_core/costmap_controller.h @@ -109,7 +109,10 @@ namespace mbf_costmap_core { * @param plan The plan to pass to the local planner * @return True if the plan was updated successfully, false otherwise */ - virtual bool setPlan(const std::vector &plan) = 0; + virtual bool setPlan(const std::vector &plan) + { + + }; /** * @brief Requests the planner to cancel, e.g. if it takes too much time From a200112b44ef75b67ec40cfb036581716a90a1b9 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 20 Feb 2020 01:34:46 +0900 Subject: [PATCH 09/30] Merge teb compatibility changes and single checkpoint change --- .../mbf_abstract_core/abstract_controller.h | 2 +- mbf_abstract_nav/CMakeLists.txt | 2 + mbf_abstract_nav/package.xml | 2 + .../src/abstract_controller_execution.cpp | 19 ++++++++- mbf_abstract_nav/src/navigate_action.cpp | 39 ++++--------------- .../costmap_navigation_server.cpp | 12 +++--- 6 files changed, 37 insertions(+), 39 deletions(-) diff --git a/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h b/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h index cddc452d..17f282c0 100644 --- a/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h +++ b/mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h @@ -112,7 +112,7 @@ namespace mbf_abstract_core{ * @param plan The plan to pass to the local planner * @return True if the plan was updated successfully, false otherwise */ - virtual bool setPlan(const std::vector &plan) {}; + virtual bool setPlan(const std::vector &plan) { }; /** * @brief Requests the planner to give feedback for the current plan diff --git a/mbf_abstract_nav/CMakeLists.txt b/mbf_abstract_nav/CMakeLists.txt index 6952b86c..3f66fd88 100644 --- a/mbf_abstract_nav/CMakeLists.txt +++ b/mbf_abstract_nav/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED roscpp std_msgs std_srvs + rr_path_follower tf xmlrpcpp ) @@ -49,6 +50,7 @@ catkin_package( roscpp std_msgs std_srvs + rr_path_follower tf xmlrpcpp DEPENDS Boost diff --git a/mbf_abstract_nav/package.xml b/mbf_abstract_nav/package.xml index 426e8d6a..cd164eba 100644 --- a/mbf_abstract_nav/package.xml +++ b/mbf_abstract_nav/package.xml @@ -26,6 +26,7 @@ mbf_abstract_core mbf_msgs mbf_utility + rr_path_follower xmlrpcpp tf @@ -42,6 +43,7 @@ mbf_abstract_core mbf_msgs mbf_utility + rr_path_follower xmlrpcpp diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp index 26dd889a..4c1f82ba 100644 --- a/mbf_abstract_nav/src/abstract_controller_execution.cpp +++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp @@ -40,6 +40,7 @@ #include "mbf_abstract_nav/abstract_controller_execution.h" #include +#include namespace mbf_abstract_nav { @@ -271,6 +272,7 @@ namespace mbf_abstract_nav // init plan std::vector plan; + std::vector plan_poses; if (!hasNewPlan()) { setState(NO_PLAN); @@ -319,8 +321,23 @@ namespace mbf_abstract_nav return; } + bool set_plan = false; + rr_path_follower::PathFollowerROS* path_follower = dynamic_cast(controller_.get()); //TODO: use dynamic_pointer_cast + if (path_follower) + { + set_plan = controller_->setPlan(plan); + } + else + { + for (const auto checkpoint: plan) + { + plan_poses.push_back(checkpoint.pose); + } + set_plan = controller_->setPlan(plan_poses); + } + // check if plan could be set - if (!controller_->setPlan(plan)) + if (!set_plan) { setState(INVALID_PLAN); condition_.notify_all(); diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 1813d297..0e1bd98e 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -342,37 +342,7 @@ bool NavigateAction::getSplitPath( forklift_interfaces::NavigatePath segment; - //single checkpoint case - if (plan.checkpoints.size()==1) - { - - segment.header = plan.header; - segment.xy_goal_tolerance = plan.xy_goal_tolerance; - segment.yaw_goal_tolerance = plan.xy_goal_tolerance; - - geometry_msgs::PoseStamped robot_pose; - - robot_info_.getRobotPose(robot_pose); - - forklift_interfaces::Checkpoint start; - - start.pose = robot_pose; - start.spin_turn = false; - segment.checkpoints.push_back(start); - segment.checkpoints.push_back(plan.checkpoints[0]); - - result.push_back(segment); - - ROS_INFO_STREAM_NAMED("navigate","Single checkpoint"); - - for (const auto& point : segment.checkpoints) - { - ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.spin_turn)); - } - segment.checkpoints.clear(); - return true; - } for (size_t i = 0 ; i < plan.checkpoints.size(); i++) { @@ -384,6 +354,13 @@ bool NavigateAction::getSplitPath( if (i<1) { segment.checkpoints.push_back(plan.checkpoints[i]); + if (plan.checkpoints.size()==1) + { + result.push_back(segment); + ROS_INFO_STREAM_NAMED("navigate", "Single checkpoint:"); + segment.checkpoints.clear(); + return true; + } } else if (i( planner_plugin_loader_.createInstance(planner_type)); std::string planner_name = planner_plugin_loader_.getName(planner_type); - ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded."); + ROS_INFO_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded."); } catch (const pluginlib::PluginlibException &ex_mbf_core) { - ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin." + ROS_INFO_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin." << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what()); try { @@ -189,11 +189,11 @@ mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControll { controller_ptr = controller_plugin_loader_.createInstance(controller_type); std::string controller_name = controller_plugin_loader_.getName(controller_type); - ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded."); + ROS_INFO_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded."); } catch (const pluginlib::PluginlibException &ex_mbf_core) { - ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;" + ROS_INFO_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;" << " we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what()); try { @@ -217,7 +217,7 @@ bool CostmapNavigationServer::initializeControllerPlugin( const std::string& name, const mbf_abstract_core::AbstractController::Ptr& controller_ptr) { - ROS_DEBUG_STREAM("Initialize controller \"" << name << "\"."); + ROS_INFO_STREAM("Initialize controller \"" << name << "\"."); if (!tf_listener_ptr_) { @@ -234,7 +234,7 @@ bool CostmapNavigationServer::initializeControllerPlugin( mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr = boost::static_pointer_cast(controller_ptr); costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), local_costmap_ptr_.get()); - ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized."); + ROS_INFO_STREAM("Controller plugin \"" << name << "\" initialized."); return true; } From 37f4ca5769c1fc6a06cec8fa59c94d28fe8e133f Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Thu, 20 Feb 2020 13:50:55 +0530 Subject: [PATCH 10/30] added smooth turn logic --- .../mbf_abstract_nav/navigate_action.h | 3 + mbf_abstract_nav/src/navigate_action.cpp | 63 +++++++++++-------- 2 files changed, 40 insertions(+), 26 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h index 9fcdef3b..f54efb60 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h @@ -83,6 +83,9 @@ class NavigateAction void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback); + bool isSmoothTurnPossible(const forklift_interfaces::Checkpoint& previous, + const forklift_interfaces::Checkpoint& current, const forklift_interfaces::Checkpoint& next); + bool getSplitPath( const forklift_interfaces::NavigatePath &plan, std::vector &result); diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 0e1bd98e..6b4a3579 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -39,6 +39,8 @@ */ #include +#include +#include #include "mbf_abstract_nav/MoveBaseFlexConfig.h" #include "mbf_abstract_nav/navigate_action.h" @@ -136,11 +138,8 @@ void NavigateAction::start(GoalHandle &goal_handle) return; } - - bool split_result; - // call function to split path between spin turns - split_result = getSplitPath(plan, path_segments_); + bool split_result = getSplitPath(plan, path_segments_); if(!split_result) { @@ -152,10 +151,7 @@ void NavigateAction::start(GoalHandle &goal_handle) } // start navigating with the split path - startNavigate(); - - - + startNavigate(); } void NavigateAction::startNavigate() @@ -328,6 +324,32 @@ void NavigateAction::actionExePathFeedback( } } + +bool NavigateAction::isSmoothTurnPossible(const forklift_interfaces::Checkpoint& previous, + const forklift_interfaces::Checkpoint& current, const forklift_interfaces::Checkpoint& next) +{ + double initial_orientation = tf2::getYaw(previous.pose.pose.orientation); + double initial_slope = std::atan2((current.pose.pose.position.y - previous.pose.pose.position.y), + (current.pose.pose.position.x - previous.pose.pose.position.x)); + + double orientation = tf2::getYaw(current.pose.pose.orientation); + double slope = std::atan2((next.pose.pose.position.y - current.pose.pose.position.y), + (next.pose.pose.position.x - current.pose.pose.position.x)); + + // check if robot is facing forwards in both the segments + if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope)) < 2e-1) && + (std::abs(angles::shortest_angular_distance(orientation, slope)) < 2e-1)) { + return true; + } + + // check if robot is facing backwards in both the segments + if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope+M_PI)) < 2e-1) && + (std::abs(angles::shortest_angular_distance(orientation, slope+M_PI)) < 2e-1)) { + return true; + } + return false; +} + bool NavigateAction::getSplitPath( const forklift_interfaces::NavigatePath &plan, std::vector &result) @@ -341,9 +363,6 @@ bool NavigateAction::getSplitPath( } forklift_interfaces::NavigatePath segment; - - - for (size_t i = 0 ; i < plan.checkpoints.size(); i++) { @@ -362,14 +381,10 @@ bool NavigateAction::getSplitPath( return true; } } - else if (i Date: Wed, 4 Mar 2020 18:58:49 +0530 Subject: [PATCH 11/30] added navigate action --- mbf_abstract_nav/src/navigate_action.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 6b4a3579..e9c775a7 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -383,7 +383,8 @@ bool NavigateAction::getSplitPath( } else if (i Date: Tue, 10 Mar 2020 14:39:26 +0900 Subject: [PATCH 12/30] Temp fix for new goal recieved while spin turning --- mbf_abstract_nav/src/navigate_action.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index e9c775a7..8a2ae4f6 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -138,6 +138,7 @@ void NavigateAction::start(GoalHandle &goal_handle) return; } + // call function to split path between spin turns bool split_result = getSplitPath(plan, path_segments_); @@ -176,6 +177,9 @@ void NavigateAction::startNavigate() break; case SPIN_TURN: // state for executing spin turn + action_client_spin_turn_.cancelGoal(); + ros::Duration(0.5).sleep(); + action_client_spin_turn_.sendGoal( spin_turn_goal_, boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), From 89fe0eccb5ba743662adcfa7cc223ba3d11849dc Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Wed, 11 Mar 2020 18:34:23 +0900 Subject: [PATCH 13/30] Revert "Temp fix for new goal recieved while spin turning" This reverts commit bc04f6b2d72f56264e3da8ad449ae493840ea8a3. --- mbf_abstract_nav/src/navigate_action.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 8a2ae4f6..e9c775a7 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -138,7 +138,6 @@ void NavigateAction::start(GoalHandle &goal_handle) return; } - // call function to split path between spin turns bool split_result = getSplitPath(plan, path_segments_); @@ -177,9 +176,6 @@ void NavigateAction::startNavigate() break; case SPIN_TURN: // state for executing spin turn - action_client_spin_turn_.cancelGoal(); - ros::Duration(0.5).sleep(); - action_client_spin_turn_.sendGoal( spin_turn_goal_, boost::bind(&NavigateAction::actionSpinTurnDone, this, _1, _2), From 71569bc119adc5b4260f647b753abbd59b4b4fc5 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Wed, 11 Mar 2020 18:50:35 +0900 Subject: [PATCH 14/30] Added a wait to finish previous spin goal before accepting new navigate goal --- mbf_abstract_nav/src/navigate_action.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index e9c775a7..0b1510f6 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -94,6 +94,13 @@ void NavigateAction::cancel() void NavigateAction::start(GoalHandle &goal_handle) { + if (action_state_ == ACTIVE) + { + if(!action_client_spin_turn_.getState().isDone()) + { + action_client_spin_turn_.waitForResult(ros::Duration(30.0)); + } + } action_state_ = SPLIT_PATH; goal_handle.setAccepted(); From 8d2cf121f90466f2c046e0792f675c9252b99fa5 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 20 Mar 2020 19:10:15 +0530 Subject: [PATCH 15/30] added spin at end --- mbf_abstract_nav/src/navigate_action.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 0b1510f6..d31ffe27 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -408,7 +408,10 @@ bool NavigateAction::getSplitPath( } else { - segment.checkpoints.push_back(plan.checkpoints[i]); + ROS_INFO("Plan requires final node spin to be : %d", plan.checkpoints[i].node.spin_turn); + forklift_interfaces::Checkpoint checkpoint = plan.checkpoints[i]; + checkpoint.spin_turn = plan.checkpoints[i].node.spin_turn; + segment.checkpoints.push_back(checkpoint); result.push_back(segment); segment.checkpoints.clear(); } From a167500406f118aadbf03b401a410c11e4bd3a22 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 20 Mar 2020 20:12:02 +0530 Subject: [PATCH 16/30] removed spin turn check for last node --- mbf_abstract_nav/src/navigate_action.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index d31ffe27..1bda4b02 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -503,10 +503,11 @@ void NavigateAction::actionExePathDone( action_state_ = SPIN_TURN; //set state to execute spin spin_turn_goal_.angle = yaw_goal; - if (fabs(min_angle)<10.0) + //removing to achieve high tolearance at the goal and spin turn server should take care of the threshold!! + /*if (fabs(min_angle)<10.0) { action_state_ = NAVIGATE; //set state to navigate because angle below spin threshold - } + }*/ } else From 507749c979bf9908d0f4c2f1d260a365f8015755 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Wed, 29 Apr 2020 18:10:29 +0900 Subject: [PATCH 17/30] Added returning error code for spin turn failure --- mbf_abstract_nav/src/navigate_action.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index e9c775a7..d6847c7a 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -439,6 +439,11 @@ void NavigateAction::actionSpinTurnDone( if(result.status != 2) { ROS_ERROR_STREAM_NAMED("navigate", "Action \"spin_turn\" failed :" << result); + forklift_interfaces::NavigateResult navigate_result; + navigate_result.status = forklift_interfaces::NavigateResult::SPIN_FAILURE; + navigate_result.remarks = "Spin turn failed!"; + goal_handle_.setAborted(navigate_result, state.getText()); + } else { From 21ffb45015dcfacd8245b3d1708583d3f93810ca Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Wed, 13 May 2020 15:25:57 +0900 Subject: [PATCH 18/30] added new error --- mbf_abstract_nav/src/navigate_action.cpp | 39 ++++++++++++++++-------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index cf5256d0..223c11a2 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -158,7 +158,31 @@ void NavigateAction::start(GoalHandle &goal_handle) } // start navigating with the split path - startNavigate(); + startNavigate(); + + // double check if the plan request has + if (action_state_ == SUCCEEDED) + { + geometry_msgs::PoseStamped robot_pose; + robot_info_.getRobotPose(robot_pose); + ROS_INFO_STREAM_NAMED("navigate", "Succeeded from navigation, double checking for orientation from mbf ..."); + + if (mbf_utility::distance(robot_pose, plan.checkpoints.back().pose) < plan.xy_goal_tolerance + && mbf_utility::angle(robot_pose, plan.checkpoints.back().pose) < plan.yaw_goal_tolerance) + { + navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; + navigate_result.remarks = "Action navigate completed successfully!"; + navigate_result.final_pose = robot_pose; + goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); + } + else + { + navigate_result.status = forklift_interfaces::NavigateResult::MISSED_GOAL; + navigate_result.remarks = "Requested pose in the plan was not reached"; + navigate_result.final_pose = robot_pose; + goal_handle_.setAborted(navigate_result, navigate_result.remarks); + } + } } void NavigateAction::startNavigate() @@ -256,15 +280,7 @@ void NavigateAction::runNavigate() } } else - { - - forklift_interfaces::NavigateResult navigate_result; - navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; - navigate_result.remarks = "Action navigate completed successfully!"; - navigate_result.final_pose = robot_pose_; - ROS_INFO_STREAM_NAMED("navigate", "Navigate action completed successfully"); - goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); - + { action_state_ = SUCCEEDED; return; } @@ -522,9 +538,6 @@ void NavigateAction::actionExePathDone( if(path_segments_.empty()) { - navigate_result.remarks = "Action navigate completed successfully!"; - ROS_INFO_STREAM_NAMED("navigate", "Navigate action completed successfully"); - goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); action_state_ = SUCCEEDED; } path_segments_.erase(path_segments_.begin()); //erase the done segment From 26208eba3c19bfadf0094eee1a3aedc59b3b56ef Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Wed, 13 May 2020 19:02:31 +0900 Subject: [PATCH 19/30] state machine fixes --- .../mbf_abstract_nav/navigate_action.h | 3 ++ mbf_abstract_nav/src/navigate_action.cpp | 48 +++++++++++-------- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h index f54efb60..f7aa6d5a 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h @@ -40,6 +40,8 @@ #ifndef MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_ #define MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_ +#include + #include #include @@ -169,6 +171,7 @@ class NavigateAction }; NavigateActionState action_state_; + std::mutex action_mutex_; }; } /* mbf_abstract_nav */ diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 223c11a2..27b5f4f2 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -158,38 +158,41 @@ void NavigateAction::start(GoalHandle &goal_handle) } // start navigating with the split path + action_state_ = NAVIGATE; startNavigate(); + ROS_INFO("Completed Navigation"); // double check if the plan request has if (action_state_ == SUCCEEDED) { geometry_msgs::PoseStamped robot_pose; robot_info_.getRobotPose(robot_pose); - ROS_INFO_STREAM_NAMED("navigate", "Succeeded from navigation, double checking for orientation from mbf ..."); - - if (mbf_utility::distance(robot_pose, plan.checkpoints.back().pose) < plan.xy_goal_tolerance - && mbf_utility::angle(robot_pose, plan.checkpoints.back().pose) < plan.yaw_goal_tolerance) + ROS_INFO("Succeeded from navigation, double checking for orientation from mbf"); + if (mbf_utility::distance(robot_pose, plan.checkpoints.back().pose) <= plan.xy_goal_tolerance + && mbf_utility::angle(robot_pose, plan.checkpoints.back().pose) <= plan.yaw_goal_tolerance) { + ROS_INFO_STREAM_NAMED("navigate", "Plan complete with desired goal tolerance"); navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; navigate_result.remarks = "Action navigate completed successfully!"; navigate_result.final_pose = robot_pose; - goal_handle_.setSucceeded(navigate_result, navigate_result.remarks); + goal_handle.setSucceeded(navigate_result, navigate_result.remarks); } else { + ROS_INFO_STREAM_NAMED("navigate", "Plan failed as the robot did not reach with desired goal tolerance"); navigate_result.status = forklift_interfaces::NavigateResult::MISSED_GOAL; navigate_result.remarks = "Requested pose in the plan was not reached"; navigate_result.final_pose = robot_pose; - goal_handle_.setAborted(navigate_result, navigate_result.remarks); + goal_handle.setAborted(navigate_result, navigate_result.remarks); } } } void NavigateAction::startNavigate() { - action_state_ = NAVIGATE; - while (action_state_ != SUCCEEDED || action_state_ != FAILED) + while (action_state_ != SUCCEEDED && action_state_ != FAILED) { + action_mutex_.lock(); switch (action_state_) { case NAVIGATE: @@ -221,6 +224,8 @@ void NavigateAction::startNavigate() break; } ros::spinOnce(); + action_mutex_.unlock(); + ros::Duration(0.1).sleep(); } } @@ -475,8 +480,7 @@ void NavigateAction::actionSpinTurnDone( { ROS_INFO_STREAM_NAMED("navigate", "Action \"spin_turn\" completed successfully"); action_state_ = NAVIGATE; - } - + } } } @@ -485,9 +489,9 @@ void NavigateAction::actionExePathDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result_ptr) { - action_state_ = FAILED; - - ROS_DEBUG_STREAM_NAMED("navigate", "Action \"exe_path\" finished."); + std::lock_guard lck (action_mutex_); + action_state_ = FAILED; + ROS_INFO_STREAM_NAMED("navigate", "Action \"exe_path\" finished."); const mbf_msgs::ExePathResult& result = *(result_ptr.get()); const forklift_interfaces::NavigateGoal& goal = *(goal_handle_.getGoal().get()); @@ -499,11 +503,19 @@ void NavigateAction::actionExePathDone( navigate_result.angle_to_goal = result.angle_to_goal; navigate_result.final_pose = result.final_pose; - ROS_DEBUG_STREAM_NAMED("exe_path", "Current state:" << state.toString()); + ROS_INFO_STREAM_NAMED("exe_path", "Current state:" << state.toString()); switch (state.state_) { case actionlib::SimpleClientGoalState::SUCCEEDED: + + path_segments_.erase(path_segments_.begin()); //erase the done segment + if(path_segments_.empty()) + { + action_state_ = SUCCEEDED; + ROS_INFO("Succeeded....."); + return; + } // check if we need a spin turn at the last checkpoint if(path_segments_.front().checkpoints.back().spin_turn) { @@ -534,13 +546,7 @@ void NavigateAction::actionExePathDone( else { action_state_ = NAVIGATE; - } - - if(path_segments_.empty()) - { - action_state_ = SUCCEEDED; - } - path_segments_.erase(path_segments_.begin()); //erase the done segment + } break; case actionlib::SimpleClientGoalState::ABORTED: From ff70d1c54ce45406a85665ec6c26323e7f50c494 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Wed, 13 May 2020 19:45:43 +0900 Subject: [PATCH 20/30] spin turn tests --- mbf_abstract_nav/src/navigate_action.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 27b5f4f2..2d91d32b 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -160,7 +160,6 @@ void NavigateAction::start(GoalHandle &goal_handle) // start navigating with the split path action_state_ = NAVIGATE; startNavigate(); - ROS_INFO("Completed Navigation"); // double check if the plan request has if (action_state_ == SUCCEEDED) @@ -232,7 +231,6 @@ void NavigateAction::startNavigate() void NavigateAction::runNavigate() { - action_state_ = FAILED; ROS_INFO_STREAM_NAMED("navigate", "Segments remaning: " << path_segments_.size()); if(!path_segments_.empty()) @@ -509,13 +507,6 @@ void NavigateAction::actionExePathDone( { case actionlib::SimpleClientGoalState::SUCCEEDED: - path_segments_.erase(path_segments_.begin()); //erase the done segment - if(path_segments_.empty()) - { - action_state_ = SUCCEEDED; - ROS_INFO("Succeeded....."); - return; - } // check if we need a spin turn at the last checkpoint if(path_segments_.front().checkpoints.back().spin_turn) { @@ -546,7 +537,13 @@ void NavigateAction::actionExePathDone( else { action_state_ = NAVIGATE; - } + } + if(path_segments_.empty()) + { + action_state_ = SUCCEEDED; + return; + } + path_segments_.erase(path_segments_.begin()); //erase the done segment break; case actionlib::SimpleClientGoalState::ABORTED: From 913c7e1b49aef6fdb60129c0eb0ccd5c00688a93 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Thu, 14 May 2020 07:32:50 +0900 Subject: [PATCH 21/30] added distance and angle to goal --- mbf_abstract_nav/src/navigate_action.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 2d91d32b..d737aa2c 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -166,9 +166,12 @@ void NavigateAction::start(GoalHandle &goal_handle) { geometry_msgs::PoseStamped robot_pose; robot_info_.getRobotPose(robot_pose); - ROS_INFO("Succeeded from navigation, double checking for orientation from mbf"); - if (mbf_utility::distance(robot_pose, plan.checkpoints.back().pose) <= plan.xy_goal_tolerance - && mbf_utility::angle(robot_pose, plan.checkpoints.back().pose) <= plan.yaw_goal_tolerance) + navigate_result.angle_to_goal = mbf_utility::angle(robot_pose, plan.checkpoints.back().pose); + navigate_result.dist_to_goal = mbf_utility::distance(robot_pose, plan.checkpoints.back().pose); + ROS_INFO("Succeeded from navigation, double checking for orientation from mbf with \ + dist_to_goal: %.4f, angle_to_goal: %.4f", navigate_result.dist_to_goal, navigate_result.angle_to_goal); + if (navigate_result.dist_to_goal <= plan.xy_goal_tolerance + && navigate_result.angle_to_goal <= plan.yaw_goal_tolerance) { ROS_INFO_STREAM_NAMED("navigate", "Plan complete with desired goal tolerance"); navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; From edf7e6d7e02e269e3ad339147d3982cd6b128194 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 15 May 2020 15:03:51 +0900 Subject: [PATCH 22/30] keeping it downwards compatible --- mbf_abstract_nav/src/navigate_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index d737aa2c..9f66bd45 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -170,8 +170,8 @@ void NavigateAction::start(GoalHandle &goal_handle) navigate_result.dist_to_goal = mbf_utility::distance(robot_pose, plan.checkpoints.back().pose); ROS_INFO("Succeeded from navigation, double checking for orientation from mbf with \ dist_to_goal: %.4f, angle_to_goal: %.4f", navigate_result.dist_to_goal, navigate_result.angle_to_goal); - if (navigate_result.dist_to_goal <= plan.xy_goal_tolerance - && navigate_result.angle_to_goal <= plan.yaw_goal_tolerance) + if ((navigate_result.dist_to_goal <= plan.xy_goal_tolerance || plan.xy_goal_tolerance <= 1e-5) + && (navigate_result.angle_to_goal <= plan.yaw_goal_tolerance || plan.yaw_goal_tolerance <= 1e-5)) { ROS_INFO_STREAM_NAMED("navigate", "Plan complete with desired goal tolerance"); navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; From ec4255f86824114e2927504554dc6c8037b8ab8d Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 15 May 2020 17:03:50 +0900 Subject: [PATCH 23/30] added for cancel --- mbf_abstract_nav/src/navigate_action.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 9f66bd45..484af452 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -192,7 +192,7 @@ void NavigateAction::start(GoalHandle &goal_handle) void NavigateAction::startNavigate() { - while (action_state_ != SUCCEEDED && action_state_ != FAILED) + while (action_state_ != SUCCEEDED && action_state_ != FAILED && action_state_ != CANCELED) { action_mutex_.lock(); switch (action_state_) @@ -225,8 +225,8 @@ void NavigateAction::startNavigate() default: break; } - ros::spinOnce(); action_mutex_.unlock(); + ros::spinOnce(); ros::Duration(0.1).sleep(); } } @@ -474,7 +474,7 @@ void NavigateAction::actionSpinTurnDone( forklift_interfaces::NavigateResult navigate_result; navigate_result.status = forklift_interfaces::NavigateResult::SPIN_FAILURE; navigate_result.remarks = "Spin turn failed!"; - goal_handle_.setAborted(navigate_result, state.getText()); + //goal_handle_.setAborted(navigate_result, state.getText()); } else From 43490da1f4d1a1e3435a8216d1706606e927268e Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 15 May 2020 17:14:42 +0900 Subject: [PATCH 24/30] revert back setAborted --- mbf_abstract_nav/src/navigate_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 484af452..2b57c5b2 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -474,7 +474,7 @@ void NavigateAction::actionSpinTurnDone( forklift_interfaces::NavigateResult navigate_result; navigate_result.status = forklift_interfaces::NavigateResult::SPIN_FAILURE; navigate_result.remarks = "Spin turn failed!"; - //goal_handle_.setAborted(navigate_result, state.getText()); + goal_handle_.setAborted(navigate_result, state.getText()); } else From d0aa95a4a7f871a5049c772755f3fb929f121275 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 15 May 2020 21:24:12 +0900 Subject: [PATCH 25/30] enabled default oscillation --- mbf_abstract_nav/src/mbf_abstract_nav/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py b/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py index bb812cdf..c1f687e3 100644 --- a/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py +++ b/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py @@ -35,6 +35,6 @@ def add_mbf_abstract_nav_params(gen): "How much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails) them", 15.0, 0, 100) gen.add("oscillation_timeout", double_t, 0, - "How long in seconds to allow for oscillation before executing recovery behaviors.", 0.0, 0, 60) + "How long in seconds to allow for oscillation before executing recovery behaviors.", 10, 0, 60) gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10) From d24e91392bf37f160494aefc474b272fe7b316e3 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Sun, 17 May 2020 11:56:02 +0900 Subject: [PATCH 26/30] rest oscillation distance before sending goal. should not get triggered during spin --- mbf_abstract_nav/src/navigate_action.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index 2b57c5b2..a8fbc7fe 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -203,6 +203,7 @@ void NavigateAction::startNavigate() break; case EXE_PATH: // state for executing current segment + last_oscillation_reset_ = ros::Time::now(); // important to reset the oscillation action_client_exe_path_.sendGoal( exe_path_goal_, boost::bind(&NavigateAction::actionExePathDone, this, _1, _2), From 28fc4d90e024c4b88b899c29bc3e0ddba072557e Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Sun, 17 May 2020 18:12:49 +0900 Subject: [PATCH 27/30] default for oscillation is 60 seconds --- mbf_abstract_nav/src/mbf_abstract_nav/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py b/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py index c1f687e3..76498560 100644 --- a/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py +++ b/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py @@ -35,6 +35,6 @@ def add_mbf_abstract_nav_params(gen): "How much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails) them", 15.0, 0, 100) gen.add("oscillation_timeout", double_t, 0, - "How long in seconds to allow for oscillation before executing recovery behaviors.", 10, 0, 60) + "How long in seconds to allow for oscillation before executing recovery behaviors.", 60, 0, 60) gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must move to be considered not to be oscillating.", 0.5, 0, 10) From 882e60a9898ca13f4e50f776702658760cf79f3d Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Fri, 29 May 2020 20:17:11 +0900 Subject: [PATCH 28/30] last point check --- mbf_abstract_nav/src/navigate_action.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index a8fbc7fe..db16f7fe 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -240,9 +240,9 @@ void NavigateAction::runNavigate() if(!path_segments_.empty()) { - ROS_INFO_STREAM_NAMED("navigate","Spin turn: "<< static_cast(path_segments_.front().checkpoints.front().spin_turn)); + ROS_INFO_STREAM_NAMED("navigate","Spin turn: "<< static_cast(path_segments_.front().checkpoints.front().node.spin_turn)); //check if the first checkpoint needs spin turn - if(path_segments_.front().checkpoints.front().spin_turn) + if(path_segments_.front().checkpoints.front().node.spin_turn >0) { const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; @@ -261,7 +261,7 @@ void NavigateAction::runNavigate() if (fabs(min_angle)<10.0) { - path_segments_.front().checkpoints.front().spin_turn = false; + path_segments_.front().checkpoints.front().node.spin_turn = false; action_state_ = NAVIGATE; return; } @@ -270,7 +270,7 @@ void NavigateAction::runNavigate() action_state_ = SPIN_TURN; - path_segments_.front().checkpoints.front().spin_turn = false; + path_segments_.front().checkpoints.front().node.spin_turn = false; return; } @@ -413,15 +413,13 @@ bool NavigateAction::getSplitPath( } else if (i(point.spin_turn)); + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.node.spin_turn)); } } @@ -512,7 +509,7 @@ void NavigateAction::actionExePathDone( case actionlib::SimpleClientGoalState::SUCCEEDED: // check if we need a spin turn at the last checkpoint - if(path_segments_.front().checkpoints.back().spin_turn) + if(path_segments_.front().checkpoints.back().node.spin_turn >= 0) { const auto orientation = path_segments_.front().checkpoints.back().pose.pose.orientation; geometry_msgs::PoseStamped robot_pose; From bb01e07481802ae0b01c3d84914aacb57b93fd95 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Sun, 31 May 2020 12:24:57 +0900 Subject: [PATCH 29/30] fixes for intermediate and first node --- mbf_abstract_nav/src/navigate_action.cpp | 43 ++++++++++++++++-------- 1 file changed, 29 insertions(+), 14 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index db16f7fe..ccac8bad 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -166,12 +166,12 @@ void NavigateAction::start(GoalHandle &goal_handle) { geometry_msgs::PoseStamped robot_pose; robot_info_.getRobotPose(robot_pose); - navigate_result.angle_to_goal = mbf_utility::angle(robot_pose, plan.checkpoints.back().pose); - navigate_result.dist_to_goal = mbf_utility::distance(robot_pose, plan.checkpoints.back().pose); + navigate_result.angle_to_goal = mbf_utility::angle(robot_pose, goal.path.checkpoints.back().pose); + navigate_result.dist_to_goal = mbf_utility::distance(robot_pose, goal.path.checkpoints.back().pose); ROS_INFO("Succeeded from navigation, double checking for orientation from mbf with \ dist_to_goal: %.4f, angle_to_goal: %.4f", navigate_result.dist_to_goal, navigate_result.angle_to_goal); - if ((navigate_result.dist_to_goal <= plan.xy_goal_tolerance || plan.xy_goal_tolerance <= 1e-5) - && (navigate_result.angle_to_goal <= plan.yaw_goal_tolerance || plan.yaw_goal_tolerance <= 1e-5)) + if ((navigate_result.dist_to_goal <= goal.path.xy_goal_tolerance || goal.path.xy_goal_tolerance <= 1e-5) + && (navigate_result.angle_to_goal <= goal.path.yaw_goal_tolerance || goal.path.yaw_goal_tolerance <= 1e-5)) { ROS_INFO_STREAM_NAMED("navigate", "Plan complete with desired goal tolerance"); navigate_result.status = forklift_interfaces::NavigateResult::SUCCESS; @@ -242,7 +242,7 @@ void NavigateAction::runNavigate() ROS_INFO_STREAM_NAMED("navigate","Spin turn: "<< static_cast(path_segments_.front().checkpoints.front().node.spin_turn)); //check if the first checkpoint needs spin turn - if(path_segments_.front().checkpoints.front().node.spin_turn >0) + if(path_segments_.front().checkpoints.front().node.spin_turn >=0) { const auto orientation = path_segments_.front().checkpoints.front().pose.pose.orientation; @@ -261,7 +261,7 @@ void NavigateAction::runNavigate() if (fabs(min_angle)<10.0) { - path_segments_.front().checkpoints.front().node.spin_turn = false; + path_segments_.front().checkpoints.front().node.spin_turn = 0; action_state_ = NAVIGATE; return; } @@ -270,7 +270,7 @@ void NavigateAction::runNavigate() action_state_ = SPIN_TURN; - path_segments_.front().checkpoints.front().node.spin_turn = false; + path_segments_.front().checkpoints.front().node.spin_turn = 0; return; } @@ -362,19 +362,24 @@ bool NavigateAction::isSmoothTurnPossible(const forklift_interfaces::Checkpoint& double initial_slope = std::atan2((current.pose.pose.position.y - previous.pose.pose.position.y), (current.pose.pose.position.x - previous.pose.pose.position.x)); + ROS_INFO("Initial orientation %f, initial angle: %f", initial_orientation, initial_slope); + double orientation = tf2::getYaw(current.pose.pose.orientation); double slope = std::atan2((next.pose.pose.position.y - current.pose.pose.position.y), (next.pose.pose.position.x - current.pose.pose.position.x)); + ROS_INFO("Next orientation %f, Next angle: %f", orientation, slope); + + // check if robot is facing forwards in both the segments - if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope)) < 2e-1) && - (std::abs(angles::shortest_angular_distance(orientation, slope)) < 2e-1)) { + if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope)) < 3e-1) && + (std::abs(angles::shortest_angular_distance(orientation, slope)) < 3e-1)) { return true; } // check if robot is facing backwards in both the segments - if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope+M_PI)) < 2e-1) && - (std::abs(angles::shortest_angular_distance(orientation, slope+M_PI)) < 2e-1)) { + if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope+M_PI)) < 3e-1) && + (std::abs(angles::shortest_angular_distance(orientation, slope+M_PI)) < 3e-1)) { return true; } return false; @@ -413,17 +418,26 @@ bool NavigateAction::getSplitPath( } else if (i 0) || !(isSmoothTurnPossible(plan.checkpoints[i-1], plan.checkpoints[i], plan.checkpoints[i+1]))){ segment.checkpoints.push_back(plan.checkpoints[i]); result.push_back(segment); segment.checkpoints.clear(); forklift_interfaces::Checkpoint checkpoint = plan.checkpoints[i]; - checkpoint.node.spin_turn = false; + checkpoint.node.spin_turn = 0; segment.checkpoints.push_back(checkpoint); } else { + // treat -1 as smooth turn if it is not the last node. segment.checkpoints.push_back(plan.checkpoints[i]); } } @@ -442,7 +456,8 @@ bool NavigateAction::getSplitPath( ROS_INFO_STREAM_NAMED("navigate","Split segments:"); for (const auto& point : segment.checkpoints) { - ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << "spin turn:" << static_cast(point.node.spin_turn)); + ROS_INFO_STREAM_NAMED("navigate","["<< point.pose.pose.position.x << "," << point.pose.pose.position.y << "]" << + "spin turn:" << static_cast(point.node.spin_turn)); } } From 2fca80e1d8f4de7d106105cd0edc83f20eed9fc7 Mon Sep 17 00:00:00 2001 From: Praveen Ramanujam Date: Sun, 31 May 2020 13:11:54 +0900 Subject: [PATCH 30/30] fixes for straight line --- mbf_abstract_nav/src/navigate_action.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/mbf_abstract_nav/src/navigate_action.cpp b/mbf_abstract_nav/src/navigate_action.cpp index ccac8bad..b1c2c98e 100644 --- a/mbf_abstract_nav/src/navigate_action.cpp +++ b/mbf_abstract_nav/src/navigate_action.cpp @@ -261,7 +261,7 @@ void NavigateAction::runNavigate() if (fabs(min_angle)<10.0) { - path_segments_.front().checkpoints.front().node.spin_turn = 0; + path_segments_.front().checkpoints.front().node.spin_turn = -1; action_state_ = NAVIGATE; return; } @@ -270,8 +270,7 @@ void NavigateAction::runNavigate() action_state_ = SPIN_TURN; - path_segments_.front().checkpoints.front().node.spin_turn = 0; - + path_segments_.front().checkpoints.front().node.spin_turn = -1; return; } else @@ -370,6 +369,16 @@ bool NavigateAction::isSmoothTurnPossible(const forklift_interfaces::Checkpoint& ROS_INFO("Next orientation %f, Next angle: %f", orientation, slope); + // check if its straight line segment + if (std::abs(angles::shortest_angular_distance(initial_orientation, orientation)) < 3e-1) { + ROS_INFO("Expecting straight line checkpoint: %d and checkpoint: %d", previous.node.node_id, current.node.node_id); + return true; + } + + // if it is not straight line, force spin turn + if (current.node.spin_turn > 0) { + return false; + } // check if robot is facing forwards in both the segments if((std::abs(angles::shortest_angular_distance(initial_orientation, initial_slope)) < 3e-1) && @@ -426,7 +435,7 @@ bool NavigateAction::getSplitPath( ROS_ERROR_STREAM_NAMED("navigate", "Terminating as the spin turn flag is set to -1"); break; } - else if((plan.checkpoints[i].node.spin_turn > 0) || !(isSmoothTurnPossible(plan.checkpoints[i-1], plan.checkpoints[i], plan.checkpoints[i+1]))){ + else if(!(isSmoothTurnPossible(plan.checkpoints[i-1], plan.checkpoints[i], plan.checkpoints[i+1]))){ segment.checkpoints.push_back(plan.checkpoints[i]); result.push_back(segment); segment.checkpoints.clear();