From 463d84765b3641e8ccdd066b92f7b73d63527357 Mon Sep 17 00:00:00 2001 From: oscardegroot Date: Tue, 3 Dec 2024 16:19:33 +0100 Subject: [PATCH] Fix bug in state retrieval. Set build mode release to ensure fast running. Update example. --- CMakeLists.txt | 4 + config/params.yaml | 22 +- .../guidance_planner/types/space_time_point.h | 6 +- launch/ros1_example.launch | 2 +- rviz/ros1_clean.rviz | 188 ++++++++++++++++++ src/global_guidance.cpp | 5 +- src/prm.cpp | 2 + src/ros1_example.cpp | 30 ++- 8 files changed, 239 insertions(+), 20 deletions(-) create mode 100644 rviz/ros1_clean.rviz diff --git a/CMakeLists.txt b/CMakeLists.txt index 71d1a4d..84e39db 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/config/params.yaml b/config/params.yaml index 854a37a..c84c2e3 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -3,8 +3,8 @@ 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: @@ -12,19 +12,19 @@ guidance_planner: 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: diff --git a/include/guidance_planner/types/space_time_point.h b/include/guidance_planner/types/space_time_point.h index 282057e..c3f9a3d 100644 --- a/include/guidance_planner/types/space_time_point.h +++ b/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 diff --git a/launch/ros1_example.launch b/launch/ros1_example.launch index d23e3a6..ba7c20b 100755 --- a/launch/ros1_example.launch +++ b/launch/ros1_example.launch @@ -8,6 +8,6 @@ - + diff --git a/rviz/ros1_clean.rviz b/rviz/ros1_clean.rviz new file mode 100644 index 0000000..1034e5b --- /dev/null +++ b/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: + 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: + 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 diff --git a/src/global_guidance.cpp b/src/global_guidance.cpp index 1696d48..973ab87 100644 --- a/src/global_guidance.cpp +++ b/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)); } diff --git a/src/prm.cpp b/src/prm.cpp index 49f5d2e..161d1ab 100644 --- a/src/prm.cpp +++ b/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) diff --git a/src/ros1_example.cpp b/src/ros1_example.cpp index 192c628..b207088 100755 --- a/src/ros1_example.cpp +++ b/src/ros1_example.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -41,16 +42,39 @@ void ManualObstacles(std::vector &obstacles) void RandomizeObstacles(std::vector &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)