Skip to content

Commit

Permalink
Add support for apriltag params
Browse files Browse the repository at this point in the history
  • Loading branch information
@davidirobinson
davidirobinson committed Jun 21, 2022
1 parent 738e667 commit 5e94306
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 6 deletions.
2 changes: 1 addition & 1 deletion multisense_lib/sensor_api
Submodule sensor_api updated 6 files
+15 −0 source/LibMultiSense/details/public.cc
+12 −2 source/LibMultiSense/include/MultiSense/MultiSenseChannel.hh
+94 −0 source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh
+1 −0 source/LibMultiSense/include/MultiSense/details/channel.hh
+1 −0 source/LibMultiSense/include/MultiSense/details/wire/Protocol.hh
+99 −0 source/LibMultiSense/include/MultiSense/details/wire/SysApriltagParamsMessage.hh
23 changes: 22 additions & 1 deletion multisense_ros/cfg/multisense.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ class SensorConfig(object):
class SupportedFeatures(Enum):
NONE = 0
GROUND_SURFACE = 1
APRILTAG = 2

sensorConfigList = []
sensorConfigList.append(SensorConfig(name="sl_bm_cmv2000", sgm=False, motor=True, imu=False, lighting=True , aux=False, ar0234=False, features=False))
Expand Down Expand Up @@ -231,7 +232,8 @@ for feature_name, feature in SupportedFeatures.__members__.items():
if feature == SupportedFeatures.GROUND_SURFACE:
gen.add("ground_surface_profile", bool_t, 0, "GroundSurfaceProfile", False);

gen.add("apriltag_profile", bool_t, 0, "AprilTagProfile", False);
if feature == SupportedFeatures.APRILTAG:
gen.add("apriltag_profile", bool_t, 0, "AprilTagProfile", False);

#endif
#endif
Expand Down Expand Up @@ -334,6 +336,25 @@ for feature_name, feature in SupportedFeatures.__members__.items():
gen.add("ground_surface_spline_draw_resolution", double_t, 0, "Resolution to draw resulting B-Spline model with in RVIZ", 0.1, 0.01, 1.0)
#endif

if feature == SupportedFeatures.APRILTAG:
apriltag_family_enum = gen.enum([gen.const("tag25h9", str_t, "tag25h9", "tag25h9"),
gen.const("tag16h5", str_t, "tag16h5", "tag16h5"),
gen.const("tag36h10", str_t, "tag36h10", "tag36h10"),
gen.const("tag36h11", str_t, "tag36h11", "tag36h11"),
gen.const("tagCircle21h7", str_t, "tagCircle21h7", "tagCircle21h7"),
gen.const("tagCircle49h12", str_t, "tagCircle49h12", "tagCircle49h12"),
gen.const("tagCustom48h12", str_t, "tagCustom48h12", "tagCustom48h12"),
gen.const("tagStandard41h12", str_t, "tagStandard41h12", "tagStandard41h12"),
gen.const("tagStandard52h13", str_t, "tagStandard52h13", "tagStandard52h13")],
"Available apriltag families")
gen.add("apriltag_family", str_t, 0, "Apriltag family to detect", "tagStandard52h13", edit_method=apriltag_family_enum)
gen.add("apriltag_quad_detection_blur_sigma", double_t, 0, "Sigma of the Gaussian blur applied to the image before quad_detection, specified in full resolution pixels. Kernel size = 4*sigma, rounded up to the next odd number. (<0.5 results in no blur)", 0.75, 0.0, 5.0)
gen.add("apriltag_quad_detection_decimate", double_t, 0, "Amount to decimate image before detecting quads. Values < 1.0. (0.5 decimation reduces height/width by half)", 1.0, 0.0, 1.0)
gen.add("apriltag_min_border_width", int_t, 0, "Minimum border width that can be considered a valid tag. Used to filter contours based on area and perimeter. Units are in undecimated image pixels.", 5, 0, 10)
gen.add("apriltag_refine_quad_edges", bool_t, 0, "Whether or not to refine the edges before attempting to decode", False)
gen.add("apriltag_decode_sharpening", double_t, 0, "How much to sharpen the quad before sampling the pixels. 0 to turn off, large values are stronger sharpening", 0.25, 0.0, 1.0)
#endif

# Generate package name based on camera cfg and supported features
if feature == SupportedFeatures.NONE:
package_name = cfg.name
Expand Down
11 changes: 10 additions & 1 deletion multisense_ros/include/multisense_ros/reconfigure.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,11 @@
#include <multisense_ros/mono_cmv4000Config.h>
#include <multisense_ros/s27_sgm_AR0234Config.h>
#include <multisense_ros/s27_sgm_AR0234_ground_surfaceConfig.h>
#include <multisense_ros/s27_sgm_AR0234_apriltagConfig.h>
#include <multisense_ros/ks21_sgm_AR0234Config.h>
#include <multisense_ros/ks21_sgm_AR0234_ground_surfaceConfig.h>
#include <multisense_ros/ks21_sgm_AR0234_apriltagConfig.h>

#include <multisense_ros/camera_utilities.h>
#include <multisense_ros/ground_surface_utilities.h>

Expand Down Expand Up @@ -94,6 +97,8 @@ class Reconfigure {

void callback_s27_AR0234_ground_surface (multisense_ros::s27_sgm_AR0234_ground_surfaceConfig& dyn, uint32_t level);
void callback_ks21_AR0234_ground_surface (multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig& dyn, uint32_t level);
void callback_s27_AR0234_apriltag (multisense_ros::s27_sgm_AR0234_apriltagConfig& dyn, uint32_t level);
void callback_ks21_AR0234_apriltag (multisense_ros::ks21_sgm_AR0234_apriltagConfig& dyn, uint32_t level);

//
// Internal helper functions
Expand All @@ -114,8 +119,10 @@ class Reconfigure {
template<class T> void configurePtp(const T& dyn);
template<class T> void configureStereoProfile(crl::multisense::image::Config &cfg, const T& dyn);
template<class T> void configureStereoProfileWithGroundSurface(crl::multisense::image::Config &cfg, const T& dyn);
template<class T> void configureStereoProfileWithApriltag(crl::multisense::image::Config &cfg, const T& dyn);
template<class T> void configureExtrinsics(const T& dyn);
template<class T> void configureGroundSurfaceParams(const T& dyn);
template<class T> void configureApriltagParams(const T& dyn);

//
// CRL sensor API
Expand Down Expand Up @@ -156,6 +163,8 @@ class Reconfigure {
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234Config> > server_ks21_sgm_AR0234_;
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_ground_surfaceConfig> > server_s27_AR0234_ground_surface_;
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig> > server_ks21_sgm_AR0234_ground_surface_;
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig> > server_s27_AR0234_apriltag_;
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_apriltagConfig> > server_ks21_sgm_AR0234_apriltag_;

//
// Cached values for supported sub-systems (these may be unavailable on
Expand Down Expand Up @@ -192,7 +201,7 @@ class Reconfigure {
std::function<void (crl::multisense::system::ExternalCalibration)> extrinsics_callback_;

//
// Extrinsics callback to modify pointcloud
// Callback to modify spline drawing parameters

std::function<void (ground_surface_utilities::SplineDrawParameters)> spline_draw_parameters_callback_;
};
Expand Down
87 changes: 84 additions & 3 deletions multisense_ros/src/reconfigure.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,11 +87,16 @@ Reconfigure::Reconfigure(Channel* driver,
return;
}

// Check which algorithms may be supported
const bool ground_surface_supported =
std::any_of(deviceModes.begin(), deviceModes.end(), [](const auto &mode) {
return (mode.supportedDataSources & Source_Ground_Surface_Spline_Data) &&
(mode.supportedDataSources & Source_Ground_Surface_Class_Image); });

const bool apriltag_supported =
std::any_of(deviceModes.begin(), deviceModes.end(), [](const auto &mode) {
return mode.supportedDataSources & Source_AprilTag_Detections; });

if (deviceInfo.lightingType != 0 || system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21 == deviceInfo.hardwareRevision)
{
lighting_supported_ = true;
Expand Down Expand Up @@ -169,6 +174,12 @@ Reconfigure::Reconfigure(Channel* driver,
new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_ground_surfaceConfig>(device_nh_));
server_s27_AR0234_ground_surface_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234_ground_surface, this,
std::placeholders::_1, std::placeholders::_2));
} else if (apriltag_supported) {
server_s27_AR0234_apriltag_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig> > (
new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig>(device_nh_));
server_s27_AR0234_apriltag_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234_apriltag, this,
std::placeholders::_1, std::placeholders::_2));
} else {
server_s27_AR0234_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config> > (
Expand All @@ -184,6 +195,12 @@ Reconfigure::Reconfigure(Channel* driver,
new dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig>(device_nh_));
server_ks21_sgm_AR0234_ground_surface_->setCallback(std::bind(&Reconfigure::callback_ks21_AR0234_ground_surface, this,
std::placeholders::_1, std::placeholders::_2));
} else if (apriltag_supported) {
server_ks21_sgm_AR0234_apriltag_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_apriltagConfig> > (
new dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_apriltagConfig>(device_nh_));
server_ks21_sgm_AR0234_apriltag_->setCallback(std::bind(&Reconfigure::callback_ks21_AR0234_apriltag, this,
std::placeholders::_1, std::placeholders::_2));
} else {
server_ks21_sgm_AR0234_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234Config> > (
Expand Down Expand Up @@ -280,6 +297,12 @@ Reconfigure::Reconfigure(Channel* driver,
new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_ground_surfaceConfig>(device_nh_));
server_s27_AR0234_ground_surface_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234_ground_surface, this,
std::placeholders::_1, std::placeholders::_2));
} else if (apriltag_supported) {
server_s27_AR0234_apriltag_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig> > (
new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig>(device_nh_));
server_s27_AR0234_apriltag_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234_apriltag, this,
std::placeholders::_1, std::placeholders::_2));
} else {
server_s27_AR0234_ =
std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config> > (
Expand Down Expand Up @@ -743,7 +766,6 @@ template<class T> void Reconfigure::configureStereoProfile(crl::multisense::imag
profile |= (dyn.high_contrast_profile ? crl::multisense::High_Contrast : profile);
profile |= (dyn.show_roi_profile ? crl::multisense::Show_ROIs : profile);
profile |= (dyn.full_res_aux_profile ? crl::multisense::Full_Res_Aux_Cam : profile);
profile |= (dyn.apriltag_profile ? crl::multisense::AprilTag : profile);

cfg.setCameraProfile(profile);
}
Expand All @@ -756,6 +778,17 @@ template<class T> void Reconfigure::configureStereoProfileWithGroundSurface(crl:
profile |= (dyn.show_roi_profile ? crl::multisense::Show_ROIs : profile);
profile |= (dyn.full_res_aux_profile ? crl::multisense::Full_Res_Aux_Cam : profile);
profile |= (dyn.ground_surface_profile ? crl::multisense::Ground_Surface : profile);

cfg.setCameraProfile(profile);
}

template<class T> void Reconfigure::configureStereoProfileWithApriltag(crl::multisense::image::Config &cfg, const T& dyn)
{
crl::multisense::CameraProfile profile = crl::multisense::User_Control;
profile |= (dyn.detail_disparity_profile ? crl::multisense::Detail_Disparity : profile);
profile |= (dyn.high_contrast_profile ? crl::multisense::High_Contrast : profile);
profile |= (dyn.show_roi_profile ? crl::multisense::Show_ROIs : profile);
profile |= (dyn.full_res_aux_profile ? crl::multisense::Full_Res_Aux_Cam : profile);
profile |= (dyn.apriltag_profile ? crl::multisense::AprilTag : profile);

cfg.setCameraProfile(profile);
Expand Down Expand Up @@ -791,7 +824,7 @@ template<class T> void Reconfigure::configureExtrinsics(const T& dyn)
template<class T> void Reconfigure::configureGroundSurfaceParams(const T& dyn)
{
//
// Update calibration on camera via libmultisense
// Update ground surface params on camera via libmultisense
crl::multisense::system::GroundSurfaceParams params;

params.ground_surface_number_of_levels_x = dyn.ground_surface_spline_resolution_x;
Expand Down Expand Up @@ -838,6 +871,28 @@ template<class T> void Reconfigure::configureGroundSurfaceParams(const T& dyn)
);
}

template<class T> void Reconfigure::configureApriltagParams(const T& dyn)
{
//
// Update apriltag params on camera via libmultisense
crl::multisense::system::ApriltagParams params;

params.family = dyn.apriltag_family;
params.quad_detection_blur_sigma = dyn.apriltag_quad_detection_blur_sigma;
params.quad_detection_decimate = dyn.apriltag_quad_detection_decimate;
params.min_border_width = dyn.apriltag_min_border_width;
params.refine_quad_edges = dyn.apriltag_refine_quad_edges;
params.decode_sharpening = dyn.apriltag_decode_sharpening;

// Update ground surface params on camera
Status status = driver_->setApriltagParams(params);
if (Status_Ok != status) {
ROS_ERROR("Reconfigure: failed to set apriltag params: %s",
Channel::statusString(status));
return;
}
}

#define GET_CONFIG() \
image::Config cfg; \
Status status = driver_->getImageConfig(cfg); \
Expand Down Expand Up @@ -971,7 +1026,32 @@ template<class T> void Reconfigure::configureGroundSurfaceParams(const T& dyn)
configureGroundSurfaceParams(dyn); \
} while(0)

#define S27_SGM_APRILTAG() do { \
GET_CONFIG(); \
configureSgm(cfg, dyn); \
configureStereoProfileWithApriltag(cfg, dyn); \
configureAutoWhiteBalance(cfg, dyn); \
configureAuxCamera(cfg, dyn); \
configureCamera(cfg, dyn); \
configureBorderClip(dyn); \
configurePtp(dyn); \
configurePointCloudRange(dyn); \
configureExtrinsics(dyn); \
configureApriltagParams(dyn); \
} while(0)

#define KS21_SGM_APRILTAG() do { \
GET_CONFIG(); \
configureSgm(cfg, dyn); \
configureStereoProfileWithApriltag(cfg, dyn); \
configureCamera(cfg, dyn); \
configureBorderClip(dyn); \
configureLeds(dyn); \
configurePtp(dyn); \
configurePointCloudRange(dyn); \
configureExtrinsics(dyn); \
configureApriltagParams(dyn); \
} while(0)
//
// The dynamic reconfigure callbacks (MultiSense S* & feature variations)

Expand All @@ -985,9 +1065,10 @@ void Reconfigure::callback_mono_cmv2000 (multisense_ros::mono_cmv2000Config
void Reconfigure::callback_mono_cmv4000 (multisense_ros::mono_cmv4000Config& dyn, uint32_t level) { (void) level; MONO_BM_IMU(); }
void Reconfigure::callback_s27_AR0234 (multisense_ros::s27_sgm_AR0234Config& dyn, uint32_t level) { (void) level; S27_SGM(); }
void Reconfigure::callback_ks21_AR0234 (multisense_ros::ks21_sgm_AR0234Config& dyn, uint32_t level) { (void) level; KS21_SGM(); }

void Reconfigure::callback_s27_AR0234_ground_surface (multisense_ros::s27_sgm_AR0234_ground_surfaceConfig& dyn, uint32_t level) { (void) level; S27_SGM_GROUND_SURFACE(); }
void Reconfigure::callback_ks21_AR0234_ground_surface (multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig& dyn, uint32_t level) { (void) level; KS21_SGM_GROUND_SURFACE(); }
void Reconfigure::callback_s27_AR0234_apriltag (multisense_ros::s27_sgm_AR0234_apriltagConfig& dyn, uint32_t level) { (void) level; S27_SGM_APRILTAG(); }
void Reconfigure::callback_ks21_AR0234_apriltag (multisense_ros::ks21_sgm_AR0234_apriltagConfig& dyn, uint32_t level) { (void) level; KS21_SGM_APRILTAG(); }

//
// BCAM (Sony IMX104)
Expand Down

0 comments on commit 5e94306

Please sign in to comment.