Merge pull request #898 from JuliaRobotics/master

v0.13.4-rc1

Author: dehann

Project.toml

@@ -2,7 +2,7 @@ name = "Caesar"
 uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.13.3"
+version = "0.13.4"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

docs/src/examples/custom_relative_factors.md

@@ -20,6 +20,17 @@ end
 ```
 New relative factors should either inheret from `<:AbstractManifoldMinimize`, `<:AbstractRelativeMinimize`, or `<:AbstractRelativeRoots`.  These are all subtypes of `<:AbstractRelative`.  There are only two abstract super types, `<:AbstractPrior` and `<:AbstractRelative`.
 
+## Summary of Sampling Data Representation
+
+| Usage       | `<:AbstractPrior`  |  `<:AbstractRelative` |
+|-------------|--------------------|-----------------------|
+| `getSample` | point `p` on Manifold | tangent `X` at some `p` (e.g. identity) |
+
+| Usage             |    |
+|-------------------|----|
+| `sampleTangent`   | tangent at point `p` or the identity element for groups |
+| `rand` / `sample` | coordinates |
+
 ## Specialized Dispatch (`getManifold`, `getSample`)
 
 Relative factors involve computaton, these computations must be performed on some manifold.  Custom relative factors require that the [`getManifold`](@ref) function be overridded.  Here two examples are given for reference:
@@ -47,7 +58,7 @@ function getSample(cf::CalcFactor{<:Pose2Pose2})
 end
 ```
 
-The return type for `getSample` is unrestricted, and will be passed to the residual function "as-is".
+The return type for `getSample` is unrestricted, and will be passed to the residual function "as-is", but must return values representing a tangent vector for `<:AbstractRelative`
 
 !!! note
     Default dispatches in `IncrementalInference` will try use `cf.factor.Z` to `samplePoint` on manifold (for `<:AbstractPrior`) or `sampleTangent` (for `<:AbstractRelative`), which simplifies new factor definitions.  If, however, you wish to build more complicated sampling processes, then simply define your own `getSample(cf::CalcFactor{<:MyFactor})` function.
@@ -70,11 +81,11 @@ function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
 end
 ```
 
+It is recommended to leave the incoming types unrestricted.  If you must define the types, make sure to allow sufficient dispatch freedom (i.e. dispatch to concrete types) and not force operations to "non-concrete" types.  Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work of inferring the concrete types.
+
 !!! note
     At present (2021) the residual function should return the residual value as a coordinate (not as tangent vectors or manifold points).  Ongoing work is in progress, and likely to return residual values as manifold tangent vectors instead.
 
-It is recommended to leave the incoming types unrestricted.  If you must define the types, make sure to allow sufficient dispatch freedom (i.e. dispatch to concrete types) and not force operations to "non-concrete" types.  Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work of inferring the concrete types.
-
 ### Serialization
 
 Serialization of factors is also discussed in more detail at [Standardized Factor Serialization](@ref factor_serialization).

docs/src/examples/using_pcl.md

@@ -1,6 +1,6 @@
-# Pointclouds and PCL Types
+# [Pointclouds and PCL Types](@id pointclouds_and_pcl)
 
-## Introduction Caesar._PCL
+## Introduction `Caesar._PCL`
 
 A wide ranging and well used [point cloud library exists called PCL](https://pointclouds.org/) which is implemented in C++.  To get access to many of those features and bridge the Caesar.jl suite of packages, the base `PCL.PointCloud` types have been implemented in Julia and reside under `Caesar._PCL`.  The main types of interest:
 - [`Caesar._PCL.PointCloud`](@ref),

docs/src/examples/using_ros.md

@@ -53,10 +53,21 @@ Distributed.@everywhere using PyCall
 
 Caesar.jl has native by optional package tools relating to RobotOS.jl (leveraging [Requires.jl](https://github.com/JuliaPackaging/Requires.jl)):
 ```julia
-using RobotOS, Caesar
-Distributed.@everywhere using Caesar
+using RobotOS
+
+@rosimport sensor_msgs.msg: PointCloud2
+
+rostypegen()
+
+using Colors, Caesar
+Distributed.@everywhere using Colors, Caesar
 ```
 
+Colors.jl is added as a conditional requirement to get `Caesar._PCL.PointCloud` support ([see PCL page here](@ref pointclouds_and_pcl)).
+
+!!! note
+    Imports and type generation are necessary for RobotOS and Caesar to work properly.
+
 ## Prepare Any Outer Objects
 
 Usually a factor graph or detectors, or some more common objects are required.  For the example lets just say a basic SLAMWrapper containing a regular `fg=initfg()`:

src/ros/Utils/RosbagSubscriber.jl

@@ -48,13 +48,15 @@ mutable struct RosbagSubscriber
   syncBuffer::Dict{Symbol,Tuple{DateTime, Int}} # t,ns,msgdata
   nextMsgChl::Symbol
   nextMsgTimestamp::Tuple{DateTime, Int}
+  compression::Any
   # constructors
 end
 RosbagSubscriber(bagfile::AbstractString;
+                  compression=nothing,
                   channels::Vector{Symbol}=Symbol[],
                   callbacks::Dict{Symbol,Function}=Dict{Symbol,Function}(),
                   readers::Dict{Symbol,PyObject}=Dict{Symbol,PyObject}(),
-                  syncBuffer::Dict{Symbol,Tuple{DateTime, Int}}=Dict{Symbol,Tuple{DateTime, Int}}() ) = RosbagSubscriber(bagfile,channels,callbacks,readers,syncBuffer, :null, (unix2datetime(0),0))
+                  syncBuffer::Dict{Symbol,Tuple{DateTime, Int}}=Dict{Symbol,Tuple{DateTime, Int}}() ) = RosbagSubscriber(bagfile,channels,callbacks,readers,syncBuffer, :null, (unix2datetime(0),0),compression)
 #
 
 # loss of accuracy (Julia only Millisecond)
@@ -97,8 +99,8 @@ function loop!(rbs::RosbagSubscriber, args...)
     rbs.callbacks[rbs.nextMsgChl](msg, args...)
     true
   else
-    println("false, dont know how to handle message time as typeof(msgT)=$(typeof(msgT))")
-    @show msg
+    @warn "Unsure about decoding this topic:msg type pair, make sure the subscribed topic names are spelled exactly right." rbs.nextMsgChl msgT maxlog=10
+    # @show msg
     false
   end
 end
@@ -114,5 +116,5 @@ function (rbs::RosbagSubscriber)( chl::AbstractString,
   # include the type converter, see ref: https://github.com/jdlangs/RobotOS.jl/blob/21a7088461a21bc9b24cd2763254d5043d5b1800/src/callbacks.jl#L23
   rbs.callbacks[cn] = (m)->callback(convert(MT,m[2]),args...)
   rbs.syncBuffer[cn] = (unix2datetime(0), 0)
-  rbs.readers[cn] = RosbagParser(rbs.bagfile, chl)
+  rbs.readers[cn] = RosbagParser(rbs.bagfile, chl) #; rbs.compression)
 end

src/ros/Utils/rosbagReader.py

@@ -6,11 +6,11 @@
 import sys
 
 class RosbagParser:
-    def __init__(self, bag_file_name, topic_name):
+    def __init__(self, bag_file_name, topic_name): #, compression=None):
         #bag_file_name = sys.argv[1]
         #topic_name = sys.argv[2]
 
-        self.bag = rosbag.Bag(bag_file_name)
+        self.bag = rosbag.Bag(bag_file_name, 'r') #, compression=compression)
         self.bag_contents = self.bag.read_messages(topics=[topic_name])
 
         self.idx = 0