Merge pull request #17 from dfki-ric/example_case

Example case

Author: haider8645

README.md

@@ -126,16 +126,21 @@ This provides instructions for setting up a test environment using **Gazebo Fort
 ## Prerequisites
 
 ### 1. Install ROS2 Humble
-Ensure you have **ROS2 Humble** installed on your system.
-
-```
-sudo apt install ros-humble-desktop-full
-```
+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. Get the repos 
+### 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
@@ -146,6 +151,8 @@ You can clone the repo `ros2_humble_gazebo_sim` anywhere in your system. Here we
 ```
 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:
@@ -154,10 +161,11 @@ export IGN_GAZEBO_RESOURCE_PATH=/path/to/your_ros2_workspace/ros2_humble_gazebo_
 ```
 
 ### 5. Building the ROS 2 Workspace
-Before launching the simulation, build your ROS 2 workspace with the release flag:
+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
 ```
 
@@ -168,16 +176,107 @@ 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/ugv_nav4d with the location of the ugv_nav4d library. Add this command to your terminal:
+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/ugv_nav4d/build/install/env.sh
+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
 
-ros2 launch ugv_nav4d_ros2 ugv_nav4d_ros2.launch.py
+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

doc/figures/gazebo_teleop.png

@@ -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
+)

doc/figures/mls_visualization_rviz2.png

@@ -0,0 +1,130 @@
+#define BOOST_TEST_MODULE EnvironmentXYZThetaTestModule
+
+#include <fstream>
+#include <cstdlib>
+
+#include "ugv_nav4d/EnvironmentXYZTheta.hpp"
+#include "ugv_nav4d/Mobility.hpp"
+#include <sbpl_spline_primitives/SplinePrimitivesConfig.hpp>
+#include <traversability_generator3d/TraversabilityConfig.hpp>
+
+#include <maps/grid/MLSMap.hpp>
+
+#include <pcl/io/ply_io.h>
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+
+#include <boost/test/included/unit_test.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+
+using namespace ugv_nav4d;
+
+std::string mlsBinFile;
+
+// Define EnvironmentXYZThetaTest fixture for Boost Test
+struct EnvironmentXYZThetaTest {
+    EnvironmentXYZThetaTest();
+    ~EnvironmentXYZThetaTest();
+
+    void loadMlsMap();
+
+    typedef EnvironmentXYZTheta::MLGrid MLSBase;
+
+    EnvironmentXYZTheta* environment;
+    maps::grid::MLSMapSloped mlsMap;
+    Mobility mobility;
+    sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig;
+    traversability_generator3d::TraversabilityConfig traversabilityConfig;
+    bool map_loaded = false;
+};
+
+EnvironmentXYZThetaTest::EnvironmentXYZThetaTest() {
+    std::cout << "Called SetUp()" << std::endl;
+
+    splinePrimitiveConfig.gridSize = 0.3;
+    splinePrimitiveConfig.numAngles = 16;
+    splinePrimitiveConfig.numEndAngles = 8;
+    splinePrimitiveConfig.destinationCircleRadius = 6;
+    splinePrimitiveConfig.cellSkipFactor = 0.1;
+    splinePrimitiveConfig.splineOrder = 4.0;
+
+    mobility.translationSpeed = 0.5;
+    mobility.rotationSpeed = 0.5;
+    mobility.minTurningRadius = 1;
+    mobility.spline_sampling_resolution = 0.05;
+    mobility.remove_goal_offset = true;
+    mobility.multiplierForward = 1;
+    mobility.multiplierBackward = 3;
+    mobility.multiplierPointTurn = 3;
+    mobility.multiplierLateral = 4;
+    mobility.multiplierForwardTurn = 2;
+    mobility.multiplierBackwardTurn = 4;
+    mobility.multiplierLateralCurve = 4;
+    mobility.searchRadius = 1.0;
+    mobility.searchProgressSteps = 0.1;
+    mobility.maxMotionCurveLength = 100;
+
+    traversabilityConfig.maxStepHeight = 0.25;
+    traversabilityConfig.maxSlope = 0.45;
+    traversabilityConfig.inclineLimittingMinSlope = 0.2;
+    traversabilityConfig.inclineLimittingLimit = 0.1;
+    traversabilityConfig.costFunctionDist = 0.0;
+    traversabilityConfig.minTraversablePercentage = 0.4;
+    traversabilityConfig.robotHeight = 1.2;
+    traversabilityConfig.robotSizeX = 1.35;
+    traversabilityConfig.robotSizeY = 0.85;
+    traversabilityConfig.distToGround = 0.0;
+    traversabilityConfig.slopeMetricScale = 1.0;
+    traversabilityConfig.slopeMetric = traversability_generator3d::NONE;
+    traversabilityConfig.gridResolution = 0.3;
+    traversabilityConfig.initialPatchVariance = 0.0001;
+    traversabilityConfig.allowForwardDownhill = true;
+    traversabilityConfig.enableInclineLimitting = false;
+
+    loadMlsMap();
+}
+
+EnvironmentXYZThetaTest::~EnvironmentXYZThetaTest(){
+    std::cout << "Called TearDown()" << std::endl;
+    delete environment;  
+}
+
+void EnvironmentXYZThetaTest::loadMlsMap() {
+    std::string filename;
+
+    if (mlsBinFile.empty()) {
+        filename = "cave_circuit_mls.bin";
+    } else {
+        filename = mlsBinFile;
+    }
+
+    std::ifstream file(filename, std::ios::binary);
+    if (!file.is_open()) {
+        map_loaded = false;
+        return;
+    }
+
+    try {
+        boost::archive::binary_iarchive ia(file);
+        ia >> mlsMap;  // Deserialize into mlsMap
+    } catch (const std::exception& e) {
+        map_loaded = false;
+    }
+    map_loaded = true;
+}
+
+// Define the test suite and test cases
+BOOST_FIXTURE_TEST_SUITE(EnvironmentXYZThetaTestSuite, EnvironmentXYZThetaTest)
+
+BOOST_AUTO_TEST_CASE(check_planner_init_failure_wrong_grid_resolutions) {
+    std::shared_ptr<MLSBase> mlsPtr = std::make_shared<MLSBase>(mlsMap);
+    try {
+        environment = new EnvironmentXYZTheta(mlsPtr, traversabilityConfig, splinePrimitiveConfig, mobility);
+    } catch (const std::exception& ex) {
+        std::cout << "Exception: \n" << ex.what() << "\n";
+        BOOST_CHECK_NE(splinePrimitiveConfig.gridSize, traversabilityConfig.gridResolution);
+    }
+}
+
+BOOST_AUTO_TEST_SUITE_END()