hubero_core & ros - fixed delayed reception of navigation result (cau…

…sed switching active<->finished in MoveToGoal), added Nav::finish call when goal reached (Actor)
rayvburn · Feb 20, 2022 · e0bf60f · e0bf60f
1 parent b55f42b
commit e0bf60f
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 28 deletions.
diff --git 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
@@ -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<bool, Pose3> 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
@@ -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<bool, Pose3> 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()
 	);