Peek ahead on next maneuver

src/DUNE/Maneuvers/Maneuver.cpp

@@ -46,6 +46,8 @@ namespace DUNE
     {
       bind<IMC::StopManeuver>(this);
       bind<IMC::PathControlState>(this);
+      // always consume PeekManeuver, even if inactive
+      bind<IMC::PeekManeuver>(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)
     {

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

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;

src/Maneuver/Multiplexer/Goto.hpp

@@ -59,17 +59,24 @@ 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;
         path.end_z = maneuver->z;
         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<const IMC::Goto*>(planman->data.get());
+        peek_dpath.dpath.set(convertManeuverToPath(m));
+        m_task->dispatch(peek_dpath);
+      }
+
       ~Goto(void)
       { }
     };

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)
       {

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)
     {

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

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);
       }