From 3037051909d64ff684bb17bae92251053533f556 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:04:39 +0100 Subject: [PATCH 1/6] Plan/Engine/Plan: add function to peek into the upcoming maneuver --- src/Plan/Engine/Plan.cpp | 17 +++++++++++++++++ src/Plan/Engine/Plan.hpp | 5 +++++ 2 files changed, 22 insertions(+) diff --git a/src/Plan/Engine/Plan.cpp b/src/Plan/Engine/Plan.cpp index 11796f4bc1..02e4e52b6a 100644 --- a/src/Plan/Engine/Plan.cpp +++ b/src/Plan/Engine/Plan.cpp @@ -250,6 +250,23 @@ namespace Plan return loadManeuverFromId(m_last_id); } + IMC::PlanManeuver* + Plan::peekNextManeuver(void) + { + IMC::PlanManeuver* pman = NULL; + if(m_curr_node->trans.size() > 0) + { + std::string id = m_curr_node->trans[0]->dest_man; + PlanMap::iterator itr = m_graph.find(id); + + if (itr != m_graph.end()) + { + pman = (&itr->second)->pman; + } + } + return pman; + } + float Plan::updateProgress(const IMC::ManeuverControlState* mcs) { diff --git a/src/Plan/Engine/Plan.hpp b/src/Plan/Engine/Plan.hpp index 87c7358f71..1204f27baa 100644 --- a/src/Plan/Engine/Plan.hpp +++ b/src/Plan/Engine/Plan.hpp @@ -139,6 +139,11 @@ namespace Plan IMC::PlanManeuver* loadNextManeuver(void); + //! Peek next maneuver message + //! @return NULL if maneuver id is invalid + IMC::PlanManeuver* + peekNextManeuver(void); + //! Get current maneuver id //! @return current id string inline std::string From 611658b5d731e12d5262ff5372cfff9bc2c2fada Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:06:14 +0100 Subject: [PATCH 2/6] Plan/Engine: dispatch the next maneuver on start of the current maneuver --- src/Plan/Engine/Task.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Plan/Engine/Task.cpp b/src/Plan/Engine/Task.cpp index 4de3fe09bf..8e9859a231 100644 --- a/src/Plan/Engine/Task.cpp +++ b/src/Plan/Engine/Task.cpp @@ -936,6 +936,14 @@ namespace Plan changeMode(IMC::PlanControlState::PCS_EXECUTING, pman->maneuver_id + DTR(": executing maneuver"), pman->maneuver_id, pman->data.get(), TYPE_INF); + IMC::PlanManeuver* next_man = m_plan->peekNextManeuver(); + if(next_man != NULL) + { + IMC::PeekManeuver peek_man; + peek_man.man.set(*next_man); + dispatch(peek_man); + debug("Dispatching peek maneuver!"); + } m_plan->maneuverStarted(pman->maneuver_id); } From 34c2a18736e3f92976ea06587c6decb7d87a2370 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:07:46 +0100 Subject: [PATCH 3/6] /DUNE/Maneuvers/Maneuver: consume PeekManeuver from PlanEngine and call virtual onPeekManeuver --- src/DUNE/Maneuvers/Maneuver.cpp | 9 +++++++++ src/DUNE/Maneuvers/Maneuver.hpp | 10 ++++++++++ 2 files changed, 19 insertions(+) diff --git a/src/DUNE/Maneuvers/Maneuver.cpp b/src/DUNE/Maneuvers/Maneuver.cpp index de786fdd2d..7ef552a458 100644 --- a/src/DUNE/Maneuvers/Maneuver.cpp +++ b/src/DUNE/Maneuvers/Maneuver.cpp @@ -46,6 +46,8 @@ namespace DUNE { bind(this); bind(this); + // always consume PeekManeuver, even if inactive + bind(this,true); } Maneuver::~Maneuver(void) @@ -164,6 +166,13 @@ namespace DUNE onPathControlState(pcs); } + void + Maneuver::consume(const IMC::PeekManeuver* pman) + { + debug("Calling onPeekManeuver"); + onPeekManeuver(pman); + } + void Maneuver::signalError(const std::string& msg) { diff --git a/src/DUNE/Maneuvers/Maneuver.hpp b/src/DUNE/Maneuvers/Maneuver.hpp index cd3190347a..370942d3a3 100644 --- a/src/DUNE/Maneuvers/Maneuver.hpp +++ b/src/DUNE/Maneuvers/Maneuver.hpp @@ -86,6 +86,13 @@ namespace DUNE (void)pcs; } + //! On Peek Maneuver + virtual void + onPeekManeuver(const IMC::PeekManeuver* pman) + { + (void)pman; + } + //! On task activation //! Should be used only by parent class Maneuver void @@ -166,6 +173,9 @@ namespace DUNE void consume(const IMC::PathControlState* pcs); + void + consume(const IMC::PeekManeuver* pman); + //! Set or reconfigure control loops used by maneuver task. //! @param mask mask identifying controllers that should be made active. void From 394bbd7250583794211889f85c57fc2eaa925c61 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:09:11 +0100 Subject: [PATCH 4/6] Maneuver/Multiplexer/AbstractMux: add virtual onPeekManeuver --- src/Maneuver/Multiplexer/AbstractMux.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/Maneuver/Multiplexer/AbstractMux.hpp b/src/Maneuver/Multiplexer/AbstractMux.hpp index 0a8140a00d..e7fb4990fa 100644 --- a/src/Maneuver/Multiplexer/AbstractMux.hpp +++ b/src/Maneuver/Multiplexer/AbstractMux.hpp @@ -104,6 +104,12 @@ namespace Maneuver (void)msg; } + virtual void + onPeekManeuver(const IMC::PeekManeuver* pman) + { + (void)pman; + } + protected: //! Pointer to task Maneuvers::Maneuver* m_task; From 73580f19cc58c1a7681ade5ea80026f1f00380b9 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:10:44 +0100 Subject: [PATCH 5/6] Maneuver/Multiplexer: implement onPeekManeuver, which calls onPeekManeuver of the peeked maneuver --- src/Maneuver/Multiplexer/Task.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/Maneuver/Multiplexer/Task.cpp b/src/Maneuver/Multiplexer/Task.cpp index 4bd7cdd17b..f344d011b4 100644 --- a/src/Maneuver/Multiplexer/Task.cpp +++ b/src/Maneuver/Multiplexer/Task.cpp @@ -500,6 +500,26 @@ namespace Maneuver m_maneuvers[m_type]->onPathControlState(pcs); } + void + onPeekManeuver(const IMC::PeekManeuver* pman) + { + // figure out what maneuver pman is, and call onPeekManeuver for that maneuver + // as the maneuver type might be different from the current maneuver + const IMC::PlanManeuver* planman = pman->man.get(); + const IMC::Maneuver* maneuver = planman->data.get(); + MultiplexMap::const_iterator itr = m_map.find(maneuver->getId()); + if (itr == m_map.end()) + { + //Only warn, as the current maneuver might be supported + war(DTR("unsupported maneuver. Not peeking forward.")); + return; + } + + ManeuverType type = (ManeuverType)itr->second; + debug("Calling specific onPeekManeuver for %s", maneuver->getName()); + m_maneuvers[type]->onPeekManeuver(pman); + } + void onStateReport(void) { From c89dbb24b1ba13af940f7031fb7076a3e4e1bd3f Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Mon, 18 Mar 2019 16:13:43 +0100 Subject: [PATCH 6/6] Maneuver/Multiplexer/Goto: implement specific onPeekManeuver Translates the peeked maneuver to a PeekDesiredPath, that is dispatched --- src/Maneuver/Multiplexer/Goto.hpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/Maneuver/Multiplexer/Goto.hpp b/src/Maneuver/Multiplexer/Goto.hpp index b68a53472b..6b10473db4 100644 --- a/src/Maneuver/Multiplexer/Goto.hpp +++ b/src/Maneuver/Multiplexer/Goto.hpp @@ -59,6 +59,13 @@ namespace Maneuver { m_task->setControl(IMC::CL_PATH); + IMC::DesiredPath dpath = convertManeuverToPath(maneuver); + m_task->dispatch(dpath); + } + + IMC::DesiredPath + convertManeuverToPath(const IMC::Goto* maneuver) + { IMC::DesiredPath path; path.end_lat = maneuver->lat; path.end_lon = maneuver->lon; @@ -66,10 +73,10 @@ namespace Maneuver path.end_z_units = maneuver->z_units; path.speed = maneuver->speed; path.speed_units = maneuver->speed_units; - - m_task->dispatch(path); + return path; } + //! On PathControlState message //! @param[in] pcs pointer to PathControlState message void @@ -81,6 +88,17 @@ namespace Maneuver m_task->signalProgress(pcs->eta); } + void + onPeekManeuver(const IMC::PeekManeuver* pman) + { + m_task->debug("Dispatching peeked DesiredPath"); + IMC::PeekDesiredPath peek_dpath; + const IMC::PlanManeuver* planman = pman->man.get(); + const IMC::Goto* m = static_cast(planman->data.get()); + peek_dpath.dpath.set(convertManeuverToPath(m)); + m_task->dispatch(peek_dpath); + } + ~Goto(void) { } };