From 72fad6f7b6240be7759cb972cc95ceee0649b2a5 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 20 Nov 2022 08:12:37 +0100 Subject: [PATCH] update examples after last commit class rename --- README.md | 7 +- .../selfdriving-simulator-gui.cpp | 210 ++++++++---------- ...{wp-params.yaml => nav-engine-params.yaml} | 0 3 files changed, 100 insertions(+), 117 deletions(-) rename share/{wp-params.yaml => nav-engine-params.yaml} (100%) diff --git a/README.md b/README.md index 275c3d7..137fd9b 100644 --- a/README.md +++ b/README.md @@ -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 \ diff --git a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp index 49495b9..6c2569f 100644 --- a/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp +++ b/apps/selfdriving-simulator-gui/selfdriving-simulator-gui.cpp @@ -88,10 +88,10 @@ TCLAP::ValueArg arg_cost_local_yaml_file( "Input .yaml file with local obstacle points costmap parameters", false, "", "points-costmap.yaml", cmd); -TCLAP::ValueArg arg_waypoint_seq_yaml_file( - "", "waypoint-sequencer-parameters", +TCLAP::ValueArg 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 arg_waypoints_yaml_file( "", "waypoints", "Input .yaml file with waypoints", false, "", @@ -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(&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(&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(&we); - pc && pc->getPoints()) - { - obsPts->insertAnotherMap( - pc->getPoints().get(), mrpt::poses::CPose3D::Identity()); - } + if (auto pc = dynamic_cast(&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; } @@ -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: @@ -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(); }; @@ -621,26 +614,26 @@ void prepare_selfdriving_window( pnNav->add("Nav Status: "); auto btnReq = pnNav->add("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("suspend()") - ->setCallback([]() { sd->navigator.suspend(); }); - pnNav->add("resume()") - ->setCallback([]() { sd->navigator.resume(); }); - pnNav->add("cancel()") - ->setCallback([]() { sd->navigator.cancel(); }); + pnNav->add("suspend()")->setCallback([]() { + sd->navigator.suspend(); + }); + pnNav->add("resume()")->setCallback([]() { + sd->navigator.resume(); + }); + pnNav->add("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", @@ -691,25 +684,22 @@ void prepare_selfdriving_window( pnPlanner->add(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("Goal global vel:"); auto edStateGoalVel = @@ -717,27 +707,25 @@ void prepare_selfdriving_window( edStateGoalVel->setEditable(true); auto btnDoPlan = pnPlanner->add("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; + } + }); } // ------------------------------- @@ -748,18 +736,15 @@ void prepare_selfdriving_window( const auto cbViewCostmaps = pnViz->add("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; @@ -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; @@ -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; } diff --git a/share/wp-params.yaml b/share/nav-engine-params.yaml similarity index 100% rename from share/wp-params.yaml rename to share/nav-engine-params.yaml