Skip to content

Commit

Permalink
update examples after last commit class rename
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Nov 20, 2022
1 parent 672eb1b commit 72fad6f
Show file tree
Hide file tree
Showing 3 changed files with 100 additions and 117 deletions.
7 changes: 4 additions & 3 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,23 +65,24 @@ build-Release/bin/path-planner-cli \
GUI with live navigation simulator:

```
# Holonomic robot:
build-Release/bin/selfdriving-simulator-gui \
--waypoints share/mvsim-demo-waypoints01.yaml \
-s share/mvsim-demo.xml \
-p share/ptgs_holonomic_robot.ini \
--waypoint-sequencer-parameters share/wp-params.yaml \
--nav-engine-parameters share/nav-engine-params.yaml \
--planner-parameters share/mvsim-demo-astar-planner-params.yaml \
--prefer-waypoints-parameters share/costmap-prefer-waypoints.yaml \
--global-costmap-parameters share/costmap-obstacles.yaml \
--local-costmap-parameters share/costmap-obstacles.yaml \
-v DEBUG
```
```
# Ackermann vehicle:
build-Release/bin/selfdriving-simulator-gui \
--waypoints share/mvsim-demo-waypoints01.yaml \
-s share/mvsim-demo.xml \
-p share/ptgs_ackermann_vehicle.ini \
--nav-engine-parameters share/nav-engine-params.yaml \
--planner-parameters share/mvsim-demo-astar-planner-params.yaml \
--prefer-waypoints-parameters share/costmap-prefer-waypoints.yaml \
--global-costmap-parameters share/costmap-obstacles.yaml \
Expand Down
210 changes: 96 additions & 114 deletions apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,10 +88,10 @@ TCLAP::ValueArg<std::string> arg_cost_local_yaml_file(
"Input .yaml file with local obstacle points costmap parameters", false, "",
"points-costmap.yaml", cmd);

TCLAP::ValueArg<std::string> arg_waypoint_seq_yaml_file(
"", "waypoint-sequencer-parameters",
TCLAP::ValueArg<std::string> arg_nav_engine_yaml_file(
"", "nav-engine-parameters",
"Input .yaml file with parameters for NavEngine", false, "",
"wp-sequencer.yaml", cmd);
"nav-engine.yaml", cmd);

TCLAP::ValueArg<std::string> arg_waypoints_yaml_file(
"", "waypoints", "Input .yaml file with waypoints", false, "",
Expand Down Expand Up @@ -184,49 +184,44 @@ static mrpt::maps::CSimplePointsMap::Ptr world_to_static_obstacle_points(
{
auto obsPts = mrpt::maps::CSimplePointsMap::Create();

world.runVisitorOnWorldElements(
[&](mvsim::WorldElementBase& we)
{
if (auto grid = dynamic_cast<mvsim::OccupancyGridMap*>(&we); grid)
{ // get grid occupied cells:
mrpt::maps::CSimplePointsMap pts;
grid->getOccGrid().getAsPointCloud(pts);
obsPts->insertAnotherMap(
&pts, mrpt::poses::CPose3D::Identity());
}
world.runVisitorOnWorldElements([&](mvsim::WorldElementBase& we) {
if (auto grid = dynamic_cast<mvsim::OccupancyGridMap*>(&we); grid)
{ // get grid occupied cells:
mrpt::maps::CSimplePointsMap pts;
grid->getOccGrid().getAsPointCloud(pts);
obsPts->insertAnotherMap(&pts, mrpt::poses::CPose3D::Identity());
}
#ifdef MVSIM_HAS_POINTCLOUD
if (auto pc = dynamic_cast<mvsim::PointCloud*>(&we);
pc && pc->getPoints())
{
obsPts->insertAnotherMap(
pc->getPoints().get(), mrpt::poses::CPose3D::Identity());
}
if (auto pc = dynamic_cast<mvsim::PointCloud*>(&we);
pc && pc->getPoints())
{
obsPts->insertAnotherMap(
pc->getPoints().get(), mrpt::poses::CPose3D::Identity());
}
#endif
});
world.runVisitorOnBlocks(
[&](mvsim::Block& b)
});
world.runVisitorOnBlocks([&](mvsim::Block& b) {
mrpt::maps::CSimplePointsMap pts;
const auto shape = b.blockShape();
const double minDistBetweenPts = 0.1;
ASSERT_(!shape.empty());
for (size_t i = 0; i < shape.size(); i++)
{
mrpt::maps::CSimplePointsMap pts;
const auto shape = b.blockShape();
const double minDistBetweenPts = 0.1;
ASSERT_(!shape.empty());
for (size_t i = 0; i < shape.size(); i++)
const size_t ip1 = (i + 1) % shape.size();
const auto pt0 = shape.at(i);
const auto pt1 = shape.at(ip1);
// sample:
const double dist = (pt1 - pt0).norm();
const size_t nSamples = std::ceil(dist / minDistBetweenPts);
const auto dirVector = (pt1 - pt0).unitarize();
for (size_t k = 0; k < nSamples; k++)
{
const size_t ip1 = (i + 1) % shape.size();
const auto pt0 = shape.at(i);
const auto pt1 = shape.at(ip1);
// sample:
const double dist = (pt1 - pt0).norm();
const size_t nSamples = std::ceil(dist / minDistBetweenPts);
const auto dirVector = (pt1 - pt0).unitarize();
for (size_t k = 0; k < nSamples; k++)
{
const auto pt = pt0 + dirVector * k * dist / (nSamples + 1);
pts.insertPointFast(pt.x, pt.y, 0);
}
const auto pt = pt0 + dirVector * k * dist / (nSamples + 1);
pts.insertPointFast(pt.x, pt.y, 0);
}
obsPts->insertAnotherMap(&pts, mrpt::poses::CPose3D(b.getPose()));
});
}
obsPts->insertAnotherMap(&pts, mrpt::poses::CPose3D(b.getPose()));
});

return obsPts;
}
Expand Down Expand Up @@ -325,10 +320,10 @@ void prepare_selfdriving(mvsim::World& world)
arg_cost_prefer_waypoints_yaml_file.getValue()));
}

if (arg_waypoint_seq_yaml_file.isSet())
if (arg_nav_engine_yaml_file.isSet())
{
sd->navigator.config_.loadFrom(mrpt::containers::yaml::FromFile(
arg_waypoint_seq_yaml_file.getValue()));
arg_nav_engine_yaml_file.getValue()));
}

// all mandaroty fields filled in now:
Expand Down Expand Up @@ -524,13 +519,11 @@ void prepare_selfdriving_window(
auto lck = mrpt::lockHelper(gui->background_scene_mtx);

// navigator 3D visualization interface:
sd->navigator.config_.on_viz_pre_modify = [world, &gui]()
{
sd->navigator.config_.on_viz_pre_modify = [world, &gui]() {
world->m_gui_user_objects_mtx.lock();
gui->background_scene_mtx.lock();
};
sd->navigator.config_.on_viz_post_modify = [world, &gui]()
{
sd->navigator.config_.on_viz_post_modify = [world, &gui]() {
world->m_gui_user_objects_mtx.unlock();
gui->background_scene_mtx.unlock();
};
Expand Down Expand Up @@ -621,26 +614,26 @@ void prepare_selfdriving_window(
pnNav->add<nanogui::Label>("Nav Status: ");

auto btnReq = pnNav->add<nanogui::Button>("requestNavigation()");
btnReq->setCallback(
[world]()
{
// Update global obstacles, in case the MVSIM world has changed:
auto obsPts = world_to_static_obstacle_points(*world);
sd->navigator.config_.globalMapObstacleSource =
selfdriving::ObstacleSource::FromStaticPointcloud(obsPts);
btnReq->setCallback([world]() {
// Update global obstacles, in case the MVSIM world has changed:
auto obsPts = world_to_static_obstacle_points(*world);
sd->navigator.config_.globalMapObstacleSource =
selfdriving::ObstacleSource::FromStaticPointcloud(obsPts);

sd->navigator.request_navigation(sd->waypts);
});
sd->navigator.request_navigation(sd->waypts);
});

pnNav->add<nanogui::Button>("suspend()")
->setCallback([]() { sd->navigator.suspend(); });
pnNav->add<nanogui::Button>("resume()")
->setCallback([]() { sd->navigator.resume(); });
pnNav->add<nanogui::Button>("cancel()")
->setCallback([]() { sd->navigator.cancel(); });
pnNav->add<nanogui::Button>("suspend()")->setCallback([]() {
sd->navigator.suspend();
});
pnNav->add<nanogui::Button>("resume()")->setCallback([]() {
sd->navigator.resume();
});
pnNav->add<nanogui::Button>("cancel()")->setCallback([]() {
sd->navigator.cancel();
});
}
const auto lambdaUpdateNavStatus = [lbNavStatus]()
{
const auto lambdaUpdateNavStatus = [lbNavStatus]() {
const auto state = sd->navigator.current_status();
lbNavStatus->setCaption(mrpt::format(
"Nav status: %s",
Expand Down Expand Up @@ -691,53 +684,48 @@ void prepare_selfdriving_window(
pnPlanner->add<nanogui::TextBox>(dummyState.pose.asString());
edStateGoalPose->setEditable(true);

pickBtn->setCallback(
[glTargetSign, edStateGoalPose]()
{
activeActionMouseHandler =
[glTargetSign, edStateGoalPose](MouseEvent e)
pickBtn->setCallback([glTargetSign, edStateGoalPose]() {
activeActionMouseHandler = [glTargetSign,
edStateGoalPose](MouseEvent e) {
edStateGoalPose->setValue(e.pt.asString());
glTargetSign->setLocation(
e.pt + mrpt::math::TVector3D(0, 0, 0.05));
glTargetSign->setVisibility(true);

// Click -> end mode:
if (e.leftBtnDown)
{
edStateGoalPose->setValue(e.pt.asString());
glTargetSign->setLocation(
e.pt + mrpt::math::TVector3D(0, 0, 0.05));
glTargetSign->setVisibility(true);

// Click -> end mode:
if (e.leftBtnDown)
{
activeActionMouseHandler = {};
glTargetSign->setVisibility(false);
}
};
});
activeActionMouseHandler = {};
glTargetSign->setVisibility(false);
}
};
});

pnPlanner->add<nanogui::Label>("Goal global vel:");
auto edStateGoalVel =
pnPlanner->add<nanogui::TextBox>(dummyState.vel.asString());
edStateGoalVel->setEditable(true);

auto btnDoPlan = pnPlanner->add<nanogui::Button>("Do path planning...");
btnDoPlan->setCallback(
[=]()
btnDoPlan->setCallback([=]() {
try
{
try
{
selfdriving::SE2_KinState stateStart;
stateStart.pose.fromString(edStateStartPose->value());
stateStart.vel.fromString(edStateStartVel->value());
selfdriving::SE2_KinState stateStart;
stateStart.pose.fromString(edStateStartPose->value());
stateStart.vel.fromString(edStateStartVel->value());

selfdriving::SE2orR2_KinState stateGoal;
stateGoal.state = selfdriving::PoseOrPoint::FromString(
edStateGoalPose->value());
stateGoal.vel.fromString(edStateGoalVel->value());
selfdriving::SE2orR2_KinState stateGoal;
stateGoal.state = selfdriving::PoseOrPoint::FromString(
edStateGoalPose->value());
stateGoal.vel.fromString(edStateGoalVel->value());

on_do_single_path_planning(*world, stateStart, stateGoal);
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
}
});
on_do_single_path_planning(*world, stateStart, stateGoal);
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
}
});
}

// -------------------------------
Expand All @@ -748,18 +736,15 @@ void prepare_selfdriving_window(
const auto cbViewCostmaps =
pnViz->add<nanogui::CheckBox>("View costmap");
cbViewCostmaps->setChecked(true);
cbViewCostmaps->setCallback(
[](bool /*checked*/)
{
//
});
cbViewCostmaps->setCallback([](bool /*checked*/) {
//
});
}

// ----------------------------------
// Custom event handlers
// ----------------------------------
const auto lambdaHandleMouseOperations = [gui, world]()
{
const auto lambdaHandleMouseOperations = [gui, world]() {
MRPT_START

static mrpt::math::TPoint3D lastMousePt;
Expand Down Expand Up @@ -794,8 +779,7 @@ void prepare_selfdriving_window(
MRPT_END
};

const auto lambdaCollectSensors = [&]()
{
const auto lambdaCollectSensors = [&]() {
if (!sd->navigator.config_.vehicleMotionInterface) return;
if (sd->sdThreadParams.isClosing()) return;

Expand Down Expand Up @@ -852,11 +836,9 @@ void mvsim_server_thread_update_GUI(GUI_ThreadParams& tp)
static bool firstTime = true;
if (firstTime)
{
tp.world->enqueue_task_to_run_in_gui_thread(
[&]() {
prepare_selfdriving_window(
tp.world->gui_window(), tp.world);
});
tp.world->enqueue_task_to_run_in_gui_thread([&]() {
prepare_selfdriving_window(tp.world->gui_window(), tp.world);
});
firstTime = false;
}

Expand Down
0 share/wp-params.yaml → share/nav-engine-params.yaml
View file
File renamed without changes.

0 comments on commit 72fad6f

Please sign in to comment.