-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #10 from JuliaRobotics/23Q3/refac/handlers
handlers and tests
- Loading branch information
Showing
19 changed files
with
1,218 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,55 +1,76 @@ | ||
module PyCaesarRobotOSExt | ||
|
||
using PyCall | ||
using OpenSSL # workaround attempt, https://github.com/JuliaWeb/OpenSSL.jl/issues/9 | ||
using RobotOS | ||
using Caesar | ||
using Dates | ||
using DocStringExtensions | ||
|
||
import Base: convert | ||
using Colors | ||
using ImageMagick, FileIO | ||
# using Images | ||
# likely packages needed to ingest various different sensor data types and serialize them for NVA | ||
using TimeZones | ||
using Random | ||
using JSON3 | ||
using BSON | ||
# , Serialization | ||
using FixedPointNumbers | ||
using StaticArrays | ||
|
||
# dont load Caesar until after rostypegen | ||
using Caesar | ||
# import Caesar: unmarshal | ||
|
||
# FIXME DEPRECATE UPGRADE TBD | ||
import Unmarshal: unmarshal | ||
|
||
import Caesar._PCL as _PCL | ||
|
||
import Base: convert | ||
|
||
# weakdeps type and memver prototype import for overwritten definition pattern | ||
import PyCaesar: RosbagSubscriber, RosbagWriter | ||
import PyCaesar: loop!, getROSPyMsgTimestamp, nanosecond2datetime | ||
import PyCaesar: handleMsg!, handleMsg_OVRLPRECOMP! | ||
|
||
|
||
## | ||
|
||
@rosimport std_msgs.msg: Header | ||
|
||
@rosimport tf2_msgs.msg: TFMessage | ||
|
||
@rosimport nav_msgs.msg: Odometry | ||
|
||
# standard types | ||
@rosimport sensor_msgs.msg: PointCloud2 | ||
@rosimport sensor_msgs.msg: PointField | ||
@rosimport sensor_msgs.msg: LaserScan | ||
@rosimport sensor_msgs.msg: CompressedImage | ||
@rosimport sensor_msgs.msg: Image | ||
@rosimport sensor_msgs.msg: Imu | ||
@rosimport tf2_msgs.msg: TFMessage | ||
@rosimport geometry_msgs.msg: TransformStamped | ||
@rosimport geometry_msgs.msg: Transform | ||
@rosimport sensor_msgs.msg: NavSatFix | ||
@rosimport sensor_msgs.msg: RegionOfInterest | ||
@rosimport sensor_msgs.msg: CameraInfo | ||
|
||
@rosimport geometry_msgs.msg: Vector3 | ||
@rosimport geometry_msgs.msg: Quaternion | ||
@rosimport nav_msgs.msg: Odometry | ||
|
||
# FIXME, note this is not working, functions using Main. as workaround | ||
@rosimport std_msgs.msg: Header | ||
@rosimport sensor_msgs.msg: PointField | ||
@rosimport sensor_msgs.msg: PointCloud2 | ||
|
||
# FIXME not sure if this should be run here | ||
@rosimport geometry_msgs.msg: Point | ||
@rosimport geometry_msgs.msg: Pose | ||
@rosimport geometry_msgs.msg: Transform | ||
@rosimport geometry_msgs.msg: TransformStamped | ||
@rosimport geometry_msgs.msg: Twist | ||
@rosimport geometry_msgs.msg: TwistWithCovariance | ||
@rosimport geometry_msgs.msg: TwistWithCovarianceStamped | ||
|
||
# thismodule(m::Module, args...) = (@info("this module is getting", m); m) | ||
# macro thismodule(args...) | ||
# :( thismodule(__module__) ) | ||
# end | ||
# load type definitions into this modules context | ||
rostypegen(@__MODULE__) | ||
|
||
## | ||
|
||
include("Utils/RosbagSubscriber.jl") | ||
include("services/ROSConversions.jl") | ||
include("services/PCLROSConversions.jl") | ||
include("services/IngestROSSetup.jl") | ||
|
||
end |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,143 @@ | ||
|
||
|
||
|
||
include("StdROSHandlers/ROSJSONtoJSON.jl") | ||
|
||
# http://wiki.ros.org/laser_geometry | ||
lg = pyimport("laser_geometry.laser_geometry") | ||
|
||
include("StdROSHandlers/SystemState.jl") | ||
|
||
# actual handlers | ||
include("StdROSHandlers/IngestLidar3D.jl") | ||
include("StdROSHandlers/IngestRadar2D.jl") | ||
include("StdROSHandlers/IngestLaserScan2D.jl") | ||
include("StdROSHandlers/IngestIMU.jl") | ||
include("StdROSHandlers/IngestCameraRaw.jl") | ||
include("StdROSHandlers/IngestCameraInfo.jl") | ||
include("StdROSHandlers/IngestTFMessage.jl") | ||
include("StdROSHandlers/IngestOdometry.jl") | ||
include("StdROSHandlers/IngestGPS.jl") | ||
include("StdROSHandlers/IngestTwist.jl") | ||
|
||
|
||
|
||
## Automation code that converts user options on which topics and types to subscribe to in a ROSbag. | ||
|
||
function SubscribeROSTopicOptions(;kw...) | ||
di = Dict{String,Any}() | ||
for (k,v) in kw | ||
di[string(k)] = v | ||
end | ||
di | ||
end | ||
|
||
|
||
Base.@kwdef struct SubscribeROSTopicInput | ||
topicname::String | ||
msgtype | ||
callback | ||
end | ||
|
||
|
||
function addROSSubscriber!(bagSub::RosbagSubscriber, inp::SubscribeROSTopicInput, subsargs::Tuple=() ) | ||
@show inp.msgtype | ||
bagSub(inp.topicname, inp.msgtype, inp.callback, subsargs ) | ||
end | ||
|
||
""" | ||
_suppressPushExisting! | ||
Piece of defensive coding, if graph upload from ROS with BashingMapper breaks during process, this helps resume the "upload" | ||
""" | ||
function _suppressPushExisting!( | ||
nfg::AbstractDFG, | ||
state::SystemState, | ||
vars::AbstractVector{Symbol}, | ||
userPushNva::Bool, | ||
userPushBlobs::Bool | ||
) | ||
# skip if not allow suppress | ||
!state.suppressPush && (return false) | ||
lbl = Symbol("x$(state.var_index)") | ||
if lbl in vars | ||
state.pushNva = false | ||
# not a perfect check, assumes all or nothing is already up there | ||
state.pushBlobs = if 0 === length(listBlobEntries(nfg, lbl)) | ||
@warn "might upload blobs for $lbl" | ||
userPushBlobs | ||
return false | ||
else | ||
@warn "skipping uploads of $lbl" | ||
false | ||
return true | ||
end | ||
else | ||
# regular case of nothing up on nfg yet so just add as stuff as normal | ||
state.pushNva = userPushNva | ||
state.pushBlobs = userPushBlobs | ||
return false | ||
end | ||
# | ||
error("should get here") | ||
end | ||
|
||
|
||
## a common main loop | ||
|
||
function main( | ||
nfg::AbstractDFG, | ||
bagfile, | ||
userWantsSubsOnROS; | ||
iters::Integer=50, | ||
stripeKeyframe=5, | ||
pushNva=true, | ||
pushBlobs=false, | ||
cur_variable = "x0", # should already be uploaded in a previous step | ||
var_index = 1 # this is the next variable to add -- i.e. "x1" | ||
) | ||
# | ||
|
||
@info "Hit CTRL+C to exit and save the graph..." | ||
|
||
if pushNva | ||
x0 = NvaSDK.getVariable(client, context, cur_variable) |> fetch | ||
@assert !(x0 === nothing) "The indicated starting variable $cur_variable was not found" | ||
end | ||
|
||
# subscriber for the bagfile | ||
bagSubscriber = RosbagSubscriber(bagfile) | ||
|
||
# System state | ||
systemstate = SystemState(; | ||
stripeKeyframe, # TODO remove, use handler option instead | ||
pushBlobs, | ||
pushNva, | ||
cur_variable, | ||
var_index | ||
) | ||
|
||
# all the subscribers the user requested | ||
for (usrInp, usrOpt) in userWantsSubsOnROS | ||
@info "Adding ROS subscriber " context usrInp usrOpt | ||
addROSSubscriber!(bagSubscriber, usrInp, (client, context, systemstate, usrOpt)) | ||
end | ||
|
||
vars = listVariables(nfg) | ||
|
||
@info "subscribers have been set up; entering main loop" | ||
# loop_rate = Rate(20.0) | ||
while loop!(bagSubscriber) | ||
# defensive suppress state.pushNva and .pushBlobs if variable already there -- i.e. trying to resume an upload | ||
_suppressPushExisting!(nfg, systemstate, vars, pushNva, pushBlobs) | ||
# regular processing steps | ||
iters -= 1 | ||
iters < 0 ? break : nothing | ||
end | ||
|
||
@info "Exiting" | ||
systemstate | ||
end | ||
|
||
|
||
## |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,48 @@ | ||
|
||
""" | ||
$SIGNATURES | ||
Message callback for camera images. | ||
""" | ||
function handleMsg!( | ||
msg::sensor_msgs.msg.CompressedImage, | ||
dfg, | ||
state::SystemState, | ||
options=Dict() | ||
) | ||
|
||
lbl = "IMG_CENTER_$(state.cur_variable)_$(msg.header.seq)" | ||
@info "handleCameraCompressed!" lbl maxlog=10 | ||
|
||
entries = Tuple{String, String}[] | ||
|
||
prvLbl = state.prv_variable === "" ? "x0" : state.prv_variable | ||
if state.pushBlobs | ||
res = addData(dfg, lbl, msg.data) | ||
# not super well synched, but good enough for basic demonstration | ||
addBlobEntry!(dfg, prvLbl, res, lbl, "image/jpeg") | ||
# description="ImageMagick.readblob(imgBytes)" | ||
push!(entries, (lbl, res)) | ||
end | ||
|
||
# add the data entries to the local factor graph too | ||
if state.pushBlobs && state.pushLocal | ||
vari = Caesar.getVariable(state.localfg, Symbol(prvLbl)) | ||
timestamp = now(localzone()) | ||
for (lb,id) in entries | ||
fde = Caesar.BlobStoreEntry( | ||
Symbol(lb), | ||
UUID(id), | ||
:nva, | ||
"", | ||
"", # $(userId)|$(robotId)|$(sessionId)|$(prvLbl)", | ||
"", | ||
"image/jpeg", | ||
now(localzone()) | ||
) | ||
de = addBlobEntry!(vari, fde) | ||
end | ||
end | ||
|
||
nothing | ||
end |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,63 @@ | ||
|
||
|
||
|
||
""" | ||
$SIGNATURES | ||
Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element. | ||
""" | ||
function handleMsg!( | ||
msg::sensor_msgs.msg.CameraInfo, | ||
dfg, | ||
state::SystemState, | ||
options=Dict() | ||
) | ||
@info "handleCamInfo" state.cur_variable state.prv_variable maxlog=10 | ||
|
||
if state.cur_variable === nothing | ||
# skip if we don't have a variable yet. | ||
return nothing | ||
end | ||
|
||
if !haskey(state.workspace, "CamInfo_keyframe") | ||
state.workspace["CamInfo_keyframe"] = state.prv_variable | ||
end | ||
|
||
# blob label is user specified or Mapper default | ||
bllb = get(options, "name", CAMINFO_BLOBNAME()) | ||
|
||
# quick and dirty synchronization of camera messages to lidar messages. | ||
# NOTE storing only the first image after a new pose is created | ||
if state.workspace["CamInfo_keyframe"] == state.prv_variable | ||
return nothing | ||
end | ||
|
||
msgdict = unmarshal(msg) | ||
# @info "TRYING TO ADD" msg msgdict | ||
|
||
timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/10^9 | ||
@info "[$timestamp] CamInfo sample on $(state.cur_variable)" | ||
|
||
bllb *= "_"*string(msg.header.seq) #msgdict["header"]["seq"]) | ||
blob = JSON.json(msgdict) |> Vector{UInt8} | ||
mime = "application/json/octet-stream" | ||
|
||
if state.pushBlobs | ||
blobid = addData(dfg, bllb, blob) | ||
|
||
# | ||
addBlobEntry!( | ||
dfg, | ||
# next of pair for now as rest works like that | ||
state.cur_variable, | ||
blobid, | ||
bllb, | ||
length(blob), | ||
mime | ||
) | ||
end | ||
|
||
state.workspace["CamInfo_keyframe"] = state.prv_variable | ||
|
||
nothing | ||
end |
Oops, something went wrong.