Fix bug in state retrieval. Set build mode release to ensure fast run…

…ning. Update example.

CMakeLists.txt

@@ -5,6 +5,10 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
 find_package(OpenMP)
 
 if(OPENMP_FOUND)

config/params.yaml

@@ -3,28 +3,28 @@ guidance_planner:
     output: false
     visuals: false
 
-  T: 4.8 # Time horizon [s]
-  N: 12  # Discrete time steps [#]
+  T: 4.0 # Time horizon [s]
+  N: 20  # Discrete time steps [#]
   seed: 1 # Seed of the visibility-PRM. -1 = random
 
   homotopy:
     n_paths: 4 # Number of guidance trajectories
     comparison_function: Homology # Homology (default) Winding or UVD
     winding:
       pass_threshold: 1.74 # half of pi
-      use_non_passing: true
+      use_non_passing: false
     use_learning: false
     track_selected_homology_only: false
 
-  predictions_are_constant_velocity: false
+  predictions_are_constant_velocity: true
 
   dynamics:
     connections: Straight # Straight (default) or Dubins
     turning_radius: 0.305 # [m]
 
   sampling:
-    n_samples: 1000 # Max number of samples for PRM
-    timeout: 200 # Timeout for PRM sampling [ms]
+    n_samples: 50 #1000 # Max number of samples for PRM
+    timeout: 10 #200 # Timeout for PRM sampling [ms]
     margin: 5.0 # [m] sampled outside of goals 
 
   max_velocity: 3.0 # Maximum velocity of connections between nodes
@@ -40,7 +40,7 @@ guidance_planner:
     consistency: 0  # How much better should a new trajectory be to be selected [%]
 
   goals: # Only used when `LoadReferencePath` is used to set the goals
-    longitudinal: 6 # Number of goals in direction of the path
+    longitudinal: 3 # Number of goals in direction of the path
     vertical: 3 # Number of goals in direction orthogonal to the path
 
   spline_optimization: # Settings when the splines are optimized and used as reference trajectory
@@ -49,16 +49,16 @@ guidance_planner:
     geometric: 25.
     smoothness: 10.
     collision: 0.5
-    velocity_tracking: 0.0 #0.01
+    velocity_tracking: 0.01
 
   visuals:
-    transparency: 0.7 # The least transparent the obstacle visualization is (0-1)
-    visualize_all_samples: true # Visualizes all PRM samples
+    transparency: 0.97 # The least transparent the obstacle visualization is (0-1)
+    visualize_all_samples: false # Visualizes all PRM samples
     visualize_homology: false
     show_indices: false
 
   enable:
-    dynamically_propagate_nodes: false #true  # Propagate the nodes in time (dropping them)
+    dynamically_propagate_nodes: true   # Propagate the nodes in time (dropping them)
     project_from_obstacles: false # Project the guidance trajectory from obstacles if enabled (not necessary by default)
 
   test_node:

include/guidance_planner/types/space_time_point.h

@@ -61,8 +61,8 @@ namespace GuidancePlanner
 
         operator Vector() const { return _vec; } // Cast to Vector
 
-        TVector State() const { return _vec.block(0, 0, T - 1, 1); }
-        void SetState(const TVector &val) const { _vec.block(0, 0, T - 1, 1) = val; }
+        TVector State() const { return _vec.block(0, 0, T, 1); }
+        void SetState(const TVector &val) const { _vec.block(0, 0, T, 1) = val; }
 
         PosTimeVector PosTime() const
         {
@@ -83,7 +83,7 @@ namespace GuidancePlanner
 
         double &operator()(int i) { return _vec(i); }
 
-        static int numStates() { return T - 1; }
+        static int numStates() { return T; }
         static int numPositions() { return P; }
 
         SpaceTimePointDim operator+(const SpaceTimePointDim &other) const

launch/ros1_example.launch

@@ -8,6 +8,6 @@
 
     <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="rqt_reconfigure" respawn="false"/>
 
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find guidance_planner)/rviz/ros1_example.rviz" output="log"/>
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find guidance_planner)/rviz/ros1_clean.rviz" output="log"/>
 
 </launch>  

rviz/ros1_clean.rviz

@@ -0,0 +1,188 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 99
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Grid1
+        - /Grid1/Offset1
+      Splitter Ratio: 0.8436657786369324
+    Tree Height: 772
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_plugin_tutorials/Teleop
+    Name: Teleop
+    Topic: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 0; 0; 0
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Billboards
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 5
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 8
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /example/guidance_planner/obstacles_3d
+      Name: Obstacles
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /example/guidance_planner/start_and_goals
+      Name: Start and Goals
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /example/guidance_planner/graph
+      Name: Graph
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /example/guidance_planner/trajectories
+      Name: Trajectories
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /example/guidance_planner/samples
+      Name: All Samples
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /example/guidance_planner/geometric_paths
+      Name: Geometric Paths
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /example/guidance_planner/spline
+      Name: Spline Points
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /example/people
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /nn_jackal/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 13.901430130004883
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.800000011920929
+      Focal Point:
+        X: 3.6608831882476807
+        Y: -0.4716482162475586
+        Z: 0.5641281604766846
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.9197971224784851
+      Target Frame: <Fixed Frame>
+      Yaw: 2.4019925594329834
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000175000003a2fc0200000008fc0000003d000000790000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008f00fffffffb0000001200530065006c0065006300740069006f006e0000000000000001df0000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f00700000000331000000b50000004500ffffff0000000100000120000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650000000000000007800000035c00fffffffb0000000800540069006d00650100000000000004500000000000000000000005bd000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Teleop:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 72
+  Y: 27

src/global_guidance.cpp

@@ -823,8 +823,9 @@ namespace GuidancePlanner
       for (int k = 0; k < Config::N; k++)
       {
         // Transparent
-        disc.setColorInt(obstacle.id_, (1. - config_->visuals_transparency_) * std::pow(((double)(Config::N - k + 2)) / (double)Config::N + 2, 2.), RosTools::Colormap::BRUNO);
-
+        // disc.setColorInt(obstacle.id_, (1. - config_->visuals_transparency_) * std::pow(((double)(Config::N - k + 2)) / (double)Config::N + 2, 2.), RosTools::Colormap::BRUNO);
+        // disc.setColorInt(2, (1. - config_->visuals_transparency_) * std::pow(((double)(Config::N - k + 2)) / (double)Config::N + 2, 2.), RosTools::Colormap::BRUNO);
+        disc.setColor(25. / 256., 138. / 256., 89. / 256., (1. - config_->visuals_transparency_) * std::pow(((double)(Config::N - k + 2)) / (double)Config::N + 2, 2.));
         disc.addPointMarker(Eigen::Vector3d(obstacle.positions_[k](0), obstacle.positions_[k](1), (float)k * Config::DT));
       }
 

src/prm.cpp

@@ -79,6 +79,7 @@ namespace GuidancePlanner
       environment_->SetPosition(start);
       PRM_LOG("Static obstacles size: " << static_obstacles.size());
       environment_->LoadObstacles(obstacles, static_obstacles);
+      PRM_LOG("Obstacles loaded");
     }
 
     /* Start */
@@ -100,6 +101,7 @@ namespace GuidancePlanner
     // Add goals that are collision free
     {
       PROFILE_SCOPE("Setting goals");
+      PRM_LOG("Setting goals");
       goals_.clear();
       int goal_i = 0;
       for (auto &goal : goals)

src/ros1_example.cpp

@@ -4,6 +4,7 @@
 #include <ros_tools/profiling.h>
 #include <ros_tools/logging.h>
 #include <ros_tools/visuals.h>
+#include <ros_tools/convertions.h>
 
 #include <string>
 #include <stdexcept>
@@ -41,16 +42,39 @@ void ManualObstacles(std::vector<GuidancePlanner::Obstacle> &obstacles)
 
 void RandomizeObstacles(std::vector<GuidancePlanner::Obstacle> &obstacles)
 {
+    auto &publisher = VISUALS.getPublisher("people");
+    auto &model = publisher.getNewModelMarker();
+    model.setColor(25. / 256., 138. / 256., 89. / 256.);
+
+    auto &line = publisher.getNewLine();
+    line.setColor(25. / 256., 138. / 256., 89. / 256., 1.0);
+    line.setScale(0.1, 0.1);
+
+    int num_obstacles = 4;
     obstacles.clear();
     RosTools::RandomGenerator obstacle_randomizer;
-    for (int i = 0; i < 6; i++)
+
+    for (int i = 0; i < num_obstacles; i++)
     {
-        Eigen::Vector2d start(2.0 + obstacle_randomizer.Double() * 4.0, -2. + 2. * obstacle_randomizer.Double());
-        Eigen::Vector2d goal(2.0 + obstacle_randomizer.Double() * 4.0, -2. + 2. * obstacle_randomizer.Double());
+        Eigen::Vector2d start(3.0 + obstacle_randomizer.Double() * 4.0, -3. + 6. * obstacle_randomizer.Double());
+        Eigen::Vector2d goal(-2.0 + obstacle_randomizer.Double() * 4.0, -3. + 6. * obstacle_randomizer.Double());
         Eigen::Vector2d vel = (goal - start).normalized();
 
         obstacles.emplace_back(i, start, vel, Config::DT, Config::N, 0.5);
+
+        model.setOrientation(RosTools::angleToQuaternion(std::atan2(vel(1), vel(0)) + M_PI_2));
+        model.addPointMarker(start);
+
+        Eigen::Vector3d cur(start(0), start(1), 0.);
+        Eigen::Vector3d prev = cur;
+        for (int k = 0; k < Config::N; k++)
+        {
+            cur += Eigen::Vector3d(vel(0) * Config::DT, vel(1) * Config::DT, Config::DT);
+            line.addLine(prev, cur);
+            prev = cur;
+        }
     }
+    publisher.publish();
 }
 
 int main(int argc, char **argv)