diff --git a/Examples/PubSub/CMakeLists.txt b/Examples/PubSub/CMakeLists.txt
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/CMakeLists.txt
@@ -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)
diff --git a/Examples/PubSub/Makefile b/Examples/PubSub/Makefile
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/Makefile
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
diff --git a/Examples/PubSub/PubSub.cabal b/Examples/PubSub/PubSub.cabal
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/PubSub.cabal
@@ -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
diff --git a/Examples/PubSub/Setup.hs b/Examples/PubSub/Setup.hs
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/Setup.hs
@@ -0,0 +1,5 @@
+import Distribution.Simple
+import Ros.Internal.SetupUtil
+
+main = defaultMainWithHooks $
+ simpleUserHooks { confHook = rosConf }
diff --git a/Examples/PubSub/mainpage.dox b/Examples/PubSub/mainpage.dox
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b PubSub is an example of publishing and subscribing to Topics using roshask.
+
+
+
+
+\section codeapi Code API
+
+
+
+
+*/
diff --git a/Examples/PubSub/manifest.xml b/Examples/PubSub/manifest.xml
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/manifest.xml
@@ -0,0 +1,15 @@
+
+
+
+ PubSub
+
+
+ Anthony Cowley
+ BSD
+
+ http://ros.org/wiki/PubSub
+
+
+
+
+
diff --git a/Examples/PubSub/src/Listener.hs b/Examples/PubSub/src/Listener.hs
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/src/Listener.hs
@@ -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"
diff --git a/Examples/PubSub/src/Talker.hs b/Examples/PubSub/src/Talker.hs
new file mode 100644
--- /dev/null
+++ b/Examples/PubSub/src/Talker.hs
@@ -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)
diff --git a/Examples/Turtle/CMakeLists.txt b/Examples/Turtle/CMakeLists.txt
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/CMakeLists.txt
@@ -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)
diff --git a/Examples/Turtle/Makefile b/Examples/Turtle/Makefile
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/Makefile
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
diff --git a/Examples/Turtle/Setup.hs b/Examples/Turtle/Setup.hs
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/Setup.hs
@@ -0,0 +1,5 @@
+import Distribution.Simple
+import Ros.Internal.SetupUtil
+
+main = defaultMainWithHooks $
+ simpleUserHooks { confHook = rosConf }
diff --git a/Examples/Turtle/Turtle.cabal b/Examples/Turtle/Turtle.cabal
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/Turtle.cabal
@@ -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
diff --git a/Examples/Turtle/mainpage.dox b/Examples/Turtle/mainpage.dox
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b Turtle is ...
+
+
+
+
+\section codeapi Code API
+
+
+
+
+*/
diff --git a/Examples/Turtle/manifest.xml b/Examples/Turtle/manifest.xml
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/manifest.xml
@@ -0,0 +1,16 @@
+
+
+
+ Turtle
+
+
+ Anthony Cowley
+ BSD
+
+ http://ros.org/wiki/Turtle
+
+
+
+
+
+
diff --git a/Examples/Turtle/src/AngleNum.hs b/Examples/Turtle/src/AngleNum.hs
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/src/AngleNum.hs
@@ -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π, 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
diff --git a/Examples/Turtle/src/Turtle.hs b/Examples/Turtle/src/Turtle.hs
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/src/Turtle.hs
@@ -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)
diff --git a/Examples/Turtle/src/Turtle2.hs b/Examples/Turtle/src/Turtle2.hs
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/src/Turtle2.hs
@@ -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)
diff --git a/Examples/Turtle/src/Turtle3.hs b/Examples/Turtle/src/Turtle3.hs
new file mode 100644
--- /dev/null
+++ b/Examples/Turtle/src/Turtle3.hs
@@ -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
diff --git a/LICENSE b/LICENSE
new file mode 100644
--- /dev/null
+++ b/LICENSE
@@ -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.
diff --git a/Setup.hs b/Setup.hs
new file mode 100644
--- /dev/null
+++ b/Setup.hs
@@ -0,0 +1,2 @@
+import Distribution.Simple
+main = defaultMain
diff --git a/Tests/AllTests.hs b/Tests/AllTests.hs
new file mode 100644
--- /dev/null
+++ b/Tests/AllTests.hs
@@ -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 ]
diff --git a/Tests/ServiceClientTests/ServiceClientTest.hs b/Tests/ServiceClientTests/ServiceClientTest.hs
new file mode 100644
--- /dev/null
+++ b/Tests/ServiceClientTests/ServiceClientTest.hs
@@ -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
diff --git a/roshask.cabal b/roshask.cabal
new file mode 100644
--- /dev/null
+++ b/roshask.cabal
@@ -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
+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 () is a software
+ framework developed by Willow Garage
+ () 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
+ 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
diff --git a/src/Ros/Graph/Master.hs b/src/Ros/Graph/Master.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Graph/Master.hs
@@ -0,0 +1,94 @@
+{-| Client functionality for the
+-}
+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"
diff --git a/src/Ros/Graph/ParameterServer.hs b/src/Ros/Graph/ParameterServer.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Graph/ParameterServer.hs
@@ -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
diff --git a/src/Ros/Graph/Slave.hs b/src/Ros/Graph/Slave.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Graph/Slave.hs
@@ -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
diff --git a/src/Ros/Internal/DepFinder.hs b/src/Ros/Internal/DepFinder.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/DepFinder.hs
@@ -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 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 "msg"
+
+--TODO: refactor with findMessages
+findServices :: FilePath -> IO [FilePath]
+findServices pkgRoot =
+ do e <- doesDirectoryExist dir
+ if e then F.find (depth "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
diff --git a/src/Ros/Internal/Header.hs b/src/Ros/Internal/Header.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Header.hs
@@ -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"
diff --git a/src/Ros/Internal/Log.hs b/src/Ros/Internal/Log.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Log.hs
@@ -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
diff --git a/src/Ros/Internal/Msg/HeaderSupport.hs b/src/Ros/Internal/Msg/HeaderSupport.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Msg/HeaderSupport.hs
@@ -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
diff --git a/src/Ros/Internal/Msg/MsgInfo.hs b/src/Ros/Internal/Msg/MsgInfo.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Msg/MsgInfo.hs
@@ -0,0 +1,5 @@
+module Ros.Internal.Msg.MsgInfo where
+
+class MsgInfo a where
+ sourceMD5 :: a -> String
+ msgTypeName :: a -> String
diff --git a/src/Ros/Internal/Msg/SrvInfo.hs b/src/Ros/Internal/Msg/SrvInfo.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Msg/SrvInfo.hs
@@ -0,0 +1,5 @@
+module Ros.Internal.Msg.SrvInfo where
+
+class SrvInfo a where
+ srvMD5 :: a -> String
+ srvTypeName :: a -> String
diff --git a/src/Ros/Internal/PathUtil.hs b/src/Ros/Internal/PathUtil.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/PathUtil.hs
@@ -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
diff --git a/src/Ros/Internal/RosBinary.hs b/src/Ros/Internal/RosBinary.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/RosBinary.hs
@@ -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
diff --git a/src/Ros/Internal/RosTime.hs b/src/Ros/Internal/RosTime.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/RosTime.hs
@@ -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
diff --git a/src/Ros/Internal/RosTypes.hs b/src/Ros/Internal/RosTypes.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/RosTypes.hs
@@ -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)
diff --git a/src/Ros/Internal/SetupUtil.hs b/src/Ros/Internal/SetupUtil.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/SetupUtil.hs
@@ -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'
+
diff --git a/src/Ros/Internal/Util/AppConfig.hs b/src/Ros/Internal/Util/AppConfig.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Util/AppConfig.hs
@@ -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
diff --git a/src/Ros/Internal/Util/ArgRemapping.hs b/src/Ros/Internal/Util/ArgRemapping.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Util/ArgRemapping.hs
@@ -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" ]
+-}
diff --git a/src/Ros/Internal/Util/BytesToVector.hs b/src/Ros/Internal/Util/BytesToVector.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Util/BytesToVector.hs
@@ -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)
diff --git a/src/Ros/Internal/Util/RingChan.hs b/src/Ros/Internal/Util/RingChan.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Util/RingChan.hs
@@ -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)
diff --git a/src/Ros/Internal/Util/StorableMonad.hs b/src/Ros/Internal/Util/StorableMonad.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Internal/Util/StorableMonad.hs
@@ -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)
diff --git a/src/Ros/Logging.hs b/src/Ros/Logging.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Logging.hs
@@ -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)
diff --git a/src/Ros/Node.hs b/src/Ros/Node.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node.hs
@@ -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'
diff --git a/src/Ros/Node/BinaryIter.hs b/src/Ros/Node/BinaryIter.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node/BinaryIter.hs
@@ -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
diff --git a/src/Ros/Node/ConnectionHeader.hs b/src/Ros/Node/ConnectionHeader.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node/ConnectionHeader.hs
@@ -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
diff --git a/src/Ros/Node/RosTcp.hs b/src/Ros/Node/RosTcp.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node/RosTcp.hs
@@ -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
diff --git a/src/Ros/Node/RunNode.hs b/src/Ros/Node/RunNode.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node/RunNode.hs
@@ -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
diff --git a/src/Ros/Node/Type.hs b/src/Ros/Node/Type.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Node/Type.hs
@@ -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')
diff --git a/src/Ros/Rate.hs b/src/Ros/Rate.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Rate.hs
@@ -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
diff --git a/src/Ros/Service.hs b/src/Ros/Service.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Service.hs
@@ -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
diff --git a/src/Ros/Service/ServiceTypes.hs b/src/Ros/Service/ServiceTypes.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Service/ServiceTypes.hs
@@ -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)
+
diff --git a/src/Ros/Topic.hs b/src/Ros/Topic.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic.hs
@@ -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)
+
diff --git a/src/Ros/Topic/PID.hs b/src/Ros/Topic/PID.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic/PID.hs
@@ -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
diff --git a/src/Ros/Topic/Stamped.hs b/src/Ros/Topic/Stamped.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic/Stamped.hs
@@ -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
diff --git a/src/Ros/Topic/Stats.hs b/src/Ros/Topic/Stats.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic/Stats.hs
@@ -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
diff --git a/src/Ros/Topic/Transformers.hs b/src/Ros/Topic/Transformers.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic/Transformers.hs
@@ -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')
diff --git a/src/Ros/Topic/Util.hs b/src/Ros/Topic/Util.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Topic/Util.hs
@@ -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))
diff --git a/src/Ros/Util/PID.hs b/src/Ros/Util/PID.hs
new file mode 100644
--- /dev/null
+++ b/src/Ros/Util/PID.hs
@@ -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)
+ #-}
diff --git a/src/executable/Analysis.hs b/src/executable/Analysis.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Analysis.hs
@@ -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
diff --git a/src/executable/FieldImports.hs b/src/executable/FieldImports.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/FieldImports.hs
@@ -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
diff --git a/src/executable/Gen.hs b/src/executable/Gen.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Gen.hs
@@ -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"]
diff --git a/src/executable/Instances/Binary.hs b/src/executable/Instances/Binary.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Instances/Binary.hs
@@ -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)
diff --git a/src/executable/Instances/NFData.hs b/src/executable/Instances/NFData.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Instances/NFData.hs
@@ -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)
diff --git a/src/executable/Instances/Storable.hs b/src/executable/Instances/Storable.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Instances/Storable.hs
@@ -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 ' '
+
diff --git a/src/executable/MD5.hs b/src/executable/MD5.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/MD5.hs
@@ -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
diff --git a/src/executable/Main.hs b/src/executable/Main.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Main.hs
@@ -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))
diff --git a/src/executable/Parse.hs b/src/executable/Parse.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Parse.hs
@@ -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
diff --git a/src/executable/PkgInit.hs b/src/executable/PkgInit.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/PkgInit.hs
@@ -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 (pkgNamepkgName++".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
+
diff --git a/src/executable/ResolutionTypes.hs b/src/executable/ResolutionTypes.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/ResolutionTypes.hs
@@ -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)
diff --git a/src/executable/Unregister.hs b/src/executable/Unregister.hs
new file mode 100644
--- /dev/null
+++ b/src/executable/Unregister.hs
@@ -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!"