Go to the documentation of this file.
38 std::string CAbstractPTGBasedReactive::TNavigationParamsPTG::getAsText()
const
41 CWaypointsNavigator::TNavigationParamsWaypoints::getAsText();
42 s +=
"restrict_PTG_indices: ";
48 bool CAbstractPTGBasedReactive::TNavigationParamsPTG::isEqual(
54 return o !=
nullptr &&
55 CWaypointsNavigator::TNavigationParamsWaypoints::isEqual(rhs) &&
60 CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(
62 bool enableLogFile,
const std::string& sLogDir)
64 m_enableConsoleOutput(enableConsoleOutput),
65 m_navlogfiles_dir(sLogDir)
131 "[CAbstractPTGBasedReactive::enableLogFile] Stopping "
145 "[CAbstractPTGBasedReactive::enableLogFile] Creating rnav log "
152 "Could not create directory for navigation logs: `%s`",
156 std::string filToOpen;
157 for (
unsigned int nFile = 0;; nFile++)
167 std::unique_ptr<CFileGZOutputStream> fil(
169 bool ok = fil->
open(filToOpen, 1 );
173 "Error opening log file: `%s`", filToOpen.c_str());
182 "[CAbstractPTGBasedReactive::enableLogFile] Logging to "
187 catch (
const std::exception& e)
190 "[CAbstractPTGBasedReactive::enableLogFile] Exception: %s",
216 for (
size_t i = 0; i < nPTGs; i++)
222 "Non-registered holonomic method className=`%s`",
236 m_navProfiler,
"CAbstractPTGBasedReactive::performNavigationStep()");
259 for (
size_t i = 0; i < nPTGs; i++)
270 newLogRec.
infoPerPTG[i].ptg = std::dynamic_pointer_cast<
289 const bool target_changed_since_last_iteration =
292 if (target_changed_since_last_iteration)
298 std::vector<CAbstractNavigator::TargetInfo> targets;
300 auto p =
dynamic_cast<
303 if (p && !p->multiple_targets.empty())
305 targets = p->multiple_targets;
312 const size_t nTargets = targets.size();
327 "CAbstractPTGBasedReactive::performNavigationStep().STEP2_"
336 "Error while loading and sorting the obstacles. Robot will be "
340 CPose2D rel_cur_pose_wrt_last_vel_cmd_NOP,
341 rel_pose_PTG_origin_wrt_sense_NOP;
344 std::vector<mrpt::math::TPose2D>() ,
348 rel_cur_pose_wrt_last_vel_cmd_NOP.
asTPose(),
349 rel_pose_PTG_origin_wrt_sense_NOP.
asTPose(),
352 tim_start_iteration);
365 relPoseSense(0, 0, 0), relPoseVelCmd(0, 0, 0);
398 newLogRec.
values[
"timoff_obstacles"] = timoff_obstacles;
399 newLogRec.
values[
"timoff_obstacles_avr"] =
406 newLogRec.
values[
"timoff_curPoseVelAge"] = timoff_curPoseVelAge;
407 newLogRec.
values[
"timoff_curPoseVelAge_avr"] =
411 const double timoff_pose2sense =
412 timoff_obstacles - timoff_curPoseVelAge;
414 double timoff_pose2VelCmd;
417 timoff_curPoseVelAge;
418 newLogRec.
values[
"timoff_pose2sense"] = timoff_pose2sense;
419 newLogRec.
values[
"timoff_pose2VelCmd"] = timoff_pose2VelCmd;
421 if (std::abs(timoff_pose2sense) > 1.25)
423 "timoff_pose2sense=%e is too large! Path extrapolation may "
426 if (std::abs(timoff_pose2VelCmd) > 1.25)
428 "timoff_pose2VelCmd=%e is too large! Path extrapolation "
429 "may be not accurate.",
438 rel_pose_PTG_origin_wrt_sense = relPoseVelCmd - relPoseSense;
449 for (
auto& t : targets)
457 "Different frame_id's but no frame_tf object "
458 "attached (or it expired)!: target_frame_id=`%s` != "
459 "pose_frame_id=`%s`",
460 t.target_frame_id.c_str(),
464 frame_tf->lookupTransform(
466 robot_frame2map_frame);
469 "frame_tf: target_frame_id=`%s` as seen from "
470 "pose_frame_id=`%s` = %s",
471 t.target_frame_id.c_str(),
473 robot_frame2map_frame.
asString().c_str());
475 t.target_coords = robot_frame2map_frame + t.target_coords;
483 std::vector<TPose2D> relTargets;
486 targets.begin(), targets.end(),
487 std::back_inserter(relTargets),
489 return e.target_coords - curPoseExtrapol;
494 const double relTargetDist = relTargets.begin()->norm();
510 "CAbstractPTGBasedReactive::performNavigationStep().update_PTG_"
513 for (
size_t i = 0; i < nPTGs; i++)
519 vector<TCandidateMovementPTG> candidate_movs(
523 for (
size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
527 "CAbstractPTGBasedReactive::performNavigationStep().eval_"
536 holoMethod->setAssociatedPTG(this->
getPTG(indexPTG));
544 ptg, indexPTG, relTargets, rel_pose_PTG_origin_wrt_sense, ipf,
545 cm, newLogRec,
false ,
551 bool is_all_ptg_collision =
true;
552 for (
size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
554 bool is_collision =
true;
556 for (
const auto o : obs)
560 is_collision =
false;
566 is_all_ptg_collision =
false;
570 if (is_all_ptg_collision)
580 bool NOP_not_too_old =
true;
581 bool NOP_not_too_close_and_have_to_slowdown =
true;
582 double NOP_max_time = -1.0, NOP_At = -1.0;
583 double slowdowndist = .0;
596 const bool can_do_nop_motion =
598 !target_changed_since_last_iteration && last_sent_ptg &&
607 (NOP_not_too_close_and_have_to_slowdown =
611 ->getTargetApproachSlowDownDistance())
616 if (!NOP_not_too_old)
619 "PTG-continuation not allowed: previous command timed-out "
620 "(At=%.03f > Max_At=%.03f)",
621 NOP_At, NOP_max_time);
623 if (!NOP_not_too_close_and_have_to_slowdown)
626 "PTG-continuation not allowed: target too close and must start "
627 "slow-down (trgDist=%.03f < SlowDownDist=%.03f)",
628 relTargetDist, slowdowndist);
632 rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0);
634 if (can_do_nop_motion)
638 "CAbstractPTGBasedReactive::performNavigationStep().eval_NOP");
653 bool valid_odom, valid_pose;
656 tim_send_cmd_vel_corrected, robot_odom_at_send_cmd, valid_odom);
658 tim_send_cmd_vel_corrected, robot_pose_at_send_cmd, valid_pose);
660 if (valid_odom && valid_pose)
662 ASSERT_(last_sent_ptg !=
nullptr);
664 std::vector<TPose2D> relTargets_NOPs;
666 targets.begin(), targets.end(),
667 std::back_inserter(relTargets_NOPs),
668 [robot_pose_at_send_cmd](
670 return e.target_coords - robot_pose_at_send_cmd;
674 rel_pose_PTG_origin_wrt_sense_NOP =
675 robot_odom_at_send_cmd -
677 rel_cur_pose_wrt_last_vel_cmd_NOP =
687 [
"rel_cur_pose_wrt_last_vel_cmd_NOP(interp)"] =
688 rel_cur_pose_wrt_last_vel_cmd_NOP.asString();
690 [
"robot_odom_at_send_cmd(interp)"] =
701 candidate_movs[nPTGs], newLogRec,
705 rel_cur_pose_wrt_last_vel_cmd_NOP);
711 candidate_movs[nPTGs].speed =
721 int best_ptg_idx =
m_multiobjopt->decide(candidate_movs, mo_info);
727 for (
unsigned int i = 0; i < newLogRec.
infoPerPTG.size(); i++)
737 "MultiObjMotOptmzr_msg%03i", idx++)] = le;
743 if (best_ptg_idx >= 0)
745 selectedHolonomicMovement = &candidate_movs[best_ptg_idx];
750 const bool best_is_NOP_cmdvel = (best_ptg_idx == int(nPTGs));
756 if (best_is_NOP_cmdvel)
763 "\nERROR calling changeSpeedsNOP()!! Stopping robot and "
764 "finishing navigation\n");
768 newLogRec, relTargets, best_ptg_idx,
770 best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
771 rel_pose_PTG_origin_wrt_sense_NOP,
774 tim_start_iteration);
783 for (
size_t i = 0; i < nPTGs; i++)
790 double cmd_vel_speed_ratio = 1.0;
791 if (selectedHolonomicMovement)
794 m_timelogger,
"navigationStep.selectedHolonomicMovement");
795 cmd_vel_speed_ratio =
801 new_vel_cmd->isStopCmd())
804 "Best velocity command is STOP (no way found), calling "
820 newLogRec.
timestamps[
"tim_send_cmd_vel"] = tim_send_cmd_vel;
824 "\nERROR calling changeSpeeds()!! Stopping robot "
825 "and finishing navigation\n");
830 newLogRec, relTargets, best_ptg_idx,
831 new_vel_cmd, nPTGs, best_is_NOP_cmdvel,
832 rel_cur_pose_wrt_last_vel_cmd_NOP,
833 rel_pose_PTG_origin_wrt_sense_NOP,
836 tim_start_iteration);
845 selectedHolonomicMovement
850 selectedHolonomicMovement->
props[
"holo_stage_eval"];
858 (selectedHolonomicMovement->
props[
"is_slowdown"] != 0.0);
866 tim_start_iteration, tim_send_cmd_vel);
868 newLogRec.
values[
"timoff_sendVelCmd"] = timoff_sendVelCmd;
869 newLogRec.
values[
"timoff_sendVelCmd_avr"] =
882 const double tim_changeSpeed =
891 "Timing warning: Suspicious executionPeriod=%.03f ms is far "
892 "above the average of %.03f ms",
903 "T=%.01lfms Exec:%.01lfms|%.01lfms "
905 new_vel_cmd ? new_vel_cmd->asString().c_str() :
"NOP",
906 selectedHolonomicMovement ? selectedHolonomicMovement->
speed
915 newLogRec, relTargets, best_ptg_idx, new_vel_cmd, nPTGs,
916 best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
917 rel_pose_PTG_origin_wrt_sense_NOP, executionTimeValue,
918 tim_changeSpeed, tim_start_iteration);
921 catch (
const std::exception& e)
924 std::string(
"[CAbstractPTGBasedReactive::performNavigationStep] "
925 "Stopping robot and finishing navigation due to "
927 std::string(e.what()));
932 "[CAbstractPTGBasedReactive::performNavigationStep] Stopping robot "
933 "and finishing navigation due to untyped exception.");
939 CLogFileRecord& newLogRec,
const std::vector<TPose2D>& relTargets,
941 const int nPTGs,
const bool best_is_NOP_cmdvel,
944 const double executionTimeValue,
const double tim_changeSpeed,
951 m_navProfiler,
"CAbstractPTGBasedReactive::STEP8_GenerateLogRecord()");
963 newLogRec.
cmd_vel = new_vel_cmd;
964 newLogRec.
values[
"estimatedExecutionPeriod"] =
966 newLogRec.
values[
"executionTime"] = executionTimeValue;
968 newLogRec.
values[
"time_changeSpeeds()"] = tim_changeSpeed;
969 newLogRec.
values[
"time_changeSpeeds()_avr"] =
971 newLogRec.
values[
"CWaypointsNavigator::navigationStep()"] =
973 newLogRec.
values[
"CAbstractNavigator::navigationStep()"] =
975 newLogRec.
timestamps[
"tim_start_iteration"] = tim_start_iteration;
977 newLogRec.
nPTGs = nPTGs;
981 rel_cur_pose_wrt_last_vel_cmd_NOP;
983 rel_pose_PTG_origin_wrt_sense_NOP;
1009 const std::vector<mrpt::math::TPose2D>& WS_Targets,
1010 const std::vector<CAbstractPTGBasedReactive::PTGTarget>& TP_Targets,
1012 const bool this_is_PTG_continuation,
1014 const unsigned int ptg_idx4weights,
1021 "CAbstractPTGBasedReactive::calc_move_candidate_scores()");
1036 size_t selected_trg_idx = 0;
1038 double best_trg_angdist = std::numeric_limits<double>::max();
1039 for (
size_t i = 0; i < TP_Targets.size(); i++)
1042 TP_Targets[i].target_alpha, cm.
direction));
1043 if (angdist < best_trg_angdist)
1045 best_trg_angdist = angdist;
1046 selected_trg_idx = i;
1050 ASSERT_(selected_trg_idx < WS_Targets.size());
1051 const auto WS_Target = WS_Targets[selected_trg_idx];
1052 const auto TP_Target = TP_Targets[selected_trg_idx];
1054 const double target_d_norm = TP_Target.target_dist;
1058 const double target_WS_d = WS_Target.norm();
1061 const double d = std::min(in_TPObstacles[move_k], 0.99 * target_d_norm);
1079 const double TARGET_SLOW_APPROACHING_DISTANCE =
1085 const double f = std::min(
1087 Vf + target_WS_d * (1.0 - Vf) / TARGET_SLOW_APPROACHING_DISTANCE);
1091 "Relative speed reduced %.03f->%.03f based on Euclidean "
1092 "nearness to target.",
1099 cm.
props[
"ptg_idx"] = ptg_idx4weights;
1100 cm.
props[
"ref_dist"] = ref_dist;
1101 cm.
props[
"target_dir"] = TP_Target.target_alpha;
1102 cm.
props[
"target_k"] = TP_Target.target_k;
1103 cm.
props[
"target_d_norm"] = target_d_norm;
1104 cm.
props[
"move_k"] = move_k;
1105 double& move_cur_d = cm.
props[
"move_cur_d"] =
1108 cm.
props[
"is_PTG_cont"] = this_is_PTG_continuation ? 1 : 0;
1109 cm.
props[
"num_paths"] = in_TPObstacles.size();
1110 cm.
props[
"WS_target_x"] = WS_Target.x;
1111 cm.
props[
"WS_target_y"] = WS_Target.y;
1112 cm.
props[
"robpose_x"] = pose.
x;
1113 cm.
props[
"robpose_y"] = pose.
y;
1114 cm.
props[
"robpose_phi"] = pose.
phi;
1115 cm.
props[
"ptg_priority"] =
1118 const bool is_slowdown =
1119 this_is_PTG_continuation
1122 target_d_norm < 0.99 * in_TPObstacles[move_k]);
1123 cm.
props[
"is_slowdown"] = is_slowdown ? 1 : 0;
1124 cm.
props[
"holo_stage_eval"] =
1125 this_is_PTG_continuation
1127 : (hlfr && !hlfr->dirs_eval.empty() &&
1128 hlfr->dirs_eval.rbegin()->size() == in_TPObstacles.size())
1129 ? hlfr->dirs_eval.rbegin()->at(move_k)
1133 double& colfree = cm.
props[
"collision_free_distance"];
1136 colfree = in_TPObstacles[move_k];
1144 if (this_is_PTG_continuation)
1147 double cur_norm_d = .0;
1148 bool is_exact, is_time_based =
false;
1149 uint32_t cur_ptg_step = 0;
1155 if (std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.
x) > maxD ||
1156 std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.
y) > maxD)
1159 rel_cur_pose_wrt_last_vel_cmd_NOP.
x,
1160 rel_cur_pose_wrt_last_vel_cmd_NOP.
y, cur_k, cur_norm_d);
1165 is_time_based =
true;
1167 const double NOP_At =
1179 log.
TP_Robot.
x = cos(cur_a) * cur_norm_d;
1180 log.
TP_Robot.
y = sin(cur_a) * cur_norm_d;
1192 "PTG-continuation not allowed, cur. pose out of PTG domain.";
1195 bool WS_point_is_unique =
true;
1205 const uint32_t predicted_step =
1210 WS_point_is_unique =
1211 WS_point_is_unique &&
1215 "isBijectiveAt(): k=%i step=%i -> %s",
1216 static_cast<int>(cur_k),
static_cast<int>(cur_ptg_step),
1217 WS_point_is_unique ?
"yes" :
"no");
1219 if (!WS_point_is_unique)
1223 cur_ptg_step = predicted_step;
1228 log.
TP_Robot.
x = cos(cur_a) * cur_norm_d;
1229 log.
TP_Robot.
y = sin(cur_a) * cur_norm_d;
1237 predicted_rel_pose);
1238 const auto predicted_pose_global =
1247 "mismatchDistance=%.03f cm", 1e2 * predicted2real_dist);
1249 if (predicted2real_dist >
1251 .max_distance_predicted_actual_path &&
1253 (target_d_norm - cur_norm_d) * ref_dist > 2.0 ))
1258 "PTG-continuation not allowed, mismatchDistance above "
1267 "PTG-continuation not allowed, couldn't get PTG step for "
1281 colfree = WS_point_is_unique
1282 ? std::min(in_TPObstacles[move_k], in_TPObstacles[cur_k])
1283 : in_TPObstacles[move_k];
1289 colfree -= cur_norm_d;
1293 move_cur_d = cur_norm_d;
1298 cm.
props[
"dist_eucl_final"] =
1304 using map_d2d_t = std::map<double, double>;
1305 map_d2d_t pathDists;
1307 const int num_steps = ceil(D * 2.0);
1308 for (
int i = 0; i < num_steps; i++)
1310 pathDists[i / double(num_steps)] =
1315 WS_Target.x, WS_Target.y, move_k, pathDists,
1318 const auto it = std::min_element(
1319 pathDists.begin(), pathDists.end(),
1320 [colfree](map_d2d_t::value_type& l, map_d2d_t::value_type& r)
1321 ->
bool { return (l.second < r.second) && l.first < colfree; });
1322 cm.
props[
"dist_eucl_min"] = (it != pathDists.end())
1329 double& hysteresis = cm.
props[
"hysteresis"];
1334 hysteresis = this_is_PTG_continuation ? 1.0 : 0.;
1342 if (
typeid(*ptr1) ==
typeid(*ptr2))
1346 desired_cmd->getVelCmdLength());
1348 double simil_score = 0.5;
1349 for (
size_t i = 0; i < desired_cmd->getVelCmdLength(); i++)
1353 desired_cmd->getVelCmdElement(i) -
1358 hysteresis = simil_score;
1365 double& clearance = cm.
props[
"clearance"];
1367 move_k, target_d_norm * 1.01,
false );
1369 move_k, target_d_norm * 0.5,
false );
1371 move_k, target_d_norm * 0.9,
true );
1373 move_k, target_d_norm * 0.5,
true );
1378 double& eta = cm.
props[
"eta"];
1383 const double path_len_meters = d * ref_dist;
1386 uint32_t target_step;
1395 double discount_time = .0;
1396 if (this_is_PTG_continuation)
1406 eta -= discount_time;
1419 double cmdvel_speed_scale = 1.0;
1422 if (in_movement.
speed == 0)
1427 new_vel_cmd->setToStop();
1436 new_vel_cmd->cmdVel_scale(in_movement.
speed);
1437 cmdvel_speed_scale *= in_movement.
speed;
1447 cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
1454 catch (
const std::exception& e)
1457 "[CAbstractPTGBasedReactive::generate_vel_cmd] Exception: "
1460 return cmdvel_speed_scale;
1476 for (
size_t i = 0; i < N; i++)
1480 const std::vector<double>& tp_obs =
1488 bool is_into_domain =
1490 if (!is_into_domain)
continue;
1492 ASSERT_(wp_k <
int(tp_obs.size()));
1494 const double collision_free_dist = tp_obs[wp_k];
1495 if (collision_free_dist > 1.01 * wp_norm_d)
1523 ptg_alpha_index = -1;
1526 colfreedist_move_k = .0;
1527 was_slowdown =
false;
1529 original_holo_eval = .0;
1540 const std::vector<mrpt::math::TPose2D>& relTargets,
1543 const bool this_is_PTG_continuation,
1550 m_navProfiler,
"CAbstractPTGBasedReactive::build_movement_candidate()");
1554 const size_t idx_in_log_infoPerPTGs =
1562 bool use_this_ptg =
true;
1565 if (navpPTG && !navpPTG->restrict_PTG_indices.empty())
1567 use_this_ptg =
false;
1569 i < navpPTG->restrict_PTG_indices.size() && !use_this_ptg; i++)
1571 if (navpPTG->restrict_PTG_indices[i] == indexPTG)
1572 use_this_ptg =
true;
1577 double timeForTPObsTransformation = .0, timeForHolonomicMethod = .0;
1580 bool any_TPTarget_is_valid =
false;
1583 for (
const auto& trg : relTargets)
1589 if (!ptg_target.
valid_TP)
continue;
1591 any_TPTarget_is_valid =
true;
1598 ipf.
targets.emplace_back(ptg_target);
1602 if (!any_TPTarget_is_valid)
1605 "mov_candidate_%u",
static_cast<unsigned int>(indexPTG))] =
1606 "PTG discarded since target(s) is(are) out of domain.";
1636 for (
size_t i = 0; i < Ki; i++) ipf.
TP_Obstacles[i] *= _refD;
1638 timeForTPObsTransformation =
tictac.
Tac();
1641 "navigationStep.STEP3_WSpaceToTPSpace",
1642 timeForTPObsTransformation);
1647 if (!this_is_PTG_continuation)
1663 for (
const auto& t : ipf.
targets)
1665 ni.
targets.push_back(t.TP_Target);
1679 const int kDirection =
1681 double obsFreeNormalizedDistance = ipf.
TP_Obstacles[kDirection];
1690 obsFreeNormalizedDistance = std::min(
1691 obsFreeNormalizedDistance,
1692 std::max(0.90, obsFreeNormalizedDistance - d));
1695 double velScale = 1.0;
1699 if (obsFreeNormalizedDistance <
1702 if (obsFreeNormalizedDistance <=
1707 (obsFreeNormalizedDistance -
1714 cm.
speed *= velScale;
1719 "navigationStep.STEP4_HolonomicMethod",
1720 timeForHolonomicMethod);
1734 m_timelogger,
"navigationStep.calc_move_candidate_scores");
1738 newLogRec.
infoPerPTG[idx_in_log_infoPerPTGs], newLogRec,
1739 this_is_PTG_continuation, rel_cur_pose_wrt_last_vel_cmd_NOP,
1740 indexPTG, tim_start_iteration, HLFR);
1743 cm.
props[
"original_col_free_dist"] =
1754 const bool fill_log_record =
1756 if (fill_log_record)
1759 newLogRec.
infoPerPTG[idx_in_log_infoPerPTGs];
1760 if (!this_is_PTG_continuation)
1764 "NOP cmdvel (prev PTG idx=%u)",
1771 for (
const auto& t : ipf.
targets)
1788 robot_absolute_speed_limits.loadConfigFile(c, s);
1799 min_normalized_free_space_for_ptg_continuation,
double);
1810 robot_absolute_speed_limits.saveToConfigFile(c, s);
1813 string lstHoloStr =
"# List of known classes:\n";
1817 for (
const auto& cl : lst)
1819 string(
"# - `") + string(cl->className) + string(
"`\n");
1823 string(
"C++ class name of the holonomic navigation method to run in "
1824 "the transformed TP-Space.\n") +
1828 string lstDecidersStr =
"# List of known classes:\n";
1832 for (
const auto& cl : lst)
1834 string(
"# - `") + string(cl->className) + string(
"`\n");
1837 motion_decider_method,
1838 string(
"C++ class name of the motion decider method.\n") +
1843 "Maximum distance up to obstacles will be considered (D_{max} in "
1847 "Time constant (in seconds) for the low-pass filter applied to "
1848 "kinematic velocity commands (default=0: no filtering)");
1850 secure_distance_start,
1851 "In normalized distance [0,1], start/end of a ramp function that "
1852 "scales the holonomic navigator output velocity.");
1854 secure_distance_end,
1855 "In normalized distance [0,1], start/end of a ramp function that "
1856 "scales the holonomic navigator output velocity.");
1859 "Whether to use robot pose inter/extrapolation to improve accuracy "
1862 max_distance_predicted_actual_path,
1863 "Max distance [meters] to discard current PTG and issue a new vel cmd "
1866 min_normalized_free_space_for_ptg_continuation,
1867 "Min normalized dist [0,1] after current pose in a PTG continuation to "
1870 enable_obstacle_filtering,
1871 "Enabled obstacle filtering (params in its own section)");
1874 "Enable exact computation of clearance (default=false)");
1876 max_dist_for_timebased_path_prediction,
1877 "Max dist [meters] to use time-based path prediction for NOP "
1883 : holonomic_method(),
1884 ptg_cache_files_directory(
"."),
1886 robot_absolute_speed_limits()
1902 c,
"CAbstractPTGBasedReactive");
1909 filter->options.loadFromConfigFile(c,
"CPointCloudFilterByDistance");
1921 "Non-registered CMultiObjectiveMotionOptimizerBase className=`%s`",
1941 c,
"CAbstractPTGBasedReactive");
1960 for (
const auto& cl : lst)
1983 for (
const auto& cl : lst)
2001 o->setTargetApproachSlowDownDistance(dist);
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds
mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod
Runtime estimation of execution period of the method.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
std::map< std::string, double > values
Known values:
mrpt::system::TTimeStamp timestamp
mrpt::math::TPose2D asTPose() const
mrpt::system::CTimeLogger m_timelogger
A complete time logger.
static CMultiObjectiveMotionOptimizerBase::Ptr Factory(const std::string &className) noexcept
Class factory from C++ class name.
virtual void evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t &inout_realdist2clearance, bool treat_as_obstacle=true) const
Evals the robot clearance for each robot pose along path k, for the real distances in the key of the ...
~CAbstractPTGBasedReactive() override
std::shared_ptr< CObject > Ptr
double secure_distance_start
In normalized distances, the start and end of a ramp function that scales the velocity output from th...
virtual bool implementSenseObstacles(mrpt::system::TTimeStamp &obs_timestamp)=0
Return false on any fatal error.
Implementation of pointcloud filtering based on requisities for minimum neigbouring points in both,...
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
The struct for configuring navigation requests to CAbstractPTGBasedReactive and derived classes.
void copy_container_typecasting(const src_container &src, dst_container &trg)
Copy all the elements in a container (vector, deque, list) into a different one performing the approp...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
CParameterizedTrajectoryGenerator * PTG
The associated PTG.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
double desiredDirection
The results from the holonomic method.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const =0
Returns an empty kinematic velocity command object of the type supported by this PTG.
std::vector< std::string > log_entries
Optionally, debug logging info will be stored here by the implementor classes.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
std::shared_ptr< CHolonomicLogFileRecord > Ptr
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const =0
Converts a discretized "alpha" value into a feasible motion command or action.
double targetRelSpeed
Desired relative speed [0,1] at target.
ClearanceDiagram clearance
Clearance for each path.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
double starting_robot_dir
Default to 0, they can be used to reflect a robot starting at a position not at (0,...
mrpt::math::LowPassFilter_IIR1 meanExecutionTime
double m_last_curPoseVelUpdate_robot_time
std::shared_ptr< CPointCloudFilterBase > Ptr
TRobotPoseVel poseVel
Robot pose & velocities and timestamp of when it was queried.
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
bool open(const std::string &fileName, int compress_level=1, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Open a file for write, choosing the compression level.
double phi
Orientation (rads)
std::string pose_frame_id
map frame ID for pose
mrpt::system::TTimeStamp m_infoPerPTG_timestamp
bool m_enableKeepLogRecords
See enableKeepLogRecords.
mrpt::system::TTimeStamp m_WS_Obstacles_timestamp
mrpt::math::TTwist2D velLocal
bool directoryExists(const std::string &fileName)
Test if a given directory exists (it fails if the given path refers to an existing file).
int16_t ptg_index_NOP
Negative means no NOP mode evaluation, so the rest of "NOP variables" should be ignored.
virtual double evalPathRelativePriority(uint16_t k, double target_distance) const
Query the PTG for the relative priority factor (0,1) of this PTG, in comparison to others,...
int target_k
The discrete version of target_alpha.
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
void initialize() override
Must be called for loading collision grids, or the first navigation command may last a long time to b...
std::vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
mrpt::system::TTimeStamp tim_send_cmd_vel
Timestamp of when the cmd was sent.
CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState
double desiredDirection
The desired motion direction, in the range [-PI, PI].
TargetInfo target
Navigation target.
double original_holo_eval
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
std::vector< CAbstractHolonomicReactiveMethod::Ptr > m_holonomicMethod
The holonomic navigation algorithm (one object per PTG, so internal states are maintained)
void enableLogFile(bool enable)
Enables/disables saving log files.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
mrpt::system::TParameters< double > props
List of properties.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
double colfreedist_move_k
TP-Obstacles in the move direction at the instant of picking this movement.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd()=0
Gets the emergency stop command for the current robot.
std::vector< PTGTarget > targets
double speedfilter_tau
Time constant (in seconds) for the low-pass filter applied to kinematic velocity commands (default=0:...
double getClearance(uint16_t k, double TPS_query_distance, bool integrate_over_path) const
Gets the clearance for path k and distance TPS_query_distance in one of two modes:
void setHolonomicMethod(const std::string &method, const mrpt::config::CConfigFileBase &cfgBase)
Selects which one from the set of available holonomic methods will be used into transformed TP-Space,...
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState
double vx
Velocity components: X,Y (m/s)
int ptg_index
0-based index of used PTG
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::LowPassFilter_IIR1 timoff_obstacles_avr
std::string PTG_desc
A short description for the applied PTG.
The struct for configuring navigation requests to CWaypointsNavigator and derived classes.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match.
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
virtual CAbstractHolonomicReactiveMethod * getHoloMethod(int idx)
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code.
mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd
Last velocity commands.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::recursive_mutex m_critZoneLastLog
Critical zones.
mrpt::io::CStream * m_prev_logfile
The current log file stream, or nullptr if not being used.
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed.
mrpt::math::TPose2D relTarget
Current relative target location.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
This class extends CAbstractNavigator with the capability of following a list of waypoints.
mrpt::math::CVectorFloat TP_Obstacles
Distances until obstacles, in "pseudometers", first index for -PI direction, last one for PI directio...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
bool evaluate_clearance
Default: false.
std::string holonomic_method
C++ class name of the holonomic navigation method to run in the transformed TP-Space.
std::vector< double > TP_Obstacles
One distance per discretized alpha value, describing the "polar plot" of TP obstacles.
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
virtual void sendApparentCollisionEvent()
Callback: Apparent collision event (i.e.
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits
Params related to speed limits.
mrpt::system::CTicTac tictac
double secure_distance_end
void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
double Tac() noexcept
Stops the stopwatch.
void registerUserMeasure(const std::string_view &event_name, const double value, const bool is_time=false) noexcept
std::map< std::string, std::string > additional_debug_msgs
Additional debug traces.
bool m_PTGsMustBeReInitialized
#define MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR_NO_DEFAULT() for REQUIRED variables config file object named c and ...
virtual void loggingGetWSObstaclesAndShape(CLogFileRecord &out_log)=0
Generates a pointcloud of obstacles, and the robot shape, to be saved in the logging record for the c...
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
void setTargetApproachSlowDownDistance(const double dist)
Changes this parameter in all inner holonomic navigator instances [m].
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
double starting_robot_dist
mrpt::math::LowPassFilter_IIR1 timoff_curPoseAndSpeed_avr
The struct for configuring navigation requests.
double target_alpha
TP-Target.
virtual size_t getPTG_count() const =0
Returns the number of different PTGs that have been setup.
A base class for holonomic reactive navigation methods.
mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt
double timeForHolonomicMethod
mrpt::math::LowPassFilter_IIR1 timoff_sendVelCmd_avr
mrpt::math::TPoint2D TP_Target
The Target, in TP-Space (x,y)
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target.
bool enable_obstacle_filtering
#define MRPT_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s
std::vector< const TRuntimeClassId * > getAllRegisteredClassesChildrenOf(const TRuntimeClassId *parent_id)
Like getAllRegisteredClasses(), but filters the list to only include children clases of a given base ...
double leave(const std::string_view &func_name) noexcept
End of a named section.
virtual CParameterizedTrajectoryGenerator * getPTG(size_t i)=0
Gets the i'th PTG.
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for.
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
int round(const T value)
Returns the closer integer (int) to x.
void deleteHolonomicObjects()
Delete m_holonomicMethod.
bool STEP2_SenseObstacles()
void build_movement_candidate(CParameterizedTrajectoryGenerator *ptg, const size_t indexPTG, const std::vector< mrpt::math::TPose2D > &relTargets, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, TInfoPerPTG &ipf, TCandidateMovementPTG &holonomicMovement, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, mrpt::nav::CAbstractHolonomicReactiveMethod &holoMethod, const mrpt::system::TTimeStamp tim_start_iteration, const TNavigationParams &navp=TNavigationParams(), const mrpt::math::TPose2D &relPoseVelCmd_NOP=mrpt::math::TPose2D(0, 0, 0))
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const =0
saves all available parameters, in a forma loadable by initialize()
mrpt::system::CTicTac totalExecutionTime
This class allows loading and storing values and vectors of different types from a configuration text...
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
Saves data to a file and transparently compress the data using the given compression level.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Clearance information for one particular PTG and one set of obstacles.
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
CHolonomicLogFileRecord::Ptr HLFR
Other useful info about holonomic method execution.
Stores a candidate movement in TP-Space-based navigation.
double speed_scale
[0,1] scale of the raw cmd_vel as generated by the PTG
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
mrpt::math::TPoint2D TP_Robot
Robot location in TP-Space: normally (0,0), except during "NOP cmd vel" steps.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
CRobot2NavInterface & m_robot
The navigator-robot interface.
void enableApproachTargetSlowDown(bool enable)
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
Dynamic state that may affect the PTG path parameterization.
void enter(const std::string_view &func_name) noexcept
Start of a named section.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
void performNavigationStep() override
The main method for the navigator.
bool m_closing_navigator
Signal that the destructor has been called, so no more calls are accepted from other threads.
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value.
Virtual base class for multi-objective motion choosers, as used for reactive navigation engines.
void Tic() noexcept
Starts the stopwatch.
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and,...
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
virtual double getTargetApproachSlowDownDistance() const =0
Returns the actual value of this parameter [m], as set via the children class options structure.
static CAbstractHolonomicReactiveMethod::Ptr Factory(const std::string &className) noexcept
The structure used to store all relevant information about each transformation into TP-Space.
virtual double getPathStepDuration() const =0
Returns the duration (in seconds) of each "step".
mrpt::math::TPose2D robotPoseOdometry
This CStream derived class allow using a memory buffer as a CStream.
bool m_enableConsoleOutput
Enables / disables the console debug output.
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
std::string sprintf_vector(const char *fmt, const VEC &V)
Generates a string for a vector in the format [A,B,C,...] to std::cout, and the fmt string for each v...
TSentVelCmd m_lastSentVelCmd
virtual bool isBijectiveAt(uint16_t k, uint32_t step) const
Returns true if a given TP-Space point maps to a unique point in Workspace, and viceversa.
virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
std::shared_ptr< CVehicleVelCmd > Ptr
std::vector< TInfoPerPTG > m_infoPerPTG
Temporary buffers for working with each PTG during a navigationStep()
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
void STEP8_GenerateLogRecord(CLogFileRecord &newLogRec, const std::vector< mrpt::math::TPose2D > &relTargets, int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr &new_vel_cmd, int nPTGs, const bool best_is_NOP_cmdvel, const math::TPose2D &rel_cur_pose_wrt_last_vel_cmd_NOP, const math::TPose2D &rel_pose_PTG_origin_wrt_sense_NOP, const double executionTimeValue, const double tim_changeSpeed, const mrpt::system::TTimeStamp &tim_start_iteration)
double getRefDistance() const
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr
double getLastTime(const std::string &name) const
Return the last execution time of the given "section", or 0 if it hasn't ever been called "enter" wit...
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const override
Implements the way to waypoint is free function in children classes: true must be returned if,...
virtual double generate_vel_cmd(const TCandidateMovementPTG &in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr &new_vel_cmd)
Return the [0,1] velocity scale of raw PTG cmd_vel.
bool m_init_done
Whether loadConfigFile() has been called or not.
double direction
TP-Space movement direction.
std::string m_navlogfiles_dir
Default: "./reactivenav.logs".
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
double getLastOutput() const
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards)
double speed
TP-Space movement speed, normalized to [0,1].
Output for CAbstractHolonomicReactiveMethod::navigate()
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
double getTargetApproachSlowDownDistance() const
Returns this parameter for the first inner holonomic navigator instances [m] (should be the same in a...
double max_dist_for_timebased_path_prediction
Max dist [meters] to use time-based path prediction for NOP evaluation.
uint32_t nPTGs
The number of PTGS:
void preDestructor()
To be called during children destructors to assure thread-safe destruction, and free of shared object...
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string §ion) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
void calc_move_candidate_scores(TCandidateMovementPTG &holonomicMovement, const std::vector< double > &in_TPObstacles, const mrpt::nav::ClearanceDiagram &in_clearance, const std::vector< mrpt::math::TPose2D > &WS_Targets, const std::vector< PTGTarget > &TP_Targets, CLogFileRecord::TInfoPerPTG &log, CLogFileRecord &newLogRec, const bool this_is_PTG_continuation, const mrpt::math::TPose2D &relPoseVelCmd_NOP, const unsigned int ptg_idx4weights, const mrpt::system::TTimeStamp tim_start_iteration, const mrpt::nav::CHolonomicLogFileRecord::Ptr &hlfr)
Scores holonomicMovement.
TAbstractPTGNavigatorParams params_abstract_ptg_navigator
mrpt::math::TPose2D relPoseVelCmd
int32_t nSelectedPTG
The selected PTG.
This base provides a set of functions for maths stuff.
mrpt::nav::ClearanceDiagram clearance
Clearance for each path.
double filter(double x)
Processes one input sample, updates the filter state and return the filtered value.
std::list< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
void updateNavDynamicState(const TNavDynamicState &newState, const bool force_update=false)
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynam...
std::vector< size_t > restrict_PTG_indices
(Default=empty) Optionally, a list of PTG indices can be sent such that the navigator will restrict i...
mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter
Default: none.
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
std::string motion_decider_method
C++ class name of the motion chooser.
bool valid_TP
For each PTG, whether target falls into the PTG domain.
std::vector< double > final_evaluation
The final evaluation score for each candidate.
TAbstractPTGNavigatorParams()
mrpt::math::TPose2D robotPoseLocalization
The robot pose (from odometry and from the localization/SLAM system).
virtual void STEP1_InitPTGs()=0
std::unique_ptr< TNavigationParams > m_copy_prev_navParams
A copy of last-iteration navparams, used to detect changes.
mrpt::math::TTwist2D velGlobal
mrpt::system::CTicTac executionTime
virtual void STEP3_WSpaceToTPSpace(const size_t ptg_idx, std::vector< double > &out_TPObstacles, mrpt::nav::ClearanceDiagram &out_clearance, const mrpt::math::TPose2D &rel_pose_PTG_origin_wrt_sense, const bool eval_clearance)=0
Builds TP-Obstacles from Workspace obstacles for the given PTG.
Virtual base for velocity commands of different kinematic models of planar mobile robot.
bool createDirectory(const std::string &dirName)
Creates a directory.
void getLastLogRecord(CLogFileRecord &o)
Provides a copy of the last log record with information about execution.
mrpt::math::TPose2D relPoseSense
int ptg_alpha_index
Path index for selected PTG.
std::vector< mrpt::math::TPoint2D > TP_Targets
Target(s) location in TP-Space.
mrpt::system::CTicTac timerForExecutionPeriod
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
virtual void navigate(const NavInput &ni, NavOutput &no)=0
Invokes the holonomic navigation algorithm itself.
double timeForTPObsTransformation
Time, in seconds.
This is the base class for any user-defined PTG.
std::string std::string format(std::string_view fmt, ARGS &&... args)
std::unique_ptr< mrpt::io::CStream > m_logFile
Base for all high-level navigation commands.
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
void onStartNewNavigation() override
Called whenever a new navigation has been started.
CLogFileRecord lastLogRecord
The last log.
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
std::recursive_mutex m_nav_cs
mutex for all navigation methods
double getScorePriority() const
When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG.
void onStartNewNavigation() override
Called whenever a new navigation has been started.
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance()
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |