From 1ff5621e8e42f2c62d0f123277e63de2afb3fc8d Mon Sep 17 00:00:00 2001 From: rayvburn Date: Sat, 12 Feb 2022 04:20:40 +0100 Subject: [PATCH] hubero_core & ros - fixed delayed reception of navigation result (caused switching active<->finished in MoveToGoal), added Nav::finish call when goal reached (Actor) --- hubero_core/src/actor.cpp | 6 +++++ .../include/hubero_ros/navigation_ros.h | 13 ----------- hubero_ros/src/navigation_ros.cpp | 23 +++++++------------ 3 files changed, 14 insertions(+), 28 deletions(-) diff --git a/hubero_core/src/actor.cpp b/hubero_core/src/actor.cpp index 5f168d5b..4b5aa316 100644 --- a/hubero_core/src/actor.cpp +++ b/hubero_core/src/actor.cpp @@ -272,6 +272,12 @@ void Actor::addFsmSuperTransitionHandlers( TaskMoveToGoal::State::FINISHED, std::bind(&TaskMoveToGoal::finish, task_move_to_goal_ptr) ); + + task_move_to_goal_ptr->addStateTransitionHandler( + TaskMoveToGoal::State::ACTIVE, + TaskMoveToGoal::State::FINISHED, + std::bind(&NavigationBase::finish, navigation_ptr) + ); } // static diff --git a/hubero_ros/include/hubero_ros/navigation_ros.h b/hubero_ros/include/hubero_ros/navigation_ros.h index 14a6b799..87500db2 100644 --- a/hubero_ros/include/hubero_ros/navigation_ros.h +++ b/hubero_ros/include/hubero_ros/navigation_ros.h @@ -155,19 +155,6 @@ class NavigationRos: public NavigationBase { void callbackResult(const move_base_msgs::MoveBaseActionResult::ConstPtr& msg); /// @} - /** - * @defgroup mbinterfaceaction ROS move_base action interface callbacks - * @{ - */ - /** - * @brief Called when action is done - */ - void callbackActionDone( - const actionlib::SimpleClientGoalState& state, - const move_base_msgs::MoveBaseResultConstPtr& msg - ); - /// @} - /** * @brief Finds transform between coordinate systems using ROS TF buffer * @return std::tuple first element is true if transform (second elem) is valid diff --git a/hubero_ros/src/navigation_ros.cpp b/hubero_ros/src/navigation_ros.cpp index 06292013..4a48f378 100644 --- a/hubero_ros/src/navigation_ros.cpp +++ b/hubero_ros/src/navigation_ros.cpp @@ -283,11 +283,15 @@ bool NavigationRos::setGoal(const Pose3& pose, const std::string& frame) { nav_goal_.target_pose.header.stamp = ros::Time::now(); nav_goal_.target_pose.pose = ignPoseToMsgPose(pose); - // SimpleActiveCallback: we subscribe more extensive action state feedback - // SimpleFeedbackCallback: feedback returns current pose of the 'base', which we know quite well + /* + * - SimpleDoneCallback: returns action status with a big delay (compared to topic data) that interferes HuBeRo + * task-switching logic + * - SimpleActiveCallback: we subscribe more extensive action state feedback + * - SimpleFeedbackCallback: feedback returns current pose of the 'base', which we know quite well + */ nav_action_client_ptr_->sendGoal( nav_goal_, - std::bind(&NavigationRos::callbackActionDone, this, std::placeholders::_1, std::placeholders::_2), + MoveBaseActionClient::SimpleDoneCallback(), MoveBaseActionClient::SimpleActiveCallback(), MoveBaseActionClient::SimpleFeedbackCallback() ); @@ -467,17 +471,6 @@ void NavigationRos::callbackResult(const move_base_msgs::MoveBaseActionResult::C feedback_ = fb_type; } -void NavigationRos::callbackActionDone( - const actionlib::SimpleClientGoalState& state, - const move_base_msgs::MoveBaseResultConstPtr& msg -) { - auto fb_type = NavigationRos::convertSimpleClientStateToTaskFeedback(state.state_); - if (fb_type == TASK_FEEDBACK_UNDEFINED) { - return; - } - feedback_ = fb_type; -} - std::tuple NavigationRos::findTransform(const std::string& frame_source, const std::string& frame_target) const { Pose3 transform; bool success = false; @@ -601,7 +594,7 @@ nav_msgs::Path NavigationRos::computePlan( // restore previous goal nav_action_client_ptr_->sendGoal( nav_goal_, - std::bind(&NavigationRos::callbackActionDone, this, std::placeholders::_1, std::placeholders::_2), + MoveBaseActionClient::SimpleDoneCallback(), MoveBaseActionClient::SimpleActiveCallback(), MoveBaseActionClient::SimpleFeedbackCallback() );