diff --git a/.travis.yml b/.travis.yml index de7abb96c..febd54ca5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,4 @@ language: julia -dist: trusty -sudo: required os: - linux - osx @@ -9,6 +7,10 @@ julia: - nightly notifications: email: false +addons: + apt: + packages: + - hdf5-tools matrix: allow_failures: - os: osx diff --git a/examples/auv/Sandshark/Sandshark.jl b/examples/marine/auv/Sandshark/Sandshark.jl similarity index 100% rename from examples/auv/Sandshark/Sandshark.jl rename to examples/marine/auv/Sandshark/Sandshark.jl diff --git a/examples/rov/asfm/ASfMExample.jl b/examples/marine/rov/asfm/ASfMExample.jl similarity index 100% rename from examples/rov/asfm/ASfMExample.jl rename to examples/marine/rov/asfm/ASfMExample.jl diff --git a/examples/rov/asfm/LRBEroll45deg.jl b/examples/marine/rov/asfm/LRBEroll45deg.jl similarity index 100% rename from examples/rov/asfm/LRBEroll45deg.jl rename to examples/marine/rov/asfm/LRBEroll45deg.jl diff --git a/examples/rov/asfm/LRBEyaw90deg.jl b/examples/marine/rov/asfm/LRBEyaw90deg.jl similarity index 100% rename from examples/rov/asfm/LRBEyaw90deg.jl rename to examples/marine/rov/asfm/LRBEyaw90deg.jl diff --git a/examples/rov/asfm/LRBEzpitch45deg.jl b/examples/marine/rov/asfm/LRBEzpitch45deg.jl similarity index 100% rename from examples/rov/asfm/LRBEzpitch45deg.jl rename to examples/marine/rov/asfm/LRBEzpitch45deg.jl diff --git a/examples/rov/asfm/animateSonarSim.jl b/examples/marine/rov/asfm/animateSonarSim.jl similarity index 100% rename from examples/rov/asfm/animateSonarSim.jl rename to examples/marine/rov/asfm/animateSonarSim.jl diff --git a/examples/marine/rov/highend/lcmHandlers.jl b/examples/marine/rov/highend/lcmHandlers.jl new file mode 100644 index 000000000..af4c3bfff --- /dev/null +++ b/examples/marine/rov/highend/lcmHandlers.jl @@ -0,0 +1,167 @@ +""" +Adds pose nodes to graph with a prior on Z, pitch, and roll. +""" +function handle_poses!(slam::SyncrSLAM, + msg::pose_node_t) + id = msg.id + println("[Caesar.jl] Received pose msg for x$(id)") + + mean = msg.mean + covar = msg.covar + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) + # + node_label = Symbol("x$(id)") + varRequest = VariableRequest(node_label, "Pose3", nothing, ["POSE"]) + resp = addVariable(slam.syncrconf, slam.robotId, slam.sessionId, varRequest) + # + if id == 0 + println("[Caesar.jl] First pose") + # this is the first msg, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = RoME.PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + packedPrior = convert(RoME.PackedPriorPose3, initPosePrior) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(initPosePrior)), string(typeof(packedPrior)), "JSON", JSON.json(packedPrior)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slam) + end +end + +""" +Handle ZPR priors on poses. +""" +function handle_priors!(slam::SyncrSLAM, + msg::prior_zpr_t) + + id = msg.id + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + # 1. Build up the prior + z = msg.z + pitch = msg.pitch + roll = msg.roll + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = RoME.PartialPriorRollPitchZ(rp_dist, z_dist) + + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_prior_rpz = convert(RoME.PackedPartialPriorRollPitchZ, prior_rpz) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(prior_rpz)), string(typeof(packed_prior_rpz)), "JSON", JSON.json(packed_prior_rpz)) + fctRequest = FactorRequest([node_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) +end + +""" +Handle partial x, y, and heading odometry constraints between Pose3 variables. +""" +function handle_partials!(slam::SyncrSLAM, + msg::Any) + println(" --- Handling odometry change...") + return + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYaw(xyh_dist) + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_xyh_factor = convert(RoME.PackedPartialPose3XYYaw, xyh_factor) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) + fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) +end + +""" +Handle loop closure proposals with chance of being a null hypothesis likelihood. +""" +function handle_loops!(slam::SyncrSLAM, + msg::pose_pose_xyh_nh_t) + + println(" --- Handling loop...") + # return nothing + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + confidence = msg.confidence + + println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = RoME.PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH + # 2. Pack the prior (we can automate this step soon, but for now it's hand cranking) + packed_xyh_factor = convert(RoME.PackedPartialPose3XYYawNH, xyh_factor) + + # 3. Build the factor request (again, we can make this way easier and transparent once it's stable) + fctBody = FactorBody(string(typeof(xyh_factor)), string(typeof(packed_xyh_factor)), "JSON", JSON.json(packed_xyh_factor)) + fctRequest = FactorRequest([origin_label; destination_label], fctBody, false, false) + @show resp = addFactor(slam.syncrconf, slam.robotId, slam.sessionId, fctRequest) +end + +""" +Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client +""" +function handle_clouds!(slam::SyncrSLAM, + msg::point_cloud_t) + id = msg.id + last_pose = "x$(id)" + println("[Caesar.jl] Got cloud $id") + + # 2d arrays of points and colors (from LCM data into arrays{arrays}) + # @show size(msg.points) + # @show size(msg.colors) + # points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + points = JSON.json(msg.points) + # @show "HERREEEEEE" + # colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] + colors = JSON.json(msg.colors) + + node = getNode(slam.syncrconf, slam.robotId, slam.sessionId, last_pose) + pointCloud = BigDataElementRequest("PointCloud", "Mongo", "Pointcloud from HAUV", points, "application/binary") + colors = BigDataElementRequest("Colors", "Mongo", "Colors from HAUV", colors, "application/binary") + addDataElement(slam.syncrconf, slam.robotId, slam.sessionId, node.id, pointCloud) + addDataElement(slam.syncrconf, slam.robotId, slam.sessionId, node.id, colors) + # serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + # appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) + # serialized_colors = BSONObject(Dict("colors" => colors)) + # appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) +end diff --git a/examples/lcmserver/.gitignore b/examples/marine/rov/highend/lcmserver/.gitignore similarity index 100% rename from examples/lcmserver/.gitignore rename to examples/marine/rov/highend/lcmserver/.gitignore diff --git a/examples/lcmserver/README.md b/examples/marine/rov/highend/lcmserver/README.md similarity index 100% rename from examples/lcmserver/README.md rename to examples/marine/rov/highend/lcmserver/README.md diff --git a/examples/lcmserver/data.txt b/examples/marine/rov/highend/lcmserver/data.txt similarity index 100% rename from examples/lcmserver/data.txt rename to examples/marine/rov/highend/lcmserver/data.txt diff --git a/examples/lcmserver/drawtoolsfromdb.jl b/examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl similarity index 100% rename from examples/lcmserver/drawtoolsfromdb.jl rename to examples/marine/rov/highend/lcmserver/drawtoolsfromdb.jl diff --git a/examples/lcmserver/hauvvisualizationdebug.jl b/examples/marine/rov/highend/lcmserver/hauvvisualizationdebug.jl similarity index 100% rename from examples/lcmserver/hauvvisualizationdebug.jl rename to examples/marine/rov/highend/lcmserver/hauvvisualizationdebug.jl diff --git a/examples/lcmserver/lcmdbservice.jl b/examples/marine/rov/highend/lcmserver/lcmdbservice.jl similarity index 100% rename from examples/lcmserver/lcmdbservice.jl rename to examples/marine/rov/highend/lcmserver/lcmdbservice.jl diff --git a/examples/lcmserver/lcmpose3server.jl b/examples/marine/rov/highend/lcmserver/lcmpose3server.jl similarity index 100% rename from examples/lcmserver/lcmpose3server.jl rename to examples/marine/rov/highend/lcmserver/lcmpose3server.jl diff --git a/examples/lcmserver/pointclouddev.jl b/examples/marine/rov/highend/lcmserver/pointclouddev.jl similarity index 100% rename from examples/lcmserver/pointclouddev.jl rename to examples/marine/rov/highend/lcmserver/pointclouddev.jl diff --git a/examples/lcmserver/resolvingPartialXYH.jl b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl similarity index 100% rename from examples/lcmserver/resolvingPartialXYH.jl rename to examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl diff --git a/examples/lcmserver/resolvingPartialXYH2.jl b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH2.jl similarity index 100% rename from examples/lcmserver/resolvingPartialXYH2.jl rename to examples/marine/rov/highend/lcmserver/resolvingPartialXYH2.jl diff --git a/examples/lcmserver/server.jl b/examples/marine/rov/highend/lcmserver/server.jl similarity index 100% rename from examples/lcmserver/server.jl rename to examples/marine/rov/highend/lcmserver/server.jl diff --git a/examples/lcmserver/server_debug.jl b/examples/marine/rov/highend/lcmserver/server_debug.jl similarity index 100% rename from examples/lcmserver/server_debug.jl rename to examples/marine/rov/highend/lcmserver/server_debug.jl diff --git a/examples/marine/rov/highend/synchrony.jl b/examples/marine/rov/highend/synchrony.jl new file mode 100644 index 000000000..24d7baaab --- /dev/null +++ b/examples/marine/rov/highend/synchrony.jl @@ -0,0 +1,98 @@ +#= +LCM Server: an LCM interface to Caesar.jl +=# + +using Base.Dates +using Caesar +using Unmarshal +using CaesarLCMTypes +using TransformUtils, Rotations, CoordinateTransformations +using Distributions +using LCMCore +using LibBSON + +using RobotTestDatasets + +include(joinpath(dirname(@__FILE__), "synchronySDKIntegration.jl")) + + +""" +Store robot parameters in the centralized database system. +""" +function setRobotParameters!(sslaml::SyncrSLAM) + rovconf = Dict{String, String}() # TODO relax to Dict{String, Any} + rovconf["robot"] = "hovering-rov" + rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" + rovconf["bTc_format"] = "xyzqwqxqyqz" + # currently unused, but upcoming + rovconf["pointcloud_description_name"] = "BSONpointcloud" + rovconf["pointcloud_color_description_name"] = "BSONcolors" + + # Actually modify the databases + Syncr.updateRobotConfig(sslaml.syncrconf, sslaml.robotId, rovconf) + + nothing +end + +# Include the LCM handlers... +include(joinpath(dirname(@__FILE__), "lcmHandlers.jl")) + +# this function handles lcm msgs +function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) + # handle traffic + # TODO: handle termination + while true + if !handle(lcm_node) + break + end + end +end + +# 0. Constants +println("[Caesar.jl] defining constants.") +robotId = "HROV" +sessionId = "LCM_27" +sessionId = strip(sessionId) + +# create a SLAM container object +slam_client = SyncrSLAM(robotId, sessionId, nothing) + +# initialize a new session ready for SLAM using the built in SynchronySDK +println("[Caesar.jl] Setting up remote solver") +initialize!(slam_client) + +# Set up the robot +setRobotParameters!(slam_client) +robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) + +# TODO - should have a function that allows first pose and prior to be set by user. + +# create new handlers to pass in additional data +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) + +# create LCM object and subscribe to messages on the following channels +logfile = robotdata("rovlcm_singlesession_01") +lcm_node = LCMLog(logfile) # for direct log file access + +# poses +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_node_t) + +# factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# loop closures come in via p3p3nh factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) + +# sensor data +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) + +println("[Caesar.jl] Running LCM listener") +listener!(lcm_node) + +println(" --- Done! Now we can run the solver on this dataset!") + +# Todo - call Sychrony to start the solver diff --git a/examples/marine/rov/highend/synchronySDKIntegration.jl b/examples/marine/rov/highend/synchronySDKIntegration.jl new file mode 100644 index 000000000..8dc3870d4 --- /dev/null +++ b/examples/marine/rov/highend/synchronySDKIntegration.jl @@ -0,0 +1,134 @@ +# SynchronySDK integration file + +using SynchronySDK +using DocStringExtensions # temporary while $(SIGNATURES) is in use in this file + +const Syncr = SynchronySDK + +""" + SyncrSLAM + +An object definition containing the require variables to leverage the server side SLAM solution per user, robot, and session. +""" +mutable struct SyncrSLAM + robotId::AbstractString + sessionId::AbstractString + syncrconf::Union{Void, SynchronySDK.SynchronyConfig} + robot + session + + # Constructor + SyncrSLAM( + robotId::AbstractString, + sessionId::AbstractString, + syncrconf::Union{Void, SynchronySDK.SynchronyConfig}; + robot=nothing, + session=nothing + ) = new( + robotId, + sessionId, + syncrconf, + robot, + session + ) +end + + +""" +$(SIGNATURES) + +Get Synchrony configuration, default filepath location assumed as `~/Documents/synchronyConfig.json`. +""" +function loadSyncrConfig(; + filepath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # + println(" - Retrieving Synchrony Configuration...") + configFile = open(filepath) + configData = JSON.parse(readstring(configFile)) + close(configFile) + Unmarshal.unmarshal(SynchronyConfig, configData) # synchronyConfig = +end + +""" +$(SIGNATURES) + +Confirm that the robot already exists, create if it doesn't. +""" +function syncrRobot( + synchronyConfig::SynchronyConfig, + robotId::AS + ) where {AS <: AbstractString} + # + println(" - Creating or retrieving robot '$robotId'...") + robot = nothing + if(SynchronySDK.isRobotExisting(synchronyConfig, robotId)) + println(" -- Robot '$robotId' already exists, retrieving it...") + robot = getRobot(synchronyConfig, robotId) + else + # Create a new one + println(" -- Robot '$robotId' doesn't exist, creating it...") + newRobot = RobotRequest(robotId, "My New Bot", "Description of my neat robot", "Active") + robot = addRobot(synchronyConfig, newRobot) + end + println(robot) + robot +end + +""" +$(SIGNATURES) + +Get sessions, if it already exists, add to it. +""" +function syncrSession( + synchronyConfig::SynchronyConfig, + robotId::AS, + sessionId::AS + ) where {AS<: AbstractString} + # + println(" - Creating or retrieving data session '$sessionId' for robot...") + session = nothing + if(SynchronySDK.isSessionExisting(synchronyConfig, robotId, sessionId)) + println(" -- Session '$sessionId' already exists for robot '$robotId', retrieving it...") + session = getSession(synchronyConfig, robotId, sessionId) + else + # Create a new one + println(" -- Session '$sessionId' doesn't exist for robot '$robotId', creating it...") + newSessionRequest = SessionDetailsRequest(sessionId, "Submersible vehicle.") + session = addSession(synchronyConfig, robotId, newSessionRequest) + end + println(session) + session +end + +""" +$(SIGNATURES) + +Intialize the `sslaml` object using configuration file defined in `syncrconfpath`. +""" +function initialize!(sslaml::SyncrSLAM; + syncrconfpath::AS=joinpath(ENV["HOME"],"Documents","synchronyConfig.json") + ) where {AS <: AbstractString} + # 1. Get a Synchrony configuration + sslaml.syncrconf = loadSyncrConfig(filepath=syncrconfpath) + + # 2. Confirm that the robot already exists, create if it doesn't. + sslaml.robot = syncrRobot(sslaml.syncrconf, sslaml.robotId) + + # Make sure that this session is not already populated + if isSessionExisting(slam_client.syncrconf, sslaml.robotId, sslaml.sessionId) + warn("There is already a session named '$sessionId' for robot '$robotId'. This example will fail if it tries to add duplicate nodes. We strongly recommend providing a new session name.") + print(" Should we delete it? [Y/N] - ") + if lowercase(strip(readline(STDIN))) == "y" + deleteSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) + println(" -- Session '$sessionId' deleted!") + else + warn("Okay, but this may break the example! Caveat Userus!") + end + end + + # 3. Create or retrieve the session. + sslaml.session = syncrSession(sslaml.syncrconf, sslaml.robotId, sslaml.sessionId) + + nothing +end diff --git a/examples/parameters/robots01.jl b/examples/parameters/robots01.jl index 4313fa76c..8dbd604eb 100644 --- a/examples/parameters/robots01.jl +++ b/examples/parameters/robots01.jl @@ -1,74 +1,75 @@ # example for loading robot description to database -using Caesar -using JSON -using Neo4j -using CloudGraphs +using Caesar, SynchronySDK -addrdict = nothing -include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) -# prepare the factor graph with just one node -# (will prompt on stdin for db credentials) -cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) - -# Turtlebot robot -turtlebot = Dict() -turtlebot["robot"] = "mrg-turtlebot" -turtlebot["CAMK"] = [ 570.34222412 0.0 319.5; - 0.0 570.34222412 239.5; - 0.0 0.0 1.0] -# -turtlebot["imshape"]=[480,640] -turtlebot["bTc"] = [0.0;0.0;0.6; 0.5; -0.5; 0.5; -0.5] -turtlebot["bTc_format"] = "xyzqwqxqyqz" - -# robotdata = json(turtlebot).data -# resp = JSON.parse(String(take!(IOBuffer(robotdata)))) -# tryunpackalltypes!(resp) -# resp - -# Actually modify the database -insertrobotdatafirstpose!(cloudGraph, "SESSTURT21", turtlebot) -frd = fetchrobotdatafirstpose(cloudGraph, "SESSTURT21") - - - - -# A different robot - -hauv = Dict() -hauv["robot"] = "hauv" -hauv["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] -hauv["bTc_format"] = "xyzqwqxqyqz" -hauv["depth_pointcloud_description"] = "BSONpointcloud" -hauv["depth_color_description"] = ["BSONcolor"] - -# robotdata = json(hauv).data -hauv - +rovconf = Dict() +rovconf["robot"] = "hovering-rov" +rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" +rovconf["bTc_format"] = "xyzqwqxqyqz" +rovconf["pointcloud_description_name"] = "BSONpointcloud" +rovconf["pointcloud_color_description_name"] = "BSONcolors" # Actually modify the databases -insertrobotdatafirstpose!(cloudGraph, "SESSHAUVDEV", hauv) -frd = fetchrobotdatafirstpose(cloudGraph, "SESSHAUVDEV") - - - - - - - - - - - +updateRobotConfig(synchronyConfig, robotId, rovconf) +## Obsolete usege below +# +# addrdict = nothing +# include(joinpath(dirname(@__FILE__),"..","database","blandauthremote.jl")) +# # prepare the factor graph with just one node +# # (will prompt on stdin for db credentials) +# cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict) +# +# +# +# # Turtlebot robot +# turtlebot = Dict() +# turtlebot["robot"] = "mrg-turtlebot" +# turtlebot["CAMK"] = [ 570.34222412 0.0 319.5; +# 0.0 570.34222412 239.5; +# 0.0 0.0 1.0] +# # +# turtlebot["imshape"]=[480,640] +# turtlebot["bTc"] = [0.0;0.0;0.6; 0.5; -0.5; 0.5; -0.5] +# turtlebot["bTc_format"] = "xyzqwqxqyqz" +# +# # robotdata = json(turtlebot).data +# # resp = JSON.parse(String(take!(IOBuffer(robotdata)))) +# # tryunpackalltypes!(resp) +# # resp +# +# # Actually modify the database +# insertrobotdatafirstpose!(cloudGraph, "SESSTURT21", turtlebot) +# frd = fetchrobotdatafirstpose(cloudGraph, "SESSTURT21") +# +# +# +# +# # A different robot +# +# hauv = Dict() +# hauv["robot"] = "hauv" +# hauv["bTc"] = [0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0] +# hauv["bTc_format"] = "xyzqwqxqyqz" +# hauv["depth_pointcloud_description"] = "BSONpointcloud" +# hauv["depth_color_description"] = ["BSONcolor"] +# +# # robotdata = json(hauv).data +# hauv +# +# +# # Actually modify the databases +# insertrobotdatafirstpose!(cloudGraph, "SESSHAUVDEV", hauv) +# frd = fetchrobotdatafirstpose(cloudGraph, "SESSHAUVDEV") +# +# diff --git a/examples/tobedeleted/highend/synchrony.jl b/examples/tobedeleted/highend/synchrony.jl new file mode 100644 index 000000000..40722e004 --- /dev/null +++ b/examples/tobedeleted/highend/synchrony.jl @@ -0,0 +1,280 @@ +#= +LCM Server: an LCM interface to Caesar.jl + +=# + +using Caesar +using LCMCore, CaesarLCMTypes +using TransformUtils, Rotations, CoordinateTransformations, Distributions +using LibBSON, Unmarshal + +using RobotTestDatasets + +# Does: using SynchronySDK +include(joinpath(dirname(@__FILE__), "synchronySDKIntegration.jl")) + + +""" +Store robot parameters in the centralized database system. +""" +function setRobotParameters!(sslaml::SyncrSLAM) + rovconf = Dict{String, String}() # TODO relax to Dict{String, Any} + rovconf["robot"] = "hovering-rov" + rovconf["bTc"] = "[0.0;0.0;0.0; 1.0; 0.0; 0.0; 0.0]" + rovconf["bTc_format"] = "xyzqwqxqyqz" + # currently unused, but upcoming + rovconf["pointcloud_description_name"] = "BSONpointcloud" + rovconf["pointcloud_color_description_name"] = "BSONcolors" + + # Actually modify the databases + Syncr.updateRobotConfig(sslaml.syncrconf, sslaml.robotId, rovconf) + + nothing +end + + +""" +Adds pose nodes to graph with a prior on Z, pitch, and roll. +""" +function handle_poses!(slaml::SyncrSLAM, + msg::pose_node_t) + # + id = msg.id + println("[Caesar.jl] Received pose msg for x$(id)") + + mean = msg.mean + covar = msg.covar + t = [mean[1], mean[2], mean[3]] + qw = mean[4] + qxyz = [mean[5], mean[6], mean[7]] + q = Quaternion(qw,qxyz) # why not a (w,x,y,z) constructor? + pose = SE3(t,q) + euler = Euler(q) + + node_label = Symbol("x$(id)") + xn = addNode!(slaml, node_label, Pose3) # this is an incremental inference call + slaml.lastposesym = node_label; # update object + + if id == 1 + println("[Caesar.jl] First pose") + # this is the first msg, and it does not carry odometry, but the prior on the first node. + + # add 6dof prior + initPosePrior = PriorPose3( MvNormal( veeEuler(pose), diagm([covar...]) ) ) + addFactor!(slaml, [xn], initPosePrior) + + # auto init is coming, this code will likely be removed + initializeNode!(slaml, node_label) + + # set robot parameters in the first pose, this will become a separate node in the future + println("[Caesar.jl] Setting robot parameters") + setRobotParameters!(slaml) + end +end + + +""" +Handle ZPR priors on poses. +""" +function handle_priors!(slam::SyncrSLAM, + msg::prior_zpr_t) + + id = msg.id + println("[Caesar.jl] Adding prior on RPZ to x$(id)") + + node_label = Symbol("x$(id)") + xn = getVert(slam,node_label) + + z = msg.z + pitch = msg.pitch + roll = msg.roll + + var_z = msg.var_z + var_pitch = msg.var_pitch + var_roll = msg.var_roll + + rp_dist = MvNormal( [roll;pitch], diagm([var_roll, var_pitch])) + z_dist = Normal(z, var_z) + prior_rpz = PartialPriorRollPitchZ(rp_dist, z_dist) + addFactor!(slam, [xn], prior_rpz) +end + +""" +Handle partial x, y, and heading odometry constraints between Pose3 variables. +""" +function handle_partials!(slam::SyncrSLAM, + msg::pose_pose_xyh_t) + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + println("[Caesar.jl] Adding XYH odometry constraint between(x$(origin_id), x$(destination_id))") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + + origin_label, destination_label + xo = getVert(slam,origin_label) + xd = getVert(slam,destination_label) + + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYaw(xyh_dist) + addFactor!(slam, [xo;xd], xyh_factor) + + initializeNode!(slam, destination_label) + println() +end + +""" +Handle loop closure proposals with chance of being a null hypothesis likelihood. +""" +function handle_loops!(slaml::SyncrSLAM, + msg::pose_pose_xyh_nh_t) + + origin_id = msg.node_1_id + destination_id = msg.node_2_id + origin_label = Symbol("x$(origin_id)") + destination_label = Symbol("x$(destination_id)") + + delta_x = msg.delta_x + delta_y = msg.delta_y + delta_yaw = msg.delta_yaw + + var_x = msg.var_x + var_y = msg.var_y + var_yaw = msg.var_yaw + confidence = msg.confidence + + xo = getVert(slaml.fg,origin_label) + xd = getVert(slaml.fg,destination_label) + + # if (destination_id - origin_id == 1) + # warn("Avoiding parallel factor! See: https://github.com/dehann/IncrementalInference.jl/issues/63To ") + # return + # end + + println("[Caesar.jl] Adding XYH-NH loop closure constraint between (x$(origin_id), x$(destination_id))") + xyh_dist = MvNormal([delta_x, delta_y, delta_yaw], diagm([var_x, var_y, var_yaw])) + xyh_factor = PartialPose3XYYawNH(xyh_dist ,[1.0-confidence, confidence]) # change to NH + addFactor!(slaml, [xo;xd], xyh_factor ) + + # println("[Caesar.jl] Adding P3P3NH loop closure constraint between (x$(origin_id), x$(destination_id))") + + # # line below fails! + # lcf = Pose3Pose3NH( MvNormal(veeEuler(rel_pose), diagm(1.0./covar)), [0.5;0.5]) # define 50/50% hypothesis + # lcf_label = Symbol[origin_label;destination_label] + + # addFactor!(slaml, lcf_label, lcf) +end + + +# type Cloud +# points:: +# colors:: +# end + +# function add_pointcloud!(slaml::SyncrSLAM, nodeID::Symbol, cloud::Cloud ) +# # fetch from database +# vert = getVert(slaml.fg, nodeID, api=IncrementalInference.dlapi) + +# # add points blob +# serialized_point_cloud = BSONObject(Dict("pointcloud" => cloud.points)) +# appendvertbigdata!(slaml.fg, vert, "BSONpointcloud", string(serialized_point_cloud).data) + +# # add colors blob +# serialized_colors = BSONObject(Dict("colors" => cloud.colors)) +# appendvertbigdata!(slaml.fg, vert, "BSONcolors", string(serialized_colors).data) +# end + + + +""" +Callback for caesar_point_cloud_t msgs. Adds point cloud to SLAM_Client +""" +function handle_clouds!(slaml::SyncrSLAM, + msg::point_cloud_t) + # TODO: interface here should be as simple as slam_client.add_pointcloud(nodeID, pc::SomeCloudType) + + # TODO: check for empty clouds! + + id = msg.id + + last_pose = Symbol("x$(id)") + println("[Caesar.jl] Got cloud $id") + + # 2d arrays of points and colors (from LCM data into arrays{arrays}) + points = [[pt[1], pt[2], pt[3]] for pt in msg.points] + colors = [[UInt8(c.data[1]),UInt8(c.data[2]),UInt8(c.data[3])] for c in msg.colors] + + + # TODO: check if vert exists or not (may happen if msgs are lost or out of order) + vert = getVert(slaml, last_pose, api=IncrementalInference.dlapi) # fetch from database + + # push to mongo (using BSON as a quick fix) + # (for deserialization, see src/DirectorVisService.jl:cachepointclouds!) + serialized_point_cloud = BSONObject(Dict("pointcloud" => points)) + appendvertbigdata!(slaml, vert, "BSONpointcloud", string(serialized_point_cloud).data) + serialized_colors = BSONObject(Dict("colors" => colors)) + appendvertbigdata!(slaml, vert, "BSONcolors", string(serialized_colors).data) +end + +# this function handles lcm msgs +function listener!(lcm_node::Union{LCMCore.LCM, LCMCore.LCMLog}) + # handle traffic + # TODO: handle termination + while true + handle(lcm_node) + end +end + + +# 0. Constants +println("[Caesar.jl] defining constants.") +robotId = "HROV" +sessionId = "LCM_01" + +# create a SLAM container object +slam_client = SyncrSLAM(robotId=robotId,sessionId=sessionId) + +# initialize a new session ready for SLAM using the built in SynchronySDK +println("[Caesar.jl] Setting up remote solver") +initialize!(slam_client) + +setRobotParameters!(slam_client) +# TEST: Getting robotConfig +@show robotConfig = Syncr.getRobotConfig(slam_client.syncrconf, slam_client.robotId) + +# TODO - should have a function that allows first pose and prior to be set by user. + +# create new handlers to pass in additional data +lcm_pose_handler = (channel, message_data) -> handle_poses!(slam_client, message_data ) +lcm_odom_handler = (channel, message_data) -> handle_partials!(slam_client, message_data ) +lcm_prior_handler = (channel, message_data) -> handle_priors!(slam_client, message_data ) +lcm_cloud_handler = (channel, message_data) -> handle_clouds!(slam_client, message_data ) +lcm_loop_handler = (channel, message_data) -> handle_loops!(slam_client, message_data ) + +# create LCM object and subscribe to messages on the following channels +logfile = robotdata("rovlcm_singlesession_01") +lcm_node = LCMLog(logfile) # for direct log file access +# lcm_node = LCM() # for UDP Ethernet traffic version + +# poses +subscribe(lcm_node, "CAESAR_POSES", lcm_pose_handler, pose_pose_nh_t) +# factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH", lcm_odom_handler, pose_pose_xyh_t) +subscribe(lcm_node, "CAESAR_PARTIAL_ZPR", lcm_prior_handler, prior_zpr_t) +# loop closures come in via p3p3nh factors +subscribe(lcm_node, "CAESAR_PARTIAL_XYH_NH", lcm_loop_handler, pose_pose_xyh_nh_t) +# sensor data +subscribe(lcm_node, "CAESAR_POINT_CLOUDS", lcm_cloud_handler, point_cloud_t) + + +println("[Caesar.jl] Running LCM listener") +listener!(lcm_node)