Python example: mrpt_nav_example.py

Waypoint sequences and trajectory generators (PTGs) with mrpt.nav.

Modules: mrpt.nav, mrpt.math

 1#!/usr/bin/env python3
 2"""
 3Waypoint sequences and trajectory generators (PTGs) with mrpt.nav.
 4
 5Demonstrates:
 6  - TWaypoint / TWaypointSequence / TWaypointStatus
 7  - CParameterizedTrajectoryGenerator factory
 8  - CLogFileRecord
 9"""
10
11import mrpt.nav as nav
12from mrpt.math import TPoint2D
13
14# ---------------------------------------------------------------------------
15# TWaypoint / TWaypointSequence
16# ---------------------------------------------------------------------------
17wp1 = nav.TWaypoint()
18wp1.target = TPoint2D(5.0, 3.0)
19wp1.allowed_distance = 0.5
20wp1.allow_skip = True
21print(f"Waypoint 1: target=({wp1.target.x}, {wp1.target.y}), tol={wp1.allowed_distance}")
22
23wp2 = nav.TWaypoint()
24wp2.target = TPoint2D(10.0, 0.0)
25wp2.allowed_distance = 0.3
26
27seq = nav.TWaypointSequence()
28seq.append(wp1)
29seq.append(wp2)
30print(f"Waypoint sequence ({len(seq.waypoints)} waypoints):")
31print(seq.getAsText())
32
33# ---------------------------------------------------------------------------
34# CParameterizedTrajectoryGenerator factory
35# ---------------------------------------------------------------------------
36# Build a minimal PTG config in INI format
37ptg_ini = (
38    "[PTG]\n"
39    "K=1.0\n"
40    "v_max_mps=0.5\n"
41    "w_max_dps=60\n"
42    "T_c_deg_s=0.5\n"
43    "turningRadiusReference=0.10\n"
44    "num_paths=31\n"
45    "refDistance=3.0\n"
46    "resolution=0.10\n"
47    "score_priority=1.0\n"
48)
49ptg = nav.CParameterizedTrajectoryGenerator.CreatePTG("CPTG_DiffDrive_C", ptg_ini, "PTG")
50if ptg:
51    # initialize() requires a robot shape definition; skip here for brevity
52    desc = ptg.getDescription()
53    print(f"PTG created: {type(ptg).__name__}")
54    print(f"  description: {desc}")
55    print(f"  isInitialized: {ptg.isInitialized()}")
56else:
57    print("PTG creation returned None")
58
59# ---------------------------------------------------------------------------
60# CLogFileRecord
61# ---------------------------------------------------------------------------
62log = nav.CLogFileRecord()
63print(f"CLogFileRecord created (type={type(log).__name__})")
64
65print("\nAll mrpt.nav examples passed.")