packages feed

roshask (empty) → 0.2

raw patch · 71 files changed

+5318/−0 lines, 71 filesdep +BoundedChandep +Cabaldep +SafeSemaphoresetup-changed

Dependencies added: BoundedChan, Cabal, SafeSemaphore, attoparsec, base, binary, bytestring, containers, data-default-generics, deepseq, directory, filemanip, filepath, haxr, mtl, network, parsec, process, pureMD5, roshask, snap-core, snap-server, stm, storable-tuple, tasty, tasty-hunit, template-haskell, testpack, time, transformers, unix, uri, utf8-string, vector, vector-space, xml

Files

+ Examples/PubSub/CMakeLists.txt view
@@ -0,0 +1,32 @@+cmake_minimum_required(VERSION 2.4.6)+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)++# Set the build type.  Options are:+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage+#  Debug          : w/ debug symbols, w/o optimization+#  Release        : w/o debug symbols, w/ optimization+#  RelWithDebInfo : w/ debug symbols, w/ optimization+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries+#set(ROS_BUILD_TYPE RelWithDebInfo)++rosbuild_init()++#set the default path for built executables to the "bin" directory+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)+#set the default path for built libraries to the "lib" directory+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)++#uncomment if you have defined messages+#rosbuild_genmsg()+#uncomment if you have defined services+#rosbuild_gensrv()++#common commands for building c++ executables and libraries+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)+#target_link_libraries(${PROJECT_NAME} another_library)+#rosbuild_add_boost_directories()+#rosbuild_link_boost(${PROJECT_NAME} thread)+#rosbuild_add_executable(example examples/example.cpp)+#target_link_libraries(example ${PROJECT_NAME})++add_custom_target(roshask ALL roshask dep ${PROJECT_SOURCE_DIR} COMMAND cd ${PROJECT_SOURCE_DIR} && cabal install)
+ Examples/PubSub/Makefile view
@@ -0,0 +1,1 @@+include $(shell rospack find mk)/cmake.mk
+ Examples/PubSub/PubSub.cabal view
@@ -0,0 +1,26 @@+Name:                ROS-PubSub+Version:             0.0+Synopsis:            I am code+Cabal-version:       >=1.6+Category:            Robotics+Build-type:          Custom++Executable talker+  Build-Depends:  base >= 4.2 && < 6,+                  vector >= 0.7,+                  time >= 1.1,+                  roshask == 0.1.*,+                  ROS-std-msgs+  GHC-Options:    -Odph -main-is Talker+  Main-Is:        Talker.hs+  Hs-Source-Dirs: src++Executable listener+  Build-Depends:  base >= 4.2 && < 6,+                  vector >= 0.7,+                  time >= 1.1,+                  roshask == 0.1.*,+                  ROS-std-msgs+  GHC-Options:    -Odph -main-is Listener+  Main-Is:        Listener.hs+  Hs-Source-Dirs: src
+ Examples/PubSub/Setup.hs view
@@ -0,0 +1,5 @@+import Distribution.Simple+import Ros.Internal.SetupUtil++main = defaultMainWithHooks $+       simpleUserHooks { confHook = rosConf }
+ Examples/PubSub/mainpage.dox view
@@ -0,0 +1,26 @@+/**+\mainpage+\htmlinclude manifest.html++\b PubSub is an example of publishing and subscribing to Topics using roshask.++<!-- +Provide an overview of your package.+-->+++\section codeapi Code API++<!--+Provide links to specific auto-generated API documentation within your+package that is of particular interest to a reader. Doxygen will+document pretty much every part of your code, so do your best here to+point the reader to the actual API.++If your codebase is fairly large or has different sets of APIs, you+should use the doxygen 'group' tag to keep these APIs together. For+example, the roscpp documentation has 'libros' group.+-->+++*/
+ Examples/PubSub/manifest.xml view
@@ -0,0 +1,15 @@+<package>+  <description brief="PubSub">++     PubSub++  </description>+  <author>Anthony Cowley</author>+  <license>BSD</license>+  <review status="unreviewed" notes=""/>+  <url>http://ros.org/wiki/PubSub</url>+  <depend package="std_msgs"/>++</package>++
+ Examples/PubSub/src/Listener.hs view
@@ -0,0 +1,8 @@+module Listener (main) where+import Ros.Node+import qualified Ros.Std_msgs.String as S++showMsg :: S.String -> IO ()+showMsg = putStrLn . ("I heard " ++) . S._data++main = runNode "listener" $ runHandler showMsg =<< subscribe "chatter"
+ Examples/PubSub/src/Talker.hs view
@@ -0,0 +1,11 @@+module Talker (main) where+import Data.Time.Clock (getCurrentTime)+import Ros.Node+import Ros.Topic (repeatM)+import qualified Ros.Std_msgs.String as S++sayHello :: Topic IO S.String+sayHello = repeatM (fmap mkMsg getCurrentTime)+  where mkMsg = S.String . ("Hello world " ++) . show++main = runNode "talker" $ advertise "chatter" (topicRate 1 sayHello)
+ Examples/Turtle/CMakeLists.txt view
@@ -0,0 +1,32 @@+cmake_minimum_required(VERSION 2.4.6)+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)++# Set the build type.  Options are:+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage+#  Debug          : w/ debug symbols, w/o optimization+#  Release        : w/o debug symbols, w/ optimization+#  RelWithDebInfo : w/ debug symbols, w/ optimization+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries+#set(ROS_BUILD_TYPE RelWithDebInfo)++rosbuild_init()++#set the default path for built executables to the "bin" directory+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)+#set the default path for built libraries to the "lib" directory+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)++#uncomment if you have defined messages+#rosbuild_genmsg()+#uncomment if you have defined services+#rosbuild_gensrv()++#common commands for building c++ executables and libraries+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)+#target_link_libraries(${PROJECT_NAME} another_library)+#rosbuild_add_boost_directories()+#rosbuild_link_boost(${PROJECT_NAME} thread)+#rosbuild_add_executable(example examples/example.cpp)+#target_link_libraries(example ${PROJECT_NAME})++add_custom_target(roshask ALL roshask dep ${PROJECT_SOURCE_DIR} COMMAND cd ${PROJECT_SOURCE_DIR} && cabal install)
+ Examples/Turtle/Makefile view
@@ -0,0 +1,1 @@+include $(shell rospack find mk)/cmake.mk
+ Examples/Turtle/Setup.hs view
@@ -0,0 +1,5 @@+import Distribution.Simple+import Ros.Internal.SetupUtil++main = defaultMainWithHooks $+       simpleUserHooks { confHook = rosConf }
+ Examples/Turtle/Turtle.cabal view
@@ -0,0 +1,42 @@+Name:                ROS-Turtle+Version:             0.0+Synopsis:            I am code+Cabal-version:       >=1.6+Category:            Robotics+Build-type:          Custom++Executable Turtle+  Build-Depends:  base >= 4.2 && < 6,+                  vector > 0.7,+                  time >= 1.1,+                  roshask == 0.1.*,+                  ROS-std-msgs,+                  ROS-turtlesim-msgs+  GHC-Options:    -O2 -main-is Turtle+  Main-Is:        Turtle.hs+  Hs-Source-Dirs: src++Executable Turtle2+  Build-Depends:  base >= 4.2 && < 6,+                  vector > 0.7,+                  time >= 1.1,+                  roshask == 0.1.*,+                  vector-space,+                  ROS-std-msgs,+                  ROS-turtlesim-msgs+  GHC-Options:    -O2 -main-is Turtle2+  Main-Is:        Turtle2.hs+  Other-Modules:  AngleNum+  Hs-Source-Dirs: src++Executable Turtle3+  Build-Depends:  base >= 4.2 && < 6,+                  vector > 0.7,+                  time >= 1.1,+                  roshask == 0.1.*,+                  ROS-std-msgs,+                  ROS-turtlesim-msgs+  GHC-Options:    -O2 -main-is Turtle3+  Main-Is:        Turtle3.hs+  Other-Modules:  AngleNum+  Hs-Source-Dirs: src
+ Examples/Turtle/mainpage.dox view
@@ -0,0 +1,26 @@+/**+\mainpage+\htmlinclude manifest.html++\b Turtle is ... ++<!-- +Provide an overview of your package.+-->+++\section codeapi Code API++<!--+Provide links to specific auto-generated API documentation within your+package that is of particular interest to a reader. Doxygen will+document pretty much every part of your code, so do your best here to+point the reader to the actual API.++If your codebase is fairly large or has different sets of APIs, you+should use the doxygen 'group' tag to keep these APIs together. For+example, the roscpp documentation has 'libros' group.+-->+++*/
+ Examples/Turtle/manifest.xml view
@@ -0,0 +1,16 @@+<package>+  <description brief="Turtle">++     Turtle++  </description>+  <author>Anthony Cowley</author>+  <license>BSD</license>+  <review status="unreviewed" notes=""/>+  <url>http://ros.org/wiki/Turtle</url>+  <depend package="std_msgs"/>+  <depend package="turtlesim"/>++</package>++
+ Examples/Turtle/src/AngleNum.hs view
@@ -0,0 +1,48 @@+{-# LANGUAGE TypeFamilies, GeneralizedNewtypeDeriving #-}+-- |Define a lightweight wrapper, 'Angle', for floating point numeric+-- types that comes with a 'Num' instance that wraps around at+-- 0 and 2&#x03C0, and properly handles angle differencing.+module AngleNum (Angle, angle, fromAngle, toDegrees) where+import Data.Function+import Data.VectorSpace++wrapAngle :: (Floating a, Ord a) => a -> a+wrapAngle theta +  | theta < 0    = theta + 2 * pi+  | theta > 2*pi = theta - 2 * pi+  | otherwise    = theta++angleDiff :: (Floating a, Ord a) => a -> a -> a+angleDiff x y = wrapAngle (x' + pi - y') - pi+  where x' = if x < 0 then x + 2*pi else x+        y' = if y < 0 then y + 2*pi else y++-- |Representation of an angle in radians.+newtype Angle a = Angle { fromAngle :: a } +  deriving (Eq, Ord, Show, Fractional, Floating)++-- |Produce an 'Angle' value within the range [0,2pi].+angle :: (Floating a, Ord a) => a -> Angle a+angle = Angle . wrapAngle++-- |Convert an 'Angle' into a floating point number of degrees.+toDegrees :: Floating a => Angle a -> a+toDegrees = (* (180 / pi)) . fromAngle++instance (Floating a, Ord a) => Num (Angle a) where+  Angle x + Angle y = Angle . wrapAngle $ x + y+  (*) = ((Angle . wrapAngle) .) . (*) `on` fromAngle+  Angle x - Angle y = Angle $ angleDiff x y+  negate (Angle x) = Angle (negate x)+  abs (Angle x) = Angle (abs x)+  signum (Angle x) = Angle (signum x)+  fromInteger = Angle . wrapAngle . fromInteger++instance (Floating a, Ord a, AdditiveGroup a) => AdditiveGroup (Angle a) where+  zeroV = Angle 0+  (^+^) = ((Angle . wrapAngle) . ) . (^+^) `on` fromAngle+  negateV = negate++instance (Floating a, Ord a, AdditiveGroup a) => VectorSpace (Angle a) where+  type Scalar (Angle a) = a+  s *^ Angle x = Angle . wrapAngle $ s * x
+ Examples/Turtle/src/Turtle.hs view
@@ -0,0 +1,50 @@+{-# LANGUAGE TemplateHaskell #-}+module Turtle (main) where+import Data.Complex+import Ros.Node+import Ros.Topic (cons, repeatM)+import Ros.TopicUtil (tee, filterBy, everyNew, interruptible, gate, share)+import Ros.Turtlesim.Pose+import Ros.Turtlesim.Velocity+import Ros.Logging+import System.IO (hFlush, stdout)++-- A type synonym for a 2D point.+type Point = Complex Float++-- A Topic of user-supplied waypoint trajectories.+getTraj :: Topic IO [Point]+getTraj = repeatM (do putStr "Enter waypoints: " >> hFlush stdout+                      $(logInfo "Waiting for new traj")+                      (map (uncurry (:+)) . read) `fmap` getLine)++wrapAngle theta +  | theta < 0    = theta + 2 * pi+  | theta > 2*pi = theta - 2 * pi+  | otherwise    = theta++angleDiff x y = wrapAngle (x' + pi - y') - pi+  where x' = if x < 0 then x + 2*pi else x+        y' = if y < 0 then y + 2*pi else y++-- Produce a unit value every time a goal is reached.+arrivalTrigger :: Topic IO Point -> Topic IO Pose -> Topic IO ()+arrivalTrigger goals poses = fmap (const ()) $+                             filterBy (fmap arrived goals) (fmap p2v poses)+  where arrived goal pose = magnitude (goal - pose) < 1.5+        p2v (Pose x y _ _ _) = x :+ y++-- Navigate to a goal given a current pose estimate.+navigate :: (Point, Pose) -> Velocity+navigate (goal, pos) = Velocity (min 2 (magnitude v)) angVel+  where v        = goal - (x pos :+ y pos)+        thetaErr = (angleDiff (phase v) (theta pos)) * (180 / pi)+        angVel   = signum thetaErr * (min 2 (abs thetaErr))++main = runNode "HaskellBTurtle" $+       do enableLogging (Just Warn)+          poses <- subscribe "/turtle1/pose"+          (t1,t2) <- liftIO . tee . interruptible $ getTraj+          let goals = gate t1 (cons () (arrivalTrigger t2 poses))+          advertise "/turtle1/command_velocity" $+                    fmap navigate (everyNew goals poses)
+ Examples/Turtle/src/Turtle2.hs view
@@ -0,0 +1,53 @@+{-# LANGUAGE TemplateHaskell #-}+module Turtle2 (main) where+import Prelude hiding (dropWhile)+import Control.Applicative+import Control.Arrow+import Data.Complex+import AngleNum+import Ros.Node+import Ros.Topic (repeatM, force, dropWhile, metamorphM, yieldM)+import Ros.TopicUtil (everyNew, interruptible)+import Ros.Turtlesim.Pose+import Ros.Turtlesim.Velocity+import Ros.Logging+import System.IO (hFlush, stdout)++-- A type synonym for a 2D point.+type Point = Complex Float++-- A Topic of user-supplied waypoint trajectories.+getTraj :: Topic IO [Point]+getTraj = repeatM (do putStr "Enter waypoints: " >> hFlush stdout+                      $(logInfo "Waiting for new traj")+                      map (uncurry (:+)) . read <$> getLine)++-- Produce a new goal 'Point' every time a goal is reached.+destinations :: (Functor m, Monad m) => +                Topic m Point -> Topic m Pose -> Topic m Point+destinations goals poses = metamorphM (start (p2v <$> poses)) goals+  where start t g = yieldM g (go g t)+        go g t g' = force (dropWhile (keepGoing g) t) >>= yieldM g' . go g'+        keepGoing goal pose = magnitude (goal - pose) > 1.5+        p2v (Pose x y _ _ _) = x :+ y++-- Compute linear distance to goal and bearing to goal+toGoal :: (Pose,Point) -> (Float, Angle Float)+toGoal (pos,goal) = (magnitude v, angle $ phase v)+  where v = goal - (x pos :+ y pos)++-- Steer based on a current pose estimate and distance from goal+steering :: Pose -> (Float, Angle Float) -> Velocity+steering pos (dpos, thetaDesired) = Velocity (min 2 dpos) angVel+  where thetaErr = toDegrees $ thetaDesired - angle (theta pos)+        angVel = signum thetaErr * min 2 (abs thetaErr)++navigate :: (Pose, Point) -> Velocity+navigate = uncurry ($) . (steering . fst &&& toGoal)++main = runNode "HaskellBTurtle" $+       do enableLogging (Just Warn)+          poses <- subscribe "/turtle1/pose"+          let goals = destinations (interruptible getTraj) poses+          advertise "/turtle1/command_velocity" +                    (navigate <$> everyNew poses goals)
+ Examples/Turtle/src/Turtle3.hs view
@@ -0,0 +1,58 @@+{-# LANGUAGE TemplateHaskell #-}+module Turtle3 (main) where+import Prelude hiding (dropWhile)+import Control.Applicative+import Control.Arrow+import System.IO (hFlush, stdout)+import Data.Complex+import Ros.Logging+import Ros.Node+import Ros.Topic (repeatM, force, dropWhile, metamorphM, yieldM)+import Ros.TopicUtil (everyNew, interruptible, forkTopic, topicOn, subsample)+import Ros.Util.PID (pidTimedIO)+import AngleNum+import Ros.Turtlesim.Pose+import Ros.Turtlesim.Velocity++-- A type synonym for a 2D point.+type Point = Complex Float++-- A Topic of user-supplied waypoint trajectories.+getTraj :: Topic IO [Point]+getTraj = repeatM (do putStr "Enter waypoints: " >> hFlush stdout+                      $(logInfo "Waiting for new traj")+                      map (uncurry (:+)) . read <$> getLine)++-- Produce a new goal 'Point' every time a 'Pose' topic reaches the+-- vicinity of the previous goal.+destinations :: (Functor m, Monad m) => +                Topic m Point -> Topic m Pose -> Topic m Point+destinations goals poses = metamorphM (start (p2v <$> poses)) goals+  where start t g = yieldM g (go g t)+        go g t g' = let keepGoing x = magnitude (g - x) > 0.5+                    in force (dropWhile keepGoing t) >>= yieldM g' . go g'+        p2v (Pose x y _ _ _) = x :+ y++-- Compute linear and angular error to goal.+toGoal :: (Pose,Point) -> (Float, Float)+toGoal (pos,goal) = (magnitude v, thetaErr)+  where v = goal - (x pos :+ y pos)+        thetaErr = toDegrees $ angle (phase v) - angle (theta pos)++-- Run a PID loop on angular velocity to converge to a bearing for the+-- goal.+steering :: Topic IO (Float,Float) -> Topic IO Velocity+steering = topicOn snd (Velocity . fst) (($ 0) <$> pidTimedIO 0.01 0.5 0)++-- Compute position and bearing error to goal, clamp linear and+-- angular velocities, and generate a steering command.+navigate :: Topic IO (Pose,Point) -> Topic IO Velocity+navigate = steering . fmap ((clamp *** id) . toGoal)+  where clamp x = signum x * min 2 (abs x)++main = runNode "HaskellBTurtle" $+       do enableLogging (Just Warn)+          poses <- subscribe "/turtle1/pose"+          let goals = destinations (interruptible getTraj) poses+              commands = subsample 15 $ navigate $ everyNew poses goals+          advertise "/turtle1/command_velocity" commands
+ LICENSE view
@@ -0,0 +1,30 @@+Copyright (c)2010, Anthony Cowley++All rights reserved.++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are met:++    * Redistributions of source code must retain the above copyright+      notice, this list of conditions and the following disclaimer.++    * Redistributions in binary form must reproduce the above+      copyright notice, this list of conditions and the following+      disclaimer in the documentation and/or other materials provided+      with the distribution.++    * Neither the name of Anthony Cowley nor the names of other+      contributors may be used to endorse or promote products derived+      from this software without specific prior written permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ Tests/AllTests.hs view
@@ -0,0 +1,9 @@+module Main where+import qualified MsgGen+import Test.Tasty++main :: IO ()+main = do genTests <- MsgGen.tests+          defaultMain $+            testGroup "roshask executable"+                      [ genTests ]
+ Tests/ServiceClientTests/ServiceClientTest.hs view
@@ -0,0 +1,106 @@+{-# LANGUAGE CPP #-}+module Main where+import qualified Ros.Test_srvs.AddTwoIntsRequest as Req+import qualified Ros.Test_srvs.AddTwoIntsResponse as Res+import Ros.Test_srvs.EmptyRequest+import Ros.Test_srvs.EmptyResponse+import Ros.Service (callService)+import Ros.Service.ServiceTypes+import Test.Tasty+import Test.Tasty.HUnit+import GHC.Int+import qualified Data.Int as Int+import Ros.Internal.Msg.SrvInfo+import Ros.Internal.RosBinary+import Control.Applicative ((<$>))+import Control.Exception+import Test.HUnit.Tools++-- To run:+-- 1. start ros: run "roscore"+-- 2. in another terminal start the add_two_ints server:+--      python roshask/Tests/ServiceClientTests/add_two_ints_server.py+-- 3. in a new terminal make sure $ROS_MASTER_URI is correct and run+--      cabal test servicetest --show-details=always+type Response a = IO (Either ServiceResponseExcept a)++main :: IO ()+main = defaultMain $ testGroup "Service Tests" [+  addIntsTest 4 7+  , notOkTest 100 100+  , requestResponseDontMatchTest+  , noProviderTest+  , connectionHeaderBadMD5Test+  , emptyServiceTest]+       +addIntsTest :: GHC.Int.Int64 -> GHC.Int.Int64 -> TestTree+addIntsTest x y = testCase ("add_two_ints, add " ++ show x ++ " + " ++ show y) $+  do res <- callService "/add_two_ints" Req.AddTwoIntsRequest{Req.a=x, Req.b=y} :: Response  Res.AddTwoIntsResponse+     Right (Res.AddTwoIntsResponse (x + y)) @=? res++-- add_two_ints_server returns None (triggering the NotOkError) if both a and b are 100+notOkTest :: GHC.Int.Int64 -> GHC.Int.Int64 -> TestTree+notOkTest x y = testCase ("NotOKError, add_two_ints, add " ++ show x ++ " + " ++ show y) $+  do res <- callService "/add_two_ints" Req.AddTwoIntsRequest{Req.a=x, Req.b=y} :: Response Res.AddTwoIntsResponse+     Left (NotOkExcept "service cannot process request: service handler returned None") @=? res++-- tests that an error is returned if the server is not registered with the master+noProviderTest :: TestTree+noProviderTest = testCase ("service not registered error") $+  do res <- callService "/not_add_two_ints" Req.AddTwoIntsRequest{Req.a=x, Req.b=y} :: Response Res.AddTwoIntsResponse+     Left (MasterExcept "lookupService failed, code: -1, statusMessage: no provider") @=? res+  where+    x = 10+    y = 10+++requestResponseDontMatchTest :: TestTree+requestResponseDontMatchTest =+  testGroup "check request and response" [testMd5, testName]+  where+    testMd5 = testCase ("check md5") $ do+      assertRaises "Failed to detect mismatch"+        (ErrorCall "Request and response type do not match")+        --(callService "/add_two_ints" (Req.AddTwoIntsRequest 1 1) :: IO (Either ServiceResponseExcept BadMD5))+        (callService "/add_two_ints" (Req.AddTwoIntsRequest 1 1) :: Response BadMD5)+    testName =  testCase ("check name") $ do+      assertRaises "Failed to detect mismatch"+        (ErrorCall "Request and response type do not match")+        (callService "/add_two_ints" (Req.AddTwoIntsRequest 1 1) :: IO (Either ServiceResponseExcept BadName))++connectionHeaderBadMD5Test :: TestTree+connectionHeaderBadMD5Test = testCase "connection header wrong MD5 error" $ +  do res <- callService "/add_two_ints" $ BadMD5 10 :: Response BadMD5+     Left (ConHeadExcept "Connection header from server has error, connection header is: [(\"error\",\"request from [roshask]: md5sums do not match: [6a2e34150c00229791cc89ff309fff22] vs. [6a2e34150c00229791cc89ff309fff21]\")]") @=? res++emptyServiceTest :: TestTree+emptyServiceTest =+  testCase "emptyService" $+  do res <- callService "/empty_srv" $ EmptyRequest :: Response EmptyResponse+     Right (EmptyResponse) @=? res++data BadMD5 = BadMD5 {a :: Int.Int64} deriving (Show, Eq)++instance SrvInfo BadMD5 where+  srvMD5 _ = "6a2e34150c00229791cc89ff309fff22"+  srvTypeName _ = "test_srvs/AddTwoInts"++instance RosBinary BadMD5 where+  put obj' = put (a obj')+  get = BadMD5 <$> get++data BadName = BadName Int.Int64 deriving (Show, Eq)++instance SrvInfo BadName where+  srvMD5 _ = "6a2e34150c00229791cc89ff309fff21"+  srvTypeName _ = "test_srvs/AddTwoIntu"++instance RosBinary BadName where+  put (BadName x) = put x+  get = BadName <$> get++#if MIN_VERSION_base(4,7,0)+#else+instance Eq ErrorCall where+    x == y = (show x) == (show y)+#endif
+ roshask.cabal view
@@ -0,0 +1,189 @@+Name:                roshask+Version:             0.2+Synopsis:            Haskell support for the ROS robotics framework.+License:             BSD3+License-file:        LICENSE+Cabal-version:       >=1.10+Author:              Anthony Cowley+Maintainer:          Anthony Cowley <acowley@seas.upenn.edu>+Copyright:           (c) 2010 Anthony Cowley+Category:            Robotics+Build-type:          Simple+Bug-reports:         http://github.com/acowley/roshask/issues+Homepage:            http://github.com/acowley/roshask+Tested-with:         GHC >= 7.6+Description:         Tools for working with ROS in Haskell.+                     .+                     ROS (<http://www.ros.org>) is a software+                     framework developed by Willow Garage+                     (<http://http://www.willowgarage.com/>) that aims+                     to provide a standard software architecture for+                     robotic systems. The main idea of the framework+                     is to support the development and execution of+                     loosely coupled /Node/s connected by typed+                     /Topic/s. Each Node represents a locus of+                     processing, ideally with a minimal interface+                     specified in terms of the types of Topics it+                     takes as input and offers as output.+                     .+                     This package provides libraries for creating new+                     ROS Nodes in Haskell, along with the @roshask@+                     executable for creating new ROS packages and+                     generating Haskell code from message definition+                     files (see the ROS documentation for information+                     on message types).+                     .+                     See+                     <http://github.com/acowley/roshask/wiki> for more+                     information on getting started.++Extra-source-files:  Examples/PubSub/CMakeLists.txt+                     Examples/PubSub/Makefile+                     Examples/PubSub/PubSub.cabal+                     Examples/PubSub/Setup.hs+                     Examples/PubSub/mainpage.dox+                     Examples/PubSub/manifest.xml+                     Examples/PubSub/src/Talker.hs+                     Examples/PubSub/src/Listener.hs+                     Examples/Turtle/CMakeLists.txt+                     Examples/Turtle/Makefile+                     Examples/Turtle/Turtle.cabal+                     Examples/Turtle/Setup.hs+                     Examples/Turtle/mainpage.dox+                     Examples/Turtle/manifest.xml+                     Examples/Turtle/src/*.hs++Source-repository head+  type:     git+  location: http://github.com/acowley/roshask.git++Flag logging+ Description: Enable ROS Logging support.+ Default:     True++Library+  -- Modules exported by the library.+  Exposed-modules:     Ros.Node+                       Ros.Topic+                       Ros.Topic.Util+                       Ros.Topic.PID+                       Ros.Topic.Transformers+                       Ros.Topic.Stamped+                       Ros.Rate+                       Ros.Internal.DepFinder+                       Ros.Internal.Msg.MsgInfo+                       Ros.Internal.Msg.SrvInfo+                       Ros.Internal.Msg.HeaderSupport+                       Ros.Internal.Util.BytesToVector+                       Ros.Internal.Util.StorableMonad+                       Ros.Internal.RosTime+                       Ros.Internal.RosTypes+                       Ros.Internal.RosBinary+                       Ros.Internal.SetupUtil+                       Ros.Internal.PathUtil+                       Ros.Util.PID+                       Ros.Service+                       Ros.Service.ServiceTypes++  -- The Log and Header message types must be generated by a+  -- bootstrapped roshask.+  if flag(logging)+    Exposed-modules:   Ros.Logging+    Other-modules:     Ros.Internal.Log+                       Ros.Internal.Header+                       Ros.Node.RosTcp+    Build-depends:     template-haskell++  -- Packages needed in order to build this package.+  Build-depends:       base >= 4.5 && < 6,+                       binary >= 0.7.2.1,+                       bytestring,+                       containers,+                       network >= 2.3,+                       Cabal >= 1.20,+                       stm >= 2.1.2,+                       mtl >= 2.2.1,+                       time >= 1.1,+                       BoundedChan >= 1.0.0.2,+                       parsec >= 3.1,+                       process >= 1.0.1.3,+                       SafeSemaphore,+                       snap-core >= 0.9,+                       snap-server >= 0.9,+                       storable-tuple >= 0.0.2,+                       transformers >= 0.4,+                       haxr >= 3000.10.3,+                       utf8-string >= 0.3.6,+                       uri >= 0.1.5,+                       vector-space,+                       filemanip > 0.3.6,+                       vector >= 0.10,+                       filepath > 1.1,+                       xml >= 1.3.5,+                       directory > 1.0++  if !os(windows)+    Build-depends: unix+++  GHC-Options:         -O2 -Wall+  Hs-Source-Dirs:      src+  default-language: Haskell2010+  -- Modules not exported by this package.+  Other-modules:      Ros.Graph.Master Ros.Graph.Slave Ros.Graph.ParameterServer+                      Ros.Node.Type Ros.Node.RunNode Ros.Node.BinaryIter+                      Ros.Node.ConnectionHeader Ros.Topic.Stats+                      Ros.Internal.Util.RingChan+                      Ros.Internal.Util.ArgRemapping+                      Ros.Internal.Util.AppConfig+                      Paths_roshask++-- The ROS .msg definition parser and Haskell code generation utility.+Executable roshask+  Build-Depends:  base >= 4.2 && < 6,+                  bytestring,+                  containers,+                  deepseq,+                  mtl >= 2,+                  vector >= 0.10,+                  binary >= 0.7.2.1,+                  filepath > 1.1,+                  attoparsec > 0.8,+                  directory > 1.0,+                  process >= 1.0.1.2,+                  xml >= 1.3.5,+                  pureMD5 >= 2.1,+                  filemanip > 0.3.6,+                  data-default-generics >= 0.3,+                  roshask+  default-language: Haskell2010+  GHC-Options:    -O2 -Wall+  Main-Is:        Main.hs+  Other-modules:  Analysis Gen MD5 Parse PkgInit ResolutionTypes+                  FieldImports Unregister+                  Instances.Binary Instances.Storable Instances.NFData+                  Paths_roshask+  Hs-Source-Dirs: src/executable++test-suite testexe+  type: exitcode-stdio-1.0+  hs-source-dirs: Tests, src/executable+  main-is: AllTests.hs+  ghc-options: -Wall -O0+  default-language: Haskell2010+  build-depends: base >= 4.6 && < 5, bytestring, tasty, tasty-hunit, roshask,+                 containers, mtl,+                 filepath, attoparsec, transformers,+                 pureMD5 >= 2.1++test-suite servicetest+  type: exitcode-stdio-1.0+  hs-source-dirs: Tests/ServiceClientTests+  main-is: ServiceClientTest.hs+  ghc-options: -Wall -O0+  default-language: Haskell2010+  build-depends: base >= 4.6 && < 5, bytestring, tasty, tasty-hunit, roshask,+                 containers, mtl, filepath, attoparsec, transformers,+                 pureMD5 >= 2.1,+                 data-default-generics,+                 testpack
+ src/Ros/Graph/Master.hs view
@@ -0,0 +1,94 @@+{-| Client functionality for the <http://wiki.ros.org/ROS/Master_API ROS Master API>+-}+module Ros.Graph.Master where+import Network.XmlRpc.Client+import Ros.Internal.RosTypes+import Ros.Service.ServiceTypes+import Network.XmlRpc.Internals (fromValue, toValue)+import Control.Monad.Except (ExceptT(..))+import System.IO.Error (catchIOError)+import Control.Monad.Error (runErrorT)++-- |Subscribe the caller to the specified topic. In addition to+-- receiving a list of current publishers, the subscriber will also+-- receive notifications of new publishers via the publisherUpdate+-- API. Takes the URI of the master, the caller_id, the topic name,+-- the topic type (must be a package-resource name, i.e. the .msg+-- name), and the API URI of the subscriber to register (used for new+-- publisher notifications). Returns a list of XML-RPC API URIs for+-- nodes currently publishing the specified topic.+registerSubscriber :: URI -> String -> TopicName -> TopicType -> String -> +                      IO (Int, String, [String])+registerSubscriber = flip remote "registerSubscriber"++-- |Unregister the caller as a subscriber of the topic. Takes the URI+-- of the master, the caller_id, the topic name, and the API URI of+-- the subscriber to unregister. Returns zero if the caller was not+-- registered as a subscriber.+unregisterSubscriber :: URI -> String -> TopicName -> String -> +                        IO (Int, String, Int)+unregisterSubscriber = flip remote "unregisterSubscriber"++-- |Register the caller as a publisher the topic. Takes the URI of the+-- master, the caller_id, the topic name, the topic type (must be a+-- package-resource name, i.e. the .msg name), and the API URI of the+-- publisher to register. Returns a list of the XML-RPC URIs of+-- current subscribers of the topic.+registerPublisher :: URI -> String -> TopicName -> TopicType -> String -> +                     IO (Int, String, [String])+registerPublisher = flip remote "registerPublisher"++-- |Unregister the caller as a publisher of the topic. Takes the URI of+-- the master, caller_id, the topic name, and the API URI of the+-- publisher to unregister. Returns zero if the caller was not+-- registered as a publisher.+unregisterPublisher :: URI -> String -> TopicName -> String -> +                       IO (Int, String, Int)+unregisterPublisher = flip remote "unregisterPublisher"++-- |@lookupService u s1 s2@ where @u@ is the URI of the ROS master, @s1@ is the ROS caller ID, and @s2@ is the ROS fully-qualified name of the service+--+-- In the result tuple @(code, statusMessage, serviceUrl)@, @code@ should be 1 upon a sucessful call. If @code@ is not 1 then @statusMessage@ may contain an error message. The host name and port of the service can be extracted from the @serviceUrl@+--+lookupService :: URI -> String -> ServiceName -> ExceptT ServiceResponseExcept IO (Int, String, String)+lookupService u s1 s2 = ExceptT . (flip catchIOError) handler $ do+  let res = call u "lookupService" (fmap toValue [s1, s2]) >>= fromValue+  err <- runErrorT res+  return $ case err of+    Left x -> Left $ MasterExcept $ "Could not look up service with master. Got message: " ++ x+    Right y -> Right y+  where+    handler x =  return . Left . MasterExcept $+                 "Could not look up service with master. Is the ROS master (roscore) running? Got exception: " ++ show x++-- | ROS API: registerService(caller_id, service, service_api, caller_api)+-- Register the caller as a provider of the specified service.+-- Parameters+-- caller_id (str)+-- ROS caller ID+-- service (str)+-- Fully-qualified name of service+-- service_api (str)+-- ROSRPC Service URI+-- caller_api (str)+-- XML-RPC URI of caller node+-- Returns (int, str, int)+-- (code, statusMessage, ignore)+registerService :: URI -> String -> ServiceName -> URI -> URI -> IO(Int, String, Int)+registerService = flip remote "registerService"+++-- | ROS API: unregisterService(caller_id, service, service_api)+-- Unregister the caller as a provider of the specified service.+-- Parameters+-- caller_id (str)+-- ROS caller ID+-- service (str)+-- Fully-qualified name of service+-- service_api (str)+-- API URI of service to unregister. Unregistration will only occur if current registration matches.+-- Returns (int, str, int)+-- (code, statusMessage, numUnregistered).+-- Number of unregistrations (either 0 or 1). If this is zero it means that the caller was not registered as a service provider. The call still succeeds as the intended final state is reached.+unregisterService :: URI -> String -> ServiceName -> URI -> IO(Int, String, Int)+unregisterService = flip remote "unregisterService"
+ src/Ros/Graph/ParameterServer.hs view
@@ -0,0 +1,72 @@+{-# LANGUAGE ScopedTypeVariables #-}+-- |Client interface for the ROS Parameter Server API. Note that+-- dictionary values are not supported.+module Ros.Graph.ParameterServer (deleteParam, setParam, getParam, searchParam, +                                  subscribeParam, unsubscribeParam, hasParam, +                                  getParamNames) where+import Network.XmlRpc.Client+import Network.XmlRpc.Internals (XmlRpcType)+import Ros.Internal.RosTypes++-- |Delete a parameter on the server.+deleteParam :: URI -> CallerID -> ParamName -> IO (Int,String,Int)+deleteParam = flip remote "deleteParam"++-- |Set a parameter value on the server.+setParam :: XmlRpcType a => URI -> CallerID -> ParamName -> a -> IO (Int,String,Int)+setParam = flip remote "setParam"++-- Helper for extracting a value from a tuple returned by the+-- parameter server.+handleParam :: (Int,String,a) -> IO (Maybe a)+handleParam (1,_,x) = return $ Just x+handleParam (_,_,_) = return Nothing++-- |Retrieve parameter value from server.+getParam :: XmlRpcType a => URI -> CallerID -> ParamName -> IO (Maybe a)+getParam uri caller name = handleParam =<< remote uri "getParam" caller name++-- |Search for a parameter name on the Parameter Server. The search+-- starts in the caller's namespace and proceeds upwards through+-- parent namespaces until the Parameter Server finds a matching+-- key. The first non-trivial partial match is returned.+searchParam :: URI -> CallerID -> ParamName -> IO (Maybe String)+searchParam uri caller name = handle =<< remote uri "searchParam" caller name+    where handle :: (Int, String, String) -> IO (Maybe String)+          handle (1, _, n) = return $ Just n+          handle (_, _, _) = return $ Nothing++-- |Retrieve parameter value from server and subscribe to updates to+-- that param. See paramUpdate() in the 'Ros.SlaveAPI' API.+subscribeParam :: XmlRpcType a => URI -> CallerID -> URI -> ParamName -> +                  IO (Maybe a)+subscribeParam uri caller myUri name = +    handleParam =<< remote uri "subscribeParam" caller myUri name++-- |Unsubscribe from updates to a parameter. If there is an error, the+-- status message is returned on the 'Left'; otherwise whether or not+-- the caller was previously subscribed to the parameter is returned+-- on the 'Right'.+unsubscribeParam :: URI -> CallerID -> URI -> ParamName -> IO (Either String Bool)+unsubscribeParam uri caller myUri name = +    handle =<< remote uri "unsubscribeParam" caller myUri name+    where handle :: (Int, String, Int) -> IO (Either String Bool)+          handle (1, _, 0) = return $ Right False+          handle (1, _, _) = return $ Right True+          handle (_, msg, _) = return $ Left msg++-- |Check if a parameter is stored on the server. If there is an+-- error, the status message is returned on the 'Left'; otherwise the+-- query response is returned on the 'Right'.+hasParam :: URI -> CallerID -> ParamName -> IO (Either String Bool)+hasParam uri caller name = handle =<< remote uri "hasParam" caller name+    where handle :: (Int,String,Bool) -> IO (Either String Bool)+          handle (1,_,b) = return $ Right b+          handle (_,msg,_) = return $ Left msg++-- |Get a list of all parameter names stored on this server.+getParamNames :: URI -> CallerID -> IO (Either String [String])+getParamNames uri caller = handle =<< remote uri "getParamNames" caller+    where handle :: (Int, String, [String]) -> IO (Either String [String])+          handle (1,_,names) = return $ Right names+          handle (_,msg,_) = return $ Left msg
+ src/Ros/Graph/Slave.hs view
@@ -0,0 +1,206 @@+{-# 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
+ src/Ros/Internal/DepFinder.hs view
@@ -0,0 +1,189 @@+-- Use a package's manifest.xml file to find paths to the packages on+-- which this package is dependent.+module Ros.Internal.DepFinder (findPackageDeps, findPackageDepNames, +                               findPackageDepsTrans,+                               findMessages, findMessage, findMessagesInPkg,+                               findDepsWithMessages, hasMsgsOrSrvs,+                               findServices+                              ) where+import Control.Applicative ((<$>))+import Control.Monad (when, filterM)+import Data.Maybe (mapMaybe, isNothing, fromJust)+import Data.List (find, findIndex, nub)+import System.Directory (doesFileExist, doesDirectoryExist)+import System.Environment (getEnvironment)+import System.FilePath ((</>), splitSearchPath, dropExtension, +                        takeFileName, splitPath)+import System.FilePath.Find hiding (find)+import qualified System.FilePath.Find as F+import Text.XML.Light++type Package = String++-- Find the path to a package based on the given search paths.+findPackagePath :: [FilePath] -> Package -> Maybe FilePath+findPackagePath search pkg = find ((== pkg) . last . splitPath) search++-- Get the packages listed as dependencies in an XML manifest.  NOTE:+-- In version 1.3.7, the "xml" package gained the ability to work with+-- ByteStrings via the XmlSource typeclass. Consider that upgrade if+-- performance is causing trouble.+getPackages :: String -> Maybe [Package]+getPackages = (map attrVal . +               mapMaybe (find ((==pkg).attrKey) . elAttribs) . +               findChildren dep <$>) . +              parseXMLDoc+    where pkg = QName "package" Nothing Nothing+          dep = QName "depend" Nothing Nothing++-- The catkin build system uses a @package.xml@ file that is somewhat+-- different from the older @manifest.xml@.+catkinBuildDeps :: String -> Maybe [Package]+catkinBuildDeps = fmap (map strContent . findChildren dep) . parseXMLDoc+  where dep = QName "build_depend" Nothing Nothing++-- The given path is a possible package path root, as are all of its+-- subdirectories that are stacks (indicated by the presence of a+-- stack.xml file). Returns a list of package directories.+packagePaths :: FilePath -> IO [FilePath]+packagePaths = F.find always $+               contains "manifest.xml" ||? contains "package.xml"++-- Get every package directory on the ROS search path.+getRosPaths :: IO [FilePath]+getRosPaths = +    do env <- getEnvironment+       let pPaths = case lookup "ROS_PACKAGE_PATH" env of+                      Just s -> s+                      Nothing -> error "ROS_PACKAGE_PATH not set in environment"+           -- rPath = case lookup "ROS_ROOT" env of+           --           Just s -> s+           --           Nothing -> error "ROS_ROOT not set in environment"+           allPaths = splitSearchPath pPaths+       concat <$> (mapM packagePaths =<< filterM doesDirectoryExist allPaths)++-- Packages that we will ignore for tracking down message definition+-- dependencies.+ignoredPackages :: [String]+ignoredPackages = ["genmsg_cpp", "rospack", "rosconsole", "rosbagmigration", +                   "roscpp", "rospy", "roslisp", "roslib", "boost"]++-- |Find the names of the ROS packages this package depends on as+-- indicated by the manifest.xml or package.xml file in this package's root+-- directory.+findPackageDepNames :: FilePath -> IO [String]+findPackageDepNames pkgRoot = +  let manifest = pkgRoot </> "manifest.xml"+      pkg = pkgRoot </> "package.xml"+  in do exists <- doesFileExist manifest+        existsCatkin <- doesFileExist pkg+        when (not $ exists || existsCatkin)+             (error $ "Couldn't find "++manifest++" or "++pkg)+        pkgs <- if exists+                then getPackages <$> readFile manifest+                else catkinBuildDeps <$> readFile pkg+        case pkgs of+          Nothing -> error $ "Couldn't parse package file for " ++ pkgRoot+          Just ps -> return . nub $ filter (not . (`elem` ignoredPackages)) ps++-- |Returns 'True' if the ROS package at the given 'FilePath' defines+-- any messages or services+hasMsgsOrSrvs :: FilePath -> IO Bool+hasMsgsOrSrvs = fmap (not . null) . F.find (depth <? 2) (extension ==? ".msg" ||? extension ==? ".srv")++{-+-- |Returns 'True' if the ROS package at the given 'FilePath' is a+-- roshask package (determined by the presence of a @.cabal@ file in+-- the package's root directory).+isRoshask :: FilePath -> IO Bool+isRoshask pkgPath = not . null . filter ((== ".cabal") . takeExtension) <$> +                    getDirectoryContents pkgPath+-}++-- |Find the names of the ROS packages the package at the given+-- 'FilePath' depends on as indicated by its @manifest.xml@ file. Only+-- those packages that define messages or services are returned.+findDepsWithMessages :: FilePath -> IO [String]+findDepsWithMessages pkgRoot = +  do names <- findPackageDepNames pkgRoot+     searchPaths <- getRosPaths+     filterM (maybe (return False) hasMsgsOrSrvs . findPackagePath searchPaths) names++-- |Find the paths to the packages this package depends on as+-- indicated by the manifest.xml file in this package's root+-- directory.+findPackageDeps :: FilePath -> IO [FilePath]+findPackageDeps pkgRoot = +    do pkgs <- findPackageDepNames pkgRoot+       searchPaths <- getRosPaths+       let pkgPaths = map (findPackagePath searchPaths) pkgs+       case findIndex isNothing pkgPaths of+         Just i -> putStrLn ("Looking for "++show pkgs+++                             ", dependencies of"++pkgRoot) >>+                   error ("Couldn't find path to package (1) " ++ (pkgs !! i))+         Nothing -> return $ map fromJust pkgPaths++-- |Transitive closure of 'findPackageDeps'. Find the paths to the+-- packages this package depends on as indicated by the manifest.xml+-- file in this package's root directories and the manifests in the+-- root directories of the dependencies, and so on.+findPackageDepsTrans :: FilePath -> IO [FilePath]+findPackageDepsTrans pkgRoot =+  -- searchPaths is a list of all directories on the ROS package path that have+  -- either a manifest.xml(rosbuild), or a package.xml(new replacement for manifest.xml)+  do searchPaths <- getRosPaths+     let getDeps pkg = +           do pkgDeps <- findPackageDepNames pkg+              let pkgPaths = map (findPackagePath searchPaths) pkgDeps+              case findIndex isNothing pkgPaths of+                Just i -> putStrLn ("Looking for "++show pkgDeps+++                                    ", dependencies of "++pkgRoot) >>+                          error ("Couldn't find path to package (2) " ++ +                                 (pkgDeps !! i))+                Nothing -> return $ map fromJust pkgPaths+         recurse p = do deps <- getDeps p+                        nub . (++[p]) . concat <$> mapM recurse deps+     init <$> recurse pkgRoot+     +-- |Return the full path to every .msg file in the given package+-- directory.+findMessages :: FilePath -> IO [FilePath]+findMessages pkgRoot = +  do e <- doesDirectoryExist dir+     if e then F.find (depth <? 1) (extension ==? ".msg") dir else return []+  where dir = pkgRoot </> "msg"+        +--TODO: refactor with findMessages+findServices :: FilePath -> IO [FilePath]+findServices pkgRoot =+  do e <- doesDirectoryExist dir+     if e then F.find (depth <? 1) (extension ==? ".srv") dir else return []+  where dir = pkgRoot </> "srv"++-- |Find all message definition files in a ROS package. Returns the+-- 'FilePath' to the package, and the 'FilePath' to each message+-- definition in the package.+findMessagesInPkg :: String -> IO (FilePath, [FilePath])+findMessagesInPkg pkgName = do searchPaths <- getRosPaths+                               let pkgPath = maybe err id $ +                                             findPackagePath searchPaths pkgName+                               msgs <- findMessages pkgPath+                               return (pkgPath, msgs)+    where err = error $ "Couldn't find path to package (3) " ++ pkgName++-- |Find the path to the message definition (.msg) file for a message+-- type with the given name. The first argument is a home package used+-- to resolve unqualified message names. The second argument is the+-- name of the message type either in the form "pkgName/typeName" or+-- "typeName". The specified package will be searched for using the+-- search paths indicated in the current environment (ROS_PACKAGE_PATH+-- and ROS_ROOT).+findMessage :: String -> String -> IO (Maybe FilePath)+findMessage pkg msgType = +    do searchPaths <- getRosPaths+       let pkgPath = findPackagePath searchPaths pkg+       case pkgPath of+         Just p -> find isMsg <$> findMessages p+         Nothing -> putStrLn ("Looking for "++pkg++"."++msgType) >>+                    error ("Couldn't find path to package " ++ pkg)+    where isMsg = (== msgType) . dropExtension . takeFileName
+ src/Ros/Internal/Header.hs view
@@ -0,0 +1,22 @@+{-# LANGUAGE OverloadedStrings, DeriveDataTypeable #-}+module Ros.Internal.Header where+import qualified Prelude as P+import qualified Data.Typeable as T+import Control.Applicative+import Ros.Internal.RosBinary+import Ros.Internal.Msg.MsgInfo+import Ros.Internal.RosTypes+import qualified Data.Word as Word++data Header = Header { seq :: Word.Word32+                     , stamp :: ROSTime+                     , frame_id :: P.String+                     } deriving (P.Show, P.Eq, P.Ord, T.Typeable)++instance RosBinary Header where+  put obj' = put (seq obj') *> put (stamp obj') *> put (frame_id obj')+  get = Header <$> get <*> get <*> get++instance MsgInfo Header where+  sourceMD5 _ = "2176decaecbce78abc3b96ef049fabed"+  msgTypeName _ = "std_msgs/Header"
+ src/Ros/Internal/Log.hs view
@@ -0,0 +1,51 @@+{-# LANGUAGE OverloadedStrings, DeriveDataTypeable #-}+module Ros.Internal.Log where+import qualified Prelude as P+import Prelude ((.))+import qualified Data.Typeable as T+import Control.Applicative+import Ros.Internal.RosBinary+import Ros.Internal.Msg.MsgInfo+import Ros.Internal.Msg.HeaderSupport+import qualified Data.Word as Word+import qualified Ros.Internal.Header as Header++data Log = Log { header   :: Header.Header+               , level    :: Word.Word8+               , name     :: P.String+               , msg      :: P.String+               , file     :: P.String+               , function :: P.String+               , line     :: Word.Word32+               , topics   :: [P.String]+               } deriving (P.Show, P.Eq, P.Ord, T.Typeable)++instance RosBinary Log where+  put obj' = put (header obj') *> put (level obj') *> put (name obj') *> put (msg obj') *> put (file obj') *> put (function obj') *> put (line obj') *> putList (topics obj')+  get = Log <$> get <*> get <*> get <*> get <*> get <*> get <*> get <*> getList+  putMsg = putStampedMsg++instance HasHeader Log where+  getSequence = Header.seq . header+  getFrame = Header.frame_id . header+  getStamp = Header.stamp . header+  setSequence seq x' = x' { header = (header x') { Header.seq = seq } }++instance MsgInfo Log where+  sourceMD5 _ = "acffd30cd6b6de30f120938c17c593fb"+  msgTypeName _ = "rosgraph_msgs/Log"++dEBUG :: Word.Word8+dEBUG = 1++iNFO :: Word.Word8+iNFO = 2++wARN :: Word.Word8+wARN = 4++eRROR :: Word.Word8+eRROR = 8++fATAL :: Word.Word8+fATAL = 16
+ src/Ros/Internal/Msg/HeaderSupport.hs view
@@ -0,0 +1,18 @@+-- |If a message type's first field is of type Header, its sequence+-- number is automatically incremented by the ROS Topic machinery.+module Ros.Internal.Msg.HeaderSupport where+import Data.Binary (Put)+import Data.Word (Word32)+import Ros.Internal.RosBinary (RosBinary, put)+import Ros.Internal.RosTypes (ROSTime)++class HasHeader a where+    getSequence :: a -> Word32+    getFrame    :: a -> String+    getStamp    :: a -> ROSTime+    setSequence :: Word32 -> a -> a++-- |Serialize a message after setting the sequence number in its+-- header.+putStampedMsg :: (HasHeader a, RosBinary a) => Word32 -> a -> Put+putStampedMsg n v = put $ setSequence n v
+ src/Ros/Internal/Msg/MsgInfo.hs view
@@ -0,0 +1,5 @@+module Ros.Internal.Msg.MsgInfo where++class MsgInfo a where+    sourceMD5 :: a -> String+    msgTypeName :: a -> String
+ src/Ros/Internal/Msg/SrvInfo.hs view
@@ -0,0 +1,5 @@+module Ros.Internal.Msg.SrvInfo where++class SrvInfo a where+    srvMD5 :: a -> String+    srvTypeName :: a -> String
+ src/Ros/Internal/PathUtil.hs view
@@ -0,0 +1,47 @@+module Ros.Internal.PathUtil where+import Data.Char (toUpper)+import Data.List (tails)+import System.Directory (doesFileExist)+import System.FilePath+import Paths_roshask++-- |Ensure that the first character in a String is capitalized.+cap :: String -> String+cap [] = []+cap (x:xs) = toUpper x : xs++-- |Determine if a path is a directory containing a ROS package.+isPkg :: FilePath -> IO Bool+isPkg = doesFileExist . (</> "manifest.xml")++-- |Determine if a path is a directory containing a ROS stack.+isStack :: FilePath -> IO Bool+isStack = doesFileExist . (</> "stack.xml")++-- |Identify the name of the package defining a msg.+pathToPkgName :: FilePath -> String+pathToPkgName p | hasExtension p = cap . last . init . splitPath $ p+                | otherwise = cap . last . splitPath $ p++-- |Identify the name of the stack in which a msg is defined. If the+-- package definining the message does not live in a stack, the result+-- is 'Nothing'.+stackName :: FilePath -> IO (Maybe String)+stackName = go . tails . reverse . splitPath+  where go :: [[FilePath]] -> IO (Maybe String)+        go [] = return Nothing+        go [[]] = return Nothing+        go (d:ds) = do b <- isStack . joinPath . reverse $ d+                       if b then return (Just (head d)) else go ds++-- |Given a path to a msg definition file, compute a destination+-- directory for generated Haskell code. A typical path will be under,+-- @~/.cabal/share/roshask/@.+codeGenDir :: FilePath -> IO FilePath+codeGenDir f = do s <- stackName f+                  r <- getDataDir+                  let base = case s of+                               Nothing -> r +                               Just s' -> r </> s'+                  return $ base </> pkg </> "Ros" </> pkg+  where pkg = pathToPkgName f
+ src/Ros/Internal/RosBinary.hs view
@@ -0,0 +1,140 @@+{-# LANGUAGE ScopedTypeVariables, TypeSynonymInstances, FlexibleInstances #-}+-- |Binary serialization/deserialization utilities for types used in+-- ROS messages. This module is used by generated code for .msg types.+-- NOTE: The native byte ordering of the host is used to support the+-- common scenario of same-machine transport.+module Ros.Internal.RosBinary where+import Control.Applicative ((<$>), (<*>))+import Control.Monad (replicateM)+import Data.Binary.Get+import Data.Binary.Put+import Data.Int+import qualified Data.Vector.Storable as V+import Data.Word+import Unsafe.Coerce (unsafeCoerce)++import qualified Data.ByteString as B+import qualified Data.ByteString.Char8 as BC8+import Foreign.Storable (sizeOf, Storable)++import Ros.Internal.RosTypes+import Ros.Internal.Util.BytesToVector++-- |A type class for binary serialization of ROS messages. Very like+-- the standard Data.Binary type class, but with different, more+-- compact, instances for base types and an extra class method for+-- dealing with message headers.+class RosBinary a where+    -- |Serialize a value to a ByteString.+    put :: a -> Put+    -- |Deserialize a value from a ByteString.+    get :: Get a+    -- |Serialize a ROS message given a sequence number. This number+    -- may be used by message types with headers. The default+    -- implementation ignores the sequence number.+    putMsg :: Word32 -> a -> Put+    putMsg _ = put++instance RosBinary Bool where+    put True = putWord8 1+    put False = putWord8 0+    get = (> 0) <$> getWord8++instance RosBinary Int8 where+    put = putWord8 . fromIntegral+    get = fromIntegral <$> getWord8++instance RosBinary Word8 where+    put = putWord8+    get = getWord8++instance RosBinary Int16 where+    put = putWord16host . fromIntegral+    get = fromIntegral <$> getWord16host++instance RosBinary Word16 where+    put = putWord16host+    get = getWord16host++instance RosBinary Int where+    put = putWord32host . fromIntegral+    get = fromIntegral <$> getWord32host++instance RosBinary Word32 where+    put = putWord32host+    get = getWord32host++instance RosBinary Int64 where+    put = putWord64host . fromIntegral+    get = fromIntegral <$> getWord64host++instance RosBinary Word64 where+    put = putWord64host+    get = getWord64host++instance RosBinary Float where+    put = putWord32le . unsafeCoerce+    get = unsafeCoerce <$> getWord32le++instance RosBinary Double where+    put = putWord64le . unsafeCoerce+    get = unsafeCoerce <$> getWord64le++getAscii :: Get Char+getAscii = toEnum . fromEnum <$> getWord8++putAscii :: Char -> Put+putAscii = putWord8 . toEnum . fromEnum++putUnit :: Put+putUnit = putWord8 0++getUnit :: Get ()+getUnit = getWord8 >> return ()++instance RosBinary String where+    put s = let s' = BC8.pack s+            in putInt32 (BC8.length s') >> putByteString s'+    get = getInt32 >>= (BC8.unpack <$>) . getByteString++instance RosBinary B.ByteString where+    put b = putInt32 (B.length b) >> putByteString b+    get = getInt32 >>= getByteString++instance RosBinary ROSTime where+    put (s,n) = putWord32host s >> putWord32host n+    get = (,) <$> getWord32host <*> getWord32host++putList :: RosBinary a => [a] -> Put+putList xs = putInt32 (length xs) >> mapM_ put xs++getList :: RosBinary a => Get [a]+getList = getInt32 >>= flip replicateM get++putFixedList :: RosBinary a => [a] -> Put+putFixedList = mapM_ put++getFixedList :: RosBinary a => Int -> Get [a]+getFixedList = flip replicateM get++{-+instance RosBinary ROSDuration where+    put (s,n) = putWord32host s >> putWord32host n+    get =  (,) <$> getWord32host <*> getWord32host+-}++getInt32 :: Get Int+getInt32 = fromIntegral <$> getWord32le++putInt32 :: Int -> Put +putInt32 = putWord32le . fromIntegral++instance (RosBinary a, Storable a) => RosBinary (V.Vector a) where+    put v = putInt32 (V.length v) >> putByteString (vectorToBytes v)+    get = getInt32 >>= getFixed++getFixed :: forall a. Storable a => Int -> Get (V.Vector a)+getFixed n = bytesToVector n <$> getByteString (n*(sizeOf (undefined::a)))++putFixed :: (Storable a, RosBinary a) => V.Vector a -> Put+putFixed = putByteString . vectorToBytes
+ src/Ros/Internal/RosTime.hs view
@@ -0,0 +1,48 @@+-- |Utilities for working with ROS time values.+module Ros.Internal.RosTime (ROSTime, ROSDuration, toROSTime, fromROSTime, +                             diffROSTime, getROSTime, diffSeconds) where+import Data.Time.Clock (UTCTime, NominalDiffTime)+import Data.Time.Clock.POSIX+import Data.Word (Word32)+import Ros.Internal.RosTypes++toROSTime :: UTCTime -> ROSTime+toROSTime = aux . properFraction . utcTimeToPOSIXSeconds+  where aux (s,f) = (s, truncate $ f * 1000000)++-- |Class of types that may be derived from a 'ROSTime'.+class FromROSTime a where+  fromROSTime :: ROSTime -> a++instance FromROSTime UTCTime where+  fromROSTime = posixSecondsToUTCTime . aux . fromROSTime+    where aux = realToFrac :: Double -> NominalDiffTime++-- |Convert a 'ROSTime' to the POSIX number of seconds since epoch.+instance FromROSTime Double where+  fromROSTime (s,ns) = s' + ns'+    where s' = fromIntegral s             :: Double+          ns' = fromIntegral ns / 1000000 :: Double++-- |@timeDiff t1 t2@ computes the difference @t1 - t2@.+diffROSTime :: ROSTime -> ROSTime -> ROSDuration+diffROSTime (s1,ns1) (s2,ns2)+  | dns >= 0 = (fi ds, fi dns)+  | otherwise = (fi $ ds - 1, fi $ 1000000 + dns)+  where dns = fw ns1 - fw ns2+        ds = fw s1 - fw s2+        fw :: Word32 -> Int+        fw = fromIntegral+        fi :: Int -> Word32+        fi = fromIntegral++-- |Get the current POSIX time.+getROSTime :: IO ROSTime+getROSTime = fmap (aux . properFraction) getPOSIXTime+  where aux (s,f) = (s, truncate $ f * 1000000)++-- |Compute the difference in seconds between two 'ROSTime'+-- values. The application @diffSeconds tStop tStart@ computes the+-- time interval @tStop - tStart@.+diffSeconds :: ROSTime -> ROSTime -> Double+diffSeconds t1 t2 = fromROSTime $ diffROSTime t1 t2
+ src/Ros/Internal/RosTypes.hs view
@@ -0,0 +1,21 @@+-- |Utility types for working with ROS.+module Ros.Internal.RosTypes (ROSTime, ROSDuration, URI, CallerID, TopicName, +                              NodeName, ParamName, TopicType, +                              ConnectionID, ServiceName) where+import Data.Word (Word32)+import Foreign.Storable.Tuple ()++type URI          = String+type CallerID     = String+type TopicName    = String+type NodeName     = String+type ParamName    = String+type TopicType    = String+type ConnectionID = Int+type ServiceName  = String++-- |ROSTime is a tuple of (seconds, nanoseconds)+type ROSTime = (Word32, Word32)++-- |ROSDuration is a tuple of (seconds, nanoseconds)+type ROSDuration = (Word32, Word32)
+ src/Ros/Internal/SetupUtil.hs view
@@ -0,0 +1,59 @@+{-# LANGUAGE TupleSections #-}+-- |Integration with the Cabal build system.+module Ros.Internal.SetupUtil (rosBuild, rosConf) where+import Control.Applicative+import Data.List (intercalate)+import Distribution.Simple+import Distribution.Simple.InstallDirs+import Distribution.Simple.LocalBuildInfo+import Distribution.Simple.Setup+import Distribution.PackageDescription hiding (Library)+import System.Directory (getCurrentDirectory)+import System.FilePath ((</>))+import Ros.Internal.DepFinder++data Buildable = LibraryAndExecutables [String] +               | Executables [String]++-- Add the message directories of the package's we are dependent on to+-- GHC's path.+addRosMsgPaths :: Buildable -> IO HookedBuildInfo+addRosMsgPaths targets = +    do dir <- getCurrentDirectory+       msgPaths <- map (</>"msg"</>"haskell") <$> findPackageDeps dir+       writeFile ".ghci" $ ":set -i"++ intercalate ":" msgPaths+       let binfo = emptyBuildInfo { hsSourceDirs = msgPaths }+       case targets of+         LibraryAndExecutables exes -> return (Just binfo, map (,binfo) exes)+         Executables exes -> return (Nothing, map (,binfo) exes)++-- |The @buildHook@ override integrates a new 'HookedBuildInfo' with+-- the 'PackageDescription' in order to include additional directories+-- (e.g. those with message type definitions) in the 'hsSourceDirs'+-- field.+rosBuild :: PackageDescription -> LocalBuildInfo -> UserHooks -> +            BuildFlags -> IO ()+rosBuild pkg lbi uh bfs = do binfo <- addRosMsgPaths targets+                             let pkg' = updatePackageDescription binfo pkg+                             (buildHook simpleUserHooks) pkg' lbi uh bfs+    where exeTargets = map exeName $ executables pkg+          targets = case library pkg of+                      Nothing -> Executables exeTargets+                      Just _ -> LibraryAndExecutables exeTargets++-- The @confHook@ override takes over @cabal install@'s @--bindir@ and+-- @--libdir@ options to force binary outputs into the @bin@ and @lib@+-- subdirectories of the package directory.++-- |The @confHook@ override takes over @cabal install@'s @--bindir@+-- option to force binary outputs into the @bin@ subdirectory of the+-- package directory.+rosConf :: (GenericPackageDescription, HookedBuildInfo) -> ConfigFlags -> +           IO LocalBuildInfo+rosConf x cf = do lbi <- (confHook simpleUserHooks) x cf+                  let oldDirs = installDirTemplates lbi+                      customDirs = oldDirs { bindir = toPathTemplate "bin" }+                                           -- , libdir = toPathTemplate "lib" }+                      lbi' = lbi { installDirTemplates = customDirs }+                  return lbi'+
+ src/Ros/Internal/Util/AppConfig.hs view
@@ -0,0 +1,28 @@+-- |Support for read-only executable application configurations.+module Ros.Internal.Util.AppConfig where+import Control.Monad.Reader+import Control.Concurrent++data ConfigOptions = ConfigOptions { verbosity :: Int }++type Config = ReaderT ConfigOptions IO++getVerbosity :: Config Int+getVerbosity = verbosity `fmap` ask++debug :: String -> Config ()+debug s = do v <- getVerbosity+             when (v > 0) (liftIO (putStrLn s))++forkConfig :: Config () -> Config ThreadId+forkConfig c = do r <- ask+                  liftIO . forkIO $ runReaderT c r++parseAppConfig :: [String] -> (ConfigOptions, [String])+parseAppConfig args+  | "-v" `elem` args = (ConfigOptions 1, filter (`notElem` appOpts) args)+  | otherwise = (ConfigOptions 0, args)+  where appOpts = ["-v"]++configured :: Config a -> Config (IO a)+configured c = ask >>= return . runReaderT c
+ src/Ros/Internal/Util/ArgRemapping.hs view
@@ -0,0 +1,133 @@+{-# LANGUAGE TypeSynonymInstances, FlexibleInstances #-}+-- |Parses the ROS argument remapping syntax. This is the language used+-- to remap names and assign private parameter values from a command+-- line invocation of a Node.+module Ros.Internal.Util.ArgRemapping (parseRemappings, FromParam(..), +                                       ParamVal) where+import Control.Applicative ((<$>))+import Control.Monad.Identity (Identity)+import Data.Either (partitionEithers, lefts, rights)+import Text.Parsec.Language (emptyDef)+import Text.Parsec.Token+import Text.Parsec (letter, char, alphaNum, (<|>))+import Text.Parsec.Combinator (choice)+import Text.Parsec.Prim (Parsec, runParser)++-- |The types of values supported as parameters.+data ParamVal = PInt Int+              | PBool Bool+              | PString String+              | PDouble Double+              | PList [ParamVal]+              | PUnknown+                deriving Show++-- |Mechanism to extract a Haskell value from a parsed parameter+-- value.+class FromParam a where+    fromParam :: ParamVal -> a++instance FromParam Int where +    fromParam (PInt x) = x+    fromParam x = error $ "Parameter is not an Int: " ++ show x++instance FromParam Bool where +    fromParam (PBool x) = x+    fromParam x = error $ "Parameter is not a Bool: " ++ show x++instance FromParam String where +    fromParam (PString x) = x+    fromParam x = error $ "Parameter is not a String: " ++ show x++instance FromParam Double where +    fromParam (PDouble x) = x+    fromParam x = error $ "Parameter is not a Double: " ++ show x++-- NOTE: We have to provide specific instances for different Lists in+-- order to avoid overlapping with the String instance.+instance FromParam [Int] where +    fromParam (PList xs) = map fromParam xs+    fromParam x = error $ "Parameter is not a List: " ++ show x++instance FromParam [Bool] where+    fromParam (PList xs) = map fromParam xs+    fromParam x = error $ "Parameter is not a List: " ++ show x++instance FromParam [Double] where+    fromParam (PList xs) = map fromParam xs+    fromParam x = error $ "Parameter is not a List: " ++ show x+++-- | Name rebindings are typically used to remap Topic names, but can+-- also be used to remap a parameter name. +type Names = [(String, String)]++-- | Parameter rebindings are used to assign values to private+-- parameters by prefixing the parameter name with an underscore. The+-- special keyword @__name@ can be used to remap the node name.+type Params = [(String, ParamVal)]++-- The ROS argument remapping syntax.+lexer :: GenTokenParser String u Identity+lexer = makeTokenParser $ +        emptyDef { reservedNames = ["true", "false"] +                 , identStart = letter <|> char '_' <|> char '/'+                 , identLetter = alphaNum <|> char '_' <|> char '/' }++data Sign = Positive | Negative++-- Parse an optional numeric sign character.+sign :: Parsec String () Sign+sign =     (char '-' >> return Negative) +       <|> (char '+' >> return Positive) +       <|> return Positive++applySign :: Num a => Sign -> a -> a+applySign Positive = id+applySign Negative = negate++-- Parses optionally signed integer or floating point numbers.+intOrFloat' :: Parsec String () (Either Int Double)+intOrFloat' = do s <- sign+                 num <- naturalOrFloat lexer+                 case num of+                   Left x -> return . Left $ applySign s (fromIntegral x)+                   Right x -> return . Right $ applySign s x++parseVal :: Parsec String () ParamVal+parseVal = choice [ either PInt PDouble <$> intOrFloat'+                  , const (PBool True) <$> reserved lexer "true"+                  , const (PBool False) <$> reserved lexer "false" +                  , PString <$> (stringLiteral lexer <|> identifier lexer)+                  , PList <$> brackets lexer (commaSep lexer parseVal) ]++parseBinding :: Parsec String () (Either (String, String) (String, ParamVal))+parseBinding = do name <- identifier lexer +                  _ <- symbol lexer ":="+                  case head name of+                    '_' -> do val <- parseVal+                              return $ Right (name, val)+                    _ -> do val <- identifier lexer+                            return $ Left (name, val)++-- |Parse program arguments to determine name remappings and parameter+-- settings.+parseRemappings :: [String] -> (Names, Params)+parseRemappings args = if null errors+                       then partitionEithers (rights remaps)+                       else error $ "Couldn't parse remapping "++ show errors+    where remaps = map (runParser parseBinding () "") args+          errors = lefts remaps++-- parseRemappings :: String -> (Names, Params)+-- parseRemappings args = case runParser (many1 parseBinding) () "" args of+--                          Left m -> error (show m)+--                          Right bindings -> partitionEithers bindings+{-+test :: (Names, Params)+test = parseRemappings [ "_joe:=42.1"+                       , "chatter:=/foo"+                       , "_susy:=[1,2,3]"+                       , "__name:=\"toby\"" +                       , "_topic:=bar" ]+-}
+ src/Ros/Internal/Util/BytesToVector.hs view
@@ -0,0 +1,58 @@+{-# LANGUAGE ScopedTypeVariables #-}+module Ros.Internal.Util.BytesToVector (unsafeBytesToVector, bytesToVectorL, +                                        bytesToVector, unsafeVectorToBytes, +                                        vectorToBytes) where+import qualified Data.ByteString.Lazy as BL+import qualified Data.ByteString as BS+import qualified Data.ByteString.Unsafe as BU+import qualified Data.Vector.Storable as V+import Foreign.Ptr (castPtr, plusPtr)+import Foreign.ForeignPtr (newForeignPtr_, withForeignPtr, +                           mallocForeignPtrBytes)+import Foreign.Marshal.Utils (copyBytes)+import Foreign.Storable (sizeOf)+import System.IO.Unsafe (unsafePerformIO)++-- |Construct a 'V.Vector' with the specified number of elements from+-- a strict 'BS.ByteString' without copying.+unsafeBytesToVector :: V.Storable a => Int -> BS.ByteString -> V.Vector a+unsafeBytesToVector n bs = unsafePerformIO $+                           BU.unsafeUseAsCStringLen bs $+                           (\(p,_) -> do fp <- newForeignPtr_ (castPtr p)+                                         return $ V.unsafeFromForeignPtr fp 0 n)++-- |Construct a 'V.Vector' with the specified number of elements from+-- a strict 'BS.ByteString' making a copy of the underlying data in+-- the process.+bytesToVector :: V.Storable a => Int -> BS.ByteString -> V.Vector a+bytesToVector n bs = unsafePerformIO $+                     BU.unsafeUseAsCStringLen bs $+                     (\(p,len) -> do fp <- mallocForeignPtrBytes len+                                     withForeignPtr fp $+                                       \dst -> copyBytes (castPtr dst) p len+                                     return $ V.unsafeFromForeignPtr fp 0 n)++-- |Construct a 'V.Vector' with the specified number of elements from+-- a lazy 'BL.ByteString'.+bytesToVectorL :: V.Storable a => Int -> BL.ByteString -> V.Vector a+bytesToVectorL n = bytesToVector n . BS.concat . BL.toChunks++-- |Construct a strict 'BS.ByteString' from a 'V.Vector' without+-- copying the underlying data.+unsafeVectorToBytes :: forall a. V.Storable a => V.Vector a -> BS.ByteString+unsafeVectorToBytes v = +    unsafePerformIO $+    withForeignPtr fp (\p -> let p' = castPtr $ plusPtr p offset+                             in BU.unsafePackCStringLen (p', len*sz))+    where (fp,offset,len) = V.unsafeToForeignPtr v+          sz = sizeOf (undefined::a)++-- |Construct a strict 'BS.ByteString' from a copy of the data+-- underlying a 'V.Vector'.+vectorToBytes :: forall a. V.Storable a => V.Vector a-> BS.ByteString+vectorToBytes v = +    unsafePerformIO $+    withForeignPtr fp (\p -> let p' = castPtr $ plusPtr p offset+                             in BS.packCStringLen (p', len*sz))+    where (fp,offset,len) = V.unsafeToForeignPtr v+          sz = sizeOf (undefined::a)
+ src/Ros/Internal/Util/RingChan.hs view
@@ -0,0 +1,54 @@+module Ros.Internal.Util.RingChan (RingChan, newRingChan, writeChan, +                                   readChan, getChanContents, +                                   getBuffered) where+import Control.Monad (join)+import Control.Concurrent.MVar+import Control.Concurrent.SSem (SSem)+import qualified Control.Concurrent.SSem as Sem+import qualified Data.Foldable as F+import Data.Sequence (Seq, (|>), viewl, ViewL(..))+import qualified Data.Sequence as Seq+import System.IO.Unsafe (unsafeInterleaveIO)++-- import Control.Concurrent.BoundedChan hiding writeChan++-- type RingChan = BoundedChan+-- newRingChan = newBoundedChan+++-- |A 'RingChan' is an 'MVar' containing a triple of maximum capacity, a+-- semaphore used to indicate that the chan has gone from empty to+-- non-empty, and a sequence of items.+type RingChan a = (Int, SSem, MVar (Seq a))++-- |Create a 'RingChan' with the specified maximum capacity.+newRingChan :: Int -> IO (RingChan a)+newRingChan n = do sem <- Sem.new 0+                   q <- newMVar Seq.empty+                   return (n,sem,q)++-- |If the chan is full, does nothing. NOTE: An alternative would be+-- to drop the oldest element before adding the new one.+writeChan :: RingChan a -> a -> IO ()+writeChan (n,sem,mv) x = +    join $ modifyMVar mv (\q -> if Seq.length q < n+                                then return (q |> x, Sem.signal sem)+                                else let _ :< t = viewl q+                                     in return (t |> x, return ()))+                                -- else return (q, return ()))++-- |Read an item from the channel. Blocks until an item is available.+readChan :: RingChan a -> IO a+readChan (_,sem,mv) = do Sem.wait sem+                         modifyMVar mv (\q -> let h :< t = viewl q+                                              in return (t,h))++getBuffered :: RingChan a -> IO [a]+getBuffered (_,_,xs) = F.toList `fmap` readMVar xs++-- |Return the channel's contents as a lazily realized list.+getChanContents :: RingChan a -> IO [a]+getChanContents c = unsafeInterleaveIO $ do+                      x <- readChan c+                      xs <- getChanContents c+                      return (x:xs)
+ src/Ros/Internal/Util/StorableMonad.hs view
@@ -0,0 +1,31 @@+-- |An applicative interface for working with Storable values. The idea+-- is that the underlying pointer is threaded through the computation+-- to make reading and writing consecutive values easier.+module Ros.Internal.Util.StorableMonad (peek, poke, runStorable, +                                        StorableM) where+import Control.Monad.State.Strict+import Foreign.Ptr+import Foreign.Storable hiding (peek, poke)+import qualified Foreign.Storable as S++-- |A state monad that threads a pointer through a computation.+type StorableM a = StateT (Ptr ()) IO a++-- |Action that pokes a value into the current pointer location, then+-- moves the pointer to just after the poked value.+poke :: Storable a => a -> StorableM ()+poke x = do ptr <- get+            liftIO $ S.poke (castPtr ptr) x+            put (plusPtr ptr (sizeOf x))++-- |Action that peeks a value from the current pointer location, then+-- moves the pointer to just after the peeked value.+peek :: Storable a => StorableM a+peek = do ptr <- get+          x <- liftIO $ S.peek (castPtr ptr)+          put (plusPtr ptr (sizeOf x))+          return x++-- |Run a StorableM action with the supplied initial pointer location.+runStorable :: StorableM a -> Ptr b -> IO a+runStorable s p = evalStateT s (castPtr p)
+ src/Ros/Logging.hs view
@@ -0,0 +1,86 @@+{-# LANGUAGE TemplateHaskell #-}+-- |Support for publishing log messages at various severity+-- levels. The log messages are annotated with the filename and line+-- number where they are generated.+module Ros.Logging (Log, LogLevel(..), enableLogging,+                    logDebug, logWarn, logInfo, logError, logFatal) where+import Control.Concurrent.Chan+import Control.Monad (when)+import Data.IORef+import Data.Word (Word8)+import Language.Haskell.TH+import System.IO.Unsafe++import Ros.Internal.Log (Log(Log))+import qualified Ros.Internal.Log as Log+import Ros.Internal.Header+import Ros.Node+import Ros.Topic.Util (fromList)++emptyHeader :: Header+emptyHeader = Header 0 (0,0) ""++mkLogMsg :: Word8 -> String -> Q Exp+mkLogMsg level msg = do Loc fname _ _ start _ <- location+                        let (line, _char) = start+                            litS = return . LitE . StringL+                            litI :: Integral a => a -> Q Exp+                            litI = return . LitE . IntegerL . fromIntegral+                        [|sendMsg (Log emptyHeader $(litI level) "" $(litS msg) +                                       $(litS fname) "" $(litI line) [])|]++-- |Template Haskell functions to splice in a 'Log' value. Usage: +-- +-- > $(logDebug "This is my message to you")+logDebug, logWarn, logInfo, logError, logFatal :: String -> Q Exp+logDebug = mkLogMsg Log.dEBUG+logInfo  = mkLogMsg Log.iNFO+logWarn  = mkLogMsg Log.wARN+logError = mkLogMsg Log.eRROR+logFatal = mkLogMsg Log.fATAL++-- The 'Chan' into which all log messages are funneled. This Chan's+-- contents are fed into the /rosout 'Topic'.+rosOutChan :: Chan Log+rosOutChan = unsafePerformIO $ newChan+{-# NOINLINE rosOutChan #-}++-- Stash a function that knows whether or not 'Log' messages of+-- various levels should be printed to stdout.+showLevel :: IORef (Log -> IO ())+showLevel = unsafePerformIO $ newIORef (const (return ()))+{-# NOINLINE showLevel #-}++-- Stash the node name away so that it can be inserted into runtime+-- log messages.+nodeName :: IORef String+nodeName = unsafePerformIO $ newIORef ""+{-# NOINLINE nodeName #-}++-- Publish a log message.+sendMsg :: Log -> IO ()+sendMsg msg = do n <- readIORef nodeName+                 let msg' = msg { Log.name = n }+                 ($ msg') =<< readIORef showLevel+                 writeChan rosOutChan msg'++-- Prints messages whose level is greater than or equal to the+-- specified level.+printLog :: LogLevel -> Log -> IO ()+printLog lvl = let code = 2 ^ fromEnum lvl+               in \msg -> when (Log.level msg >= code) (putStrLn (show msg))++-- |Log message levels. These allow for simple filtering of messages.+data LogLevel = Debug | Info | Warn | Error | Fatal deriving (Eq, Enum)++-- |Enable logging for this node. The argument indicates the level of+-- log messages that should be echoed to standard out. If 'Nothing',+-- then no messages are printed; if 'Just lvl', then all messages of+-- greater than or equal level are printed.+enableLogging :: Maybe LogLevel -> Node ()+enableLogging ll = do xs <- liftIO $ getChanContents rosOutChan+                      liftIO $ maybe (return ())+                                     (writeIORef showLevel . printLog)+                                     ll+                      liftIO . writeIORef nodeName =<< getName+                      advertise "/rosout" (fromList xs)
+ src/Ros/Node.hs view
@@ -0,0 +1,233 @@+{-# LANGUAGE ScopedTypeVariables, ExistentialQuantification #-}+-- |The primary entrypoint to the ROS client library portion of+-- roshask. This module defines the actions used to configure a ROS+-- Node.+module Ros.Node (Node, runNode, advertise, advertiseBuffered, +                 subscribe, getShutdownAction, runHandler, getParam, +                 getParamOpt, getName, getNamespace, +                 module Ros.Internal.RosTypes, Topic(..), topicRate, +                 module Ros.Internal.RosTime, liftIO) where+import Control.Applicative ((<$>))+import Control.Concurrent (newEmptyMVar, readMVar, putMVar)+import Control.Concurrent.BoundedChan+import Control.Concurrent.STM (newTVarIO)+import Control.Monad (when)+import Control.Monad.State (liftIO, get, put, execStateT)+import Control.Monad.Reader (ask, asks, runReaderT)+import qualified Data.Map as M+import qualified Data.Set as S+import Control.Concurrent (forkIO, ThreadId)+import Data.Dynamic+import System.Environment (getEnvironment, getArgs)+import Network.XmlRpc.Internals (XmlRpcType)++import Ros.Internal.Msg.MsgInfo+import Ros.Internal.RosBinary (RosBinary)+import Ros.Internal.RosTypes+import Ros.Internal.RosTime+import Ros.Internal.Util.AppConfig (Config, parseAppConfig, forkConfig, configured)+import Ros.Internal.Util.ArgRemapping+import Ros.Node.Type+import qualified Ros.Graph.ParameterServer as P+import Ros.Node.RosTcp (subStream, runServer)+import qualified Ros.Node.RunNode as RN+import Ros.Topic+import Ros.Topic.Stats (recvMessageStat, sendMessageStat)+import Ros.Topic.Util (topicRate, share)++-- |Maximum number of items to buffer for a subscriber.+recvBufferSize :: Int+recvBufferSize = 10++-- |Spark a thread that funnels a Stream from a URI into the given+-- Chan.+addSource :: (RosBinary a, MsgInfo a) => +             String -> (URI -> Int -> IO ()) -> BoundedChan a -> URI -> +             Config ThreadId+addSource tname updateStats c uri = +    forkConfig $ subStream uri tname (updateStats uri) >>= +                 liftIO . forever . join . fmap (writeChan c)++-- Create a new Subscription value that will act as a named input+-- channel with zero or more connected publishers.+mkSub :: forall a. (RosBinary a, MsgInfo a) => +         String -> Config (Topic IO a, Subscription)+mkSub tname = do c <- liftIO $ newBoundedChan recvBufferSize+                 let stream = Topic $ do x <- readChan c+                                         return (x, stream)+                 known <- liftIO $ newTVarIO S.empty+                 stats <- liftIO $ newTVarIO M.empty+                 r <- ask+                 let topicType = msgTypeName (undefined::a)+                     updateStats = recvMessageStat stats+                     addSource' = flip runReaderT r . addSource tname updateStats c+                     sub = Subscription known addSource' topicType stats+                 return (stream, sub)++mkPub :: forall a. (RosBinary a, MsgInfo a, Typeable a) => +         Topic IO a -> Int -> Config Publication+mkPub t n = do t' <- liftIO $ share t+               mkPubAux (msgTypeName (undefined::a)) t' (runServer t') n++mkPubAux :: Typeable a => +            String -> Topic IO a -> +            ((URI -> Int -> IO ()) -> Int -> Config (Config (), Int)) ->+            Int -> Config Publication+mkPubAux trep t runServer' bufferSize = +    do stats <- liftIO $ newTVarIO M.empty+       (cleanup, port) <- runServer' (sendMessageStat stats) bufferSize+       known <- liftIO $ newTVarIO S.empty+       cleanup' <- configured cleanup+       return $ Publication known trep port cleanup' (DynTopic t) stats++-- |Subscribe to the given Topic. Returns a 'Ros.TopicUtil.share'd 'Topic'.+subscribe :: (RosBinary a, MsgInfo a, Typeable a) => +             TopicName -> Node (Topic IO a)+subscribe name = do n <- get+                    name' <- canonicalizeName =<< remapName name+                    r <- nodeAppConfig <$> ask+                    let subs = subscriptions n+                    when (M.member name' subs) +                         (error $ "Already subscribed to "++name')+                    let pubs = publications n+                    if M.member name' pubs+                      then return . fromDynErr . pubTopic $ pubs M.! name'+                      else do (stream, sub) <- liftIO $+                                               runReaderT (mkSub name') r+                              put n { subscriptions = M.insert name' sub subs }+                              --return stream+                              liftIO $ share stream+  where fromDynErr = maybe (error msg) id . fromDynTopic+        msg = "Subscription to "++name++" at a different type than "+++              "what that Topic was already advertised at by this Node."++-- |Spin up a thread within a Node. This is typically used for message+-- handlers. Note that the supplied 'Topic' is traversed solely for+-- any side effects of its steps; the produced values are ignored.+runHandler :: (a -> IO b) -> Topic IO a -> Node ThreadId+runHandler = ((liftIO . forkIO . forever . join) .) . fmap++advertiseAux :: (Int -> Config Publication) -> Int -> TopicName -> Node ()+advertiseAux mkPub' bufferSize name = +    do n <- get+       name' <- remapName =<< canonicalizeName name+       r <- nodeAppConfig <$> ask+       let pubs = publications n+       if M.member name' pubs+         then error $ "Already advertised " ++ name'+         else do pub <- liftIO $ runReaderT (mkPub' bufferSize) r+                 put n { publications = M.insert name' pub pubs }++-- |Advertise a 'Topic' publishing a stream of 'IO' values with a+-- per-client transmit buffer of the specified size.+advertiseBuffered :: (RosBinary a, MsgInfo a, Typeable a) => +                     Int -> TopicName -> Topic IO a -> Node ()+advertiseBuffered bufferSize name s = advertiseAux (mkPub s) bufferSize name++-- |Advertise a 'Topic' publishing a stream of values produced in+-- the 'IO' monad.+advertise :: (RosBinary a, MsgInfo a, Typeable a) => +             TopicName -> Topic IO a -> Node ()+advertise = advertiseBuffered 1++-- -- |Existentially quantified message type that roshask can+-- -- serialize. This type provides a way to work with collections of+-- -- differently typed 'Topic's.+-- data SomeMsg = forall a. (RosBinary a, MsgInfo a, Typeable a) => SomeMsg a++-- -- |Advertise projections of a 'Topic' as discrete 'Topic's.+-- advertiseSplit :: [(TopicName, a -> SomeMsg)] -> Topic IO a -> Node ()+-- advertiseSplit = undefined++-- |Get an action that will shutdown this Node.+getShutdownAction :: Node (IO ())+getShutdownAction = get >>= liftIO . readMVar . signalShutdown++-- |Apply any matching renames to a given name.+remapName :: String -> Node String+remapName name = asks (maybe name id . lookup name . nodeRemaps)++-- |Convert relative names to absolute names. Leaves absolute names+-- unchanged.+canonicalizeName :: String -> Node String+canonicalizeName n@('/':_) = return n+canonicalizeName ('~':n) = do state <- get+                              let node = nodeName state+                              return $ node ++ "/" ++ n+canonicalizeName n = do (++n) . namespace <$> get++-- |Get a parameter value from the Parameter Server.+getServerParam :: XmlRpcType a => String -> Node (Maybe a)+getServerParam var = do state <- get+                        let masterUri = master state+                            myName = nodeName state+                        -- Call hasParam first because getParam only returns +                        -- a partial result (just the return code) in failure.+                        hasParam <- liftIO $ P.hasParam masterUri myName var+                        case hasParam of+                          Right True -> liftIO $ P.getParam masterUri myName var+                          _ -> return Nothing++-- |Get the value associated with the given parameter name. If the+-- parameter is not set, then 'Nothing' is returned; if the parameter+-- is set to @x@, then @Just x@ is returned.+getParamOpt :: (XmlRpcType a, FromParam a) => String -> Node (Maybe a)+getParamOpt var = do var' <- remapName =<< canonicalizeName var+                     params <- nodeParams <$> ask+                     case lookup var' params of+                       Just val -> return . Just $ fromParam val+                       Nothing -> getServerParam var'++-- |Get the value associated with the given parameter name. If the+-- parameter is not set, return the second argument as the default+-- value.+getParam :: (XmlRpcType a, FromParam a) => String -> a -> Node a+getParam var def = maybe def id <$> getParamOpt var++-- |Get the current node's name.+getName :: Node String+getName = nodeName <$> get++-- |Get the current namespace.+getNamespace :: Node String+getNamespace = namespace <$> get++-- |Run a ROS Node.+runNode :: NodeName -> Node a -> IO ()+runNode name (Node nConf) = +    do myURI <- newEmptyMVar+       sigStop <- newEmptyMVar+       env <- liftIO getEnvironment+       (conf, args) <- parseAppConfig <$> liftIO getArgs+       let getConfig' var def = maybe def id $ lookup var env+           getConfig = flip lookup env+           masterConf = getConfig' "ROS_MASTER_URI" "http://localhost:11311"+           namespaceConf = let ns = getConfig' "ROS_NAMESPACE" "/"+                           in if last ns == '/' then ns else ns ++ "/"+           (nameMap, params) = parseRemappings args+           name' = case lookup "__name" params of+                     Just x -> fromParam x+                     Nothing -> case name of+                                  '/':_ -> name+                                  _ -> namespaceConf ++ name+           -- Name remappings apply to exact strings and resolved names.+           resolve p@(('/':_),_) = [p]+           resolve (('_':n),v) = [(name'++"/"++n, v)]+           resolve (('~':n),v) = [(name'++"/"++ n, v)] --, ('_':n,v)]+           resolve (n,v) = [(namespaceConf ++ n,v), (n,v)]+           nameMap' = concatMap resolve nameMap+           params' = concatMap resolve params+       when (not $ null nameMap')+            (putStrLn $ "Remapping name(s) "++show nameMap')+       when (not $ null params') +            (putStrLn $ "Setting parameter(s) "++show params')+       case getConfig "ROS_IP" of+         Nothing -> case getConfig "ROS_HOSTNAME" of+                      Nothing -> return ()+                      Just n -> putMVar myURI $! "http://"++n+         Just ip -> putMVar myURI $! "http://"++ip+       let configuredNode = runReaderT nConf (NodeConfig params' nameMap' conf)+           initialState = NodeState name' namespaceConf masterConf myURI+                                    sigStop M.empty M.empty+           statefulNode = execStateT configuredNode initialState+       statefulNode >>= flip runReaderT conf . RN.runNode name'
+ src/Ros/Node/BinaryIter.hs view
@@ -0,0 +1,64 @@+-- |Binary iteratee-style serialization helpers for working with ROS+-- message types. This module is used by the automatically-generated+-- code for ROS .msg types.+module Ros.Node.BinaryIter (streamIn, getServiceResult) where+import Control.Applicative+import Control.Concurrent (myThreadId, killThread)+import Control.Monad.IO.Class+import Control.Monad.Trans.Maybe+import Data.Binary.Get+import qualified Data.ByteString as BS+import qualified Data.ByteString.Lazy as BL+import System.IO (Handle)+import Ros.Topic+import Ros.Internal.RosBinary (RosBinary(get))+import Ros.Service.ServiceTypes(ServiceResponseExcept(..))+import Data.ByteString.Lazy.Char8 (unpack)+import Control.Monad.Except (ExceptT(..), throwError)++-- Get the specified number of bytes from a 'Handle'. Returns a+-- wrapped-up 'Nothing' if the client shutdown (indicated by receiving+-- a message of zero length).+hGetAll :: Handle -> Int -> MaybeT IO BL.ByteString+hGetAll h n = go n []+    where go 0 acc = return . BL.fromChunks $ reverse acc+          go n' acc = do bs <- liftIO $ BS.hGet h n'+                         case BS.length bs of+                           0 -> MaybeT $ return Nothing+                           x -> go (n' - x) (bs:acc)++-- |The function that does the work of streaming members of the+-- 'RosBinary' class in from a 'Handle'.+streamIn :: RosBinary a => Handle -> Topic IO a+streamIn h = Topic go +  where go = do item <- runMaybeT $ do len <- runGet getInt <$> hGetAll h 4+                                       runGet get <$> hGetAll h len+                case item of+                  Nothing -> putStrLn "Publisher stopped" >>+                             myThreadId >>= killThread >>+                             return undefined+                  Just item' -> return (item', Topic go)++getInt :: Get Int+getInt = fromIntegral <$> getWord32le++-- | Get the result back from a service call (called by the service client)+-- (see http://wiki.ros.org/ROS/TCPROS)+getServiceResult :: RosBinary a => Handle ->  ExceptT ServiceResponseExcept IO a+getServiceResult h = do+  okByte <- runGet getWord8 <$> hGetAllET h 1 (ResponseReadExcept "Could not read okByte")+  case okByte of+    0 -> do+      len <- runGet getInt <$> hGetAllET h 4 (ResponseReadExcept "Could not read length for notOk message")+      message <- hGetAllET h len (ResponseReadExcept "Could not read notOk message")+      throwError . NotOkExcept $ unpack message+    _ -> do+      len <- runGet getInt <$> hGetAllET h 4 (ResponseReadExcept "Could not read length")+      runGet get <$> hGetAllET h len (ResponseReadExcept "Could not read response message")+  +hGetAllET ::  Handle -> Int -> ServiceResponseExcept -> ExceptT ServiceResponseExcept IO BL.ByteString+hGetAllET h n exceptMessage = do+  maybeData <- liftIO . runMaybeT $ hGetAll h n+  case maybeData of+    Nothing -> throwError exceptMessage+    Just b -> ExceptT . return $ Right b
+ src/Ros/Node/ConnectionHeader.hs view
@@ -0,0 +1,52 @@+-- |The ROS connection header contains important metadata about a+-- connection being established, including typing information and+-- routing information. How it is exchanged depends on the ROS+-- transport being used.+module Ros.Node.ConnectionHeader (genHeader, ConnHeader(..), parseHeader) where+import Control.Applicative ((<$>))+import Control.Arrow (second, (***))+import Data.Binary.Get (getWord32le, Get, getByteString, runGetIncremental)+import qualified Data.Binary.Get as G+import Data.Binary.Put (runPut, putWord32le)+import Data.ByteString.Char8 (ByteString, pack, unpack)+import qualified Data.ByteString.Char8 as B+import qualified Data.ByteString.Lazy as BL++-- |Wrapper for the Connection Header type that is a list of key-value+-- pairs.+newtype ConnHeader = ConnHeader [(String,String)]++-- |Serialize a list of key-value pairs using the ROS Connection+-- Header protocol.+genHeader :: [(String,String)] -> ByteString+genHeader = tagLength . +            B.concat . +            map (tagLength . uncurry B.append . +                 second (B.cons '=') . (pack *** pack))++-- Prefix a ByteString with its length encoded as a 4-byte little+-- endian integer.+tagLength :: ByteString -> ByteString+tagLength x = let len = runPut $ putWord32le (fromIntegral (B.length x))+              in B.append (BL.toStrict len) x++getInt :: Get Int+getInt = fromIntegral <$> getWord32le++-- Each entry in the header is a 4-byte little endian integer denoting+-- the length of the entry, the field name string, an equals sign, and+-- the field value string.+parsePair :: Get (String,String)+parsePair = do len <- fromIntegral <$> getInt+               field <- getByteString len+               case B.elemIndex '=' field of+                 Just i -> return . (unpack *** unpack . B.tail) . B.splitAt i $+                           field+                 Nothing -> error "Didn't find '=' in connection header field"++-- Keep parsing header entries until we run out of bytes.+parseHeader :: ByteString -> [(String, String)]+parseHeader bs | B.null bs = []+               | otherwise = let G.Partial k = runGetIncremental parsePair+                                 G.Done rst _ p = k (Just bs)+                             in p : parseHeader rst
+ src/Ros/Node/RosTcp.hs view
@@ -0,0 +1,362 @@+{-# LANGUAGE ScopedTypeVariables, BangPatterns #-}+module Ros.Node.RosTcp (subStream, runServer, runServers, callServiceWithMaster) where+import Control.Applicative ((<$>))+import Control.Arrow (first)+import Control.Concurrent (forkIO, killThread, newEmptyMVar, takeMVar, putMVar)+import Control.Concurrent.STM (atomically)+import Control.Concurrent.STM.TVar+import qualified Control.Exception as E+import Control.Monad.Reader+import Data.Binary.Put (runPut, putWord32le)+import Data.Binary.Get (runGet, getWord32le)+import Data.ByteString.Lazy (ByteString)+import qualified Data.ByteString as B+import qualified Data.ByteString.Lazy as BL++import Network.BSD (getHostByName, hostAddress)+import Network.Socket hiding (send, sendTo, recv, recvFrom, Stream, ServiceName)+import qualified Network.Socket as Sock+import Network.Socket.ByteString+import Prelude hiding (getContents)++import System.IO (IOMode(ReadMode), hClose)+import Text.URI (parseURI, uriRegName, uriPort)++import Ros.Node.BinaryIter (streamIn, getServiceResult)+import Ros.Internal.Msg.MsgInfo+import Ros.Internal.Msg.SrvInfo+import Ros.Internal.RosBinary+import Ros.Internal.RosTypes+import Ros.Internal.Util.RingChan+import Ros.Internal.Util.AppConfig (Config, debug, forkConfig)+import Ros.Topic (Topic(..))+import Ros.Node.ConnectionHeader+import Ros.Graph.Slave (requestTopicClient)+import Ros.Graph.Master (lookupService)+import Data.Maybe (fromMaybe)+import Ros.Service.ServiceTypes+import Control.Monad.Except+import System.IO.Error (tryIOError)++-- |Push each item from this client's buffer over the connected+-- socket.+serviceClient :: RingChan ByteString -> Socket -> IO ()+serviceClient c s = forever $ do bs <- readChan c+                                 sendBS s bs++sendBS :: Socket -> ByteString -> IO ()+sendBS sock bs =+  let len = runPut $ +            putWord32le . fromIntegral $ +            BL.length bs+  in+   sendAll sock (BL.toStrict $ BL.append len bs)++recvAll :: Socket -> Int -> IO B.ByteString+recvAll s = flip go []+    where go len acc = do bs <- recv s len+                          if B.length bs < len+                            then go (len - B.length bs) (bs:acc)+                            else return $ B.concat (reverse (bs:acc))++negotiatePub :: String -> String -> Socket -> IO ()+negotiatePub ttype md5 sock = +    do headerLength <- runGet (fromIntegral <$> getWord32le) <$>+                       BL.fromChunks . (:[]) <$> recvAll sock 4+       headerBytes <- recvAll sock headerLength+       let connHeader = parseHeader headerBytes+           wildCard = case lookup "type" connHeader of+                        Just t | t == "*" -> True+                               | t == ttype -> False+                               | otherwise -> error $ +                                              "Disagreeing Topic types: " +++                                              "publisher expected "++ttype+++                                              ", but client asked for "++t+                        Nothing -> error $ "Client did not include the "+++                                           "topic type in its "+++                                           "connection request."+       when (not wildCard) +            (case lookup "md5sum" connHeader of+               Just s | s == md5 -> return ()+                      | otherwise -> error "Disagreement on Topic type MD5"+               Nothing -> error $ "Client did not include MD5 sum "+++                                  "in its request.")+       case lookup "tcp_nodelay" connHeader of+         Just "1" -> setSocketOption sock NoDelay 0+         _ -> return ()+       sendAll sock . genHeader $+         [("md5sum",md5), ("type",ttype), ("callerid","roshask")]++-- |Accept new client connections. A new send buffer is allocated for+-- each new client and added to the client list along with an action+-- for cleaning up the client connection.+-- FIXME: cleaning up a disconnected client should be reflected at a+-- higher level, too.+acceptClients :: Socket -> TVar [(Config (), RingChan ByteString)] -> +                 (Socket -> IO ()) -> IO (RingChan ByteString) -> Config ()+acceptClients sock clients negotiate mkBuffer = forever acceptClient+    where acceptClient = do (client,_) <- liftIO $ accept sock+                            debug "Accepted client socket"+                            liftIO $ negotiate client+                            chan <- liftIO mkBuffer+                            let cleanup1 = +                                    do debug "Closing client socket"+                                       liftIO $ +                                         shutdown client ShutdownBoth `E.catch`+                                           \(_::E.SomeException) -> return ()+                            r <- ask+                            t <- liftIO . forkIO $ +                                 serviceClient chan client `E.catch` +                                   \(_::E.SomeException) -> runReaderT cleanup1 r+                            let cleanup2 = cleanup1 >>+                                           (liftIO $ killThread t)+                            liftIO . atomically $ +                              readTVar clients >>= +                              writeTVar clients . ((cleanup2,chan) :)++-- |Publish each item obtained from a 'Topic' to each connected client.+pubStream :: RosBinary a+          => Topic IO a -> TVar [(b, RingChan ByteString)] -> Config ()+pubStream t0 clients = liftIO $ go 0 t0+  where go !n t = do (x, t') <- runTopic t+                     let bytes = runPut $ putMsg n x+                     cs <- readTVarIO clients+                     mapM_ (flip writeChan bytes . snd) cs+                     go (n+1) t'++-- |Produce a publishing action associated with a list of+-- clients. This is used by runServers.+pubStreamIO :: RosBinary a => IO (TVar [(b, RingChan ByteString)] -> Config (), +                                  a -> IO ())+pubStreamIO = do m <- newEmptyMVar+                 let feed clients = +                       let go !n = do x <- takeMVar m+                                      let bytes = runPut $ putMsg n x+                                      cs <- readTVarIO clients+                                      mapM_ (flip writeChan bytes . snd) cs+                                      go (n+1)+                       in liftIO $ go 0+                 return (feed, putMVar m)++-- Negotiate a TCPROS subscriber connection.+-- Precondition: The socket is connected+negotiateSub :: Socket -> String -> String -> String -> IO ()+negotiateSub sock tname ttype md5 = +    do sendAll sock $ genHeader [ ("callerid", "roshask"), ("topic", tname)+                                , ("md5sum", md5), ("type", ttype) +                                , ("tcp_nodelay", "1") ]+       responseLength <- runGet (fromIntegral <$> getWord32le) <$>+                         BL.fromChunks . (:[]) <$> recvAll sock 4+       headerBytes <- recvAll sock responseLength+       let connHeader = parseHeader headerBytes+       case lookup "type" connHeader of+         Just t | t == ttype -> return ()+                | otherwise -> error $ "Disagreeing Topic types: " +++                                       "subscriber expected "++ttype+++                                       ", but server replied with "++t+         Nothing -> error $ "Server did not include the topic type "+++                            "in its response."+       case lookup "md5sum" connHeader of+         Just s | s == md5 -> return ()+                | otherwise -> error "Disagreement on Topic type MD5"+         Nothing -> error "Server did not include MD5 sum in its response."+       setSocketOption sock KeepAlive 1++-- |Connect to a publisher and return the stream of data it is+-- publishing.+subStream :: forall a. (RosBinary a, MsgInfo a) => +             URI -> String -> (Int -> IO ()) -> Config (Topic IO a)+subStream target tname _updateStats = +    do debug $ "Opening stream to " ++target++" for "++tname+       h <- liftIO $ +            do response <- requestTopicClient target "/roshask" tname +                                              [["TCPROS"]]+               let port = case response of+                            (1,_,("TCPROS",_,port')) -> fromIntegral port'+                            _ -> error $ "Couldn't get publisher's port for "+++                                         tname++" from node "++target+               sock <- socket AF_INET Sock.Stream defaultProtocol+               ip <- hostAddress <$> getHostByName host+               connect sock $ SockAddrInet port ip+               let md5 = sourceMD5 (undefined::a)+                   ttype = msgTypeName (undefined::a)+               negotiateSub sock tname ttype md5+               socketToHandle sock ReadMode+       --hSetBuffering h NoBuffering+       debug $ "Streaming "++tname++" from "++target+       return $ streamIn h+    where host = parseHost target++parseHost :: URI -> String+parseHost target = case parseURI target of+  Just u -> fromMaybe+            (error $ "Couldn't parse hostname "++ "from "++target)+            (uriRegName u)+  Nothing -> error $ "Couldn't parse URI "++target++parseHostAndPort :: URI -> Either ServiceResponseExcept (String, PortNumber)+parseHostAndPort target = do+  uri <- maybeToEither (ConnectExcept $ "Could not parse URI "++target) $ parseURI target+  host <- maybeToEither (ConnectExcept $ "Could not parse hostname from "++target) $ uriRegName uri+  port <- maybeToEither (ConnectExcept $ "Could not parse port from "++target) $ uriPort uri+  return (host, fromIntegral port)++maybeToEither :: a -> Maybe b -> Either a b+maybeToEither left m = case m of+  Just x -> Right x+  Nothing -> Left left++--TODO: use the correct callerID+callServiceWithMaster :: forall a b. (RosBinary a, SrvInfo a, RosBinary b, SrvInfo b) =>+                         URI -> ServiceName -> a -> IO (Either ServiceResponseExcept b)+callServiceWithMaster rosMaster serviceName message = runExceptT $ do+  checkServicesMatch message (undefined::b)+  --lookup the service with the master+  (code, statusMessage, serviceUrl) <- lookupService rosMaster callerID serviceName+  checkLookupServiceCode code statusMessage+  (host, port) <- ExceptT . return $ parseHostAndPort serviceUrl+  -- make a socket+  let makeSocket = socket AF_INET Sock.Stream defaultProtocol+      closeSocket sock = liftIO $ sClose sock+      withSocket sock = do+        ioErrorToExceptT ConnectExcept "Problem connecting to server. Got exception : " $ do+          --Connect to the socket+          ip <- hostAddress <$> getHostByName host+          connect sock $ SockAddrInet port ip+        let reqMd5 = srvMD5 message+            reqServiceType = srvTypeName message+        negotiateService sock serviceName reqServiceType reqMd5+        let bytes = runPut $ putMsg 0 message+        ioErrorToExceptT SendRequestExcept "Problem sending request. Got exception: " $+          sendBS sock bytes+        liftIO $ socketToHandle sock ReadMode+  handle <- bracketOnErrorME (liftIO makeSocket) closeSocket withSocket+  result <- getServiceResult handle+  liftIO $ hClose handle+  return result+    where+      callerID = "roshask"+      checkLookupServiceCode 1 _ = return ()+      checkLookupServiceCode code statusMessage =+        throwError $ MasterExcept+        ("lookupService failed, code: " ++ show code ++ ", statusMessage: " ++ statusMessage)+      checkServicesMatch x y =+        unless match $+          -- throw error here since the calling code needs to be changed+          error "Request and response type do not match"+        where+          match = srvMD5 x == srvMD5 y && srvTypeName x == srvTypeName y++-- | bracketOnError equivalent for MonadError+bracketOnErrorME :: MonadError e m => m a -> (a -> m b) -> (a -> m c) -> m c+bracketOnErrorME before after thing = do+  a <- before+  let handler e = after a >> throwError e+  catchError (thing a) handler++-- | Catch any IOErrors and convert them to a different type+catchConvertIO :: (String -> a) -> IO b -> IO (Either a b)+catchConvertIO excep action = do+  err <- tryIOError action+  return $ case err of+    Left e -> Left . excep $ show e+    Right r -> Right r++-- | Catch all IOErrors that might occur and convert them to a custom error type+-- with the IOError message postpended to the given string+ioErrorToExceptT :: (String -> e) -> String -> IO a -> ExceptT e IO a+ioErrorToExceptT except msg acc =+  ExceptT . catchConvertIO (\m -> except $ msg ++ m) $ acc++-- Precondition: The socket is already connected to the server+-- Exchange ROSTCP connection headers with the server+negotiateService :: Socket -> String -> String -> String -> ExceptT ServiceResponseExcept IO ()+negotiateService sock serviceName serviceType md5 = do+    headerBytes <- liftIO $+      do sendAll sock $ genHeader [ ("callerid", "roshask"), ("service", serviceName)+                                , ("md5sum", md5), ("type", serviceType) ]+         responseLength <- runGet (fromIntegral <$> getWord32le) <$>+                           BL.fromChunks . (:[]) <$> recvAll sock 4+         recvAll sock responseLength+    let connHeader = parseHeader headerBytes+    case lookup "error" connHeader of+      Nothing -> return ()+      Just _ -> throwError . ConHeadExcept $+                "Connection header from server has error, connection header is: " ++ show connHeader+                                    +-- Helper to run the publisher's side of a topic negotiation with a+-- new client.+mkPubNegotiator :: MsgInfo a => a -> Socket -> IO ()+mkPubNegotiator x = negotiatePub (msgTypeName x) (sourceMD5 x)++-- Run a publication server given a function that returns a+-- negotiation action given a client 'Socket', a function that returns+-- a publication action given a client list, a statistics updater, and+-- the size of the send buffer.+runServerAux :: (Socket -> IO ()) -> +                (TVar [(Config (), RingChan ByteString)] -> Config ()) -> +                (URI -> Int -> IO ()) -> Int -> Config (Config (), Int)+runServerAux negotiate pubAction _updateStats bufferSize = +    do r <- ask+       liftIO . withSocketsDo $ runReaderT go r+  where go = do sock <- liftIO $ socket AF_INET Sock.Stream defaultProtocol+                liftIO $ bindSocket sock (SockAddrInet aNY_PORT iNADDR_ANY)+                port <- liftIO (fromInteger . toInteger <$> socketPort sock)+                liftIO $ listen sock 5+                clients <- liftIO $ newTVarIO []+                let mkBuffer = newRingChan bufferSize+                acceptThread <- forkConfig $+                                acceptClients sock clients negotiate mkBuffer+                pubThread <- forkConfig $ pubAction clients+                let cleanup = liftIO (atomically (readTVar clients)) >>= +                              sequence_ . map fst >> +                              liftIO (shutdown sock ShutdownBoth >>+                                      killThread acceptThread >>+                                      killThread pubThread)+                return (cleanup, port)++-- |The server starts a thread that peels elements off the stream as+-- they become available and sends them to all connected+-- clients. Returns an action for cleaning up resources allocated by+-- this publication server along with the port the server is listening+-- on.+runServer :: forall a. (RosBinary a, MsgInfo a) => +             Topic IO a -> (URI -> Int -> IO ()) -> Int -> +             Config (Config (), Int)+runServer stream = runServerAux (mkPubNegotiator (undefined::a)) +                                (pubStream stream)++-- |The 'MsgInfo' type class dictionary made explicit to strip off the+-- actual message type.+data MsgInfoRcd = MsgInfoRcd { _md5, _typeName :: String }++-- |A 'Feeder' represents a 'Topic' fully prepared to accept+-- subscribers.+data Feeder = Feeder MsgInfoRcd -- Explicit MsgInfo dictionary+                     Int -- Transmit buffer size+                     (URI -> Int -> IO ()) -- Update topic stats+                     (TVar [(Config (), RingChan ByteString)] -> Config ())+                     -- 'pubStream' partial application++-- |Prepare an action for publishing messages. Arguments are a monadic+-- function for updating topic statistics, and a transmit buffer+-- size. The returned 'Feeder' value may be supplied to 'runServers',+-- while the returned 'IO' function may be used to push out new+-- messages.+feedTopic :: forall a. (MsgInfo a, RosBinary a) => +             (URI -> Int -> IO ()) -> Int -> IO (Feeder, a -> IO ())+feedTopic updateStats bufSize = +  do (feed,pub) <- pubStreamIO+     let f = Feeder info bufSize updateStats feed+     return (f, pub)+  where info = mkInfo (undefined::a)+        mkInfo x = MsgInfoRcd (msgTypeName x) (sourceMD5 x)++-- |Publish several 'Topic's. A single cleanup action for all 'Topic's+-- is returned, along with each 'Topic's server port in the order of+-- the input 'Feeder's.+runServers :: [Feeder] -> Config (Config (), [Int])+runServers = return . first sequence_ . unzip <=< mapM feed+  where feed (Feeder (MsgInfoRcd md5 typeName) bufSize stats push) = +          let pub = negotiatePub typeName md5+          in runServerAux pub push stats bufSize
+ src/Ros/Node/RunNode.hs view
@@ -0,0 +1,62 @@+module Ros.Node.RunNode (runNode) where+import Control.Concurrent (readMVar,forkIO, killThread)+import qualified Control.Concurrent.SSem as Sem+import qualified Control.Exception as E+import Control.Monad.IO.Class+import System.Posix.Signals (installHandler, Handler(..), sigINT)+import Ros.Internal.RosTypes+import Ros.Internal.Util.AppConfig (Config, debug)+import Ros.Graph.Master+import Ros.Graph.Slave++-- Inform the master that we are publishing a particular topic.+registerPublication :: RosSlave n => +                       String -> n -> String -> String -> +                       (TopicName, TopicType, a) -> Config ()+registerPublication name _n master uri (tname, ttype, _) = +    do debug $ "Registering publication of "++ttype++" on topic "+++               tname++" on master "++master+       _subscribers <- liftIO $ registerPublisher master name tname ttype uri+       return ()++-- Inform the master that we are subscribing to a particular topic.+registerSubscription :: RosSlave n =>+                        String -> n -> String -> String -> +                        (TopicName, TopicType, a) -> Config ()+registerSubscription name n master uri (tname, ttype, _) = +    do debug $ "Registring subscription to "++tname++" for "++ttype+       (r,_,publishers) <- liftIO $ registerSubscriber master name tname ttype uri+       if r == 1 +         then liftIO $ publisherUpdate n tname publishers+         else error "Failed to register subscriber with master"+       return ()++registerNode :: RosSlave s => String -> s -> Config ()+registerNode name n = +    do uri <- liftIO $ readMVar (getNodeURI n)+       let master = getMaster n+       debug $ "Starting node "++name++" at " ++ uri+       liftIO (getPublications n) >>= +         mapM_ (registerPublication name n master uri)+       liftIO (getSubscriptions n) >>= +         mapM_ (registerSubscription name n master uri)++-- |Run a ROS Node with the given name. Returns when the Node has+-- shutdown either by receiving an interrupt signal (e.g. Ctrl-C) or+-- because the master told it to stop.+runNode :: RosSlave s => String -> s -> Config ()+runNode name s = do (wait, _port) <- liftIO $ runSlave s+                    registerNode name s+                    debug "Spinning"+                    allDone <- liftIO $ Sem.new 0+                    let ignoreEx :: E.SomeException -> IO ()+                        ignoreEx _ = return ()+                        shutdown = do putStrLn "Shutting down"+                                      cleanupNode s `E.catch` ignoreEx+                                      Sem.signal allDone+                    liftIO $ setShutdownAction s shutdown+                    _ <- liftIO $ +                         installHandler sigINT (CatchOnce shutdown) Nothing+                    t <- liftIO . forkIO $ wait >> Sem.signal allDone+                    liftIO $ Sem.wait allDone+                    liftIO $ killThread t `E.catch` ignoreEx
+ src/Ros/Node/Type.hs view
@@ -0,0 +1,110 @@+{-# LANGUAGE MultiParamTypeClasses, FlexibleInstances, GADTs,+             ExistentialQuantification, GeneralizedNewtypeDeriving #-}+module Ros.Node.Type where+import Control.Applicative (Applicative(..), (<$>))+import Control.Concurrent (MVar, putMVar)+import Control.Concurrent.STM (atomically, TVar, readTVar, writeTVar)+import Control.Monad.State+import Control.Monad.Reader+import Data.Dynamic+import Data.Map (Map)+import qualified Data.Map as M+import Data.Set (Set)+import qualified Data.Set as S+import Control.Concurrent (ThreadId)+import Ros.Internal.RosTypes (URI)+import Ros.Internal.Util.ArgRemapping (ParamVal)+import Ros.Internal.Util.AppConfig (ConfigOptions)+import Ros.Graph.Slave (RosSlave(..))+import Ros.Topic (Topic)+import Ros.Topic.Stats++data Subscription = Subscription { knownPubs :: TVar (Set URI)+                                 , addPub    :: URI -> IO ThreadId+                                 , subType   :: String+                                 , subStats  :: StatMap SubStats }++data DynTopic where+  DynTopic :: Typeable a => Topic IO a -> DynTopic++fromDynTopic :: Typeable a => DynTopic -> Maybe (Topic IO a)+fromDynTopic (DynTopic t) = gcast t++data Publication = Publication { subscribers :: TVar (Set URI)+                               , pubType     :: String+                               , pubPort     :: Int+                               , pubCleanup  :: IO ()+                               , pubTopic    :: DynTopic+                               , pubStats    :: StatMap PubStats }++data NodeState = NodeState { nodeName       :: String+                           , namespace      :: String+                           , master         :: URI+                           , nodeURI        :: MVar URI+                           , signalShutdown :: MVar (IO ())+                           , subscriptions  :: Map String Subscription+                           , publications   :: Map String Publication }++type Params = [(String, ParamVal)]+type Remap = [(String,String)]++data NodeConfig = NodeConfig { nodeParams :: Params+                             , nodeRemaps :: Remap+                             , nodeAppConfig :: ConfigOptions }++-- |A 'Node' carries with it parameters, topic remappings, and some+-- state encoding the status of its subscriptions and publications.+newtype Node a = Node { unNode :: ReaderT NodeConfig (StateT NodeState IO) a }+  deriving (Functor, Applicative, Monad, MonadIO)++instance MonadState NodeState Node where+    get = Node get+    put = Node . put++instance MonadReader NodeConfig Node where+    ask = Node ask+    local f m = Node $ withReaderT f (unNode m)++instance RosSlave NodeState where+    getMaster = master+    getNodeName = nodeName+    getNodeURI = nodeURI+    getSubscriptions = atomically . mapM formatSub . M.toList . subscriptions+        where formatSub (name, sub) = let topicType = subType sub+                                      in do stats <- readTVar (subStats sub)+                                            stats' <- mapM statSnapshot . +                                                      M.toList $+                                                      stats+                                            return (name, topicType, stats')+    getPublications = atomically . mapM formatPub . M.toList . publications+        where formatPub (name, pub) = let topicType = pubType pub+                                      in do stats <- readTVar (pubStats pub)+                                            stats' <- mapM statSnapshot .+                                                      M.toList $+                                                      stats+                                            return (name, topicType, stats')+    publisherUpdate ns name uris = +        let act = join.atomically $+                  case M.lookup name (subscriptions ns) of+                    Nothing -> return (return ())+                    Just sub -> do let add = addPub sub >=> \_ -> return ()+                                   known <- readTVar (knownPubs sub) +                                   (act',known') <- foldM (connectToPub add)+                                                          (return (), known)+                                                          uris+                                   writeTVar (knownPubs sub) known'+                                   return act'+        in act+    getTopicPortTCP = ((pubPort <$> ) .) . flip M.lookup . publications+    setShutdownAction ns a = putMVar (signalShutdown ns) a+    stopNode = mapM_ (pubCleanup . snd) . M.toList . publications++-- If a given URI is not a part of a Set of known URIs, add an action+-- to effect a subscription to an accumulated action and add the URI+-- to the Set.+connectToPub :: Monad m => +                (URI -> IO ()) -> (IO (), Set URI) -> URI -> m (IO (), Set URI)+connectToPub doSub (act, known) uri = if S.member uri known+                                      then return (act, known)+                                      else let known' = S.insert uri known+                                           in return (doSub uri >> act, known')
+ src/Ros/Rate.hs view
@@ -0,0 +1,35 @@+-- |Provides a rate limiting mechanism that can be used to control the+-- rate at which 'IO' actions produce values.+module Ros.Rate (rateLimiter) where+import Control.Concurrent (threadDelay)+import Control.Monad (when)+import Data.IORef (newIORef, readIORef, writeIORef)+import Data.Time.Clock (UTCTime, getCurrentTime, diffUTCTime)+import Ros.Util.PID++timeDiff :: UTCTime -> UTCTime -> Double+timeDiff = curry $ realToFrac . uncurry diffUTCTime++-- |Produces an action that runs the supplied 'IO' action no faster+-- than given rate in Hz.+rateLimiter :: Double -> IO a -> IO (IO a)+rateLimiter hz action = do control' <- pidWithTimeIO (-0.2) (-0.02) (-0.01)+                           let control = control' period+                           prevDelay <- newIORef period+                           prevTime <- getCurrentTime >>= newIORef+                           start <- getCurrentTime+                           return $ do t1 <- getCurrentTime+                                       t0 <- readIORef prevTime+                                       let tdiff = timeDiff t1 t0 * 1000000+                                           t1' = realToFrac $ +                                                 diffUTCTime t1 start+                                       change <- control (t1', tdiff)+                                       delay <- readIORef prevDelay+                                       let delay' = delay + change+                                       when (delay' > 0)+                                            (threadDelay $ truncate delay')+                                       x <- action+                                       writeIORef prevDelay delay'+                                       writeIORef prevTime t1+                                       return x+  where period = 1000000 / hz
+ src/Ros/Service.hs view
@@ -0,0 +1,19 @@+module Ros.Service (callService) where+import System.Environment(getEnvironment)++import Ros.Node.RosTcp(callServiceWithMaster)+import Ros.Internal.RosTypes+import Ros.Internal.RosBinary+import Ros.Internal.Msg.SrvInfo+import Ros.Service.ServiceTypes+++--type NotOkError = String++callService :: (RosBinary a, SrvInfo a, RosBinary b, SrvInfo b) => ServiceName -> a -> IO (Either ServiceResponseExcept b)+callService name req =+  do+    env <- getEnvironment+    let getConfig' var def = maybe def id $ lookup var env+        master = getConfig' "ROS_MASTER_URI" "http://localhost:11311"+    callServiceWithMaster master name req
+ src/Ros/Service/ServiceTypes.hs view
@@ -0,0 +1,14 @@+module Ros.Service.ServiceTypes (ServiceResponseExcept(..)) where++-- | This type represensts the possible error cases that can occur when a service is called by a client.+-- A NotOkExcept occurs when the server replies to a service request with an error message instead of the normal+-- service message. The NotOkExcept string a string sent from the server. See http://wiki.ros.org/ROS/TCPROS+-- A ResponseReadExcept occurs when the roshask service client has problems recieving either the expected service+-- message or NotOkExcept message.+-- MasterExcept is for problems encountered while communicating with the master+-- ConHeadExcept is for an error with the connection header+-- ConnectExcept is for problems with connecting to the server+-- SendRequestExcept is for problems with sending the request+data ServiceResponseExcept = NotOkExcept String | ResponseReadExcept String | MasterExcept String | ConHeadExcept String | ConnectExcept String | SendRequestExcept String+                          deriving (Show, Eq)+
+ src/Ros/Topic.hs view
@@ -0,0 +1,271 @@+{-# LANGUAGE CPP, ScopedTypeVariables #-}+-- |The ROS Topic type and basic operations on Topics.+--+-- /Note/: Many of these operations have the same names as similar+-- operations on lists in the "Prelude". The ambiguity may be resolved+-- using either qualification (e.g. @import qualified Ros.TopicUtil as+-- T@), an explicit import list, or a @hiding@ clause.+module Ros.Topic where+#if __GLASGOW_HASKELL__ >= 710+import Prelude hiding (join)+#endif+import Control.Applicative+import Control.Arrow ((***), second)+import Control.Monad ((<=<), (>=>))+import Control.Monad.IO.Class++-- |A Topic is an infinite stream of values that steps between values+-- in a 'Monad'.+newtype Topic m a = Topic { runTopic :: m (a, Topic m a) }++instance Functor m => Functor (Topic m) where+  fmap f (Topic ma) = Topic $ fmap (f *** fmap f) ma++instance Applicative m => Applicative (Topic m) where+  pure x = let t = Topic $ pure (x, t) in t+  Topic ma <*> Topic mb = Topic $ uncurry (***) . (($) *** (<*>)) <$> ma <*> mb++-- |Return the first value produced by a 'Topic'.+head :: Functor m => Topic m a -> m a+head = fmap fst . runTopic++-- |Return the first value produced by a 'Topic' along with the+-- remaining 'Topic' data.+uncons :: Topic m a -> m (a, Topic m a)+uncons = runTopic++-- |Force evaluation of a topic until it produces a value.+force :: Monad m => Topic m a -> m (Topic m a)+force = uncons >=> return . Topic . return++-- |Prepend a single item to the front of a 'Topic'.+cons :: Monad m => a -> Topic m a -> Topic m a+cons x t = Topic $ return (x, t)++-- |Returns a 'Topic' containing all the values from the given 'Topic'+-- after the first.+tail :: Monad m => Topic m a -> Topic m a+tail = Topic . (runTopic . snd <=< runTopic)++-- |Return a 'Topic' of all the suffixes of a 'Topic'.+tails :: Monad m => Topic m a -> Topic m (Topic m a)+tails t = Topic $ do (x,t') <- runTopic t+                     return (Topic $ return (x,t'), tails t')++-- |Returns a 'Topic' containing only those elements of the supplied+-- 'Topic' for which the given predicate returns 'True'.+filter :: Monad m => (a -> Bool) -> Topic m a -> Topic m a+filter p = metamorph go+  where go x | p x = yield x go+             | otherwise = skip go+-- filter p = go +--   where go = Topic . (aux <=< runTopic)+--         aux (x, t') | p x       = return (x, go t')+--                     | otherwise = runTopic $ go t'++-- |@take n t@ returns the prefix of @t@ of length @n@.+take :: Monad m => Int -> Topic m a -> m [a]+take = aux []+  where aux acc 0 _ = return (reverse acc)+        aux acc n' t = do (x, t') <- runTopic t+                          aux (x:acc) (n'-1) t'++-- |Run a 'Topic' for the specified number of iterations, discarding+-- the values it produces.+take_ :: Monad m => Int -> Topic m a -> m ()+take_ 0 = const $ return ()+take_ n = take_ (n-1) . snd <=< runTopic++-- |@drop n t@ returns the suffix of @t@ after the first @n@ elements.+drop :: Monad m => Int -> Topic m a -> Topic m a+drop = (Topic .) . aux+  where aux 0 = runTopic+        aux n = aux (n-1) . snd <=< runTopic++-- |@dropWhile p t@ returns the suffix of @t@ after all elements+-- satisfying predicate @p@ have been dropped.+dropWhile :: Monad m => (a -> Bool) -> Topic m a -> Topic m a+dropWhile p = Topic . go+  where go = check <=< runTopic+        check (x,t) | p x       = go t+                    | otherwise = return (x, t)++-- |@takeWhile p t@ returns the longest prefix (possibly empty) of @t@+-- all of whose elements satisfy the predicate @p@.+takeWhile :: Monad m => (a -> Bool) -> Topic m a -> m [a]+takeWhile p = go []+  where go acc t = do (x,t') <- runTopic t+                      if p x then go (x:acc) t' +                             else return . reverse $ x:acc++-- |@break p t@ returns a tuple whose first element is the longest+-- prefix (possibly empty) of @t@ all of whose elements satisfy the+-- predicate @p@, and whose second element is the remainder of the+-- 'Topic'.+break :: Monad m => (a -> Bool) -> Topic m a -> m ([a], Topic m a)+break p = go []+  where go acc = check acc <=< runTopic+        check acc (x,t)+          | p x = go (x:acc) t+          | otherwise = return (reverse (x:acc), t)++-- |@splitAt n t@ returns a tuple whose first element is the prefix of+-- @t@ of length @n@, and whose second element is the remainder of the+-- 'Topic'.+splitAt :: Monad m => Int -> Topic m a -> m ([a], Topic m a)+splitAt = go []+  where go acc 0 t = return (reverse acc, t)+        go acc n t = do (x,t') <- runTopic t+                        go (x:acc) (n-1) t'++-- |Returns a 'Topic' that includes only the 'Just' values from the+-- given 'Topic'.+catMaybes :: Monad m => Topic m (Maybe a) -> Topic m a+catMaybes = metamorph go+  where go = maybe (skip go) (flip yield go)+-- catMaybes (Topic ma) = Topic $ ma >>= aux+--   where aux (Nothing, t') = runTopic $ catMaybes t'+--         aux (Just x, t')  = return (x, catMaybes t')++-- |Repeatedly execute a monadic action feeding the values into a+-- 'Topic'.+repeatM :: Monad m => m a -> Topic m a+repeatM action = go+  where go = Topic $ action >>= \x -> return (x, go)++-- |Build a 'Topic' from a seed value. The supplied function is+-- applied to the seed value to produce both a value that goes into+-- the 'Topic' and a new seed value for the next recursive call.+unfold :: Functor m => (b -> m (a,b)) -> b -> Topic m a+unfold f z0 = go z0+  where go z = Topic $ second go <$> f z++-- |A pair of an optional value and a continuation for producing more+-- such pairs. This type is used by 'metamorph' to implement a+-- streaming @unfold . fold@ composition.+newtype IterCont a b = IterCont (Maybe b, a -> IterCont a b)++instance Functor (IterCont a) where+  fmap f (IterCont (x, k)) = IterCont (fmap f x, fmap f . k)++-- |A pair of an optional value and a continuation with effects for+-- producing more such pairs. This type is used by 'metamorphM' to+-- implement a streaming @unfold . fold@ composition.+newtype IterContM m a b = IterContM (Maybe b, a -> m (IterContM m a b))++instance Monad m => Functor (IterContM m a) where+  fmap f (IterContM (x, k)) = IterContM (fmap f x,  return . fmap f <=< k)++-- |Yield a value and a continuation in a metamorphism (used with+-- 'metamorph').+yield :: b -> (a -> IterCont a b) -> IterCont a b+yield = curry IterCont . Just++-- |Do not yield a value, but provide a continuation in a metamorphism+-- (used with 'metamorph').+skip :: (a -> IterCont a b) -> IterCont a b+skip = curry IterCont Nothing++-- |Yield a value and a continuation in a monad as part of a monadic+-- metamorphism (used with 'metamorphM').+yieldM :: Monad m => b -> (a -> m (IterContM m a b)) -> m (IterContM m a b)+yieldM = (return .) . curry IterContM . Just++-- |Do not yield a value, but provide a continuation in a metamorphism+-- (used with 'metamorphM').+skipM :: Monad m => (a -> m (IterContM m a b)) -> m (IterContM m a b)+skipM = return . curry IterContM Nothing++-- |A metamorphism (cf. Jeremy Gibbons) on 'Topic's. This is an+-- /unfold/ following a /fold/ (i.e. @unfoldr . foldl@), with the+-- expectation that partial results of the /unfold/ may be returned+-- before the /fold/ is completed. The supplied function produces a+-- optional value and a continuation when applied to an element of the+-- first 'Topic'. The value is returned by the new 'Topic' if it is+-- not 'Nothing', and the continuation is used to produce the rest of+-- the returned 'Topic'.+metamorph :: Monad m => (a -> IterCont a b) -> Topic m a -> Topic m b+metamorph f t = Topic $ do (x,t') <- runTopic t+                           let IterCont (x', f') = f x+                           case x' of+                             Nothing -> runTopic $ metamorph f' t'+                             Just x'' -> return (x'', metamorph f' t')++-- |Similar to 'metamorph', but the metamorphism may have effects.+metamorphM :: Monad m => (a -> m (IterContM m a b)) -> Topic m a -> Topic m b+metamorphM f t = Topic $ do (x,t') <- runTopic t+                            IterContM (x', f') <- f x+                            case x' of+                              Nothing -> runTopic $ metamorphM f' t'+                              Just x'' -> return (x'', metamorphM f' t')++-- |Fold two functions along a 'Topic' collecting their productions in+-- a new 'Topic'.+bimetamorph :: Monad m =>+               (a -> IterCont a b) -> (a -> IterCont a b) ->+               Topic m a -> Topic m b+bimetamorph f g t = Topic $ do (x,t') <- runTopic t+                               let IterCont (y, f') = f x+                                   IterCont (z, g') = g x+                               aux y . aux z . runTopic $ bimetamorph f' g' t'+  where aux = maybe id (\x y -> return (x, Topic y))++-- |Fold two monadic functions along a 'Topic' collecting their+-- productions in a new 'Topic'.+bimetamorphM :: Monad m =>+                (a -> m (IterContM m a b)) -> (a -> m (IterContM m a b)) ->+                Topic m a -> Topic m b+bimetamorphM f g t = Topic $ do (x,t') <- runTopic t+                                IterContM (y, f') <- f x+                                IterContM (z, g') <- g x+                                aux y . aux z . runTopic $ bimetamorphM f' g' t'+  where aux = maybe id (\x y -> return (x, Topic y))++-- |Fold two functions along a 'Topic' collecting and tagging their+-- productions in a new 'Topic'.+bimetamorphE :: Monad m => +                (a -> IterCont a b) -> (a -> IterCont a c) ->+                Topic m a -> Topic m (Either b c)+bimetamorphE f g t = bimetamorph (fmap Left . f) (fmap Right . g) t++-- |Fold two monadic functions along a 'Topic' collecting and tagging+-- their productions in a new 'Topic'.+bimetamorphME :: Monad m => +                 (a -> m (IterContM m a b)) -> (a -> m (IterContM m a c)) ->+                 Topic m a -> Topic m (Either b c)+bimetamorphME f g t = +  bimetamorphM (return . fmap Left <=< f) (return . fmap Right <=< g) t++-- |Removes one level of monadic structure from the values a 'Topic'+-- produces.+join :: (Functor m, Monad m) => Topic m (m a) -> Topic m a+join t = Topic $ do (x, t') <- runTopic t+                    x' <- x+                    return (x', join t')++-- |@forever t@ runs all monadic actions a 'Topic' produces. This is+-- useful for 'Topic's whose steps produce side-effects, but not+-- useful pure values.+forever :: Monad m => Topic m a -> m b+forever = forever . snd <=< runTopic++-- |Map a monadic action over a 'Topic'.+mapM :: (Functor m, Monad m) => (a -> m b) -> Topic m a -> Topic m b+mapM = (join .) . fmap++-- |Map a monadic action of a 'Topic' purely for its side+-- effects. This function will never return.+mapM_ :: Monad m => (a -> m ()) -> Topic m a -> m ()+mapM_ f = go+  where go = uncurry (>>) . (f *** go) <=< runTopic++-- |A left-associative scan of a 'Topic' is a fold whose every+-- intermediate value is produced as a value of a new 'Topic'.+scan :: Monad m => (a -> b -> a) -> a -> Topic m b -> Topic m a+scan f z = metamorph (go z)+  where go acc x = let x' = f acc x in yield x' (go x')++-- |Print all the values produced by a 'Topic'.+showTopic :: (MonadIO m, Functor m, Show a) => Topic m a -> Topic m ()+showTopic = join . fmap (liftIO . putStrLn . show)+
+ src/Ros/Topic/PID.hs view
@@ -0,0 +1,97 @@+-- |PID related functions for 'Topic's.+module Ros.Topic.PID where+import Control.Applicative+import Ros.Topic+import Ros.Topic.Util+import qualified Ros.Util.PID as P++-- |@pidUniform2 kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the setpoints produced by+-- 'Topic' @setpoint@ using the PID gains @kp@, @ki@, and @kd@. The+-- interval between samples produced by the 'Topic' is assumed to be+-- 1.+pidUniform2 :: Fractional a => a -> a -> a -> Topic IO a -> Topic IO a -> +               Topic IO a+pidUniform2 kp ki kd setpoint t = +  Topic $ do controller <- uncurry <$> P.pidUniformIO kp ki kd+             runTopic . join $ controller <$> everyNew setpoint t++-- |@pidUniform kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the given setpoint using+-- the PID gains @kp@, @ki@, and @kd@. The interval between samples+-- produced by the 'Topic' is assumed to be 1.+pidUniform :: Fractional a => a -> a -> a -> a -> Topic IO a -> Topic IO a+pidUniform kp ki kd setpoint t = +  Topic $ do controller <- ($ setpoint) <$> P.pidUniformIO kp ki kd+             runTopic . join $ controller <$> t++-- |@pidFixed2 kp ki kd setpoint dt t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the setpoints produced by+-- 'Topic' @setpoint@ using the PID gains @kp@, @ki@, and @kd@, along+-- with an assumed fixed time interval, @dt@, between samples.+pidFixed2 :: Fractional a => a -> a -> a -> a -> Topic IO a -> Topic IO a -> +             Topic IO a+pidFixed2 kp ki kd dt setpoint t = +  Topic $ do controller <- uncurry <$> P.pidFixedIO kp ki kd dt+             runTopic . join $ controller <$> everyNew setpoint t+++-- |@pidFixed kp ki kd setpoint dt t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the given setpoint using+-- the PID gains @kp@, @ki@, and @kd@, along with an assumed fixed+-- time interval, @dt@, between samples.+pidFixed :: Fractional a => a -> a -> a -> a -> a -> Topic IO a -> Topic IO a+pidFixed kp ki kd setpoint dt t = +  Topic $ do controller <- ($ setpoint) <$> P.pidFixedIO kp ki kd dt+             runTopic . join $ controller <$> t++-- |@pidTimed2 kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the setpoints produced by+-- 'Topic' @setpoint@ using the PID gains @kp@, @ki@, and @kd@. The+-- system clock is checked for each value produced by the input+-- 'Topic' to determine the actual sampling rate.+pidTimed2 :: Fractional a => a -> a -> a -> Topic IO a -> Topic IO a -> +             Topic IO a+pidTimed2 kp ki kd setpoint t = +  Topic $ do controller <- uncurry <$> P.pidTimedIO kp ki kd+             runTopic . join $ controller <$> everyNew setpoint t++-- |@pidTimed kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the given setpoint using+-- the PID gains @kp@, @ki@, and @kd@. The system clock is checked for+-- each value produced by the input 'Topic' to determine the actual+-- sampling rate.+pidTimed :: Fractional a => a -> a -> a -> a -> Topic IO a -> Topic IO a+pidTimed kp ki kd setpoint t = +  Topic $ do controller <- ($ setpoint) <$> P.pidTimedIO kp ki kd+             runTopic . join $ controller <$> t+++-- |@pidStamped2 kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the given setpoint using+-- the PID gains @kp@, @ki@, and @kd@. Values produced by the 'Topic'+-- @t@ must be paired with a timestamp and thus have the form+-- (timeStamp, sample).+pidStamped2 :: Fractional a => a -> a -> a -> Topic IO a -> Topic IO (a,a) -> +               Topic IO a+pidStamped2 kp ki kd setpoint t =+  Topic $ do controller <- uncurry <$> P.pidWithTimeIO kp ki kd+             runTopic . join $ controller <$> everyNew setpoint t++-- |@pidStamped kp ki kd setpoint t@ runs a PID controller that+-- transforms 'Topic' @t@ of process outputs into a 'Topic' of control+-- signals designed to steer the output to the given setpoint using+-- the PID gains @kp@, @ki@, and @kd@. Values produced by the 'Topic'+-- @t@ must be paired with a timestamp and thuse have the form+-- (timeStamp, sample).+pidStamped :: Fractional a => a -> a -> a -> a -> Topic IO (a,a) -> Topic IO a+pidStamped kp ki kd setpoint t =+  Topic $ do controller <- ($ setpoint) <$> P.pidWithTimeIO kp ki kd+             runTopic . join $ controller <$> t
+ src/Ros/Topic/Stamped.hs view
@@ -0,0 +1,116 @@+{-# LANGUAGE TupleSections #-}+-- |Functions for fusing 'Topic's based on TimeStamp fields of the+-- underlying messages. This module shadows some of the functionality+-- of the "Ros.TopicUtil" module. The difference is that the functions+-- exported by this module use time stamps to correlate two+-- 'Topic's. +-- +-- The correlation uses a bracketing pair of values from one 'Topic'+-- to pick a correspondance for each value from the other+-- 'Topic'. This bracketing approach induces some latency. The most+-- common use case is calling the 'bothNew' function with a 'Topic'+-- that produces very quickly (faster than the minimum required update+-- rate), and another 'Topic' that imposes a rate limit.+module Ros.Topic.Stamped (everyNew, interpolate, batch) where+import Data.Time.Clock (getCurrentTime, diffUTCTime)+import System.Timeout+import qualified Ros.Topic as T+import Ros.Topic (Topic(..), metamorphM, yieldM)+import qualified Ros.Topic.Util as T+import Ros.Internal.Msg.HeaderSupport+import Ros.Internal.RosTime++-- |Given two consecutive values, pick the one with the closest time+-- stamp to another value.+pickNearest :: (HasHeader a, HasHeader b) => a -> a -> b -> a+pickNearest x1 x2 y+  | ty <= t1 = x1+  | t2 <= ty = x2+  | d1 < d2 = x1+  | otherwise = x2+  where t1 = getStamp x1+        t2 = getStamp x2+        ty = getStamp y+        d1 = diffROSTime ty t1+        d2 = diffROSTime t2 ty++-- |@findBrackets t1 t2@ Pairs each element of @t2@ with the pair of+-- consecutive elements from @t1@ that brackets it in time, and the +-- time interval in seconds covered by that bracket.+findBrackets :: (HasHeader a, HasHeader b) =>+                Topic IO a -> Topic IO b -> Topic IO ((a,a,Double), b)+findBrackets t1 t2 = T.concats . metamorphM (go t2) $ T.consecutive t1+  where go t (x,y) = let start = getStamp x+                         stop = getStamp y+                         bracket = (x,y, diffSeconds stop start)+                     in do (items, rest) <- T.break ((< stop) . getStamp) $+                                            T.dropWhile ((< start) . getStamp) t+                           let items' = map (bracket,) items+                           yieldM items' (go rest)++-- |Remove an element from a 'Topic' if the next element from that+-- 'Topic' is composed of elements bearing the exact same sequence+-- IDs in their headers (as obtained by 'getSequence').+removeDups :: (Functor m, Monad m, HasHeader a, HasHeader b) =>+              Topic m (a,b) -> Topic m (a,b)+removeDups = T.catMaybes . fmap check . T.consecutive+  where check ((x1,y1), x@(x2,y2))+          | getSequence x1 == getSequence x2 && +            getSequence y1 == getSequence y2 = Nothing+          | otherwise = Just x++-- |Returns a 'Topic' that produces a new pair for every value+-- produced by either of the component 'Topic's. The value of the+-- other element of the pair will be the element from the other+-- 'Topic' with the nearest time stamp. The resulting 'Topic' will+-- produce a new value at the rate of the faster component 'Topic'.+everyNew :: (HasHeader a, HasHeader b) => +            Topic IO a -> Topic IO b -> IO (Topic IO (a,b))+everyNew t1 t2 = +  do (t1a, t1b) <- T.tee t1+     (t2a, t2b) <- T.tee t2+     let bracketLeft = pickLeft `fmap` findBrackets t1a t2a+         bracketRight = pickRight `fmap` findBrackets t2b t1b+     return . removeDups $ bracketLeft `T.merge` bracketRight+  where pickLeft ((x1,x2,_), y) = (pickNearest x1 x2 y, y)+        pickRight ((y1,y2,_), x) = (x, pickNearest y1 y2 x)++-- |The application @interpolate f t1 t2@ produces a new 'Topic' that+-- pairs every element of @t2@ with an interpolation of two temporally+-- bracketing values from @t1@. The interpolation is effected with the+-- supplied function, @f@, that is given the two values to interpolate+-- and the linear ratio to find between them. This ratio is determined+-- by the time stamp of the intervening element of @t2@.+interpolate :: (HasHeader a, HasHeader b) => +               (a -> a -> Double -> a) -> Topic IO a -> Topic IO b -> +               Topic IO (a,b)+interpolate f t1 t2 = interp `fmap` findBrackets t1 t2+  where interp ((x1,x2,dt),y) = let tx1 = getStamp x1+                                    ty = getStamp y+                                in (f x1 x2 (diffSeconds ty tx1 / dt), y)++-- |Batch 'Topic' values that arrive within the given time window+-- (expressed in seconds). When a value arrives, the window opens and+-- all values received within that window are returned in a list, then+-- the next value is awaited before opening the window again. Intended+-- usage is to gather approximately simultaneous events into+-- batches. Note that the times used to batch messages are arrival+-- times rather than time stamps. This is what lets us close the+-- window, rather than having to admit any message that ever arrives+-- with a compatible time stamp.+batch :: Double -> Topic IO a -> Topic IO [a]+batch timeWindow t0 = +  Topic $ do (x,t') <- runTopic t0+             start <- getCurrentTime+             let go acc t = do now <- getCurrentTime+                               let dt = fromRational . toRational $+                                        diffUTCTime now start+                                   dMs = floor $ (timeWindow - dt) * 1000000+                               if dMs == 0+                                 then return (reverse acc, k t)+                                 else do r <- timeout dMs $ runTopic t+                                         case r of+                                           Just (x',t'') -> go (x':acc) t''+                                           Nothing -> return (reverse acc, k t)+             go [x] t'+    where k = batch timeWindow
+ src/Ros/Topic/Stats.hs view
@@ -0,0 +1,57 @@+{-# LANGUAGE TupleSections #-}+module Ros.Topic.Stats (sendMessageStat, recvMessageStat, statSnapshot,+                        StatMap, SubStats(..), PubStats(..)) where+import Control.Applicative ((<$>))+import Control.Concurrent.STM.TVar+import Control.Concurrent.STM+import Data.Map (Map)+import qualified Data.Map as M+import Ros.Internal.RosTypes++data SubStats = SubStats { bytesReceived  :: !Int+                         , subConnected   :: !Bool }++data PubStats = PubStats { bytesSent      :: !Int+                         , numSent        :: !Int+                         , pubConnected   :: !Bool }+++-- |A transactional data store for tracking all the connections for a+-- particular topic.+type StatMap a = TVar (Map URI (TVar a))++statSnapshot :: (URI, TVar a) -> STM (URI, a)+statSnapshot (uri, stat) = (uri,) <$> readTVar stat++-- |Record the fact that we've sent a message of the given number of+-- bytes to the given URI. If the number of bytes is negative, the+-- connection is marked is disconnected.+sendMessageStat :: StatMap PubStats -> URI -> Int -> IO ()+sendMessageStat tm uri numBytes = +    atomically $ do m <- readTVar tm+                    let conn = numBytes >= 0+                        nb = max 0 numBytes+                        nm = if conn then 1 else 0+                    case M.lookup uri m of+                      Nothing -> do stats <- newTVar $ PubStats nb nm conn+                                    writeTVar tm (M.insert uri stats m)+                      Just ts -> do PubStats nb' nm' _ <- readTVar ts+                                    let stats = PubStats (nb' + nb)+                                                         (nm' + nm)+                                                         conn+                                    writeTVar ts stats++-- |Record the fact that we've received a message of the given number+-- of bytes from the given URI. If the number of bytes is negative,+-- the connection is marked as disconnected.+recvMessageStat :: StatMap SubStats -> URI -> Int -> IO ()+recvMessageStat tm uri numBytes = +    atomically $ do m <- readTVar tm+                    let conn = numBytes >= 0+                        nb = max 0 numBytes+                    case M.lookup uri m of+                      Nothing -> do stats <- newTVar $ SubStats nb conn+                                    writeTVar tm (M.insert uri stats m)+                      Just ts -> do SubStats nb' _ <- readTVar ts+                                    let stats = SubStats (nb' + nb) conn+                                    writeTVar ts stats
+ src/Ros/Topic/Transformers.hs view
@@ -0,0 +1,36 @@+-- |Functions for working with 'Topic's built around monad+-- transformers. These make it possible to, for example, repeat a+-- stateful action to produce a 'Topic''s values.+module Ros.Topic.Transformers where+import Control.Arrow+import qualified Control.Monad.State.Lazy as L+import qualified Control.Monad.State.Strict as S+import Control.Monad.Reader+import Ros.Topic++-- |Run a 'Topic' built around a lazy @'L.StateT' s@ monad using a+-- given initial state.+runTopicState :: Monad m => Topic (L.StateT s m) a -> s -> Topic m a+runTopicState t s = Topic $ do ((x,t'), s') <- L.runStateT (runTopic t) s+                               return (x, runTopicState t' s')++-- |Run a 'Topic' build around a strict @'S.StateT' s@ monad using a+-- given initial state.+runTopicState' :: Monad m => Topic (S.StateT s m) a -> s -> Topic m a+runTopicState' t s = Topic $ do ((x,t'), s') <- S.runStateT (runTopic t) s+                                return (x, runTopicState' t' s')++-- |Run a 'Topic' built around a 'ReaderT r' monad using a value for+-- reading.+runTopicReader :: (Functor m, Monad m) => Topic (ReaderT r m) a -> r -> Topic m a+runTopicReader t r = go t+  where go (Topic ma) = Topic $ second go `fmap` runReaderT ma r++-- |Map a monadic function over a 'Topic', in the process lifting the+-- 'Topic' into a new monad.+liftMap :: (MonadTrans t, Monad m, Monad (t m)) => +           (a -> t m b) -> Topic m a -> Topic (t m) b+liftMap f = go+  where go (Topic ma) = Topic $ do (x,t') <- lift ma+                                   x' <- f x+                                   return (x', go t')
+ src/Ros/Topic/Util.hs view
@@ -0,0 +1,385 @@+{-# LANGUAGE ScopedTypeVariables, BangPatterns #-}+-- |Utility functions for working with 'Topic's. These functions are+-- primarily combinators for fusing two 'Topic's in various ways.+module Ros.Topic.Util where+import Prelude hiding (dropWhile, filter, splitAt, mapM)+import Control.Applicative+import Control.Arrow ((***), second)+import Control.Concurrent hiding (yield)+import Control.Concurrent.STM+import Control.Monad ((<=<), when, replicateM)+import Control.Monad.IO.Class+import Data.AdditiveGroup (AdditiveGroup, (^+^), (^-^), Sum(..))+import Data.Monoid (Monoid)+import Data.Sequence ((|>), viewl, ViewL(..))+import qualified Data.Sequence as S+import qualified Data.Foldable as F+import Ros.Rate (rateLimiter)+import Ros.Topic hiding (mapM_)++-- |Produce an infinite list from a 'Topic'.+toList :: Topic IO a -> IO [a]+toList t0 = do c <- newChan+               let feed t = do (x, t') <- runTopic t+                               writeChan c x+                               feed t'+               _ <- forkIO $ feed t0+               getChanContents c++-- |Produce a 'Topic' from an infinite list.+fromList :: Monad m => [a] -> Topic m a+fromList (x:xs) = Topic $ return (x, fromList xs)+fromList [] = error "Ran out of list elements"++-- |Tee a 'Topic' into two duplicate 'Topic's. Each returned 'Topic'+-- will receive all the values of the original 'Topic' while any+-- side-effect produced by each step of the original 'Topic' will+-- occur only once.+-- +-- This version of @tee@ lazily pulls data from the original 'Topic'+-- when it is first required by a consumer of either of the returned+-- 'Topic's. This behavior is crucial when lazily consuming the data+-- stream is preferred. For instance, using 'interruptible' with 'tee'+-- will allow for a chunk of data to be abandoned before being fully+-- consumed as long as neither consumer has forced its way too far+-- down the stream.+--+-- This function is useful when two consumers must see all the same+-- elements from a 'Topic'. If the 'Topic' was instead 'share'd, then+-- one consumer might get the first value from the 'Topic' before the+-- second consumer's buffer is created since buffer creation is lazy.+tee :: Topic IO a -> IO (Topic IO a, Topic IO a)+tee t0 = do c1 <- newTChanIO+            c2 <- newTChanIO+            signal <- newTVarIO True+            let feed c = do atomically $ do f <- isEmptyTChan c+                                            when f (writeTVar signal False)+                            atomically $ readTChan c+                produce t = do atomically $ readTVar signal >>= flip when retry+                               (x,t') <- runTopic t+                               atomically $ writeTChan c1 x >>+                                            writeTChan c2 x >>+                                            writeTVar signal True+                               produce t'+            _ <- forkIO $ produce t0+            return (repeatM (feed c1), repeatM (feed c2))++-- |This version of @tee@ eagerly pulls data from the+-- original 'Topic' as soon as it is available. This behavior is+-- undesirable when lazily consuming the data stream is preferred. For+-- instance, using 'interruptible' with 'teeEager' will likely not+-- work well. However, 'teeEager' may have slightly better performance+-- than 'tee'.+teeEager :: Topic IO a -> IO (Topic IO a, Topic IO a)+teeEager t = do c1 <- newChan+                c2 <- newChan+                let feed c = do x <- readChan c+                                return (x, Topic $ feed c)+                _ <- forkIO . forever . join $+                     (\x -> writeChan c1 x >> writeChan c2 x) <$> t+                return (Topic $ feed c1, Topic $ feed c2)++-- |Fan out one 'Topic' out to a number of duplicate 'Topic's, each of+-- which will produce the same values. Side effects caused by the+-- original 'Topic''s production will occur only once. This is useful+-- when a known number of consumers must see exactly all the same+-- elements.+fan :: Int -> Topic IO a -> IO [Topic IO a]+fan n t0 = do cs <- replicateM n newTChanIO+              signal <- newTVarIO True+              let feed c = do atomically $ do f <- isEmptyTChan c+                                              when f (writeTVar signal False)+                              atomically $ readTChan c+                  produce t = do atomically $ readTVar signal >>= flip when retry+                                 (x,t') <- runTopic t+                                 atomically $ mapM_ (flip writeTChan x) cs >>+                                              writeTVar signal True+                                 produce t'+              _ <- forkIO $ produce t0+              return $ map (repeatM . feed) cs++-- |Make a 'Topic' shareable among multiple consumers. Each consumer+-- of a Topic gets its own read buffer automatically as soon as it+-- starts pulling items from the Topic. Without calling one of+-- 'share', 'tee', or 'fan' on a Topic, the Topic's values will be+-- split among all consumers (e.g. consumer /A/ gets half the values+-- produced by the 'Topic', while consumer /B/ gets the other half+-- with some unpredictable interleaving). Note that Topics returned by+-- the @Ros.Node.subscribe@ are already shared.+share :: Topic IO a -> IO (Topic IO a)+share t0 = do cs <- newTVarIO [] -- A list for the individual client buffers+              signal <- newTVarIO True+              let addClient = atomically $ do cs0 <- readTVar cs+                                              c <- newTChan+                                              writeTVar cs (c:cs0)+                                              return c+                  feed c = do atomically $ do f <- isEmptyTChan c+                                              when f (writeTVar signal False)+                              atomically $ readTChan c+                  produce t = do atomically $ readTVar signal >>= flip when retry+                                 (x,t') <- runTopic t+                                 atomically $ do cs' <- readTVar cs+                                                 mapM_ (flip writeTChan x) cs'+                                                 writeTVar signal True+                                 produce t'+              _ <- forkIO $ produce t0+              return . Topic $ addClient >>= runTopic . repeatM . feed++-- |The application @topicRate rate t@ runs 'Topic' @t@ no faster than+-- @rate@ Hz.+topicRate :: (Functor m, MonadIO m) => Double -> Topic m a -> Topic m a+topicRate p t0 = Topic $ +                 do delay <- liftIO $ rateLimiter p (return ())+                    (x,t') <- runTopic t0+                    let go t = Topic $ liftIO delay >> second go <$> runTopic t+                    return (x, go t')++-- |Splits a 'Topic' into two 'Topic's: the elements of the first+-- 'Topic' all satisfy the given predicate, while none of the elements+-- of the second 'Topic' do.+partition :: (a -> Bool) -> Topic IO a -> IO (Topic IO a, Topic IO a)+partition p = fmap (filter p *** filter (not . p)) . tee++-- |Returns a 'Topic' whose values are consecutive values from the+-- original 'Topic'.+consecutive :: Monad m => Topic m a -> Topic m (a,a)+consecutive = metamorph startup+  where startup x = skip (go x)+        go x y    = yield (x,y) (go y)+-- consecutive t = Topic $ do (x, t') <- runTopic t+--                            runTopic $ go x t'+--   where go x t' = Topic$ do (y, t'') <- runTopic t'+--                             return ((x,y), go y t'')++-- |Interleave two 'Topic's. Items from each component 'Topic' will be+-- tagged with an 'Either' constructor and added to the combined+-- 'Topic' as they become available.+(<+>) :: Topic IO a -> Topic IO b -> Topic IO (Either a b)+(<+>) t1 t2 = Topic $ do c <- newChan+                         let aux = do x <- readChan c+                                      return (x, Topic aux)+                             feed t = do (x,t') <- runTopic t+                                         writeChan c x+                                         feed t'+                         _ <- forkIO $ feed (fmap Left t1)+                         _ <- forkIO $ feed (fmap Right t2)+                         aux+infixl 7 <+>++-- |Returns a 'Topic' that produces a new pair every time either of+-- the component 'Topic's produces a new value. The value of the+-- other element of the pair will be the newest available value. The+-- resulting 'Topic' will produce a new value at the rate of the+-- faster component 'Topic', and may contain duplicate consecutive+-- elements.+everyNew :: Topic IO a -> Topic IO b -> Topic IO (a,b)+everyNew t1 t2 = Topic $ warmup =<< runTopic (t1 <+> t2)+  where warmup (Left x, t)     = warmupR x =<< runTopic t+        warmup (Right y, t)    = warmupL y =<< runTopic t+        warmupR _ (Left x, t)  = warmupR x =<< runTopic t+        warmupR x (Right y, t) = return ((x,y), Topic $ runTopic t >>= go x y)+        warmupL _ (Right y, t) = warmupL y =<< runTopic t+        warmupL y (Left x, t)  = return ((x,y), Topic $ runTopic t >>= go x y)+        go _ y (Left x, t)     = return ((x,y), Topic $ runTopic t >>= go x y)+        go x _ (Right y, t)    = return ((x,y), Topic $ runTopic t >>= go x y)++-- |Returns a 'Topic' that produces a new pair every time both of the+-- component 'Topic's have produced a new value. The composite+-- 'Topic' will produce pairs at the rate of the slower component+-- 'Topic' consisting of the most recent value from each 'Topic'.+bothNew :: Topic IO a -> Topic IO b -> Topic IO (a,b)+bothNew t1 t2 = Topic $ warmup =<< runTopic (t1 <+> t2)+  where warmup (v,t) = go v =<< runTopic t+        go (Left _) (l@(Left _), t) = go l =<< runTopic t+        go (Left x) (Right y, t) = return ((x,y), Topic $ warmup =<< runTopic t)+        go (Right _) (r@(Right _), t) = go r =<< runTopic t+        go (Right y) (Left x, t) = return ((x,y), Topic $ warmup =<< runTopic t)++-- |Merge two 'Topic's into one. The items from each component+-- 'Topic' will be added to the combined 'Topic' as they become+-- available.+merge :: Topic IO a -> Topic IO a -> Topic IO a+merge t1 t2 = either id id <$> t1 <+> t2++-- |Apply a function to each consecutive pair of elements from a 'Topic'.+finiteDifference :: (Functor m, Monad m) => (a -> a -> b) -> Topic m a -> Topic m b+finiteDifference f = fmap (uncurry f) . consecutive++-- |Compute a running \"average\" of a 'Topic' using a user-provided+-- normalization function applied to the sum of products. The+-- arguments are a constat @alpha@ that is used to scale the current+-- average, a constant @invAlpha@ used to scale the newest value, a+-- function for adding two scaled values, a function for scaling+-- input values, a function for normalizing the sum of scaled values,+-- and finally the stream to average. Parameterizing over all the+-- arithmetic to this extent allows for the use of denormalizing+-- scaling factors, as might be used to keep all arithmetic+-- integral. An example would be scaling the average by the integer+-- 7, the new value by the integer 1, then normalizing by dividing+-- the sum of scaled values by 8.+weightedMeanNormalized :: Monad m =>+                          n -> n -> (b -> b -> c) -> (n -> a -> b) -> +                          (c -> a) -> Topic m a -> Topic m a+weightedMeanNormalized alpha invAlpha plus scale normalize = Topic . warmup+    where warmup = uncurry go <=< runTopic+          go avg t = do (x,t') <- runTopic t+                        let !avg' = normalize $ plus (scale alpha avg) +                                                     (scale invAlpha x)+                        return (avg', Topic $ go avg' t')+{-# INLINE weightedMeanNormalized #-}++-- |Perform numerical integration of a 'Topic' using Simpson's rule+-- applied at three consecutive points. This requires a function for+-- adding values from the 'Topic', and a function for scaling values+-- by a fractional number.+simpsonsRule :: (Monad m, Fractional n) => +                (a -> a -> a) -> (n -> a -> a) -> Topic m a -> Topic m a+simpsonsRule plus scale t0 = Topic $ do ([x,y], t') <- splitAt 2 t0+                                        go x y t'+  where go x y t = do (z,t') <- runTopic t+                      return (simpson x y z, Topic $ go y z t')+        simpson a mid b = scale c $ plus (plus a (scale 4 mid)) b+        c = 1 / 3+{-# INLINE simpsonsRule #-}++-- |Compute a running \"average\" of a 'Topic'. The application+-- @weightedMean alpha plus scale t@ sums the product of @alpha@ and+-- the current average with the product of @1 - alpha@ and the newest+-- value produced by 'Topic' @t@. The addition and scaling operations+-- are performed using the supplied @plus@ and @scale@ functions.+weightedMean :: (Monad m, Num n) => +                n -> (a -> a -> a) -> (n -> a -> a) -> Topic m a -> Topic m a+weightedMean alpha plus scale = weightedMean2 alpha (1 - alpha) plus scale+{-# INLINE weightedMean #-}++-- |Compute a running \"average\" of a 'Topic'. The application+-- @weightedMean2 alpha invAlpha plus scale t@ sums the product of+-- @alpha@ and the current average with the product of @invAlpha@ and+-- the newest value produced by 'Topic' @t@. The addition and scaling+-- operations are performed using the supplied @plus@ and @scale@+-- functions.+weightedMean2 :: Monad m =>+                 n -> n -> (a -> a -> a) -> (n -> a -> a) -> Topic m a -> Topic m a+weightedMean2 alpha invAlpha plus scale = Topic . warmup+    where warmup = uncurry go <=< runTopic+          go avg t = do (x, t') <- runTopic t+                        let !savg = scale alpha avg+                            !sx = scale invAlpha x+                            !avg' = plus savg sx+                        return (avg', Topic $ go avg' t')+{-# INLINE weightedMean2 #-}++-- |Use a 'Topic' of functions to filter a 'Topic' of values. The+-- application @filterBy t1 t2@ causes each function from 'Topic' @t1@+-- to be applied to values produced by @t2@ until it returns+-- 'True'. At that point, the 'filterBy' application produces the+-- accepted value of the @t2@ and moves on to the next function from+-- @t1@ which is applied to the rest of @t2@ in the same manner.+filterBy :: Monad m => Topic m (a -> Bool) -> Topic m a -> Topic m a+filterBy tf tx = Topic $ do (f, tf') <- runTopic tf+                            (x, tx') <- uncons $ dropWhile (not . f) tx+                            return (x, filterBy tf' tx')++-- |Produce elements of the first 'Topic' no faster than elements of+-- the second 'Topic' are produced.+gate :: (Applicative m, Monad m) => Topic m a -> Topic m b -> Topic m a+gate t1 t2 = const <$> t1 <*> t2++-- |Flatten a 'Topic' of 'F.Foldable' values. For example, turn a+-- @Topic m [a]@ of finite lists into a @Topic a@ by taking each+-- element from each list in sequence.+concats :: (Monad m, F.Foldable f) => Topic m (f a) -> Topic m a+concats t = Topic $ do (x, t') <- runTopic t+                       F.foldr (\x' z -> return (x', Topic z)) +                               (runTopic $ concats t') +                               x++-- |Flatten a 'Topic' of 'F.Foldable' values such that old values are+-- discarded as soon as the original 'Topic' produces a new+-- 'F.Foldable'.+interruptible :: F.Foldable t => Topic IO (t a) -> Topic IO a+interruptible s = Topic $+    do feeder <- newEmptyMVar         -- Active feeder thread+       latestItem <- newEmptyMVar     -- Next available item+       signal <- newEmptyMVar         -- Demand signal+       let feedItems ys = do ft <- tryTakeMVar feeder+                             maybe (return ()) killThread ft+                             t <- forkIO $ +                                  F.traverse_ (\y -> takeMVar signal >> +                                                     putMVar latestItem y) +                                              ys+                             putMVar feeder t +           watchForItems t = do (x,t') <- runTopic t+                                feedItems x +                                watchForItems t'+           getAll = do putMVar signal ()+                       x <- takeMVar latestItem+                       return (x, Topic getAll)+       _ <- forkIO $ watchForItems s+       getAll++-- |Pull elements from a 'Topic' in a new thread. This allows 'IO'+-- 'Topic's to run at different rates even if they are consumed by a+-- single thread.+forkTopic :: Topic IO a -> IO (Topic IO a)+forkTopic t = do c <- newChan+                 _ <- forkIO . forever . join $ fmap (writeChan c) t+                 let feed = Topic $ (\x -> (x,feed)) <$> readChan c+                 return feed++-- |Sliding window over a 'Monoid'. @slidingWindow n t@ slides a+-- window of width @n@ along 'Topic' @t@. As soon as at least @n@+-- elements have been produced by @t@, the output 'Topic' starts+-- producing the 'mconcat' of the elements in the window.+slidingWindow :: (Monad m, Monoid a) => Int -> Topic m a -> Topic m a+slidingWindow n = metamorph (fill S.empty)+  where fill w x+          | S.length w < n - 1 = skip . fill $ w |> x+          | otherwise = let w' = w |> x+                        in yield (F.fold w') (go w')+        go w x = let w' = dropOldest w |> x+                 in yield (F.fold w') (go w')+        dropOldest w = case viewl w of+                         EmptyL -> S.empty+                         _ :< w' -> w'++-- |Sliding window over an 'AdditiveGroup'. @slidingWindowG n t@+-- slides a window of width @n@ along 'Topic' @t@. As soon as at least+-- @n@ elements have been produced by @t@, the output 'Topic' starts+-- producing the total sum of the elements of the window. This+-- function is more efficient than 'slidingWindow' because the group+-- inverse operation is used to remove elements falling behind the+-- window from the running sum.+slidingWindowG :: (Monad m, AdditiveGroup a) => Int -> Topic m a -> Topic m a+slidingWindowG n = metamorph (fill S.empty)+  where fill w x+          | S.length w < n - 1 = skip . fill $ w |> x+          | otherwise = let w' = w |> x+                            s = getSum . F.fold . fmap Sum $ w'+                        in yield s (go s w')+        go s w x = case viewl w of+                     EmptyL -> yield x $ go x (S.singleton x)+                     y :< w' -> let s' = s ^+^ x ^-^ y+                                in yield s' $ go s' (w' |> x)++-- |A way of pushing a monadic action into and along a 'Topic'. The+-- application @topicOn proj inj trans t@ extracts a function from+-- @trans@ that is then applied to the result of applying @proj@ to+-- each value of 'Topic' @t@. The result of that application is+-- supplied to the result of applying @inj@ to the same values from+-- @t@ to produce a value for the output 'Topic'. A typical use case+-- is projecting out a field from the original 'Topic' @t@ using+-- @proj@ so that it may be modified by @trans@ and then injected back+-- into the original structure using @inj@.+topicOn :: (Applicative m, Monad m) =>+           (a -> b) -> (a -> c -> d) -> m (b -> m c) -> Topic m a -> Topic m d+topicOn proj inj trans t = +  Topic $ do f <- trans+             runTopic $ mapM (\x -> inj x `fmap` f (proj x)) t++-- |@subsample n t@ subsamples topic 't' by dropping 'n' elements for+-- every element produced by the result topic.+subsample :: Monad m => Int -> Topic m b -> Topic m b+subsample n = metamorph $ go n+  where go 0 x = yield x (go n)+        go i _ = skip (go (i - 1))
+ src/Ros/Util/PID.hs view
@@ -0,0 +1,130 @@+-- |Basic PID control.+module Ros.Util.PID where+import Data.IORef (newIORef, readIORef, writeIORef)+import Data.Time.Clock (getCurrentTime, diffUTCTime)++-- |A simple PID transfer function that assumes a unit sampling+-- interval. The first three parameters are the gains, the fourth+-- parameter is the desired setpoint, the fifth and sixth parameters+-- are the previous two errors, the seventh parameter is the most+-- recent system output. The return value is a tuple of the most+-- recent error and the computed controller output.+pidUniform :: Fractional a => a -> a -> a -> a -> a -> a -> a -> (a, a)+pidUniform kp ki kd obj = pidFixed kp ki kd obj 1+{-# INLINE pidUniform #-}++-- |PID controller with a fixed time interval between samples.+pidFixed :: Fractional a => a -> a -> a -> a -> a -> a -> a -> a -> (a,a)+pidFixed kp ki kd obj dt e1 e2 x = (e3, output)+  where e3 = x - obj+        invDt = 1 / dt+        scale = dt / 3+        integral = scale * (e1 + 4 * e2 + e3)+        derivative = (e3 - e2) * invDt+        output = kp * e3 + ki * integral + kd * derivative+{-# INLINE pidFixed #-}++-- |PID controller with explicit time stamps associated with each+-- sample. The order of the resultant tuples is (timeStamp, sample).+pidTimed :: Fractional a => a -> a -> a -> a -> (a,a) -> (a,a) -> (a,a) -> (a,a)+pidTimed kp ki kd obj (t1,e1) (_,e2) (t3,x) = (e3, output)+  where e3 = x - obj+        scale = (t3 - t1) / 6+        integral = scale * (e1 + 4 * e2 + e3)+        derivative = e3 - e2+        output = kp * e3 + ki * integral + kd * derivative+{-# INLINE pidTimed #-}++-- |A PID controller that maintains its own state. The first three+-- parameters are the gains, the fourth parameter is the desired+-- setpoint. The return value is an IO function that takes the newest+-- system output and returns the controller output.+pidFixedIO :: Fractional a => a -> a -> a -> a -> IO (a -> a -> IO a)+pidFixedIO kp ki kd dt = +  do e1 <- newIORef 0+     e2 <- newIORef 0+     initialized <- newIORef (0::Int)+     return $ \setpoint -> +       let pid' = pidFixed kp ki kd setpoint dt+       in \x -> +         do init' <- readIORef initialized+            case init' of+              0 -> do writeIORef e1 (x - setpoint)+                      writeIORef initialized 1+                      return 0+              1 -> do writeIORef e2 (x - setpoint)+                      writeIORef initialized 2+                      return 0+              _ -> do e1' <- readIORef e1+                      e2' <- readIORef e2+                      let (e3,c) = pid' e1' e2' x+                      writeIORef e1 e2'+                      e3 `seq` writeIORef e2 e3+                      return c++-- |A PID controller that assumes a uniform sampling interval of 1.+pidUniformIO :: Fractional a => a -> a -> a -> IO (a -> a -> IO a)+pidUniformIO kp ki kd = pidFixedIO kp ki kd 1++-- |A PID controller that uses the system clock to associate a+-- timestamp with each measurement that then used to determine the+-- sampling interval.+pidTimedIO :: Fractional a => a -> a -> a -> IO (a -> a -> IO a)+pidTimedIO kp ki kd =+  do go <- pidWithTimeIO kp ki kd+     start <- getCurrentTime+     return $ \setpoint -> \x ->+       do t <- fmap (realToFrac . flip diffUTCTime start) getCurrentTime+          go setpoint (t,x)++-- |A PID controller that takes values of the form (timeStamp, sample)+-- such that the associated timestamp is used to determine the+-- sampling rate.+pidWithTimeIO :: Fractional a => a -> a -> a -> IO (a -> (a,a) -> IO a)+pidWithTimeIO kp ki kd =+  do e1 <- newIORef undefined+     e2 <- newIORef undefined+     initialized <- newIORef (0::Int)+     return $ \setpoint ->+       let pid' = pidTimed kp ki kd setpoint+       in \(t,x) ->+         do init' <- readIORef initialized+            case init' of+              0 -> do writeIORef e1 (t, x - setpoint)+                      writeIORef initialized 1+                      return 0+              1 -> do writeIORef e2 (t, x - setpoint)+                      writeIORef initialized 2+                      return 0+              _ -> do e1' <- readIORef e1+                      e2' <- readIORef e2+                      let (e3,c) = pid' e1' e2' (t,x)+                      writeIORef e1 e2'+                      e3 `seq` writeIORef e2 (t,e3)+                      return c+                           +{-# SPECIALIZE+  pidUniformIO :: Double -> Double -> Double -> IO (Double -> Double -> IO Double)+  #-}++{-# SPECIALIZE+  pidUniformIO :: Float -> Float -> Float -> IO (Float -> Float -> IO Float)+  #-}++{-# SPECIALIZE+  pidFixedIO :: Double -> Double -> Double -> Double -> +                IO (Double -> Double -> IO Double)+  #-}++{-# SPECIALIZE+  pidFixedIO :: Float -> Float -> Float -> Float -> +                IO (Float -> Float -> IO Float)+  #-}++{-# SPECIALIZE+  pidTimedIO :: Double -> Double -> Double -> IO (Double -> Double -> IO Double)+  #-}++{-# SPECIALIZE+  pidTimedIO :: Float -> Float -> Float -> IO (Float -> Float -> IO Float)+  #-}
+ src/executable/Analysis.hs view
@@ -0,0 +1,230 @@+{-# LANGUAGE OverloadedStrings, TupleSections #-}+module Analysis (MsgInfo, liftIO, getTypeInfo, withMsg, getMsg, addMsg,+                 runAnalysis, isFlat, SerialInfo(..)) where+import Control.Applicative+import Control.Arrow ((&&&))+import Control.Monad.State+import Data.ByteString.Char8 (pack, unpack, ByteString)+import qualified Data.ByteString.Char8 as B+import qualified Data.Map as M+import Data.Maybe (isJust)+import System.FilePath (takeFileName, dropExtension)+import Ros.Internal.DepFinder (findMessagesInPkg)+import Types hiding (msgName)+import Parse+import ResolutionTypes++-- Synonym for a Msg paired with its SerialInfo. This tuple is cached+-- for message types as they are visited.+type SerialMsg = (SerialInfo, Msg)++-- Front-end to run analyses.+runAnalysis :: MsgInfo a -> IO a+runAnalysis m = evalStateT (cachePackage "rosgraph_msgs" >> m) emptyMsgContext++-- All the .msg files in a package are cached for quick lookup on+-- subsequent type resolutions.+cachePackage :: ByteString -> MsgInfo PkgCache+cachePackage pkgName = do (dir, msgs) <- liftIO $ findMessagesInPkg (unpack pkgName)+                          let cache = M.fromList (map prepMsg msgs)+                              pkg = (dir, M.empty, cache)+                          alterPkgMap (M.insert pkgName pkg)+                          return pkg+    where prepMsg = pack . dropExtension . takeFileName &&& Left++-- Get a package's 'MsgCache' from the package cache.+getPackage :: ByteString -> MsgInfo PkgCache+getPackage pkgName = +    do pkgs <- msgDefs <$> get+       maybe (cachePackage pkgName) return $ M.lookup pkgName pkgs++-- Try to get a 'Msg' from a given package. The first argument is the+-- package name, the second argument is the unqualified message name.+getMsgFromPkg :: ByteString -> ByteString -> MsgInfo (Maybe SerialMsg)+getMsgFromPkg pkgName msgName = getPackage pkgName >>= lookupMsg . msgCache+    where lookupMsg :: MsgCache -> MsgInfo (Maybe SerialMsg)+          lookupMsg cache = maybe (return Nothing) +                                  (return . Just <=< loadMsg)+                                  (M.lookup msgName cache)+          loadMsg :: (Either FilePath SerialMsg) -> MsgInfo SerialMsg+          loadMsg (Left fp) = either error addMsg =<< liftIO (parseMsg fp)+          loadMsg (Right m) = return m+          msgCache (_,_,c)  = c++getMsg :: ByteString -> MsgInfo SerialMsg+getMsg msgName = check <$>+                 if B.null msgType+                 then getMsgFromPkg "rosgraph_msgs" msgName <||>+                      getMsgFromPkg "std_msgs" msgName <||>+                      (flip getMsgFromPkg msgName . homePkg =<< get)+                 else getMsgFromPkg msgPkg (B.tail msgType)+    where (msgPkg, msgType) = B.span (/= '/') msgName+          check :: Maybe SerialMsg -> SerialMsg+          check Nothing = error $ "Couldn't resolve type " ++ unpack msgName+          check (Just m) = m+          -- checkLocal :: Maybe SerialMsg -> MsgInfo (Maybe SerialMsg)+          -- checkLocal Nothing = do home <- homePkg <$> get+          --                         getMsgFromPkg home msgName+          -- checkLocal info    = return info+          (<||>) :: (Applicative f, Alternative g) => f (g a) -> f (g a) -> f (g a)+          (<||>) = liftA2 (<|>)++isFlat :: Msg -> MsgInfo Bool+isFlat = fmap (all isStorable) . mapM (getTypeInfo . fieldType) . fields++isStorable :: SerialInfo -> Bool+isStorable = isJust . size++-- Add bindings for every MsgType referenced by this Msg to a Haskell+-- type and serialization information for that type to a 'MsgInfo'+-- context.+addMsg :: Msg -> MsgInfo SerialMsg+addMsg msg = do oldHome <- homePkg <$> get+                let pkgName = B.pack $ msgPackage msg+                    sName = pack $ shortName msg+                    tName = B.concat [sName, ".", sName]+                setHomePkg pkgName+                flat <- isFlat msg+                let ser = (if flat then defaultFlat else defaultNonFlat) $ tName+                addParsedMsg pkgName sName ser msg+                when (not (B.null oldHome)) (setHomePkg oldHome)+                return (ser,msg)++withMsg :: Msg -> MsgInfo a -> MsgInfo a+withMsg msg action = do _ <- addMsg msg+                        oldHome <- homePkg <$> get+                        setHomePkg . B.pack $ msgPackage msg+                        r <- action+                        setHomePkg oldHome+                        return r++-- Get a 'SerialInfo' value for a given 'MsgType'. If the specified+-- 'MsgType' has not yet been parsed, it will be resolved and parsed.+getTypeInfo :: MsgType -> MsgInfo SerialInfo+getTypeInfo mt = do pkg <- typeCache <$> (getPackage . homePkg =<< get)+                    aux . M.lookup mt $ pkg+    where aux Nothing = addField mt+          aux (Just info) = return info+          typeCache (_,tc,_) = tc++-- Default SerialInfo value for inductive (non-flat) values+defaultNonFlat :: ByteString -> SerialInfo+defaultNonFlat t = SerialInfo t "put" "get" Nothing++-- Default SerialInfo for a flat value.+defaultFlat :: ByteString -> SerialInfo+defaultFlat t = SerialInfo t "put" "get" (Just si)+    where si = B.concat ["sizeOf (P.undefined::", t, ")"]++-- Build a RHS for a sizeOf definition that multiplies a Storable+-- element's size by the number of elements a FixedArray of that type+-- contains.+mulSize :: Int -> SerialInfo -> ByteString+mulSize n (SerialInfo _ _ _ (Just sz)) = +    B.concat [pack $ show n, " * (", sz, ")"]+mulSize _ (SerialInfo _ _ _ Nothing) = +    error "Can't generate a Storable instance for a RFixedArray\+          \with a non-Storable element type."++-- NOTE: ROS specifies that we serialize booleans as a single byte,+-- while there is an instance of Haskell's 'Storable' class for 'Bool'+-- that uses four bytes for each boolean value. We handle individual+-- boolean values with the 'RosBinary' instance for the Haskell 'Bool'+-- type (i.e. one byte for each boolean). We must handle arrays of+-- booleans specially as the serialized form must still be one byte+-- per value.++-- | Deserialization source code string to read a vector of bytes,+-- then convert that to a vector of 'Bool's.+getBoolFromWord :: ByteString+getBoolFromWord = "P.fmap (V.map (P.> 0) :: V.Vector Word.Word8 \+                  \-> V.Vector P.Bool) get"++-- | Serialization source code string to convert a vector of 'Bool's+-- to a vector of bytes before serializing.+putWordFromBool :: ByteString+putWordFromBool = "(put . (V.map (P.fromIntegral . P.fromEnum)\+                  \ :: V.Vector P.Bool -> V.Vector Word.Word8))"++-- Add a SerialInfo value for a 'MsgType' to the 'MsgInfo' context.+addField :: MsgType -> MsgInfo SerialInfo+addField RString = ros2Hask RString >>= setTypeInfo RString . defaultNonFlat+addField t@(RVarArray RBool) =+  do lst <- vecOf RBool+     let arr = SerialInfo lst putWordFromBool getBoolFromWord Nothing+     setTypeInfo t arr+addField t@(RVarArray el) = +    do elInfo <- getTypeInfo el+       arr <- if isStorable elInfo+              then do lst <- vecOf el+                      return $ SerialInfo lst "put" "get" Nothing+              else do lst <- listOf el+                      return $ SerialInfo lst "putList" "getList" Nothing+       setTypeInfo t arr+addField t@(RFixedArray n RBool) =+  do lst <- vecOf RBool+     let arr = SerialInfo lst putWordFromBool getBoolFromWord+                          (Just . B.pack $ show n)+     setTypeInfo t arr+addField t@(RFixedArray n el) = +    do elInfo <- getTypeInfo el+       arr <- if isStorable elInfo+              then do lst <- vecOf el+                      return $ SerialInfo lst "put" "get" +                                          (Just $ mulSize n elInfo)+              else do lst <- listOf el+                      return $ SerialInfo lst "putFixedList" "getFixedList"+                                          Nothing+       setTypeInfo t arr+addField t@(RUserType n) = do userFlat <- isStorable . fst <$> getMsg n+                              t' <- ros2Hask t+                              setTypeInfo t $ (if userFlat+                                               then defaultFlat+                                               else defaultNonFlat) t'+addField t = ros2Hask t >>= setTypeInfo t . defaultFlat++-- Generate the name of the Haskell type that corresponds to a flat+-- (i.e. non-array) ROS type.+mkFlatType :: MsgType -> ByteString+mkFlatType RBool         = "P.Bool"+mkFlatType RInt8         = "Int.Int8"+mkFlatType RUInt8        = "Word.Word8"+mkFlatType RByte         = "Word.Word8"+mkFlatType RChar         = "Int.Int8"+mkFlatType RInt16        = "Int.Int16"+mkFlatType RUInt16       = "Word.Word16"+mkFlatType RInt32        = "P.Int"+mkFlatType RUInt32       = "Word.Word32"+mkFlatType RInt64        = "Int.Int64"+mkFlatType RUInt64       = "Word.Word64"+mkFlatType RFloat32      = "P.Float"+mkFlatType RFloat64      = "P.Double"+mkFlatType RTime         = "ROSTime"+mkFlatType RDuration     = "ROSDuration"+mkFlatType (RUserType t) = qualify . pack . takeFileName . unpack $ t+    where qualify b = B.concat [b, ".", b]+mkFlatType t             = error $ show t ++ " is not a flat type"++-- Given a home package name and a ROS 'MsgType', generate a Haskell+-- type name.+ros2Hask :: MsgType -> MsgInfo ByteString+ros2Hask (RFixedArray _ t) = mkArrayType t+ros2Hask (RVarArray t)     = mkArrayType t+ros2Hask RString           = return "P.String"+ros2Hask t                 = return $ mkFlatType t++vecOf :: MsgType -> MsgInfo ByteString+vecOf = getTypeInfo >=> return . B.append "V.Vector " . hType++listOf :: MsgType -> MsgInfo ByteString+listOf = getTypeInfo >=> return . buildString . hType+    where buildString t = B.concat ["[", t, "]"]++-- Make an array type declaration. If the element type of the+-- collection is flat (i.e. does not itself have a field of type+-- 'RVarArray'), then it will have a 'Storable' instance and can be+-- stored in a 'Vector'.+mkArrayType :: MsgType -> MsgInfo ByteString+mkArrayType t = toArr . isStorable =<< getTypeInfo t+    where toArr True = vecOf t+          toArr False = listOf t
+ src/executable/FieldImports.hs view
@@ -0,0 +1,71 @@+{-# LANGUAGE OverloadedStrings #-}+-- | Analyze a MsgType to determine the module imports needed for the+-- message field types.+module FieldImports (genImports) where+import Data.ByteString.Char8 (ByteString)+import qualified Data.ByteString.Char8 as B+import Data.Char (toUpper)+import Data.List (foldl')+import Data.Set (Set, singleton)+import qualified Data.Set as S+import Types++genImports :: ByteString -> [ByteString] -> [MsgType] -> ByteString+genImports pkgPath pkgMsgs fieldTypes = +    B.concat $ concatMap (\i -> ["import ", i, "\n"])+                         (S.toList (allImports fieldTypes))+    where getDeps = typeDependency pkgPath pkgMsgs+          allImports = foldl' ((. getDeps) . flip S.union) S.empty++intImport, wordImport, vectorDeps :: Set ByteString+intImport = singleton "qualified Data.Int as Int"+wordImport = singleton "qualified Data.Word as Word"+vectorDeps = S.fromList [ "qualified Data.Vector.Storable as V" ]++-- Generate a 'Set' of modules to be imported for the specified+-- 'MsgType'. The first argument is the package of the message type+-- currently being generated, the second argument is a list of all the+-- messages defined in that package (which can be referred to with+-- unqualified names), the third is the 'MsgType' to generate imports+-- for.+typeDependency :: ByteString -> [ByteString] -> MsgType -> Set ByteString+typeDependency _ _ RBool                = wordImport+typeDependency _ _ RInt8                = intImport+typeDependency _ _ RChar                = intImport+typeDependency _ _ RUInt8               = wordImport+typeDependency _ _ RByte                = wordImport+typeDependency _ _ RInt16               = intImport+typeDependency _ _ RUInt16              = wordImport+typeDependency _ _ RUInt32              = wordImport+typeDependency _ _ RUInt64              = wordImport+typeDependency _ _ RInt64               = intImport+typeDependency _ _ RTime                = singleton "Ros.Internal.RosTypes"+typeDependency _ _ RDuration            = singleton "Ros.Internal.RosTypes"+typeDependency p m (RFixedArray _ t)    = S.union vectorDeps $+                                       typeDependency p m t+typeDependency p m (RVarArray t)        = S.union vectorDeps $+                                       typeDependency p m t+typeDependency _ _ (RUserType "Header") = +    S.fromList ["qualified Ros.Std_msgs.Header as Header", +                "Ros.Internal.Msg.HeaderSupport"]+typeDependency p m (RUserType ut)       = if elem ut m+                                          then singleton $ +                                               B.concat ["qualified ", p, ut, +                                                         " as ", ut]+                                          else path2Module ut+typeDependency _ _ _                    = S.empty++-- Non built-in types are either in the specified package or in the+-- Ros.Std_msgs namespace. If a package path is given, then it is+-- converted to a Haskell hierarchical module name and prefixed by+-- "Ros.".+path2Module :: ByteString -> Set ByteString+path2Module p +    | B.elem '/' p = singleton $+                     B.concat ["qualified Ros.",+                               B.intercalate "." . map cap $ parts,+                               " as ", last parts]+    | otherwise    = singleton $+                     B.concat ["qualified Ros.Std_msgs.", p, " as ", p]+    where cap s = B.cons (toUpper (B.head s)) (B.tail s)+          parts = B.split '/' p
+ src/executable/Gen.hs view
@@ -0,0 +1,132 @@+-- |Generate Haskell source files for ROS .msg types.+{-# LANGUAGE OverloadedStrings #-}+module Gen (generateMsgType, generateSrvTypes) where+import Control.Applicative ((<$>), (<*>))+import Data.ByteString.Char8 (pack, ByteString)+import qualified Data.ByteString.Char8 as B+import Data.Char (toUpper)+import Analysis (MsgInfo, SerialInfo(..), withMsg, getTypeInfo)+import Types+import FieldImports+import Instances.Binary+import Instances.Storable+import MD5++data GenArgs = GenArgs {genExtraImport :: ByteString+                       , genPkgPath :: ByteString+                       , genPkgMsgs :: [ByteString]}++-- | pkgMsgs is a list of all the Mesages defined in the package (can be refered to+-- | with unqualified names)+generateSrvTypes :: ByteString -> [ByteString] -> Srv -> MsgInfo (ByteString, ByteString)+generateSrvTypes pkgPath pkgMsgs srv = do+  let msgs = [srvRequest srv, srvResponse srv]+  srvInfo <- mapM (genSrvInfo srv) msgs+  requestResponseMsgs <-+    mapM (generateMsgTypeExtraImport GenArgs{genExtraImport = "import Ros.Internal.Msg.SrvInfo\n"+                                               , genPkgPath=pkgPath+                                               , genPkgMsgs=pkgMsgs})+    msgs+  let [requestType, responseType] = zipWith B.append requestResponseMsgs srvInfo+  return (requestType, responseType)++generateMsgType :: ByteString -> [ByteString] -> Msg -> MsgInfo ByteString+generateMsgType pkgPath pkgMsgs =+  generateMsgTypeExtraImport GenArgs {genExtraImport=""+                                     , genPkgPath=pkgPath+                                     , genPkgMsgs=pkgMsgs}++generateMsgTypeExtraImport :: GenArgs -> Msg -> MsgInfo ByteString+generateMsgTypeExtraImport (GenArgs {genExtraImport=extraImport, genPkgPath=pkgPath, genPkgMsgs=pkgMsgs}) msg =+  do (fDecls, binInst, st, cons) <- withMsg msg $+                                    (,,,) <$> mapM generateField (fields msg)+                                          <*> genBinaryInstance msg+                                          <*> genStorableInstance msg+                                          <*> genConstants msg+     let fieldSpecs = B.intercalate lineSep fDecls+         (storableImport, storableInstance) = st+     --msgHash <- liftIO $ genHasHash msg+     msgHash <- genHasHash msg+     return $ B.concat [ modLine, "\n"+                       , imports+                       , storableImport+                       , if null fDecls+                         then dataSingleton+                         else B.concat [ dataLine+                                       , fieldSpecs+                                       , "\n"+                                       , fieldIndent+                                       , "} deriving (P.Show, P.Eq, P.Ord, T.Typeable, G.Generic)\n\n"]+                       , binInst, "\n\n"+                       , storableInstance+                       --, genNFDataInstance msg+                       , genHasHeader msg+                       , msgHash+                       , genDefault msg+                       , cons ]+    where name = shortName msg+          tName = pack $ toUpper (head name) : tail name+          modLine = B.concat ["{-# LANGUAGE OverloadedStrings, DeriveDataTypeable, DeriveGeneric #-}\n",+                              "module ", pkgPath, tName, " where"]+          imports = B.concat ["import qualified Prelude as P\n",+                              "import Prelude ((.), (+), (*))\n",+                              "import qualified Data.Typeable as T\n",+                              "import Control.Applicative\n",+                              "import Ros.Internal.RosBinary\n",+                              "import Ros.Internal.Msg.MsgInfo\n",+                              "import qualified GHC.Generics as G\n",+                              "import qualified Data.Default.Generics as D\n",+                              extraImport,+                              genImports pkgPath pkgMsgs +                                         (map fieldType (fields msg))]+                              --nfImport]+          dataLine = B.concat ["\ndata ", tName, " = ", tName, " { "]+          dataSingleton = B.concat ["\ndata ", tName, " = ", tName, +                                    " deriving (P.Show, P.Eq, P.Ord, T.Typeable, G.Generic)\n\n"]+          fieldIndent = B.replicate (B.length dataLine - 3) ' '+          lineSep = B.concat ["\n", fieldIndent, ", "]++genHasHeader :: Msg -> ByteString+genHasHeader m = +    if hasHeader m+    then let hn = fieldName (head (fields m)) -- The header field name+         in B.concat ["instance HasHeader ", pack (shortName m), " where\n",+                      "  getSequence = Header.seq . ", hn, "\n",+                      "  getFrame = Header.frame_id . ", hn, "\n",+                      "  getStamp = Header.stamp . " , hn, "\n",+                      "  setSequence seq x' = x' { ", hn, +                      " = (", hn, " x') { Header.seq = seq } }\n\n"]+    else ""++genDefault :: Msg -> ByteString+genDefault m = B.concat["instance D.Default ", pack (shortName m), "\n\n"]++genHasHash :: Msg -> MsgInfo ByteString+genHasHash m = fmap aux (msgMD5 m)+  where aux md5 = B.concat ["instance MsgInfo ", pack (shortName m),+                            " where\n  sourceMD5 _ = \"", pack md5,+                            "\"\n  msgTypeName _ = \"", pack (fullRosMsgName m),+                            "\"\n\n"]+genSrvInfo :: Srv -> Msg -> MsgInfo ByteString+genSrvInfo s m = fmap aux (srvMD5 s)+  where aux md5 = B.concat ["instance SrvInfo ", pack (shortName m),+                            " where\n  srvMD5 _ = \"", pack md5,+                            "\"\n  srvTypeName _ = \"", pack (fullRosSrvName s),+                            "\"\n\n"]++generateField :: MsgField -> MsgInfo ByteString+generateField (MsgField name t _) = do t' <- hType <$> getTypeInfo t+                                       return $ B.concat [name, " :: ", t']++genConstants :: Msg -> MsgInfo ByteString+genConstants = fmap B.concat . mapM buildLine . constants+    where escapeQuotes RString x =+            B.concat [ "\""+                     , B.intercalate "\\\"" (B.split '"' x)+                     , "\"" ]+          escapeQuotes _       x = x+          buildLine (MsgConst name rosType val _) = +              do t <- hType <$> getTypeInfo rosType+                 return $ B.concat [ name, " :: ", t, "\n"+                                   , name, " = "+                                   , escapeQuotes rosType val, "\n\n"]
+ src/executable/Instances/Binary.hs view
@@ -0,0 +1,37 @@+{-# LANGUAGE OverloadedStrings #-}+-- |Generate a Binary instance for ROS msg types.+module Instances.Binary (genBinaryInstance) where+import Control.Monad ((>=>))+import Data.ByteString.Char8 (ByteString, pack)+import qualified Data.ByteString.Char8 as B+import Types+import Analysis++serialize :: MsgType -> MsgInfo ByteString+serialize = getTypeInfo >=> return . putField++deserialize :: MsgType -> MsgInfo ByteString+deserialize = getTypeInfo >=> return . getField++putMsgHeader :: ByteString+putMsgHeader = "\n  putMsg = putStampedMsg"++genBinaryInstance :: Msg -> MsgInfo ByteString+genBinaryInstance m +  | null (fields m) = return $ +                      B.concat [ "instance RosBinary ", name'+                               , " where\n"+                               , "  put _  = pure ()\n"+                               , "  get = pure ", name']+  | otherwise = +    do puts <- mapM (\f -> serialize (fieldType f) >>= +                           return . buildPut (fieldName f)) +                    (fields m)+       gets <- mapM (deserialize . fieldType) (fields m)+       return $ B.concat ["instance RosBinary ", name', " where\n",+                          "  put obj' = ", B.intercalate " *> " puts,"\n",+                          "  get = ", name', " <$> ",+                          B.intercalate " <*> " gets,+                          if hasHeader m then putMsgHeader else ""]+    where buildPut f ser = B.concat [ser, " (", f, " obj')"]+          name' = pack (shortName m)
+ src/executable/Instances/NFData.hs view
@@ -0,0 +1,22 @@+{-# LANGUAGE OverloadedStrings #-}+-- |Generate an NFData instance for ROS msg types.+module Instances.NFData where+import Data.ByteString.Char8 (ByteString)+import qualified Data.ByteString.Char8 as B+import Types++nfImport :: ByteString+nfImport = "import qualified Control.DeepSeq as D\n"++genNFDataInstance :: Msg -> ByteString+genNFDataInstance msg = +  B.concat[ "instance D.NFData ", name, " where\n"+          , "  rnf = "+          , B.intercalate " `seqAp` " +                          (map (\n -> B.concat ["(D.rnf . ", n, ")"]) +                               fieldNames)+          , "\n"+          , "    where seqAp f g = (\\x y -> x `P.seq` y `P.seq` ()) <$> f <*> g\n\n"+          ]+    where name = B.pack (shortName msg)+          fieldNames = map fieldName (fields msg)
+ src/executable/Instances/Storable.hs view
@@ -0,0 +1,61 @@+{-# LANGUAGE OverloadedStrings #-}+-- |Generate a Storable instance for ROS msg types.+module Instances.Storable (genStorableInstance) where+import Control.Monad ((>=>))+import Data.ByteString.Char8 (ByteString, pack)+import qualified Data.ByteString.Char8 as B+import Data.Maybe (fromJust)+import Types+import Analysis++rosPoke :: MsgType -> ByteString+rosPoke (RFixedArray _ t) = B.concat [ "V.mapM_ (", rosPoke t, ")" ]+rosPoke _ = "SM.poke"++rosPeek :: MsgType -> ByteString+rosPeek (RFixedArray n t) = B.concat [ "V.replicateM ", B.pack (show n), +                                       " (", rosPeek t, ")" ]+rosPeek _ = "SM.peek"++genStorableInstance :: Msg -> MsgInfo (ByteString, ByteString)+genStorableInstance msg +  | null (fields msg) = return ("import Foreign.Storable (Storable(..))\n",+                                singletonStorable)+  | otherwise = isFlat msg >>= aux+    where aux False = return ("", "")+          aux True = do sz <- totalSize msg+                        return (smImp, stInst sz)+          --peekFields = map (const "SM.peek") (fields msg)+          peekFields = map (rosPeek . fieldType) (fields msg)+          -- pokeFields = map ((\n -> B.concat ["SM.poke (", n, " obj')"]) . +          --                   fieldName)+          --                  (fields msg)+          pokeFields = map (\f -> B.concat [ rosPoke (fieldType f)+                                           , " (", fieldName f+                                           , " obj')"])+                           (fields msg)+          name = pack (shortName msg)+          stInst sz = B.concat ["instance Storable ", name, " where\n",+                                "  sizeOf _ = ", sz,"\n",+                                "  alignment _ = 8\n",+                                "  peek = SM.runStorable (", name, " <$> ",+                                B.intercalate " <*> " peekFields, ")\n",+                                "  poke ptr' obj' = ",+                                "SM.runStorable store' ptr'\n",+                                "    where store' = ",+                                B.intercalate " *> " pokeFields,"\n\n"]+          smImp = B.concat [ "import Foreign.Storable (Storable(..))\n"+                           , "import qualified Ros.Internal.Util.StorableMonad"+                           , " as SM\n" ]+          singletonStorable = B.concat [ "instance Storable ", name, " where\n"+                                       , "  sizeOf _ = 1\n"+                                       , "  alignment _ = 1\n"+                                       , "  peek _ = pure ", name, "\n"+                                       , "  poke _ _ = pure ()\n\n" ]+++totalSize :: Msg -> MsgInfo ByteString+totalSize msg = B.intercalate sep `fmap` mapM (aux . fieldType) (fields msg)+    where aux = getTypeInfo >=> return . fromJust . size+          sep = B.append " +\n" $ B.replicate 13 ' '+
+ src/executable/MD5.hs view
@@ -0,0 +1,65 @@+{-# LANGUAGE OverloadedStrings #-}+module MD5 (msgMD5, srvMD5) where+import Data.ByteString.Char8 (ByteString)+import qualified Data.ByteString.Char8 as B+import Data.Digest.Pure.MD5 (md5)+import Data.ByteString.Lazy (fromChunks)+import Data.Map ((!))+import qualified Data.Map as M+import Parse (simpleFieldAssoc)+import Types+import Analysis+import Control.Applicative ((<$>))++tMap :: M.Map MsgType ByteString+tMap = M.fromList simpleFieldAssoc++typeText :: MsgType -> MsgInfo ByteString+typeText (RFixedArray n t) = +  case M.lookup t tMap of+    Just t' -> return $ B.concat [t', "[", B.pack . show $ n, "]"]+    Nothing -> typeText t+typeText (RVarArray t) =+  case M.lookup t tMap of+    Just t' -> return $ B.append t' "[]"+    Nothing -> typeText t+typeText (RUserType b) = fmap B.pack (getMsg b >>= msgMD5 . snd)+typeText t = return $ tMap ! t++md5sum :: ByteString -> String+md5sum = show . md5 . fromChunks . (:[])++-- The "MD5 text" of a message is the .msg text with+-- * comments removed+-- * whitespace removed+-- * package names of dependencies removed+-- * constants reordered ahead of other declarations+-- from http://www.ros.org/wiki/ROS/Technical%20Overview+--+-- For user defined types, rather than recording a field as "type+-- identifier", we record "md5 identifier".+msgMD5 :: Msg -> MsgInfo String+msgMD5 m = md5sum <$> msgMD5Text m++-- | Generate the text the md5 is computed from+msgMD5Text :: Msg -> MsgInfo ByteString+msgMD5Text m = let cs = map constantText (constants m)+               in withMsg m $+                do fs <- mapM fieldText (fields m)+                   return $ B.intercalate "\n" (cs ++ fs)+  where constantText (MsgConst _ t v n) = B.concat [tMap ! t, " ", n, "=", v]+        fieldText (MsgField _ t n) = do t' <- typeText t+                                        return $ B.concat [t', " ", n]++-- | The MD5 for the service is generated by concatenating the MD5 text+-- | for the request and response and then calculating the MD5 sum.+-- | I have not found any ROS specification for this, but this is how+-- | roslib does it (see _compute_hash in+-- | http://docs.ros.org/api/roslib/html/python/roslib.gentools-pysrc.html).+srvMD5 :: Srv -> MsgInfo String+srvMD5 s = do+  let request = srvRequest s+      response = srvResponse s+  reqText <- msgMD5Text request+  resText <- msgMD5Text response+  return . md5sum $ B.append reqText resText
+ src/executable/Main.hs view
@@ -0,0 +1,96 @@+-- |The main entry point for the roshask executable.+module Main (main) where+import Control.Applicative+import qualified Data.ByteString.Char8 as B+import System.Directory (createDirectoryIfMissing, getCurrentDirectory)+import System.Environment (getArgs)+import System.Exit (exitWith, ExitCode(..))+import System.FilePath (replaceExtension, isRelative, (</>), dropFileName, +                        takeFileName)+import Ros.Internal.DepFinder (findMessages, findPackageDeps, +                               findPackageDepsTrans)+import Ros.Internal.PathUtil (codeGenDir, pathToPkgName)++import Analysis (runAnalysis)+import Parse+import Gen+import MD5+import PkgBuilder (buildPkgMsgs)+import Unregister+import PkgInit (initPkg)++generateAndSave :: FilePath -> IO ()+generateAndSave fname = do msgType <- fst <$> generate fname+                           fname' <- hsName+                           B.writeFile fname' msgType+  where hsName = do d' <- codeGenDir fname+                    createDirectoryIfMissing True d'+                    return $ d' </> f+        f =  replaceExtension (takeFileName fname) ".hs"++-- Generate Haskell code for a message type.+generate :: FilePath -> IO (B.ByteString, String)+generate fname = +    do r <- parseMsg fname+       pkgMsgs <- map B.pack <$> findMessages dir+       case r of+         Left err -> do putStrLn $ "ERROR: " ++ err+                        exitWith (ExitFailure (-2))+         Right msg -> runAnalysis $ +                      do hMsg <- generateMsgType pkgHier pkgMsgs msg+                         md5 <- msgMD5 msg+                         return (hMsg, md5)+    where pkgHier = B.pack $ "Ros." ++ init pkgName ++ "."+          dir = dropFileName fname+          pkgName = pathToPkgName dir++-- |Run "roshask gen" on all the .msg files in each of the given+-- package directories.+buildDepMsgs :: [FilePath] -> IO ()+buildDepMsgs = runAnalysis . mapM_ buildPkgMsgs++-- When given a relative path, prepend it with the current working+-- directory.+canonicalizeName :: FilePath -> IO FilePath+canonicalizeName fname = if isRelative fname+                         then (</> fname) <$> getCurrentDirectory+                         else return fname++help :: [String]+help = [ "Usage: roshask command [[arguments]]"+       , "Available commands:"+       , "  create pkgName [[dependencies]]  -- Create a new ROS package with "+       , "                                      roshask support"+       , ""+       , "  gen file.msg                     -- Generate Haskell message code"+       , ""+       , "  dep                              -- Build all messages this package "+       , "                                      depends on"+       , ""+       , "  dep directory                    -- Build all messages the specified "+       , "                                      package depends on" +       , ""+       , "  md5 file.msg                     -- Generate an MD5 sum for a ROS "+       , "                                      message type"+       , ""+       , "  unregister                       -- Unregister all \"ROS-\"-prefixed"+       , "                                      packages using ghc-pkg. This is"+       , "                                      useful when you upgrade roshask"+       , "                                      and wish to remove all "+       , "                                      previously generated message"+       , "                                      libraries." ]++main :: IO ()+main = do args <- getArgs+          case args of+            ["gen",name] -> canonicalizeName name >>= generateAndSave+            ["md5",name] -> canonicalizeName name >>= +                            generate >>= putStrLn . snd+            ("create":pkgName:deps) -> initPkg pkgName deps+            ["unregister"] -> unregisterInteractive+            ["dep"] -> do d <- getCurrentDirectory +                          deps <- findPackageDepsTrans d+                          buildDepMsgs (deps++[d])+            ["dep",name] -> findPackageDeps name >>= (buildDepMsgs . (++[name]))+            _ -> do mapM_ putStrLn help+                    exitWith (ExitFailure (-1))
+ src/executable/Parse.hs view
@@ -0,0 +1,179 @@+{-# LANGUAGE OverloadedStrings, TupleSections #-}+-- | Parser components for the ROS message description language (@msg@+-- files). See http://wiki.ros.org/msg for reference.+module Parse (parseMsg, parseSrv, simpleFieldAssoc) where+import Prelude hiding (takeWhile)+import Control.Applicative+import Control.Arrow ((&&&))+import Data.Attoparsec.ByteString.Char8+import Data.ByteString (ByteString)+import Data.ByteString.Char8 (pack, unpack)+import qualified Data.ByteString.Char8 as B+import Data.Char (toLower, digitToInt)+import Data.Either (partitionEithers)+import Data.List (foldl')+import System.FilePath (dropExtension, takeFileName, splitDirectories)+import Types++simpleFieldTypes :: [MsgType]+simpleFieldTypes = [ RBool, RInt8, RUInt8, RInt16, RUInt16, RInt32, RUInt32, +                     RInt64, RUInt64, RFloat32, RFloat64, RString, +                     RTime, RDuration, RByte, RChar ]++simpleFieldAssoc :: [(MsgType, ByteString)]+simpleFieldAssoc = map (id &&& B.pack . map toLower . tail . show) +                       simpleFieldTypes++eatLine :: Parser ()+eatLine = manyTill anyChar (eitherP endOfLine endOfInput) *> skipSpace++parseName :: Parser ByteString+parseName = skipSpace *> identifier <* eatLine <* try comment++identifier :: Parser ByteString+identifier = B.cons <$> letter_ascii <*> takeWhile validChar+    where validChar c = any ($ c) [isDigit, isAlpha_ascii, (== '_'), (== '/')]++parseInt :: Parser Int+parseInt = foldl' (\s x -> s*10 + digitToInt x) 0 <$> many1 digit++comment :: Parser [()]+comment = many $ skipSpace *> try (char '#' *> eatLine)++simpleParser :: (MsgType, ByteString) -> Parser (ByteString, MsgType)+simpleParser (t,b) = (, t) <$> (string b *> space *> parseName)++fixedArrayParser :: (MsgType, ByteString) -> Parser (ByteString, MsgType)+fixedArrayParser (t,b) = (\len name -> (name, RFixedArray len t)) <$>+                         (string b *> char '[' *> parseInt <* char ']') <*> +                         (space *> parseName)++varArrayParser :: (MsgType, ByteString) -> Parser (ByteString, MsgType)+varArrayParser (t,b) = (, RVarArray t) <$> +                       (string b *> string "[]" *> space *> parseName)++userTypeParser :: Parser (ByteString, MsgType)+userTypeParser = choice [userSimple, userVarArray, userFixedArray]++userSimple :: Parser (ByteString, MsgType)+userSimple = (\t name -> (name, RUserType t)) <$>+             identifier <*> (space *> parseName)++userVarArray :: Parser (ByteString, MsgType)+userVarArray = (\t name -> (name, RVarArray (RUserType t))) <$>+               identifier <*> (string "[]" *> space *> parseName)++userFixedArray :: Parser (ByteString, MsgType)+userFixedArray = (\t n name -> (name, RFixedArray n (RUserType t))) <$>+                 identifier <*> +                 (char '[' *> parseInt <* char ']') <*> +                 (space *> parseName)++-- Parse constants defined in the message+constParser :: ByteString -> MsgType -> +               Parser (ByteString, MsgType, ByteString)+constParser s x = (,x,) <$> +                  (string s *> space *> identifier) <*> +                  (skipSpace *> char '=' *> skipSpace *> restOfLine <* skipSpace)+    where restOfLine :: Parser ByteString+          restOfLine = pack <$> manyTill anyChar (eitherP endOfLine endOfInput)++constParsers :: [Parser (ByteString, MsgType, ByteString)]+constParsers = map (uncurry constParser . swap) simpleFieldAssoc+  where swap (x,y) = (y,x)++-- String constants are parsed somewhat differently from numeric+-- constants. For numerical constants, we drop comments and trailing+-- spaces. For strings, we take the whole line (so comments aren't+-- stripped).+sanitizeConstants :: (a, MsgType, ByteString) -> (a, MsgType, ByteString)+sanitizeConstants c@(_, RString, _) = c+sanitizeConstants (name, t, val) = +    (name, t, B.takeWhile (\c -> c /= '#' && not (isSpace c)) val)++-- Parsers fields and constants.+fieldParsers :: [Parser (Either (ByteString, MsgType) +                                (ByteString, MsgType, ByteString))]+fieldParsers = map (comment *>) $+               map (Right . sanitizeConstants <$>) constParsers ++ +               map (Left <$>) (builtIns ++ [userTypeParser])+    where builtIns = concatMap (`map` simpleFieldAssoc)+                               [simpleParser, fixedArrayParser, varArrayParser]++mkParser :: MsgName -> String -> ByteString -> Parser Msg+mkParser sname lname txt = aux . partitionEithers <$> many (choice fieldParsers)+  where aux (fs, cs) = Msg sname lname txt+                           (map buildField fs)+                           (map buildConst cs)++buildField :: (ByteString, MsgType) -> MsgField+buildField (name,typ) = MsgField (sanitize name) typ name++buildConst :: (ByteString, MsgType, ByteString) -> MsgConst+buildConst (name,typ,val) = MsgConst (sanitize name) typ val name++{-+testMsg :: ByteString+testMsg = "# Foo bar\n\n#   \nHeader header  # a header\nuint32 aNum # a number \n  # It's not important\ngeometry_msgs/PoseStamped[] poses\nbyte DEBUG=1 #debug level\n"++test :: Result Msg+test = feed (parse (comment *> (mkParser "" "" testMsg)) testMsg) ""+-}++-- Ensure that field and constant names are valid Haskell identifiers+-- and do not coincide with Haskell reserved words.+sanitize :: ByteString -> ByteString+sanitize "data" = "_data"+sanitize "type" = "_type"+sanitize "class" = "_class"+sanitize "module" = "_module"+sanitize x = B.cons (toLower (B.head x)) (B.tail x)++pkgName :: FilePath -> String+pkgName f = let parts = splitDirectories f+                [pkg,_,_msgFile] = drop (length parts - 3) parts+            in pkg++parseMsg :: FilePath -> IO (Either String Msg)+parseMsg fname = do msgFile <- B.readFile fname+                    let tName = msgName . dropExtension . takeFileName $ fname+                        packageName = pkgName fname+                    return $ parseMsgWithName tName packageName msgFile++parseMsgWithName :: MsgName -> String -> ByteString -> Either String Msg+parseMsgWithName name packageName msgFile =+  case feed (parse parser msgFile) "" of+    Done leftOver msg+      | B.null leftOver -> Right msg+      | otherwise -> Left $ "Couldn't parse " ++ +                     unpack leftOver+    Fail _ _ctxt err -> Left err+    Partial _ -> Left "Incomplete msg definition"+  where+    parser = comment *> mkParser name packageName msgFile++-- | Parse a service file by splitting the file into a request and a response+-- | and parsing each part separately.+parseSrv :: FilePath -> IO (Either String Srv)+parseSrv fname = do+  srvFile <- B.readFile fname+  let (request, response) = splitService srvFile+      packageName = pkgName fname+      rawServiceName = dropExtension . takeFileName $ fname+  return $ do+    rqst <- parseMsgWithName (requestMsgName rawServiceName) packageName request+    resp <- parseMsgWithName (responseMsgName rawServiceName) packageName response+    return Srv{srvRequest = rqst+              , srvResponse = resp+              , srvName = msgName rawServiceName+              , srvPackage = packageName+              , srvSource = srvFile}++splitService :: ByteString -> (ByteString, ByteString)+splitService service = (request, response) where+  -- divider does not include newlines to allow it match even+  -- if there is no request or response message+  divider = "---"+  (request, dividerAndResponse) = B.breakSubstring divider service+  --Add 1 to the length of the divider to remove newline+  response = B.drop (1 + B.length divider) dividerAndResponse
+ src/executable/PkgInit.hs view
@@ -0,0 +1,112 @@+{-# LANGUAGE OverloadedStrings #-}+-- |Initialize a ROS Package with roshask support.+module PkgInit (initPkg) where+import Data.ByteString.Char8 (ByteString, pack)+import qualified Data.ByteString.Char8 as B+import Data.Char (toLower, toUpper)+import Data.List (intercalate)+import System.Directory (createDirectory)+import System.FilePath ((</>))+import System.Process (system)++import PkgBuilder (rosPkg2CabalPkg)++-- |Initialize a package with the given name in the eponymous+-- directory with the given ROS package dependencies.+initPkg :: String -> [String] -> IO ()+initPkg pkgName deps = do _ <- system pkgCmd+                          prepCMakeLists pkgName+                          prepCabal pkgName msgDeps+                          prepSetup pkgName+                          createDirectory (pkgName</>"src")+                          prepMain pkgName+                          prepLib pkgName+                          putStrLn (fyi pkgName)+    where pkgCmd = intercalate " " ("roscreate-pkg":pkgName:deps)+          msgDeps = B.intercalate ",\n" $ map (addSpaces . mkDep) deps+          addSpaces = B.append $ B.replicate 19 ' '+          mkDep = pack . rosPkg2CabalPkg++fyi :: String -> String+fyi pkgName = "Created an empty roshask package.\n" +++              "Please edit "+++              show (pkgName</>pkgName++".cabal") +++              " to specify what should be built."++-- Add an entry to the package's CMakeLists.txt file to invoke+-- roshask.+prepCMakeLists :: String -> IO ()+prepCMakeLists pkgName = B.appendFile (pkgName</>"CMakeLists.txt") cmd+    where cmd = "\nadd_custom_target(roshask ALL roshask dep ${PROJECT_SOURCE_DIR} COMMAND cd ${PROJECT_SOURCE_DIR} && cabal install)\n"++-- Generate a .cabal file and a Setup.hs that will perform the+-- necessary dependency tracking and code generation.+prepCabal :: String -> ByteString -> IO ()+prepCabal pkgName rosDeps = B.writeFile (pkgName</>(pkgName++".cabal")) $+                            B.concat [preamble,"\n",target,"\n\n",lib,"\n"]+    where preamble = format [ ("Name", B.append "ROS-" $ B.pack pkgName)+                            , ("Version","0.0")+                            , ("Synopsis","I am code")+                            , ("Cabal-version",">=1.6")+                            , ("Category","Robotics")+                            , ("Build-type","Custom") ]+          target = B.intercalate "\n" $+                   [ B.concat ["Executable ", pack pkgName]+                   , "  Build-Depends:   base >= 4.2 && < 6,"+                   , "                   vector > 0.7,"+                   , "                   time >= 1.1,"+                   , "                   ROS-rosgraph-msgs,"+                   , B.append "                   roshask == 0.1.*" moreDeps+                   , rosDeps+                   , "  GHC-Options:     -O2"+                   , "  Hs-Source-Dirs:  src"+                   , "  Main-Is:         Main.hs"+                   , B.append "  Other-Modules:   " pkgName' ]+          lib = B.intercalate "\n" $+                [ "Library"+                , "  Build-Depends:   base >= 4.2 && < 6,"+                , "                   vector > 0.7,"+                , "                   time >= 1.1,"+                , B.append "                   roshask == 0.1.*" moreDeps+                , rosDeps+                , "  GHC-Options:     -O2"+                , B.concat ["  Exposed-Modules: ", pkgName']+                , "  Hs-Source-Dirs:  src" ]  +          pkgName' = pack $ toUpper (head pkgName) : tail pkgName+          moreDeps = if B.null rosDeps then "" else ","++-- Format key-value pairs for a .cabal file+format :: [(ByteString,ByteString)] -> ByteString+format fields = B.concat $ map indent fields+    where indent (k,v) = let spaces = flip B.replicate ' ' $+                                      19 - B.length k - 1+                         in B.concat [k,":",spaces,v,"\n"]++-- Generate a Setup.hs file for use by Cabal.+prepSetup :: String -> IO ()+prepSetup pkgName = B.writeFile (pkgName</>"Setup.hs") $+                    B.concat [ "import Distribution.Simple\n"+                             , "import Ros.Internal.SetupUtil\n\n"+                             , "main = defaultMainWithHooks $\n"+                             , "       simpleUserHooks { confHook = "+                             , "rosConf }\n" ]++prepMain :: String -> IO ()+prepMain pkgName = writeFile (pkgName</>"src"</>"Main.hs") $+                   "module Main (main) where\n\+                   \import Ros.Node\n\+                   \import "++pkgName'++"\n\+                   \\n\+                   \main = runNode \""++pkgName++"\" "++nodeName++"\n"+    where pkgName' = toUpper (head pkgName) : tail pkgName+          nodeName = toLower (head pkgName) : tail pkgName++prepLib :: String -> IO ()+prepLib pkgName = writeFile (pkgName</>"src"</>pkgName'++".hs") . concat $+                  [ "module "++pkgName'++" ("++nodeName++") where\n"+                  , "import Ros.Node\n\n"+                  , nodeName++" :: Node ()\n"+                  , nodeName++" = return ()\n"]+  where pkgName' = toUpper (head pkgName) : tail pkgName+        nodeName = toLower (head pkgName) : tail pkgName+
+ src/executable/ResolutionTypes.hs view
@@ -0,0 +1,70 @@+-- |A monad within which to perform message type resolution.+module ResolutionTypes where+import Control.Monad.State+import Data.ByteString.Char8 (ByteString)+import qualified Data.ByteString.Char8 as B+import Data.Map (Map, insert, alter)+import qualified Data.Map as M++import Types++type MsgCache = Map ByteString (Either FilePath (SerialInfo, Msg))+type TypeCache = Map MsgType SerialInfo+type PkgCache = (FilePath, TypeCache, MsgCache)++-- Mapping from package names to a tuple of the package's directory+-- and a map from the names of message types defined in that package+-- to parsed 'Msg' values for those message types.+type PackageDefs = Map ByteString (FilePath, TypeCache, MsgCache)++-- The context in which message types are resolved is a pair of a+-- current home package (used to lookup unqualified message type+-- names) and a mapping from previously encountered message types to+-- serial info.+data MsgContext = MsgContext { homePkg   :: ByteString+                             -- ^Current package for unqualified names+                             , msgDefs   :: PackageDefs+                             -- ^Mapping from package name to directory +                             }++-- Code snippets for use in assembling a Haskell declaration (using+-- the hType field that represents a Haskell type), RosBinary and,+-- optionally, Storable instances for a message type that contains a+-- field of this type.+data SerialInfo = SerialInfo { hType    :: ByteString+                             , putField :: ByteString+                             , getField :: ByteString+                             , size     :: Maybe ByteString } +                  deriving Show++type MsgInfo = StateT MsgContext IO++-- | An empty 'MsgContext' from which one can begin running a+-- 'MsgInfo'.+emptyMsgContext :: MsgContext+emptyMsgContext = MsgContext B.empty M.empty++setHomePkg :: ByteString -> MsgInfo ()+setHomePkg = modify . aux+    where aux n ctxt = ctxt { homePkg = n }++setTypeInfo :: MsgType -> SerialInfo -> MsgInfo SerialInfo+setTypeInfo mt info = modify aux >> return info+    where aux ctxt = let home = homePkg ctxt+                         defs' = alter (fmap addType) home (msgDefs ctxt)+                     in ctxt { msgDefs = defs' }+          addType (p,tc,mc) = (p, insert mt info tc, mc)++-- Apply the function f to the msgDefs field of the current state.+alterPkgMap :: (PackageDefs -> PackageDefs) -> MsgInfo ()+alterPkgMap f = modify aux+    where aux ctxt = ctxt { msgDefs = f (msgDefs ctxt) }++-- Add a binding from a message type string to a parsed 'Msg' value to+-- a package's existing map.+addParsedMsg :: ByteString -> ByteString -> SerialInfo -> Msg -> MsgInfo ()+addParsedMsg pkg msgType info msg =+  do ctxt <- get+     let defs' = alter (fmap aux) pkg (msgDefs ctxt)+     put $ ctxt { msgDefs = defs' }+  where aux (p, tc, mc) = (p, tc, insert msgType (Right (info, msg)) mc)
+ src/executable/Unregister.hs view
@@ -0,0 +1,42 @@+-- |Helper to unregister all ROS-related packages known by @ghc-pkg@.+module Unregister (unregisterInteractive) where+import Control.Applicative+import Data.Char+import Data.List+import Data.Maybe+import PkgBuilder (myReadProcess, toolPaths, ToolPaths(..))+import System.Exit (ExitCode(..))+import System.IO (hFlush, stdout)+import System.Process++findROSPackages :: ToolPaths -> IO [String]+findROSPackages tools =+  mapMaybe (isROS . dropWhile isSpace) . lines+  <$> myReadProcess (ghcPkg tools ["list"])+  where isROS s+          | "ROS-" `isPrefixOf` s = Just s+          | otherwise = Nothing++unregisterPackage :: ToolPaths -> String -> IO ()+unregisterPackage tools pkg =+  do (_,_,_, ph) <-+       createProcess (ghcPkg tools ("unregister":pkg:(forceFlag tools)))+     waitForProcess ph >>= \ex -> case ex of+       ExitSuccess -> return ()+       ExitFailure e -> putStrLn $ "Unregistering "++pkg++" failed: "++show e++unregisterInteractive :: IO ()+unregisterInteractive = +  do tools <- toolPaths+     pkgs <- findROSPackages tools+     if null pkgs+       then putStrLn "No ROS packages to remove!"+       else do putStrLn "Ready to remove the following packags:"+               mapM_ (putStrLn . ("  "++)) pkgs+               putStr "Are you sure you want to continue (y/n)? "+               hFlush stdout+               s <- map toLower <$> getLine+               if s == "n" || s == "no"+                 then putStrLn "Cancelling unregistration."+                 else mapM_ (unregisterPackage tools) pkgs >>+                      putStrLn "Unregistration complete!"