30 #include <Eigen/Geometry>
32 #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
33 #include <SimoxUtility/shapes/OrientedBox.h>
34 #include <VirtualRobot/BoundingBox.h>
35 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
36 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
37 #include <VirtualRobot/Robot.h>
38 #include <VirtualRobot/SceneObjectSet.h>
39 #include <VirtualRobot/VirtualRobot.h>
40 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
41 #include <VirtualRobot/Visualization/VisualizationFactory.h>
56 nextMultipleOf(std::size_t
value, std::size_t multiple)
63 std::size_t remainder =
value % multiple;
69 return value + multiple - remainder;
72 static std::vector<std::string>
73 splitProperty(Property<std::string> prop)
75 std::string propString(prop.getValue());
76 std::vector<std::string> result =
Split(propString,
",");
83 LaserScannerSimulation::calculateGridDimension(
84 const std::vector<VirtualRobot::BoundingBox>& boundingBoxes)
const
87 for (
const auto& boundingBox : boundingBoxes)
89 box.expand_to(boundingBox.getMin());
90 box.expand_to(boundingBox.getMax());
93 const auto extents = box.extents();
95 std::size_t gridSizeX =
static_cast<std::size_t
>(std::ceil(extents.x() / gridCellSize));
96 gridSizeX = nextMultipleOf(gridSizeX,
sizeof(std::size_t) * CHAR_BIT);
98 std::size_t gridSizeY =
static_cast<std::size_t
>(std::ceil(extents.y() / gridCellSize));
99 gridSizeY = nextMultipleOf(gridSizeY,
sizeof(std::size_t) * CHAR_BIT);
101 return {.originX = box.min_x(),
102 .originY = box.min_y(),
103 .sizeX = std::max<std::size_t>(gridSizeX, 1),
104 .sizeY = std::max<std::size_t>(gridSizeY, 1)};
107 VirtualRobot::SceneObjectSetPtr
108 LaserScannerSimulation::getCollisionObjects(
109 const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects)
const
111 VirtualRobot::SceneObjectSetPtr objects(
new VirtualRobot::SceneObjectSet());
113 for (
auto&
object : sceneObjects)
116 if (robot ==
nullptr)
118 objects->addSceneObject(
object);
122 for (
const auto& rns : robot->getRobotNodeSets())
124 objects->addSceneObjects(rns);
133 LaserScannerSimulation::calculateGridDimension(
134 const std::vector<VirtualRobot::SceneObjectPtr>& sceneObjects)
const
136 VirtualRobot::SceneObjectSetPtr objectSet(
new VirtualRobot::SceneObjectSet());
138 for (
auto&
object : sceneObjects)
141 if (robot ==
nullptr)
143 objectSet->addSceneObject(
object);
151 objectSet->addSceneObject(
object);
156 return calculateGridDimension(objectSet);
159 std::vector<VirtualRobot::BoundingBox>
160 LaserScannerSimulation::boundingBoxes(VirtualRobot::SceneObjectSetPtr
const& objects)
const
162 std::vector<VirtualRobot::BoundingBox> objectBoudingBoxes;
164 for (
unsigned int objIndex = 0; objIndex < objects->getSize(); ++objIndex)
169 if (robot ==
nullptr)
171 objectBoudingBoxes.push_back(object->getCollisionModel()->getBoundingBox());
175 for (
const auto& collisionModel : robot->getCollisionModels())
177 if (collisionModel->getCollisionModelImplementation()->getPQPModel())
179 objectBoudingBoxes.push_back(collisionModel->getBoundingBox());
185 return objectBoudingBoxes;
189 LaserScannerSimulation::calculateGridDimension(
190 VirtualRobot::SceneObjectSetPtr
const& objects)
const
192 if (objects->getSize() == 0)
194 return {.originX = 0.0F, .originY = 0.0F, .sizeX = 1, .sizeY = 1};
197 const auto objectBoundingBoxes = boundingBoxes(objects);
198 ARMARX_INFO << objectBoundingBoxes.size() <<
" bounding boxes";
202 return calculateGridDimension(objectBoundingBoxes);
206 LaserScannerSimulation::fillOccupancyGrid(
207 std::vector<VirtualRobot::SceneObjectPtr>
const& sceneObjects)
210 const auto gridDim = calculateGridDimension(sceneObjects);
211 grid.init(gridDim.sizeX, gridDim.sizeY, gridDim.originX, gridDim.originY, gridCellSize);
213 ARMARX_INFO_S <<
"Creating grid with size (" << gridDim.sizeX <<
", " << gridDim.sizeY
216 VirtualRobot::CollisionCheckerPtr collisionChecker =
217 VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
218 VirtualRobot::VisualizationFactoryPtr factory(
new VirtualRobot::CoinVisualizationFactory());
221 float boxSize = 1.1f * gridCellSize;
222 VirtualRobot::VisualizationNodePtr boxVisu = factory->createBox(
223 boxSize, boxSize, boxSize, VirtualRobot::VisualizationFactory::Color::Green());
224 VirtualRobot::CollisionModelPtr boxCollisionModel(
225 new VirtualRobot::CollisionModel(boxVisu,
"", collisionChecker));
227 new VirtualRobot::SceneObject(
"my_box", boxVisu, boxCollisionModel));
229 if (not collisionChecker)
236 const auto collisionObjects = getCollisionObjects(sceneObjects);
238 for (std::size_t indexY = 0; indexY < grid.sizeY; ++indexY)
240 for (std::size_t indexX = 0; indexX < grid.sizeX; ++indexX)
242 const Eigen::Vector2f pos = grid.getCentralPosition(indexX, indexY);
243 const Eigen::Vector3f boxPos(pos.x(), pos.y(), boxPosZ);
245 Eigen::Affine3f(Eigen::Translation3f(boxPos)).matrix();
246 box->setGlobalPose(boxPose);
248 const bool collision = collisionChecker->checkCollision(box, collisionObjects);
250 grid.setOccupied(indexX, indexY, collision);
261 VirtualRobot::init(
"SimulatorViewerApp");
262 worldVisu = Component::create<ArmarXPhysicsWorldVisualization>(
266 enableVisualization = getProperty<bool>(
"visualization.enable").getValue();
267 ARMARX_INFO <<
"Visualization will be " << (enableVisualization ?
"enabled" :
"disabled");
269 topicName = getProperty<std::string>(
"LaserScannerTopicName").getValue();
270 ARMARX_INFO <<
"Reporting on topic \"" << topicName <<
"\"";
273 robotStateName = getProperty<std::string>(
"RobotStateComponentName").getValue();
274 ARMARX_INFO <<
"Using RobotStateComponent \"" << robotStateName <<
"\"";
277 debugDrawerName = getProperty<std::string>(
"DebugDrawerTopicName").getValue();
278 ARMARX_INFO <<
"Using DebugDrawerTopic \"" << debugDrawerName <<
"\"";
281 topicReplayerDummy = getProperty<bool>(
"TopicReplayerDummy").getValue();
282 updatePeriod = getProperty<int>(
"UpdatePeriod").getValue();
283 visuUpdateFrequency = getProperty<int>(
"VisuUpdateFrequency").getValue();
284 gridCellSize = getProperty<float>(
"GridCellSize").getValue();
286 auto framesStrings = splitProperty(getProperty<std::string>(
"Frames"));
287 auto deviceStrings = splitProperty(getProperty<std::string>(
"Devices"));
288 auto minAnglesStrings = splitProperty(getProperty<std::string>(
"MinAngles"));
289 auto maxAnglesStrings = splitProperty(getProperty<std::string>(
"MaxAngles"));
290 auto stepsStrings = splitProperty(getProperty<std::string>(
"Steps"));
291 auto noiseStrings = splitProperty(getProperty<std::string>(
"NoiseStdDev"));
292 std::size_t scannerSize = framesStrings.size();
293 if (deviceStrings.size() == 1)
295 deviceStrings.resize(scannerSize, deviceStrings.front());
297 else if (deviceStrings.size() != scannerSize)
299 ARMARX_WARNING <<
"Unexpected size of property Devices (expected " << scannerSize
300 <<
" but got " << deviceStrings.size() <<
")";
303 if (minAnglesStrings.size() == 1)
305 minAnglesStrings.resize(scannerSize, minAnglesStrings.front());
307 else if (minAnglesStrings.size() != scannerSize)
309 ARMARX_WARNING <<
"Unexpected size of property MinAngles (expected " << scannerSize
310 <<
" but got " << minAnglesStrings.size() <<
")";
313 if (maxAnglesStrings.size() == 1)
315 maxAnglesStrings.resize(scannerSize, maxAnglesStrings.front());
317 else if (maxAnglesStrings.size() != scannerSize)
319 ARMARX_WARNING <<
"Unexpected size of property MaxAngles (expected " << scannerSize
320 <<
" but got " << maxAnglesStrings.size() <<
")";
323 if (stepsStrings.size() == 1)
325 stepsStrings.resize(scannerSize, stepsStrings.front());
327 else if (stepsStrings.size() != scannerSize)
329 ARMARX_WARNING <<
"Unexpected size of property Steps (expected " << scannerSize
330 <<
" but got " << stepsStrings.size() <<
")";
333 if (noiseStrings.size() == 1)
335 noiseStrings.resize(scannerSize, noiseStrings.front());
337 else if (noiseStrings.size() != scannerSize)
339 ARMARX_WARNING <<
"Unexpected size of property NoiseStdDev (expected " << scannerSize
340 <<
" but got " << noiseStrings.size() <<
")";
344 scanners.reserve(scannerSize);
345 connectedDevices.clear();
346 for (std::size_t i = 0; i < scannerSize; ++i)
349 scanner.
frame = framesStrings[i];
352 scanner.
minAngle = std::stof(minAnglesStrings[i]);
353 scanner.
maxAngle = std::stof(maxAnglesStrings[i]);
355 scanner.
steps = std::stoi(stepsStrings[i]);
357 catch (std::exception
const& ex)
359 ARMARX_INFO <<
"Scanner[" << i <<
"] Config error: " << ex.what();
363 scanners.push_back(scanner);
365 LaserScannerInfo info;
366 info.device = topicReplayerDummy ? deviceStrings[i] : scanner.
frame;
367 info.frame = scanner.
frame;
370 info.stepSize = (info.maxAngle - info.minAngle) / scanner.
steps;
371 connectedDevices.push_back(info);
381 if (topicReplayerDummy)
383 ARMARX_INFO <<
"Fake connect (component is used for topic replay)";
386 topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
387 robotState = getProxy<RobotStateComponentInterfacePrx>(robotStateName);
388 sharedRobot = robotState->getSynchronizedRobot();
389 debugDrawer = getTopic<DebugDrawerInterfacePrx>(debugDrawerName);
395 scanner.frameNode = sharedRobot->getRobotNode(scanner.frame);
397 catch (std::exception
const& ex)
399 ARMARX_WARNING <<
"Error while querying robot frame: " << scanner.frame <<
" "
401 scanner.frameNode =
nullptr;
404 if (!scanner.frameNode)
406 ARMARX_WARNING <<
"Tried to use a non-existing robot node as frame for the "
407 "laser scanner simulation: "
417 if (enableVisualization)
423 std::vector<VirtualRobot::SceneObjectPtr> objects;
424 while (objects.empty())
426 worldVisu->synchronizeVisualizationData();
427 objects = worldVisu->getObjects();
429 <<
" objects from the simulator";
433 <<
"Could not get any objects from the simulator after syncing";
438 std::vector<VirtualRobot::RobotPtr>
robots;
441 worldVisu->synchronizeVisualizationData();
442 robots = worldVisu->getRobots();
447 <<
"Could not get any robots from the simulator after syncing";
457 [](
const auto& r) ->
bool { return not r->isPassive(); }),
460 ARMARX_INFO <<
"Got " << objects.size() <<
" scene objects from the simulator";
462 std::vector<VirtualRobot::SceneObjectPtr> validObjects;
465 VirtualRobot::CollisionModelPtr cm = o->getCollisionModel();
468 ARMARX_WARNING <<
"Scene object with no collision model: " << o->getName();
472 const auto pqpModel = cm->getCollisionModelImplementation()->getPQPModel();
475 validObjects.push_back(o);
484 validObjects.insert(validObjects.end(),
robots.begin(),
robots.end());
486 fillOccupancyGrid(validObjects);
490 arvizDrawer->drawOccupancyGrid(grid, boxPosZ);
493 visuThrottler = std::make_unique<Throttler>(visuUpdateFrequency);
496 &LaserScannerSimulation::updateScanData,
499 "LaserScannerSimUpdate");
539 return connectedDevices;
543 LaserScannerSimulation::updateScanData()
548 const bool updateVisu =
549 enableVisualization and visuThrottler->check(
TimeUtil::GetTime().toMicroSeconds());
553 if (!scanner.frameNode)
557 PosePtr scannerPoseP = PosePtr::dynamicCast(scanner.frameNode->getGlobalPose());
559 Eigen::Vector2f position = scannerPose.col(3).head<2>();
562 Eigen::Vector2f yWorld(0.0f, 1.0f);
563 Eigen::Vector2f yScanner = scannerRot.col(1).head<2>();
564 float theta = acos(yWorld.dot(yScanner));
565 if (yScanner.x() >= 0.0f)
570 float minAngle = scanner.minAngle;
571 float maxAngle = scanner.maxAngle;
572 int scanSteps = scanner.steps;
575 scan.reserve(scanSteps);
579 const Interval skipInterval(0.0, M_PI_2);
581 std::normal_distribution<float> dist(0.0f, scanner.noiseStdDev);
582 for (
int i = 0; i < scanSteps; ++i)
585 step.angle = minAngle + i * (maxAngle - minAngle) / (scanSteps - 1);
587 if (skipInterval.contains(step.angle))
592 const Eigen::Vector2f scanDirGlobal =
593 Eigen::Rotation2Df(step.angle + theta) * Eigen::Vector2f(0.0f, 1.0f);
594 const float distance = grid.computeDistance(position, scanDirGlobal);
597 step.distance =
distance + dist(engine);
598 scan.push_back(step);
602 topic->reportSensorValues(scanner.frame, scanner.frame, scan, now);
606 arvizDrawer->prepareScan(scan, scanner.frame, Eigen::Affine3f(scannerPose));
612 arvizDrawer->drawScans();
616 auto timeElapsed = endTime - startTime;
619 <<
"Time to simulate laser scanners: " << timeElapsed.toMilliSecondsDouble()