From 80613254ddbb4e037fcb0f37d2063f3b66905ea4 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Fri, 23 Jun 2023 12:40:10 +0200
Subject: [PATCH] better visu
---
.../interface/units/LaserScannerUnit.ice | 17 ++--
.../armem_laser_scans/server/Visu.cpp | 89 +++++++++++++------
.../libraries/armem_laser_scans/server/Visu.h | 17 ++--
3 files changed, 81 insertions(+), 42 deletions(-)
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 631974396..cf3a48562 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -46,18 +46,18 @@ module armarx
**/
struct LaserScanStep
{
- float angle;
- float distance;
-// float intensity;
+ float angle;
+ float distance;
+ float intensity;
};
struct LaserScannerInfo
{
- string device;
- string frame;
- float minAngle;
- float maxAngle;
- float stepSize;
+ string device;
+ string frame;
+ float minAngle;
+ float maxAngle;
+ float stepSize;
};
sequence<LaserScanStep> LaserScan;
@@ -82,4 +82,3 @@ module armarx
};
};
-
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index 551e4792c..ff8a7c9ca 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -1,7 +1,5 @@
#include "Visu.h"
-#include <SimoxUtility/color/Color.h>
-#include <SimoxUtility/color/ColorMap.h>
#include <algorithm>
#include <exception>
#include <string>
@@ -10,6 +8,9 @@
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <SimoxUtility/color/hsv.h>
#include <SimoxUtility/math/pose.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -32,33 +33,34 @@
namespace armarx::armem::server::laser_scans
{
+
Visu::Visu()
{
Logging::setTag("Visu");
}
-
void
Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(
p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
- defs->optional(p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
+ defs->optional(
+ p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
defs->optional(p.maxRobotAgeMs,
prefix + "maxRobotAgeMs",
"Maximum age of robot state before a new one is retrieved in milliseconds.");
+ defs->optional(p.colorByIntensity, "colorByIntensity", "");
}
-
void
- Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
+ Visu::init(const wm::CoreSegment* coreSegment,
+ armem::robot_state::VirtualRobotReader* virtualRobotReader)
{
this->coreSegment = coreSegment;
this->virtualRobotReader = virtualRobotReader;
}
-
void
Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
@@ -79,7 +81,6 @@ namespace armarx::armem::server::laser_scans
updateTask->start();
}
-
void
Visu::visualizeRun()
{
@@ -114,9 +115,10 @@ namespace armarx::armem::server::laser_scans
}
void
- Visu::visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ Visu::visualizeScan(const std::vector<ScanPoint>& points,
const std::string& sensorName,
- const std::string& agentName, const viz::Color& color)
+ const std::string& agentName,
+ const viz::Color& color)
{
viz::PointCloud pointCloud("laser_scan");
@@ -124,7 +126,30 @@ namespace armarx::armem::server::laser_scans
for (const auto& point : points)
{
- pointCloud.addPoint(point.x(), point.y(), point.z(), color);
+
+ // ARMARX_INFO << point.intensity;
+ const viz::Color specificColor = [&point, &color, this]() -> viz::Color
+ {
+ if (p.colorByIntensity)
+ {
+ Eigen::Vector3f hsv = simox::color::rgb_to_hsv(
+ Eigen::Vector3f(static_cast<float>(color.r) / 255.f,
+ static_cast<float>(color.g) / 255.f,
+ static_cast<float>(color.b) / 255.f));
+
+ // ARMARX_INFO << point.intensity;
+
+ hsv(2) = std::clamp<float>(point.intensity, 0., 1.);
+
+ const Eigen::Vector3f rgb = simox::color::hsv_to_rgb(hsv);
+
+ return viz::Color{rgb(0), rgb(1), rgb(2)};
+ }
+
+ return color;
+ }();
+
+ pointCloud.addPoint(point.point.x(), point.point.y(), point.point.z(), specificColor);
}
pointCloud.pointSizeInPixels(3);
@@ -135,22 +160,28 @@ namespace armarx::armem::server::laser_scans
arviz.commit(l);
}
- std::vector<Eigen::Vector3f>
+ std::vector<ScanPoint>
convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
const Eigen::Isometry3f& global_T_sensor)
{
- auto scanCartesian =
+ const auto scanCartesian =
armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
- for (auto& point : scanCartesian)
+ std::vector<ScanPoint> points;
+ points.reserve(scan.data.size());
+
+ for (std::size_t i = 0; i < scan.data.size(); i++)
{
- point = global_T_sensor * point;
+ const auto& point = scanCartesian.at(i);
+ const auto& raw = scan.data.at(i);
+
+ const Eigen::Vector3f pointGlobal = global_T_sensor * point;
+ points.push_back(ScanPoint{.point = pointGlobal, .intensity = raw.intensity});
}
- return scanCartesian;
+ return points;
}
-
// void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
// {
// coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
@@ -263,31 +294,35 @@ namespace armarx::armem::server::laser_scans
{
ARMARX_VERBOSE << "Visualizing `" << provider << "`";
- const auto global_T_sensor = [&]() -> Eigen::Isometry3f{
-
+ const auto global_T_sensor = [&]() -> Eigen::Isometry3f
+ {
const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
- if(not robot)
+ if (not robot)
{
- ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`" << "not available";
+ ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`"
+ << "not available";
return Eigen::Isometry3f::Identity();
}
const auto sensorNode = robot->getRobotNode(scan.header.frame);
ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
- << "` for robot `" << scan.header.agent << "`";
+ << "` for robot `" << scan.header.agent << "`";
- ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is " << sensorNode->getGlobalPosition();
+ ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is "
+ << sensorNode->getGlobalPosition();
return Eigen::Isometry3f{sensorNode->getGlobalPose()};
}();
- const std::vector<Eigen::Vector3f> points = convertScanToGlobal(scan, global_T_sensor);
- const auto color = [&]() -> simox::Color{
- if(p.uniformColor)
+ const std::vector<ScanPoint> points = convertScanToGlobal(scan, global_T_sensor);
+
+ const auto color = [&]() -> simox::Color
+ {
+ if (p.uniformColor)
{
return simox::Color::red();
}
-
+
return simox::color::GlasbeyLUT::at(i++);
}();
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
index d6c79dd12..5218e6a0f 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
@@ -33,14 +33,18 @@
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem_laser_scans/types.h>
-#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
-
namespace armarx::armem::server::laser_scans
{
+ struct ScanPoint
+ {
+ Eigen::Vector3f point;
+ float intensity;
+ };
/**
* @brief Models decay of object localizations by decreasing the confidence
@@ -54,7 +58,8 @@ namespace armarx::armem::server::laser_scans
void defineProperties(armarx::PropertyDefinitionsPtr defs,
const std::string& prefix = "visu.");
- void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
+ void init(const wm::CoreSegment* coreSegment,
+ armem::robot_state::VirtualRobotReader* virtualRobotReader);
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
@@ -70,11 +75,11 @@ namespace armarx::armem::server::laser_scans
{
bool enabled = true;
bool uniformColor = false;
+ bool colorByIntensity = true;
float frequencyHz = 5;
int maxRobotAgeMs = 100;
} p;
-
SimpleRunningTask<>::pointer_type updateTask;
armem::robot_state::VirtualRobotReader* virtualRobotReader;
@@ -84,7 +89,7 @@ namespace armarx::armem::server::laser_scans
VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
const DateTime& timestamp);
- void visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ void visualizeScan(const std::vector<ScanPoint>& points,
const std::string& sensorName,
const std::string& agentName,
const viz::Color& color);
--
GitLab