fix: PSO waypoint logic, frame alignment, and lint cleanup#1
Open
fadi-labib wants to merge 7 commits into
Open
fix: PSO waypoint logic, frame alignment, and lint cleanup#1fadi-labib wants to merge 7 commits into
fadi-labib wants to merge 7 commits into
Conversation
PSO Changes: - PSO now finds optimal WAYPOINT between start and goal (not just goal) - Fitness function: path_length + obstacle_penalty + segment_penalty - Made start/goal positions configurable parameters - Path segments checked for obstacle proximity Navigation Changes: - Waypoint follower uses state machine: WAITING → TO_WAYPOINT → TO_GOAL → COMPLETE - Robot navigates: start → PSO_waypoint → goal - Added final_goal_x, final_goal_y parameters TurtleBot3 Fixes: - Changed cmd_vel to TwistStamped (required by TurtleBot3 DiffDrive) - Removed duplicate ROS-Gazebo bridge (TurtleBot3 spawn includes one) - Scaled world obstacles for TurtleBot3 size (0.2-0.3m radius) Added RViz config for PSO particle visualization. Note: Gazebo/RViz coordinate alignment still needs investigation.
Logic fixes: - PsoSolver::step() now increments iteration_ (previously only solve() did, which the timer-driven node never calls), and AlgorithmBase::solve() no longer double-counts. The PSO node drops its duplicate counter and reads get_iteration() instead. - Align map/odom frames: parameterize the static map->odom TF with the robot spawn pose and convert map-frame PSO waypoints to odom in the follower. - Skip the PSO waypoint when the direct path is clear (path-length test) so the robot does not detour through an arbitrary collinear point. - Stop triple-counting the waypoint obstacle penalty; fold the inside-obstacle case into a single segment check at 0.3 m clearance. Test fixes (these never passed before): - Resolve TEST/TEST_F suite collision in pso_solver_test that aborted 9 tests. - Correct RobotControllerTest.Stop: stop() halts motion but keeps the goal. Cleanup: - Obstacles now live in ethobot_algorithms/config/obstacles.yaml, loaded via ROS params by the PSO node and the visualizer (SDF documented to stay in sync) instead of being hardcoded in three places. - Remove dead code (direct_path_length_, wait_for_completion_, duplicate iteration counter); delete tf2 view_frames artifacts and gitignore them. Lint compliance (colcon test now fully green: 123 tests, 0 failures): - Add MIT copyright headers to all source files. - Fix cpplint (TODO usernames, long lines), flake8 (import order, unused imports/vars, docstring), uncrustify formatting, and the package.xml member_of_group/test_depend ordering in ethobot_interfaces.
Covers the behaviour not exercised by colcon test or headless node runs: robot motion, map/odom coordinate alignment, the degenerate-waypoint early-out, and obstacle single-source-of-truth. Includes the off-origin spawn case that directly validates the map->odom fix.
The Gazebo demo launch points RViz at share/ethobot_simulation/config/ gazebo_pso.rviz, but CMakeLists only installed launch/ and worlds/. A clean build dropped the RViz config, so RViz started unconfigured. Add the config/ install rule.
- turtlebot3_pso.launch.py: publish map->odom (was odom->map), matching the conventional tree and gazebo_pso_demo. Robot spawns at origin in empty_world so identity translation is correct. - gazebo_pso_demo.launch.py: drop the dangling use_sim_time launch argument (the variable was already removed; nothing reads it). - package.xml: declare tf2_ros, which the launch files use for the static transform publisher.
- turtlebot3_pso.launch.py: pass final_goal_x/y to the waypoint follower so it drives to the same goal PSO plans for (was planning for (2,2) but driving to the (3,3) default). - waypoint_follower_node.cpp: replace empty RCLCPP_INFO format strings with a single space to silence -Wformat-zero-length while keeping log separators. - CLAUDE.md: update the project structure to list all five packages (was still showing only ethobot_interfaces as active).
- pso_solver: implement convergence detection (stagnation over consecutive steps) and report it via SwarmState.converged instead of hardcoded false. - waypoint_follower: add nav_timeout_sec; abort a phase (stop + log error) if the goal isn't reached in time, since the controller has no reactive obstacle avoidance and would otherwise hang forever. - pso_path_planning_node: cancel the timer after the final publish so it stops firing once optimization completes. - swarm_visualizer: retune for the 4x4 m workspace — parameterized fitness color range (best particles no longer all red), and reposition the info text and ground plane. - config/pso_params.yaml: replace stale (10,10)/100-iter values with the current waypoint demo; obstacles stay in obstacles.yaml (single source). - CLAUDE.md: correct the ROS2 interface tables (cmd_vel is TwistStamped, add the visualization topic) and mark unimplemented services/actions as planned.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Brings the PSO waypoint-optimization feature to a working, fully-tested state and clears long-standing lint debt. This PR contains the PSO redesign (
661e567, never pushed), the fix/cleanup work (c2381b6), and a manual test plan (38135e5).Logic fixes
PsoSolver::step()never incrementediteration_(onlysolve()did, which the timer-driven node never calls). Fixed instep();solve()no longer double-counts; the node drops its duplicate counter.map→odomTF was hardcoded to identity while the robot can spawn off-origin. Now parameterized by the spawn pose, and the follower converts map-frame waypoints into odom.Test fixes (these never passed before)
pso_solver_testmixedTEST/TEST_Fin one suite, which GoogleTest aborts at runtime — 9 tests never ran. Suite made consistent.RobotControllerTest.Stopassertedstop()clears the goal; corrected to match the API (stop()halts motion,clear_goal()abandons the goal).Cleanup
ethobot_algorithms/config/obstacles.yaml, loaded via ROS params by the PSO node and visualizer (Gazebo SDF documented to stay in sync) instead of being hardcoded in three places.direct_path_length_,wait_for_completion_, duplicate iteration counter); deleted empty tf2view_framesartifacts and gitignored them.Lint compliance
package.xmlmember_of_group/test_dependordering inethobot_interfaces.Test plan
Automated (verified locally):
colcon build— all 5 packagescolcon test— 123 tests, 0 failures, 0 errors (12 skipped)obstacles.yamlloads, iteration counter advances, converges to an obstacle-avoiding waypointManual — requires Gazebo + a display, not yet run (full steps in
docs/manual-test-plan.md):robot_x:=1.0 robot_y:=1.0) reaches goal — validates the map→odom fixload_obstacles()rejects mismatched-length arrays gracefully