visu

source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp

@@ -47,9 +47,10 @@ namespace armarx::armem::server::laser_scans
 
 
     void
-    Visu::init(const wm::CoreSegment* coreSegment)
+    Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
     {
         this->coreSegment = coreSegment;
+        this->virtualRobotReader = virtualRobotReader;
     }
 
 
@@ -116,6 +117,8 @@ namespace armarx::armem::server::laser_scans
 
         const auto color = viz::Color::red();
 
+        ARMARX_INFO << "Point cloud with " << points.size() << " points";
+
         for (const auto& point : points)
         {
             pointCloud.addPoint(point.x(), point.y(), point.z(), color);
@@ -190,7 +193,7 @@ namespace armarx::armem::server::laser_scans
     {
         ARMARX_CHECK_NOT_NULL(coreSegment);
 
-        const auto convert = [](const wm::EntityInstance& entityInstance)
+        const auto convert = [this](const wm::EntityInstance& entityInstance)
             -> ::armarx::armem::laser_scans::LaserScanStamped
         {
             const std::optional<armarx::armem::laser_scans::arondto::LaserScanStamped> dto =
@@ -209,6 +212,8 @@ namespace armarx::armem::server::laser_scans
             laserScanStamped.data =
                 armarx::armem::laser_scans::fromAron<LaserScanStep>(ndArrayNavigator);
 
+            ARMARX_INFO << "Number of steps: " << laserScanStamped.data.size();
+
             return laserScanStamped;
         };
 
@@ -249,12 +254,21 @@ namespace armarx::armem::server::laser_scans
 
         for (const auto& [provider, scan] : currentLaserScans)
         {
-            const auto& robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
 
-            const auto sensorNode = robot->getRobotNode(scan.header.frame);
-            ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
-                                              << "` for robot `" << scan.header.agent << "`";
-            const Eigen::Isometry3f global_T_sensor(sensorNode->getGlobalPose());
+            const auto global_T_sensor = [&]() ->  Eigen::Isometry3f{
+
+                const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
+                if(not robot)
+                {
+                    ARMARX_INFO << 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 << "`";
+                return Eigen::Isometry3f{sensorNode->getGlobalPose()};
+            }();
 
             const std::vector<Eigen::Vector3f> points = convertScanToGlobal(scan, global_T_sensor);
 
@@ -262,7 +276,7 @@ namespace armarx::armem::server::laser_scans
         }
     }
 
-    const VirtualRobot::RobotPtr&
+    VirtualRobot::RobotPtr
     Visu::getSynchronizedRobot(const std::string& name, const DateTime& timestamp)
     {
         if (robots.count(name) > 0)
@@ -271,9 +285,16 @@ namespace armarx::armem::server::laser_scans
         }
 
         ARMARX_CHECK_NOT_NULL(virtualRobotReader);
-        const auto robot = virtualRobotReader->getSynchronizedRobot(name, timestamp);
+        const auto robot = virtualRobotReader->getSynchronizedRobot(name, timestamp, 
+        VirtualRobot::RobotIO::RobotDescription::eStructure,false);
 
-        robots[name] = robot;
+        if(robot)
+        {
+            robots[name] = robot;
+        }else
+        {
+            return nullptr;
+        }
 
         return robots.at(name);
     }

source/RobotAPI/libraries/armem_laser_scans/server/Visu.h

@@ -53,7 +53,7 @@ namespace armarx::armem::server::laser_scans
 
         void defineProperties(armarx::PropertyDefinitionsPtr defs,
                               const std::string& prefix = "visu.");
-        void init(const wm::CoreSegment* coreSegment);
+        void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
         void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
 
 
@@ -78,7 +78,7 @@ namespace armarx::armem::server::laser_scans
 
         std::map<std::string, VirtualRobot::RobotPtr> robots;
 
-        const VirtualRobot::RobotPtr& getSynchronizedRobot(const std::string& name,
+        VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
                                                     const DateTime& timestamp);
 
         void visualizeScan(const std::vector<Eigen::Vector3f>& points,