Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Pymem simlib #797

Merged
merged 3 commits into from
May 22, 2024
Merged

Pymem simlib #797

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3cccd95
Add a 2D simulator class
RainerKuemmerle May 20, 2024
f4819c3
Add a 3D simulator class
RainerKuemmerle May 20, 2024
09c360b
Add basic tests for the simulator library
RainerKuemmerle May 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading