Example case

README.md

@@ -118,6 +118,165 @@ At the end you should see the output
 [  PASSED  ] 6 tests.
 ```
 
+---
+# ROS 2 Humble Test Environment with Gazebo Fortress
+
+This provides instructions for setting up a test environment using **Gazebo Fortress** and **ROS 2 Humble**. The setup includes configurations for using the Husky robot and ensures that the necessary resources are in place for smooth operation.
+
+## Prerequisites
+
+### 1. Install ROS2 Humble
+Ensure you have **ROS2 Humble** installed on your system. Follow the official page at [ROS2 Humble Debian Installation](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html)
+
+### 2. Install Gazebo Fortress
+If you need to install **Gazebo Fortress**, follow the instructions provided on the official page at [Gazebo Installation](https://gazebosim.org/docs/latest/ros_installation/).
+
+### 3. Install SLAM
+If you have a SLAM package which provides a pointcloud map on a topic then you can skip this step. If not then you can use [lidarslam_ros2](https://github.com/rsasaki0109/lidarslam_ros2). Please follow the build and install instructions from the original repository. Set the parameter `robot_frame_id: "husky/base_link"` for the `scanmatcher` node in [lidarslam.yaml](https://github.com/rsasaki0109/lidarslam_ros2/blob/a63b8fa2485e05251505b2bb209598285106bff2/lidarslam/param/lidarslam.yaml#L4)
+
+Install libg2o:
+
+```
+sudo apt-get install -y ros-humble-libg2o
+```
+
+### 3. Get ugv_nav4d_ros2 and a test environment for robot husky in gazebo
+
+```
+mkdir -p your_ros2_workspace/src
+cd ~/your_ros2_workspace/src
+git clone https://github.com/dfki-ric/ugv_nav4d_ros2.git
+```
+You can clone the repo `ros2_humble_gazebo_sim` anywhere in your system. Here we clone it in the `your_ros2_workspace` folder.
+```
+cd ~/your_ros2_workspace
+git clone https://github.com/dfki-ric/ros2_humble_gazebo_sim.git
+cd ros2_humble_gazebo_sim
+bash install_dependencies.bash
+```
+### 4. Husky Configuration
+To ensure that Gazebo can find the robot model, you need to export the following environment variable. Replace /path/to/ with the actual **complete** path where you clone the repository `ros2_humble_gazebo_sim`. Add this command to your terminal:
+```
+export IGN_GAZEBO_RESOURCE_PATH=/path/to/your_ros2_workspace/ros2_humble_gazebo_sim/resource:$IGN_GAZEBO_RESOURCE_PATH
+```
+
+### 5. Building the ROS 2 Workspace
+Before launching the simulation, source your env.sh from ugv_nav4d and build your ROS 2 workspace:
+
+```
+cd ~/your_ros2_workspace
+source path/to/ugv_nav4d/build/install/env.sh
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+```
+
+### 6. Start the Test Environment
+Launch the Gazebo simulation by executing the following command in your terminal:
+```
+source ~/your_ros2_workspace/install/setup.bash
+cd ~/your_ros2_workspace/ros2_humble_gazebo_sim/simulation
+ros2 launch start.launch.py
+```
+You can use the `Teleop` plugin of Gazebo for sending velocity commands to the robot. Click on the three dots in top-right corner of Gazebo window and search for `Teleop`. Select the plugin and adjust the values as shown in figure.
+
+![GazeboTeleop](doc/figures/gazebo_teleop.png)
+
+Alternative to the `Teleop` plugin, you can use a joystick for moving the robot. For this, set the argument `use_joystick:=True`. Adjust the config files in the folder `/config` of the `ros2_humble_gazebo_sim` package from Step 3. Provide the full paths to the arguments `joy_config_file` and `teleop_twist_config_file` as shown below:
+
+```
+ros2 launch start.launch.py use_joystick:=True joy_config_file:=/your_ros2_workspace/ros2_humble_gazebo_sim/simulation/config/joy_config.yaml teleop_twist_config_file:=/your_ros2_workspace/ros2_humble_gazebo_sim/simulation/config/teleop_twist_config.yaml
+```
+
+Available arguments:
+```
+'robot_name':
+    Options: husky
+    (default: 'husky')
+
+'world_file_name':
+    Options: cave_circuit, urban_circuit_practice_03
+    (default: 'cave_circuit')
+
+'use_joystick':
+    Use a real joystick.
+    (default: 'False')
+
+'joy_config_file':
+    Full path to the joy config
+    (default: 'joy_config_file')
+
+'teleop_twist_config_file':
+    Full path to the teleop twist joy config
+    (default: 'teleop_twist_config_file')
+```
+
+In a new terminal, source your workspace and start SLAM. Remap the node scanmatcher's topic `/input_cloud` to `/husky/scan/points` in the `lidarslam.launch.py`
+
+```
+ros2 launch lidarslam lidarslam.launch.py main_param_dir:=/path/to/your/lidarslam.yaml
+```
+
+
+In a new terminal, source your workspace, ugv_nav4d library, and launch the ugv_nav4d_ros2. Replace the /path/to/your/ugv_nav4d with the location of the ugv_nav4d library. Add this command to your terminal:
+
+```
+source ~/your_ros2_workspace/install/setup.bash
+source /path/to/your/ugv_nav4d/build/install/env.sh
+
+ros2 launch ugv_nav4d_ros2 ugv_nav4d.launch.py pointcloud_topic:=/map goal_topic:=/goal_pose
+```
+
+### 7. Plan
+
+In a new terminal, start Rviz2.
+```
+cd ~/your_ros2_workspace
+source ~/your_ros2_workspace/install/setup.bash
+source /path/to/your/ugv_nav4d/build/install/env.sh
+rviz2 -d src/ugv_nav4d_ros2/config/ugv_nav4d.rviz 
+```
+
+After you start to move the robot, the planner will show the following status:
+
+```
+[ugv_nav4d_ros2]: Planner state: Got Map
+[ugv_nav4d_ros2]: Initial patch added.
+[ugv_nav4d_ros2]: Planner state: Ready
+```
+
+Visualize the MLS in Rviz2 using 
+
+```
+ros2 service call /ugv_nav4d_ros2/map_publish std_srvs/srv/Trigger
+```
+![MLSVizRviz2](doc/figures/mls_visualization_rviz2.png)
+
+The gaps in the MLS map are due to the gaps in the scanned points. Move the robot around in the environment. After some time, you will see the MLS start to fill out the gaps.
+
+![MLSVizRviz2](doc/figures/mls_visualization_rviz2_2.png)
+
+Set a goal using the `2D Goal Pose` option in Rviz2 or by publishing to the topic `/ugv_nav4d_ros2/goal_pose`.
+
+![GoalPose2D](doc/figures/set_goal_pose_rviz2.png)
+
+```
+ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 4.0, y: 4.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}"
+```
+
+#### cave_circuit
+
+![MLSVizRviz2](doc/figures/mls_visualization_rviz2_3.png)
+
+If planning is successful you should see the following status in the terminal:
+
+```
+[ugv_nav4d_ros2]: FOUND_SOLUTION
+```
+
+#### urban_circuit_practice_03
+You could repeat the same steps and in Step 6 set `world_file_name:=urban_circuit_practice_03`.
+
+![MLSVizRviz2](doc/figures/mls_visualization_rviz2_4.png)
+
 ---
 ## Implementation Details
 ### Planning

src/EnvironmentXYZTheta.cpp

@@ -140,7 +140,7 @@ EnvironmentXYZTheta::ThetaNode* EnvironmentXYZTheta::createNewStateFromPose(cons
     {
         if(!travGen.expandNode(travNode))
         {
-            cout << "createNewStateFromPose: Error: " << name << " Pose " << pos.transpose() << " is not traversable" << endl;
+            LOG_INFO_S << "createNewStateFromPose: Error: " << name << " Pose " << pos.transpose() << " is not traversable";
             return nullptr;
         }
         travNode->setNotExpanded();
@@ -282,8 +282,13 @@ void EnvironmentXYZTheta::setGoal(const Eigen::Vector3d& goalPos, double theta)
         throw ObstacleCheckFailed("goal position is invalid");
     }
 
-    precomputeCost();
-    LOG_INFO_S << "Heuristic computed";
+    try {
+        precomputeCost();
+        LOG_INFO_S << "Heuristic computed";
+    }
+    catch(const std::runtime_error& ex){
+        throw ex;
+    }
     //draw greedy path
 #ifdef ENABLE_DEBUG_DRAWINGS
     V3DD::COMPLEX_DRAWING([&]()
@@ -1197,7 +1202,10 @@ void EnvironmentXYZTheta::precomputeCost()
 
     Dijkstra::computeCost(startXYZNode->getUserData().travNode, costToStart, travConf);
     Dijkstra::computeCost(goalXYZNode->getUserData().travNode, costToEnd, travConf);
-    assert(costToStart.size() == costToEnd.size());
+
+    if (costToStart.size() != costToEnd.size()){
+        throw std::runtime_error("costToStart.size() is not equal to costToEnd.size()");
+    }
 
     //FIXME this should be a config value?!
     const double maxDist = 99999999; //big enough to never occur in reality. Small enough to not cause overflows when used by accident.

src/gui/PlannerGui.cpp

@@ -298,28 +298,28 @@ void PlannerGui::setupPlanner(int argc, char** argv)
          res = atof(argv[2]);
 
     splineConfig.gridSize = res;
-    splineConfig.numAngles = 42;
-    splineConfig.numEndAngles = 21;
-    splineConfig.destinationCircleRadius = 10;
-    splineConfig.cellSkipFactor = 3.0;
-    splineConfig.generatePointTurnMotions = false;
-    splineConfig.generateLateralMotions = false;
+    splineConfig.numAngles = 16;
+    splineConfig.numEndAngles = 8;
+    splineConfig.destinationCircleRadius = 6;
+    splineConfig.cellSkipFactor = 0.1;
+    splineConfig.generatePointTurnMotions = true;
+    splineConfig.generateLateralMotions = true;
     splineConfig.generateBackwardMotions = true;
     splineConfig.generateForwardMotions = true;
     splineConfig.splineOrder = 4;
 
     mobilityConfig.translationSpeed = 0.5;
     mobilityConfig.rotationSpeed = 0.5;
     mobilityConfig.minTurningRadius = 1; // increase this to reduce the number of available motion primitives
-    mobilityConfig.searchRadius = 0.0;
+    mobilityConfig.searchRadius = 1.0;
+    mobilityConfig.searchProgressSteps = 0.1;
     mobilityConfig.multiplierForward = 1;
-    mobilityConfig.multiplierBackward = 3;
+    mobilityConfig.multiplierForwardTurn = 2;
+    mobilityConfig.multiplierBackward = 2;
+    mobilityConfig.multiplierBackwardTurn = 3;
     mobilityConfig.multiplierLateral = 4;
-    mobilityConfig.multiplierBackwardTurn = 4;
     mobilityConfig.multiplierLateralCurve = 4;
-    mobilityConfig.multiplierForwardTurn = 2;
     mobilityConfig.multiplierPointTurn = 3;
-    mobilityConfig.searchProgressSteps = 0.1;
     mobilityConfig.maxMotionCurveLength = 100;
     mobilityConfig.spline_sampling_resolution = 0.05;
     mobilityConfig.remove_goal_offset = false;

src/test/CMakeLists.txt

@@ -1,12 +1,22 @@
-find_package(GTest REQUIRED)
 add_executable(test_ugv_nav4d test_ugv_nav4d.cpp)
+add_executable(test_EnvironmentXYZTheta test_EnvironmentXYZTheta.cpp)
+
+target_include_directories(test_ugv_nav4d PUBLIC ${Boost_INCLUDE_DIR})
+target_include_directories(test_EnvironmentXYZTheta PUBLIC ${Boost_INCLUDE_DIR})
+
 
 if (ROCK_QT_VERSION_4)
-	target_link_libraries(test_ugv_nav4d GTest::GTest ugv_nav4d)
+	target_link_libraries(test_ugv_nav4d PRIVATE
+							ugv_nav4d)
+	target_link_libraries(test_EnvironmentXYZTheta PRIVATE
+							ugv_nav4d)							
 endif()
 
 if (ROCK_QT_VERSION_5)
-	target_link_libraries(test_ugv_nav4d GTest::GTest ugv_nav4d-qt5)
+	target_link_libraries(test_ugv_nav4d PRIVATE
+						  ugv_nav4d-qt5)
+	target_link_libraries(test_EnvironmentXYZTheta PRIVATE
+						  ugv_nav4d-qt5)		
 endif()
 
 # Install the binaries
@@ -15,3 +25,9 @@ install(TARGETS test_ugv_nav4d EXPORT test_ugv_nav4d-targets
 	LIBRARY DESTINATION lib
 	RUNTIME DESTINATION bin
 )
+
+install(TARGETS test_EnvironmentXYZTheta EXPORT test_EnvironmentXYZTheta-targets
+	ARCHIVE DESTINATION lib
+	LIBRARY DESTINATION lib
+	RUNTIME DESTINATION bin
+)