Slave.hs

{-# LANGUAGE CPP, OverloadedStrings #-}
module Ros.Graph.Slave (RosSlave(..), runSlave, requestTopicClient,
                       cleanupNode) where
import Control.Applicative
import Control.Concurrent (killThread, forkIO, threadDelay, MVar, putMVar,
                           isEmptyMVar, readMVar, modifyMVar_)
import Control.Concurrent.SSem (SSem)
import qualified Control.Concurrent.SSem as Sem
import Control.Monad.IO.Class (liftIO)
import qualified Data.ByteString.UTF8 ()
import qualified Data.ByteString.Lazy.UTF8 as BLU
import Snap.Http.Server (simpleHttpServe)
import Snap.Http.Server.Config (defaultConfig, setPort, Config, ConfigLog(..),
                                setVerbose, setAccessLog, setErrorLog)
-- import Snap.Types (Snap, getRequestBody, writeLBS, 
--                    getResponse, putResponse, setContentLength)
import Snap.Core (Snap, readRequestBody, writeLBS, getResponse, putResponse, 
                  setContentLength)
import Network.Socket hiding (Stream)
import qualified Network.Socket as Net
import Network.XmlRpc.Internals (Value)
import Network.XmlRpc.Server (handleCall, methods, fun)
import Network.XmlRpc.Client (remote)
#ifndef mingw32_HOST_OS
import System.Posix.Process (getProcessID)
#endif
import System.Process (readProcess)
import Ros.Internal.RosTypes
import Ros.Topic.Stats (PubStats(PubStats), SubStats(SubStats))
import Ros.Graph.Master

class RosSlave a where
    getMaster :: a -> URI
    getNodeName :: a -> String
    getNodeURI :: a -> MVar URI
    getSubscriptions :: a -> IO [(TopicName, TopicType, [(URI, SubStats)])]
    getPublications :: a -> IO [(TopicName, TopicType, [(URI, PubStats)])]
    publisherUpdate :: a -> TopicName -> [URI] -> IO ()
    getTopicPortTCP :: a -> TopicName -> Maybe Int
    setShutdownAction :: a -> IO () -> IO ()
    stopNode :: a -> IO ()

#ifdef mingw32_HOST_OS
getProcessID :: IO Int
getProcessID = return 42
#endif

-- |Unregister all of a node's publishers and subscribers, then stop
-- the node's servers.
cleanupNode :: RosSlave n => n -> IO ()
cleanupNode n = do pubs <- getPublications n
                   subs <- getSubscriptions n
                   nuri <- readMVar (getNodeURI n)
                   let nname = getNodeName n
                       master = getMaster n
                       stop f (tname,_,_) = f nname tname nuri
                   mapM_ (stop (unregisterPublisher master)) pubs
                   mapM_ (stop (unregisterSubscriber master)) subs
                   stopNode n

type RpcResult a = IO (Int, String, a)

mkPublishStats :: (TopicName, a, [(URI, PubStats)]) -> 
                  (TopicName, Int, [(Int, Int, Int, Bool)])
mkPublishStats (n, _, pstats) = (n, 0, map formatStats pstats)
    where formatStats (_, (PubStats bytesSent numSent conn)) = 
              (0, bytesSent, numSent, conn)

mkSubStats :: (TopicName, a, [(URI, SubStats)]) -> 
              (String, Int, [(Int, Int, Int, Bool)])
mkSubStats (n, _, sstats) = (n, 0, map formatStats sstats)
    where formatStats (_, (SubStats bytesReceived conn)) = 
              (0, bytesReceived, -1, conn)

getBusStats :: (RosSlave a) => a -> CallerID -> 
               RpcResult ([(String,Int,[(Int,Int,Int,Bool)])],
                          [(String,Int,[(Int,Int,Int,Bool)])],
                          (Int,Int,Int))
getBusStats n _ = do
    publishStats <- map (mkPublishStats) <$> getPublications n
    subscribeStats <- map (mkSubStats) <$> getSubscriptions n
    let serviceStats = (0,0,0)
    return (1, "", (publishStats, subscribeStats, serviceStats))

getBusInfo :: (RosSlave a) => a -> CallerID -> 
              RpcResult [(Int,String,String,String,String)]
getBusInfo n _ = do
    pubs <- concatMap formatPubs <$> getPublications n
    subs <- concatMap formatSubs <$> getSubscriptions n
    return (1, "", pubs ++ subs)
    where formatPubs (tname, _, stats) = 
              map (\(uri,_) -> (0, uri, "o", "TCPROS", tname)) stats
          formatSubs (tname, _, stats) = 
              map (\(uri,_) -> (0, uri, "i", "TCPROS", tname)) stats

getMaster' :: RosSlave a => a -> CallerID -> IO (Int, String, URI)
getMaster' n _ = return (1, "", getMaster n)

shutdown' :: RosSlave a => a -> SSem -> CallerID -> IO (Int, String, Bool)
shutdown' n q _ = stopNode n >> Sem.signal q >> return (1, "", True)

-- This requires a dependency on the unix package and so is not cross
-- platform.
getPid' :: CallerID -> RpcResult Int
getPid' _ = do pid <- getProcessID
               return (1, "", fromEnum pid)

getSubscriptions' :: RosSlave a => a -> CallerID -> RpcResult [(String, String)]
getSubscriptions' node _ = do 
  subs <- map (\(n,t,_) -> (n,t)) <$> getSubscriptions node
  return (1, "", subs)

getPublications' :: RosSlave a => a -> CallerID -> RpcResult [(String, String)]
getPublications' node _ = do 
  pubs <- map (\(n,t,_) -> (n,t)) <$> getPublications node
  return (1, "", pubs)

paramUpdate' :: RosSlave a => a -> CallerID -> String -> Value -> RpcResult Bool
paramUpdate' _n _ _paramKey _paramVal = do 
  putStrLn "paramUpdate not implemented!"
  return (1, "", True)

pubUpdate :: RosSlave a => a -> CallerID -> TopicName -> [URI] -> RpcResult Int
pubUpdate n _ topic publishers = do publisherUpdate n topic publishers
                                    return (1, "", 1)

-- Extract just the hostname or IP part of a Node's URI.
myName :: RosSlave a => a -> IO String
myName n = extractName `fmap` readMVar (getNodeURI n)
    where extractName uri = takeWhile (/=':') $ drop 7 uri

requestTopic :: RosSlave a => a -> CallerID -> TopicName -> [[Value]] -> 
                RpcResult (String,String,Int)
requestTopic n _ topic _protocols = 
    case getTopicPortTCP n topic of
      Just p -> do --putStrLn $ topic++" requested "++show p
                   host <- myName n
                   return (1, "", ("TCPROS",host,p))
      Nothing -> return (0, "Unknown topic", ("TCPROS", "", 0))

requestTopicClient :: URI -> CallerID -> TopicName -> [[String]] -> 
                      RpcResult (String,String,Int)
requestTopicClient = flip remote "requestTopic"

-- Dispatch an XML-RPC request body and return the response. The first
-- parameter is a value that provides the necessary reflective API as
-- to ROS Node state. The second parameter is a semaphore indicating
-- that the node should terminate.
slaveRPC :: (RosSlave a) => a -> SSem -> String -> IO BLU.ByteString
slaveRPC n = -- \q s -> putStrLn ("Slave call "++s)>>(handleCall (dispatch q) s)
    handleCall . dispatch
    where dispatch q = methods [ ("getBusStats", fun (getBusStats n))
                               , ("getBusInfo", fun (getBusInfo n))
                               , ("getMasterUri", fun (getMaster' n))
                               , ("shutdown", fun (shutdown' n q))
                               , ("getPid", fun getPid')
                               , ("getSubscriptions", fun (getSubscriptions' n))
                               , ("getPublications", fun (getPublications' n))
                               , ("paramUpdate", fun (paramUpdate' n))
                               , ("publisherUpdate", fun (pubUpdate n))
                               , ("requestTopic", fun (requestTopic n)) ]

-- Start a Snap webserver on the specified port with the specified
-- handler.
simpleServe :: Int -> Snap () -> IO ()
simpleServe port handler = simpleHttpServe conf handler
  where conf :: Config Snap ()
        conf = setAccessLog ConfigNoLog .
               setErrorLog ConfigNoLog . 
               setVerbose False .
               setPort port $
               defaultConfig

-- Find a free port by opening a socket, getting its port, then
-- closing it.
findFreePort :: IO Int
findFreePort = do s <- socket AF_INET Net.Stream defaultProtocol
                  bindSocket s (SockAddrInet aNY_PORT iNADDR_ANY)
                  port <- fromInteger . toInteger <$> socketPort s
                  sClose s
                  return port

-- |Run a ROS slave node. Returns an action that will wait for the
-- node to shutdown along with the port the server is running on.
runSlave :: RosSlave a => a -> IO (IO (), Int)
runSlave n = do quitNow <- Sem.new 0
                port <- findFreePort
                let myUri = getNodeURI n
                    myPort = ":" ++ show port
                myURIEmpty <- isEmptyMVar myUri
                if myURIEmpty 
                  then do myIP <- init <$> readProcess "hostname" [] ""
                          putMVar myUri $! "http://"++myIP++myPort
                  else modifyMVar_ myUri ((return $!) . (++myPort))
                t <- forkIO $ simpleServe port (rpc (slaveRPC n quitNow))
                let wait = do Sem.wait quitNow
                              -- Wait a second for the response to flush
                              threadDelay 1000000 
                              stopNode n
                              killThread t
                return (wait, port)
    where rpc f = do body <- BLU.toString <$> readRequestBody 4096
                     response <- liftIO $ f body
                     writeLBS response
                     let len = fromIntegral $ BLU.length response
                     putResponse . setContentLength len =<< getResponse