Request for Guidance on Using easy Navigation for 3D Navigation in Air and Underwater Vehicles

[peakyquest]

How can this package be adapted for use with air and underwater vehicles for 3D navigation and path planning?
Is it possible to develop a custom path planner or control server for it, since the current package is primarily designed for mobile robots?

[fmrico]

Hi @peakyquest

For that use case, you need:

1. Use Octomap or Bonxai (recommended) Maps Manager. Include in your parameter file the Maps Manager and set the parameters to the point cloud file, indicating the package and the file inside the package (remember to install)

maps_manager_node:
  ros__parameters:
    use_sim_time: true
    map_types: [bonxai]
    bonxai:
      freq: 10.0 
      plugin: easynav_bonxai_maps_manager/BonxaiMapsManager
      package: you_test_case_package
      bonxai_path_file: maps/map.pcd

2. Configure sensors, like this:

sensors_node:
  ros__parameters:
    use_sim_time: true
    forget_time: 0.5
    sensors: [laser, camera, imu]
    perception_default_frame: odom
    laser:
      topic: front_laser/points
      type: sensor_msgs/msg/PointCloud2
    camera:
      topic: front_camera/depth/color/points
      type: sensor_msgs/msg/PointCloud2
    imu:
      topic: imu/data
      type: sensor_msgs/msg/Imu

3. Set localization algorithm. You can:
   1. Develop your own localization algorithm. For that, you can start from this, but removing the NavMap parts, which forces it to be on the surface.
   2. Use the easynav_gps_localizer, which now is being reimplemented.
   3. Or you can use the lidarslam_ros2 plugin(maybe recommended at this point for a fast test), which is a wrapper, and it does not really follow the EasyNav design principles. The content in the parameter file can be like this (maybe you need a remap in anywhere):

localizer_node:
  ros__parameters:
    use_sim_time: true
    localizer_types: [lidarslam]
    lidarslam:
      plugin: easynav_lidarslam_localizer/LidarSlamLocalizer
      input_cloud: /front_laser/points
      imu: /imu/data

can_matcher:
  ros__parameters:
    use_sim_time: True
    global_frame_id: "map"
    robot_frame_id: "base_link"
    odom_frame_id: "odom"
    registration_method: "NDT"
    ndt_resolution: 2.0
    ndt_num_threads: 2
    gicp_corr_dist_threshold: 5.0
    trans_for_mapupdate: 1.5
    vg_size_for_input: 0.5
    vg_size_for_map: 0.2
    use_min_max_filter: true
    scan_min_range: 1.0
    scan_max_range: 200.0
    scan_period: 0.2
    map_publish_period: 15.0
    num_targeted_cloud: 20
    set_initial_pose: true
    initial_pose_x: 0.0
    initial_pose_y: 0.0
    initial_pose_z: 0.0
    initial_pose_qx: 0.0
    initial_pose_qy: 0.0
    initial_pose_qz: 0.0
    initial_pose_qw: 1.0
    use_imu: false
    use_odom: false
    debug_flag: false

graph_based_slam:
    ros__parameters:
      use_sim_time: True
      registration_method: "NDT"
      ndt_resolution: 1.0
      ndt_num_threads: 2
      voxel_leaf_size: 0.2
      loop_detection_period: 3000
      threshold_loop_closure_score: 0.7
      distance_loop_closure: 100.0
      range_of_searching_loop_closure: 20.0
      search_submap_num: 2
      num_adjacent_pose_cnstraints: 5
      use_save_map_in_loop: true
      debug_flag: true

3. Develop Planner for Octmap/Bonxai. This is the easier part, as A* is very simple. You can see any A* developed for other representations here and here. As a result, you can still set a nav_msgs/msg/Path in the NavState (the blackboard).
4. Develop a 3D Controller. Develop a Controller which takes a nav_msgs/msg/Path and produce a geomtry_msgs/msg/Twist (or geomtry_msgs/msg/TwistStamped) to follow a Path in 3D.

Let me know if you need more guidance here, but, for example, for the planner the implementation is easy:

void AStar3DPlanner::update(NavState & nav_state)
{
  current_path_.poses.clear();
  if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.bonxai")) {
    return;
  }

  const auto & map = nav_state.get<Bonxai::ProbabilisticMap>("map.bonxai");
  const auto robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
  const auto & goal = goals.goals.front().pose;

  auto poses = a_star_path(map, robot_pose.pose.pose, goal);
  if (!poses.empty()) {
    current_path_.header.stamp = get_node()->now();
    current_path_.header.frame_id = goals.header.frame_id;
    for (const auto & pose : poses) {
      geometry_msgs::msg::PoseStamped pose_stamped;
      pose_stamped.header.frame_id = goals.header.frame_id;
      pose_stamped.header.stamp = current_path_.header.stamp;
      pose_stamped.pose = pose;
      current_path_.poses.push_back(pose_stamped);
    }
    if (path_pub_->get_subscription_count() > 0) {
      path_pub_->publish(current_path_);
    }
  }
  nav_state.set("path", current_path_);
}

[peakyquest]

Thanks for your guidances. I will go through each thing, I will try to setup the easy navigation for quadcopter.

One more thing I need to ask, for quadcopter (outdoor example) do I need a map ? Or should I just create a global costmap and make it the window rolling ?

[fmrico]

Everything depends on your plugins and how you organize the navigation. There are many options: global/robocentric representation, not using a map, a planner that produces a straight line as a path, a controller without a path....

From https://easynavigation.github.io/developer_guide/design.html