Autonomous_Navigation_System

→ 📥 Install dependencies

Before installing the necessary dependencies, remember to source the appropriate ROS2 environment for your ROS2 version. This ensures the correct packages are installed for your distribution.

#fundamental libraries
sudo apt update
sudo apt-get install libeigen3-dev
sudo apt install libpcl-dev
sudo apt-get install libpcap-dev
sudo apt install can-utils
sudo apt-get install libqt5serialport5-dev
sudo apt-get install libpugixml-dev
sudo apt-get install libgeographic-dev geographiclib-tools


#ros2 packages
sudo apt install ros-$ROS_DISTRO-joint-state-publisher-gui
sudo apt install ros-$ROS_DISTRO-xacro
sudo apt-get install ros-$ROS_DISTRO-pcl-ros
sudo apt install ros-$ROS_DISTRO-vision-msgs
sudo apt install ros-$ROS_DISTRO-perception-pcl
sudo apt install ros-$ROS_DISTRO-pcl-msgs
sudo apt install ros-$ROS_DISTRO-vision-opencv
sudo apt install ros-$ROS_DISTRO-xacro
sudo apt install ros-$ROS_DISTRO-velodyne-msgs
sudo apt install ros-$ROS_DISTRO-diagnostic-updater
#sudo apt install ros-$ROS_DISTRO-lanelet2 (no longer required because in the repo is a custom repo)
sudo apt install ros-$ROS_DISTRO-color-util

#GTSAM librari for LIO-SAM
sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt update
sudo apt install libgtsam-dev libgtsam-unstable-dev

→ 📦 Install github repository

Clone the GitHub repository into the ros2_ws/src/Autonomous_Navigation_System/ folder.

# LIO SAM
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd LIO-SAM/
git checkout ros2
cd ..

# ndt opm ros2
git clone https://github.com/rsasaki0109/ndt_omp_ros2

# for LIDAR localition
git clone https://github.com/rsasaki0109/lidar_localization_ros2.git

# for velodyne drivers
git clone https://github.com/ros-drivers/velodyne.git
cd velodyne
git checkout humble-devel
cd ..

# for the vectornav package
git clone https://github.com/dawonn/vectornav.git -b ros2

# for the polygon represetation in rviz2 (NO LONGER REQUIERD)
# git clone https://github.com/MetroRobots/polygon_ros.git


git clone https://github.com/KIT-MRT/mrt_cmake_modules.git

→ 📢 Code Modifications Before colcon build

Before building the package, make the following changes to the file lidar_localization_component.cpp located in the src directory of lidar_localization_ros2. These adjustments will modify the default subscriber topics to match the correct topics of the car. Navigate to lidar_localization_ros2/src/lidar_localization_component.cpp and change the lines 234 and 238 for this ones:

  cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
      "points_rotated", rclcpp::SensorDataQoS(),
      std::bind(&PCLLocalization::cloudReceived, this, std::placeholders::_1));

  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
      "vectornav/imu", rclcpp::SensorDataQoS(),
      std::bind(&PCLLocalization::imuReceived, this, std::placeholders::_1));

→ 🔄 Adjusting LiDAR Settings for Different Models

If you’re using a different LiDAR model, update the topic configuration in the pointcloud_rotation package to ensure correct data processing. The package expects a specific topic, /velodyne_points, to receive point cloud data.

1. Update the Point Cloud Topic:

• Navigate to pointcloud_rotation/src/pointcloud_rotation_node.cpp.
• On line 8, change the topic in the subscription line to match your LiDAR’s point cloud topic:

sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>("/your_lidar_topic", 10, std::bind(&pointcloud_rotation_node::pointCloudCallback, this, std::placeholders::_1));

Note: The points_rotated topic is then published by this node and serves as an input for the Mapping and Localization sections.

2. Disable Rotation for Non-Rotating LiDARs:

• If your LiDAR does not require any rotation adjustments, modify the rotation parameters in global_navigation_launch/config/rotation_params.yaml.
• Update the following settings:

pointcloud_rotation_node:
  ros__parameters:
    sensor_rotation_y_: 0.20944   # Set to 0.174533 for slight rotation or 0 for no rotation

Setting sensor_rotation_y_ to 0 will effectively disable the rotation correction, which is useful if your LiDAR is already aligned with the coordinate frame.

→ 🛣️ Considerations for Creating HD Maps with Vector Map Builder

When creating a Lanelet2Map in the Vector Map Builder, follow these steps to configure the map projection:

1. Click on Change Map Project Info.
2. Select Set MGRS from Lat/Lon and input the following coordinates:
   • Latitude: 49
   • Longitude: 8.4
3. Click Convert to apply these settings.

Note: When exporting the map, you may encounter an error indicating that the component x or y is negative. This error can be safely ignored, as it does not impact the map creation process. Proceed with creating the map even if these errors appear.

when finding black spaces in the rout is beacuse the lack of points you can add points with insert point with linestring

→ 📥 Building

If it is the fist time you build the workspace follow the next commands to do not crash your computer.

colcon build --packages-select lio_sam
colcon build --packages-select ndt_omp_ros2
colcon build --packages-select lidar_localization_ros2
colcon build --packages-select vectornav_msgs
colcon build --packages-select vectornav
colcon build --packages-select velodyne_msgs
colcon build --packages-select velodyne_driver
colcon build --packages-select velodyne_laserscan
colcon build --packages-select velodyne_pointcloud
colcon build --packages-select velodyne
colcon build --packages-select lidar_imu_sync
colcon build --packages-select mapping_localization_launch
colcon build --packages-select polygon_msgs
colcon build --packages-select sdv_msgs
source install/setup.bash
colcon build --packages-select sdv_can
colcon build --packages-select sdv_control
colcon build --packages-select mrt_cmake_modules
source install/setup.bash
colcon build --packages-select lanelet2_core
colcon build --packages-select lanelet2_maps
colcon build --packages-select lanelet2_io
colcon build --packages-select lanelet2_projection
colcon build --packages-select lanelet2_traffic_rules
colcon build --packages-select lanelet2_routing
colcon build --packages-select traffic_information_msgs
colcon build --packages-select lanelet2_validation
colcon build --packages-select target_waypoint_index
source install/setup.bash
colcon build

→ 🏅 Lidar config

sudo ifconfig enp2s0 10.66.171.101

→ 🏅 Can start

Inside the ros2_ws/src/Autonomous_Navigation_System/ folder:

chmod +x start.sh
sudo ./start.sh

→ 🏅 IMU Permistion

sudo chmod 666 /dev/ttyUSB0

→ 💡 Sensor Launchers

Launch individual or combined sensor configurations as needed:

• For LiDAR only:

ros2 launch sensors_launch velodyne-VLP32C-launch.py

• For IMU only:

ros2 launch sensors_launch vectornav.launch.py 

• For LiDAR and IMU combined:

ros2 launch sensors_launch lidar_imu.launch.py 

→ 🌏 Launchers for mapping

Run the following commands to initialize mapping processes:

ros2 launch global_navigation_launch lidar_subprocessing.launch.py
ros2 launch robot_description display.launch.py
ros2 launch global_navigation_launch lio_sam.launch.py

• lidar_subprocessing.launch.py: This launch file adjusts the LiDAR rotation by 0.2 radians on the Y-axis, compensating for its setup angle in the car 🚗. It also creates a buffer to synchronize LiDAR and IMU messages, ensuring the sensor data is aligned for accurate mapping.

• display.launch.py (Robot Description): This launch configures the robot’s tf (transform) setup, where base_link 🌐 acts as the parent frame for all other frames.

• lio_sam.launch.py: This command starts the LIO-SAM (LiDAR-Inertial Odometry and Mapping) process, which integrates LiDAR and IMU data to create a detailed, real-time map 🗺️ of the environment for localization and navigation.

→ 🛰️ Configure Launchers for Localization

1. Open the localization.yaml file located at /global_navigation_launch/config/.
2. Add the path to your cloudGlobal.pcd or cloudSurf.pcd file. Update the map_path parameter as shown below:

map_path: "/home/genis/Downloads/LOAM/cloudSurf.pcd"

After updating the file, launch the system with the following commands:

ros2 launch global_navigation_launch lidar_subprocessing.launch.py
ros2 launch robot_description localization_display.launch.py
ros2 launch global_navigation_launch lidar_localization_ros2.launch.py
ros2 launch global_navigation_launch can_and_control.launch.py

• lidar_subprocessing.launch.py: This launch file compensates for the LiDAR setup on the car 🚗, applying a 0.2-radian rotation adjustment on the Y-axis. Additionally, it creates a buffer to synchronize LiDAR and IMU messages, ensuring precise alignment for accurate localization.

• localization_display.launch.py (Robot Description): This file sets up the transform (tf) hierarchy, where the velodyne frame 🌐 acts as the parent frame. In this setup, velodyne is connected to the map frame, making it the primary reference for localization, with other frames structured accordingly.

• lidar_localization_ros2.launch.py: This command launches the LiDAR-based localization process, integrating data from LiDAR and IMU sensors to create a real-time map 🗺️, enabling precise positioning within the environment.

• can_and_control.launch.py: This command launch the control and the comunication with the can bus 0.

→ 🛣️ Launchers for HD map & waypoints routing

ros2 launch map_visualizer osm_visualizer.launch.py #launch the map, crosswalks and the grid map
ros2 launch waypoints_routing waypoints.launch.py #launch the path
ros2 launch target_waypoint_index target_waypoint.launch.py #launch the waypoint target base on the lookahead max and lookahead min

to launch everthing reladed to the map and waypoints:

ros2 launch global_navigation_launch map_elements.launch.py

→ 🎥 Launchers rviz2

In you workspace path run:

rviz2 -d src/Autonomous_Navigation_System/global_navigation_launch/rviz/localization.rviz

→ 🛑 changes:

- Lanelet changes

In the file called /lanelet2_projection/LocalCartesian.cpp I change the to this when using localcartesian map type in orden to get the ele attribute from the oms correctly and not modify.

BasicPoint3d LocalCartesianProjector::forward(const GPSPoint& gps) const {
  BasicPoint3d local{0., 0., 0.};
  this->localCartesian_.Forward(gps.lat, gps.lon, gps.ele, local[0], local[1], local[2]);
  local[2] = gps.ele;
  return local;
}

also wacht that in the "lanelet2_io/io_handlers/OsmFile.cpp" this is in the code:

  static Nodes readNodes(const pugi::xml_node& osmNode) {
    Nodes nodes;
    for (auto node = osmNode.child(keyword::Node); node;  // NOLINT
         node = node.next_sibling(keyword::Node)) {
      if (isDeleted(node)) {
        continue;
      }
      const auto id = node.attribute(keyword::Id).as_llong(InvalId);
      const auto attributes = tags(node);
      const auto lat = node.attribute(keyword::Lat).as_double(0.);
      const auto lon = node.attribute(keyword::Lon).as_double(0.);
      // const auto ele = node.attribute(keyword::Elevation).as_double(0.);

      const auto ele = node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation)
                           .attribute(keyword::Value)
                           .as_double(0.);

      // std::cout << "ele: " << ele << std::endl;

      nodes[id] = Node{id, attributes, {lat, lon, ele}};
    }
    return nodes;
  }

- Polygon ros changes:

The Polygon2D structure has been modified to include a z offset, allowing for multiple polygons with different z offsets. The following files were updated to incorporate these changes:

• polygon_rviz_plugins/src/polygons_display.cpp
• polygon_rviz_plugins/src/polygon_parts.cpp
• polygon_rviz_plugins/include/polygon_rviz_plugins/polygon_base.hpp

These modifications enable the use of 3D polygons with varied z positions in the ROS environment.

Polygon2D.msg:

# Vertices of a simple polygon. Adjacent points are connected, as are the first and last.
float64 z_offset
polygon_msgs/Point2D[] points

polygon_base.hpp:

  void updateProperties()
  {
    resetOutlines();
    if (mode_property_->shouldDrawOutlines())
    {
      Ogre::ColourValue outline_color = rviz_common::properties::qtToOgre(outline_color_property_->getColor());
      for (unsigned int i = 0; i < saved_outlines_.size(); ++i)
      {
        double z_offset = saved_outlines_[i].z_offset;  // Use the z_offset from each polygon
        outline_objects_[i]->setPolygon(saved_outlines_[i], outline_color, z_offset);
      }
    }

    if (!mode_property_->shouldDrawFiller() || saved_fillers_.empty())
    {
      resetFillers();
    }
    else
    {
      for (unsigned int i = 0; i < saved_fillers_.size(); ++i)
      {
        double z_offset = saved_fillers_[i].outer.z_offset;  // Use the z_offset from each complex polygon
        filler_objects_[i]->setPolygon(saved_fillers_[i], filler_colors_[i % filler_colors_.size()], z_offset);
      }
    }
  }