Skip to content

Commit

Permalink
Merge pull request #797 from RainerKuemmerle/pymem-simlib
Browse files Browse the repository at this point in the history 
Pymem simlib
  • Loading branch information
@RainerKuemmerle
RainerKuemmerle authored May 22, 2024
2 parents 0eab20e + 09c360b commit 12778e3
Show file tree
Hide file tree
Showing 15 changed files with 709 additions and 416 deletions.
6 changes: 3 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -465,16 +465,16 @@ install(
NAMESPACE "${G2O_NAMESPACE}"
DESTINATION "${G2O_CONFIG_INSTALL_DIR}")

# Include the subdirectories
add_subdirectory(g2o)

# building unit test framework and our tests
option(BUILD_UNITTESTS "build unit test framework and the tests" OFF)
if(BUILD_UNITTESTS)
enable_testing()
add_subdirectory(unit_test)
endif()

# Include the subdirectories
add_subdirectory(g2o)

# Benchmarks
option(G2O_BUILD_BENCHMARKS "build benchmarks" OFF)
if(G2O_BUILD_BENCHMARKS)
Expand Down
8 changes: 4 additions & 4 deletions g2o/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,6 @@ add_subdirectory(autodiff)
add_subdirectory(stuff)
add_subdirectory(core)

if(G2O_BUILD_APPS)
add_subdirectory(apps)
endif(G2O_BUILD_APPS)

# Pre-canned types
add_subdirectory(types)

Expand All @@ -16,6 +12,10 @@ if (G2O_BUILD_SLAM2D_TYPES AND G2O_BUILD_SLAM3D_TYPES)
add_subdirectory(simulator)
endif()

if(G2O_BUILD_APPS)
add_subdirectory(apps)
endif(G2O_BUILD_APPS)

# Examples
if(G2O_BUILD_EXAMPLES)
add_subdirectory(examples)
Expand Down
236 changes: 39 additions & 197 deletions g2o/apps/g2o_simulator/g2o_simulator2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,227 +24,69 @@
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <optional>

#include "g2o/core/optimizable_graph.h"
#include "g2o/simulator/sensor_odometry2d.h"
#include "g2o/simulator/sensor_pointxy.h"
#include "g2o/simulator/sensor_pointxy_bearing.h"
#include "g2o/simulator/sensor_pose2d.h"
#include "g2o/simulator/sensor_segment2d.h"
#include "g2o/simulator/sensor_segment2d_line.h"
#include "g2o/simulator/sensor_segment2d_pointline.h"
#include "g2o/simulator/simulator.h"
#include "g2o/simulator/simulator2d_base.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/sampler.h"
#include "g2o/types/slam2d/se2.h"

using std::cerr;
#include "g2o/stuff/logger.h"

int main(int argc, char** argv) {
g2o::CommandArgs arg;
int nlandmarks;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasCompass;
bool hasGPS;
g2o::Simulator2D::Config simulator_config;

bool hasSegmentSensor;
int nSegments;
int segmentGridSize;
double minSegmentLength;
double maxSegmentLength;
G2O_INFO("Running 2D simulator");

std::string outputFilename;
arg.param("nlandmarks", nlandmarks, 100, "number of landmarks in the map");
arg.param("nSegments", nSegments, 1000, "number of segments");
arg.param("segmentGridSize", segmentGridSize, 50,
arg.param("nlandmarks", simulator_config.nlandmarks, 100,
"number of landmarks in the map");
arg.param("nSegments", simulator_config.nSegments, 1000,
"number of segments");
arg.param("segmentGridSize", simulator_config.segmentGridSize, 50,
"number of cells of the grid where to align the segments");
arg.param("minSegmentLength", minSegmentLength, 0.5,
arg.param("minSegmentLength", simulator_config.minSegmentLength, 0.5,
"minimal Length of a segment in the world");
arg.param("maxSegmentLength", maxSegmentLength, 3,
arg.param("maxSegmentLength", simulator_config.maxSegmentLength, 3,
"maximal Length of a segment in the world");

arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry");
arg.param("hasPointSensor", hasPointSensor, false,
arg.param("simSteps", simulator_config.simSteps, 100,
"number of simulation steps");
arg.param("worldSize", simulator_config.worldSize, 25.0, "size of the world");
arg.param("hasOdom", simulator_config.hasOdom, false,
"the robot has an odometry");
arg.param("hasPointSensor", simulator_config.hasPointSensor, false,
"the robot has a point sensor");
arg.param("hasPointBearingSensor", hasPointBearingSensor, false,
"the robot has a point bearing sensor");
arg.param("hasPoseSensor", hasPoseSensor, false,
arg.param("hasPointBearingSensor", simulator_config.hasPointBearingSensor,
false, "the robot has a point bearing sensor");
arg.param("hasPoseSensor", simulator_config.hasPoseSensor, false,
"the robot has a pose sensor");
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false,
arg.param("hasCompass", simulator_config.hasCompass, false,
"the robot has a compass");
arg.param("hasGPS", simulator_config.hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", simulator_config.hasSegmentSensor, false,
"the robot has a segment sensor");
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o",
arg.paramLeftOver("graph-output", outputFilename, "",
"graph file which will be written", true);

arg.parseArgs(argc, argv);

std::mt19937 generator;
g2o::World world;
for (int i = 0; i < nlandmarks; i++) {
auto landmark = std::make_unique<g2o::WorldObjectPointXY>();
double x = g2o::sampleUniform(-.5, .5, &generator) * (worldSize + 5);
double y = g2o::sampleUniform(-.5, .5, &generator) * (worldSize + 5);
landmark->vertex()->setEstimate(g2o::Vector2(x, y));
world.addWorldObject(std::move(landmark));
}

cerr << "nSegments = " << nSegments << '\n';

for (int i = 0; i < nSegments; i++) {
auto segment = std::make_unique<g2o::WorldObjectSegment2D>();
int ix = g2o::sampleUniform(-segmentGridSize, segmentGridSize, &generator);
int iy = g2o::sampleUniform(-segmentGridSize, segmentGridSize, &generator);
int ith = g2o::sampleUniform(0, 3, &generator);
double th = (M_PI / 2) * ith;
th = atan2(sin(th), cos(th));
double xc = ix * (worldSize / segmentGridSize);
double yc = iy * (worldSize / segmentGridSize);

double l2 =
g2o::sampleUniform(minSegmentLength, maxSegmentLength, &generator);

double x1 = xc + cos(th) * l2;
double y1 = yc + sin(th) * l2;
double x2 = xc - cos(th) * l2;
double y2 = yc - sin(th) * l2;

segment->vertex()->setEstimateP1(g2o::Vector2(x1, y1));
segment->vertex()->setEstimateP2(g2o::Vector2(x2, y2));
world.addWorldObject(std::move(segment));
}

{
auto robot = std::make_unique<g2o::Robot2D>("myRobot");

if (hasOdom) {
auto odometrySensor = std::make_unique<g2o::SensorOdometry2D>("odometry");
const g2o::Vector3 diagonal(500, 500, 5000);
odometrySensor->setInformation(diagonal.asDiagonal());
robot->addSensor(std::move(odometrySensor), world);
}

if (hasPoseSensor) {
auto poseSensor = std::make_unique<g2o::SensorPose2D>("poseSensor");
g2o::Matrix3 poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo *= 500;
poseInfo(2, 2) = 5000;
poseSensor->setInformation(poseInfo);
robot->addSensor(std::move(poseSensor), world);
}

if (hasPointSensor) {
auto pointSensor = std::make_unique<g2o::SensorPointXY>("pointSensor");
g2o::Matrix2 pointInfo = pointSensor->information();
pointInfo.setIdentity();
pointInfo *= 1000;
pointSensor->setInformation(pointInfo);
pointSensor->setFov(0.75 * M_PI);
robot->addSensor(std::move(pointSensor), world);
}

if (hasPointBearingSensor) {
auto bearingSensor =
std::make_unique<g2o::SensorPointXYBearing>("bearingSensor");
bearingSensor->setInformation(bearingSensor->information() * 1000);
robot->addSensor(std::move(bearingSensor), world);
}

if (hasSegmentSensor) {
cerr << "creating Segment Sensor\n";
auto segmentSensor =
std::make_unique<g2o::SensorSegment2D>("segmentSensor");
cerr << "segmentSensorCreated\n";
segmentSensor->setMaxRange(3);
segmentSensor->setMinRange(.1);
segmentSensor->setInformation(segmentSensor->information() * 1000);
robot->addSensor(std::move(segmentSensor), world);

auto segmentSensorLine =
std::make_unique<g2o::SensorSegment2DLine>("segmentSensorSensorLine");
segmentSensorLine->setMaxRange(3);
segmentSensorLine->setMinRange(.1);
g2o::Matrix2 m = segmentSensorLine->information();
m = m * 1000;
m(0, 0) *= 10;
segmentSensorLine->setInformation(m);
robot->addSensor(std::move(segmentSensorLine), world);

auto segmentSensorPointLine =
std::make_unique<g2o::SensorSegment2DPointLine>(
"segmentSensorSensorPointLine");
segmentSensorPointLine->setMaxRange(3);
segmentSensorPointLine->setMinRange(.1);
g2o::Matrix3 m3 = segmentSensorPointLine->information();
m3 = m3 * 1000;
m3(2, 2) *= 10;
segmentSensorPointLine->setInformation(m3);
robot->addSensor(std::move(segmentSensorPointLine), world);
}

world.addRobot(std::move(robot));
g2o::Simulator2D simulator(std::move(simulator_config));
G2O_INFO("Setting up");
simulator.setup();
G2O_INFO("Simulate");
simulator.simulate();

if (outputFilename.empty()) {
G2O_INFO("Saving to stdout");
simulator.world().graph().save(std::cout);
} else {
G2O_INFO("Saving to {}", outputFilename);
std::ofstream testStream(outputFilename);
simulator.world().graph().save(testStream);
}

for (const auto& robot : world.robots()) {
auto* rob2d = dynamic_cast<g2o::Robot2D*>(robot.get());
if (!rob2d) continue;
rob2d->move(world, g2o::SE2());
}

for (const auto& base_robot : world.robots()) {
auto* robot = dynamic_cast<g2o::Robot2D*>(base_robot.get());
if (!robot) continue;
for (int i = 0; i < simSteps; i++) {
cerr << "m";
const g2o::SE2& pose = robot->pose();
const std::optional<double> dtheta = [&]() -> std::optional<double> {
if (pose.translation().x() < -.5 * worldSize) return 0.;
if (pose.translation().x() > .5 * worldSize) return -M_PI;
if (pose.translation().y() < -.5 * worldSize) return M_PI / 2;
if (pose.translation().y() > .5 * worldSize) return -M_PI / 2;
return std::nullopt;
}();
const g2o::SE2 move = [&]() {
if (!dtheta.has_value()) {
constexpr double kPStraight = 0.7;
constexpr double kPLeft = 0.15;
// select a random move of the robot
double sampled = g2o::sampleUniform(0., 1., &generator);
if (sampled < kPStraight) return g2o::SE2(1., 0., 0.);
if (sampled < kPStraight + kPLeft) return g2o::SE2(0., 0., M_PI / 2);
return g2o::SE2(0., 0., -M_PI / 2);
}
const double mTheta = dtheta.value() - pose.rotation().angle();
g2o::SE2 result;
result.setRotation(g2o::Rotation2D(mTheta));
if (abs(move.rotation().angle()) <
std::numeric_limits<double>::epsilon()) {
result.setTranslation(g2o::Vector2(1., 0.));
}
return result;
}();
robot->relativeMove(world, move);
// do a sense
cerr << "s";
robot->sense(world);
}
}

std::ofstream testStream(outputFilename.c_str());
world.graph().save(testStream);

return 0;
return EXIT_SUCCESS;
}
Loading

0 comments on commit 12778e3

Please sign in to comment.