hubero_core - SitDown task transition handlers added (basic), fixed B…

…B map, added custom updateMemory()
rayvburn · Feb 20, 2022 · 5d9dc9d · 5d9dc9d
1 parent 108159b
commit 5d9dc9d
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 2 deletions.
diff --git a/hubero_core/include/hubero_core/tasks/task_sit_down.h b/hubero_core/include/hubero_core/tasks/task_sit_down.h
@@ -18,11 +18,21 @@ class TaskSitDown: public TaskEssentials<FsmSitDown, FsmSitDown::State, EventFsm
 		state_bb_map_ = {
 			{FsmSitDown::State::MOVING_TO_GOAL, BasicBehaviourType::BB_MOVE_TO_GOAL},
 			{FsmSitDown::State::SITTING_DOWN, BasicBehaviourType::BB_SIT_DOWN},
-			// TODO: add basic behaviour
-			{FsmSitDown::State::SITTING, BasicBehaviourType::BB_SIT_DOWN},
+			{FsmSitDown::State::SITTING, BasicBehaviourType::BB_SIT},
 			{FsmSitDown::State::STANDING_UP, BasicBehaviourType::BB_STAND_UP_FROM_SITTING},
 			{FsmSitDown::State::STANDING, BasicBehaviourType::BB_STAND}
 		};
+
+		fsm_.addTransitionHandler(
+			FsmSitDown::State::STANDING_UP,
+			FsmSitDown::State::STANDING,
+			std::bind(&TaskSitDown::finish, this)
+		);
+	}
+
+	void updateMemory(InternalMemory& memory, const std::shared_ptr<const WorldGeometryBase> world_geometry_ptr) {
+		memory.setGoal(Pose3(getGoalPosition(), Quaternion(0.0, 0.0, getGoalYaw())));
+		TaskEssentials::updateMemory(memory, world_geometry_ptr);
 	}
 
 	virtual bool request(const Vector3& pos, const double& yaw) override {

diff --git a/hubero_core/src/actor.cpp b/hubero_core/src/actor.cpp
@@ -249,6 +249,11 @@ void Actor::addFsmSuperTransitionHandlers(
 		TaskLieDown::State::LYING_DOWN,
 		std::bind(&NavigationBase::finish, navigation_ptr)
 	);
+	task_sit_down_ptr->addStateTransitionHandler(
+		TaskSitDown::State::MOVING_TO_GOAL,
+		TaskSitDown::State::SITTING_DOWN,
+		std::bind(&NavigationBase::finish, navigation_ptr)
+	);
 }
 
 // static