Skip to content

Commit

Permalink
Merge pull request zivid#81 from knzivid/roi-ros
Browse files Browse the repository at this point in the history 
Implement support for pointxyz and range leaves
  • Loading branch information
@knzivid
knzivid authored Apr 5, 2023
2 parents e866a7f + c570b8a commit bcd7770
Show file tree
Hide file tree
Showing 2 changed files with 173 additions and 27 deletions.
132 changes: 105 additions & 27 deletions zivid_camera/src/settings_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
// Settings2D::Acquisition). The utility header files are used by the zivid_camera library.
// This program is compiled and run during the build stage of the zivid_camera library.

#include <Zivid/Point.h>
#include <Zivid/Settings.h>
#include <Zivid/Settings2D.h>

Expand Down Expand Up @@ -197,23 +198,33 @@ class DynamicReconfigureCfgGenerator
{
return false;
}
if constexpr (Zivid::DataModel::HasValidRange<SettingsNode>::value)
else if constexpr (Zivid::DataModel::HasValidRange<SettingsNode>::value)
{
return SettingsNode::validRange().min();
}
if constexpr (IsEnumSetting<SettingsNode>::value)
else if constexpr (IsEnumSetting<SettingsNode>::value)
{
return *SettingsNode::validValues().begin();
}
return ValueType{ 0 };
else if constexpr (std::is_same_v<ValueType, Zivid::Range<double>>)
{
return ValueType{ 0.0, 0.0 };
}
else if constexpr (std::is_same<ValueType, Zivid::PointXYZ>::value)
{
return ValueType{ 0.0f, 0.0f, 0.0f };
}
else
{
return ValueType{ 0 };
}
}

template <typename ValueType>
auto convertValueToRosValue(ValueType value)
{
// Convert from our own setting value types to types that ROS params supports (double, int, bool)

if constexpr (std::is_same_v<ValueType, bool> || std::is_same_v<ValueType, double>)
if constexpr (std::is_same_v<ValueType, bool> || std::is_same_v<ValueType, double> ||
std::is_same_v<ValueType, int>)
{
return value;
}
Expand All @@ -229,6 +240,14 @@ class DynamicReconfigureCfgGenerator
{
return static_cast<int>(value);
}
else if constexpr (std::is_same_v<ValueType, Zivid::Range<double>>)
{
return value;
}
else if constexpr (std::is_same_v<ValueType, Zivid::PointXYZ>)
{
return value;
}
else
{
static_assert(DependentFalse<ValueType>::value, "Could not convert ValueType to ROS type.");
Expand All @@ -250,6 +269,10 @@ class DynamicReconfigureCfgGenerator
{
return "int_t";
}
else if constexpr (std::is_same_v<RosType, Zivid::Range<double>> || std::is_same_v<RosType, Zivid::PointXYZ>)
{
return "double_t";
}
else
{
static_assert(DependentFalse<RosType>::value, "Could not convert RosType to a ROS typename string.");
Expand All @@ -267,6 +290,10 @@ class DynamicReconfigureCfgGenerator
{
return std::to_string(v);
}
else if constexpr (std::is_same_v<RosType, Zivid::Range<double>> || std::is_same_v<RosType, Zivid::PointXYZ>)
{
return "0.0";
}
else
{
static_assert(DependentFalse<RosType>::value, "Could not convert RosType to a string value.");
Expand Down Expand Up @@ -305,21 +332,44 @@ class DynamicReconfigureCfgGenerator
<< boost::algorithm::join(enum_constants, ",\n ") << "\n],\n \"" << description << "\")\n";
}

ss_ << "gen.add(\"" << setting_name << "\", " << type_name << ", " << level << ", "
<< "\"" << description << "\", " << default_value_str;

if constexpr (Zivid::DataModel::HasValidRange<SettingsNode>::value)
if constexpr (std::is_same_v<std::decay_t<decltype(default_value)>, Zivid::PointXYZ>)
{
for (const auto comp : std::array{ "x", "y", "z" })
{
const auto comp_name = setting_name + "_" + comp;
ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", "
<< "\"" << description + " [" + comp + "]"
<< "\", " << default_value_str << ")\n";
}
}
else if constexpr (std::is_same_v<std::decay_t<decltype(default_value)>, Zivid::Range<double>>)
{
ss_ << ", " << valueTypeToRosTypeString(node.validRange().min()) << ", "
<< valueTypeToRosTypeString(node.validRange().max());
for (const auto comp : std::array{ "min", "max" })
{
const auto comp_name = setting_name + "_" + comp;
ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", "
<< "\"" << description + " [" + comp + "]"
<< "\", " << default_value_str << ")\n";
}
}
else if constexpr (IsEnumSetting<SettingsNode>::value)
else
{
const auto min_index = 0;
const auto max_index = node.validValues().size() - 1;
ss_ << ", " << min_index << ", " << max_index << ", edit_method=" << rosGeneratedEnumVariableName(setting_name);
ss_ << "gen.add(\"" << setting_name << "\", " << type_name << ", " << level << ", "
<< "\"" << description << "\", " << default_value_str;

if constexpr (Zivid::DataModel::HasValidRange<SettingsNode>::value)
{
ss_ << ", " << valueTypeToRosTypeString(node.validRange().min()) << ", "
<< valueTypeToRosTypeString(node.validRange().max());
}
else if constexpr (IsEnumSetting<SettingsNode>::value)
{
const auto min_index = 0;
const auto max_index = node.validValues().size() - 1;
ss_ << ", " << min_index << ", " << max_index << ", edit_method=" << rosGeneratedEnumVariableName(setting_name);
}
ss_ << ")\n";
}
ss_ << ")\n";
}

void insertEnabled()
Expand Down Expand Up @@ -396,6 +446,15 @@ class ApplyConfigToZividSettingsGenerator
<< " to setting of type " + setting_node_class_name + ".\");\n"
<< " }()\n";
}
else if constexpr (std::is_same_v<ValueType, Zivid::PointXYZ>)
{
ss_ << "static_cast<float>(" << cfg_id << "_x), static_cast<float>(" << cfg_id << "_y), static_cast<float>("
<< cfg_id << "_z)";
}
else if constexpr (std::is_same_v<ValueType, Zivid::Range<double>>)
{
ss_ << cfg_id << "_min, " << cfg_id << "_max ";
}
else
{
ss_ << cfg_id;
Expand Down Expand Up @@ -436,22 +495,41 @@ class ZividSettingsToConfigGenerator
const auto cfg_id = "cfg." + convertSettingsPathToConfigPath<SettingsRootGroup, SettingsNode>();
const auto zivid_node_class_name = fullyQualifiedZividTypeName<SettingsNode>();
const auto value_str = "s.get<" + zivid_node_class_name + ">().value()";
ss_ << " " + cfg_id + " = ";
if constexpr (std::is_same_v<ValueType, std::chrono::microseconds>)
{
ss_ << "static_cast<int>(" + value_str + ".count());\n";
}
else if constexpr (std::is_same_v<ValueType, std::size_t>)

if constexpr (std::is_same_v<ValueType, Zivid::PointXYZ>)
{
ss_ << "static_cast<int>(" + value_str + ");\n";
for (const auto comp : std::array{ "x", "y", "z" })
{
ss_ << " " + cfg_id + "_" + comp + " = "
<< "static_cast<float>(" + value_str + "." + comp + ");\n";
}
}
else if constexpr (IsEnumSetting<SettingsNode>::value)
else if constexpr (std::is_same_v<ValueType, Zivid::Range<double>>)
{
ss_ << "static_cast<int>(" + value_str + ");\n";
for (const auto comp : std::array{ "min", "max" })
{
ss_ << " " + cfg_id + "_" + comp + " = " << value_str + "." + comp + "();\n";
}
}
else
{
ss_ << value_str + ";\n";
ss_ << " " + cfg_id + " = ";
if constexpr (std::is_same_v<ValueType, std::chrono::microseconds>)
{
ss_ << "static_cast<int>(" + value_str + ".count());\n";
}
else if constexpr (std::is_same_v<ValueType, std::size_t>)
{
ss_ << "static_cast<int>(" + value_str + ");\n";
}
else if constexpr (IsEnumSetting<SettingsNode>::value)
{
ss_ << "static_cast<int>(" + value_str + ");\n";
}
else
{
ss_ << value_str + ";\n";
}
}
}

Expand Down
68 changes: 68 additions & 0 deletions zivid_camera/test/test_zivid_camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -361,6 +361,11 @@ class CaptureOutputTest : public TestWithFileCamera
return camera_.capture(Zivid::Settings2D{ Zivid::Settings2D::Acquisitions{ Zivid::Settings2D::Acquisition{} } });
}

Zivid::PointCloud captureViaSDK(const Zivid::Settings& settings)
{
return camera_.capture(settings).pointCloud();
}

void compareFloat(float a, float b, float delta = 1e-6f) const
{
if (std::isnan(a))
Expand Down Expand Up @@ -460,6 +465,69 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZ)
}
}

#if (ZIVID_CORE_VERSION_MAJOR >= 2 && ZIVID_CORE_VERSION_MINOR >= 9) || ZIVID_CORE_VERSION_MAJOR > 2
TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI)
{
auto points_sub = subscribe<sensor_msgs::PointCloud2>(points_xyz_topic_name);
dynamic_reconfigure::Client<zivid_camera::SettingsConfig> settings_client("/zivid_camera/settings/");
zivid_camera::SettingsConfig configOriginal;
ASSERT_TRUE(settings_client.getDefaultConfiguration(configOriginal, dr_get_max_wait_duration));
auto config = configOriginal;
config.region_of_interest_box_enabled = true;
config.region_of_interest_box_point_o_x = 1;
config.region_of_interest_box_point_o_y = 0;
config.region_of_interest_box_point_o_z = 619;
config.region_of_interest_box_point_a_x = 111;
config.region_of_interest_box_point_a_y = 2;
config.region_of_interest_box_point_a_z = 620;
config.region_of_interest_box_point_b_x = 3;
config.region_of_interest_box_point_b_y = 110;
config.region_of_interest_box_point_b_z = 621;
config.region_of_interest_box_extents_min = -10;
config.region_of_interest_box_extents_max = 10;
ASSERT_TRUE(settings_client.setConfiguration(config));

enableFirst3DAcquisitionAndCapture();
const auto& point_cloud = points_sub.lastMessage();
ASSERT_TRUE(point_cloud.has_value());

const auto point_cloud_sdk = captureViaSDK(Zivid::Settings{
Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} },
Zivid::Settings::RegionOfInterest::Box{
Zivid::Settings::RegionOfInterest::Box::Enabled::yes,
Zivid::Settings::RegionOfInterest::Box::PointO{ 1, 0, 619 },
Zivid::Settings::RegionOfInterest::Box::PointA{ 111, 2, 620 },
Zivid::Settings::RegionOfInterest::Box::PointB{ 3, 110, 621 },
Zivid::Settings::RegionOfInterest::Box::Extents{ -10, 10 },
},
});

auto expected = point_cloud_sdk.copyData<Zivid::PointXYZ>();
const auto numExpectedNaNZ = [&] {
size_t count = 0;
for (size_t i = 0; i < expected.size(); ++i)
{
count += std::isnan(expected(i).z);
}
return count;
}();
ASSERT_EQ(numExpectedNaNZ, 2154265);

for (size_t i = 0; i < expected.size(); ++i)
{
const uint8_t* point_ptr = &point_cloud->data[i * point_cloud->point_step];
const float x = *reinterpret_cast<const float*>(&point_ptr[0]);
const float y = *reinterpret_cast<const float*>(&point_ptr[4]);
const float z = *reinterpret_cast<const float*>(&point_ptr[8]);
comparePointCoordinate(x, expected(i).x);
comparePointCoordinate(y, expected(i).y);
comparePointCoordinate(z, expected(i).z);
}

ASSERT_TRUE(settings_client.setConfiguration(configOriginal));
}
#endif

TEST_F(CaptureOutputTest, testCapture3DColorImage)
{
auto color_image_sub = subscribe<sensor_msgs::Image>(color_image_color_topic_name);
Expand Down

0 comments on commit bcd7770

Please sign in to comment.