[WIP] Nav2 3D Map server support

[ShivamPR21]

nav2 map server for 3D #1787

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1787
Primary OS tested on	(Ubuntu)
Robotic platform tested on	N/A

[SteveMacenski]

Before I review this - that pcd file needs to be downloaded on test use, that cannot live in this repo or in the git history, you will need to remove it and rebase. It's 11MB, way too big for git storage.

[SteveMacenski]

You also un-linted a bunch of code, you must revert all of that. 2 spaces, functions start lower case, etc. ament_cpplint and ament_uncrustify will scream bloody murder when CI finishes

[SteveMacenski]

CMake Error at CMakeLists.txt:18 (find_package):
  By not providing "Findpcl_conversions.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "pcl_conversions", but CMake did not find one.

  Could not find a package configuration file provided by "pcl_conversions"
  with any of the following names:

    pcl_conversionsConfig.cmake
    pcl_conversions-config.cmake

Looks like you missed a dependency

[SteveMacenski]

I can't seriously review this until you fix all the linting mistakes introduced. Take a slow, critical look at your work and why you made changes. Run ament_cpplint and ament_uncrustify, its going to scream about hundreds of errors introduced in this PR.

Your builds are failing too. Ping me back when you have builds and tests passing and I can start reviewing at that point.

[ShivamPR21]

@SteveMacenski The error messages due to badly formatted code are now fixed the build is still failing this time even after adding dependencies and I think you should have a look at it and start reviewing it again.

[SteveMacenski]

nav2_map_server: Cannot locate rosdep definition for [pcl_conversions]

You have dependency errors, you need to add this to the underlay workspace repos file.

boost

What's boost for? That's a really extreme dependency to add, typically we try to avoid that at pretty much all costs.

C++14 has some filesystem capabilities, but I'm not sure exactly you need. The yaml parser didn't require it for the 2D maps so I imagine you can make something to work without it here as well. The PCD files are taken care of by PCL so that shouldn't be an issue either.

Lets get this building and passing tests before I dig into it. @AlexeyMerzlyakov can you also review?

Sorry it took me awhile to get to this, I've been slammed. Can you talk through your design process? I'm not seeing much patterns to follow in reviewing. I think understanding your thought process and why you designed it the way you did is important. To be fair, map_server has never been in a really great design state, but looks to make things considerably more complicated

[ShivamPR21]

@SteveMacenski dependency on BOOST is removable and will be done soon. As far as design is concerned the main motive is to augment the existing one for 3D capabilities rather than changing them which could break other usages of this.

For map_server the library now checks while configuring which one should be enabled.
Once any of 2D/3D is enabled the user won't be able to change it to another one, he can still change other parameters related to enabled capabilities.
In short, once you entered a YALM file related to 2D you would be able to reconfigure it with 2D only.(For the other one launch another instance).

For map_saver the library still hosts both services as I don't think on should be launching another instance of map_saver because it has nothing to retain. Therefore a single instance enables both the services {node_name}/save_map and {node_name}/save_map3D.

In the case of map_saver it doesn't need a combined message like PCD2(PointCloud2+origin/orientation) as the origin/orientation info is given while request, it needs a PointCloud2 msg topic name to read the map.

[ShivamPR21]

@SteveMacenski a lot of your points about map_server raised an issue of complexity can you explain what does it mean, is it about adding another library or something else.

[ShivamPR21]

To be fair, map_server has never been in a really great design state, but looks to make things considerably more complicated

What are your expectations with map_server or some other user perspective user, what are the points which define the term "complicated" in this scenario?

[ShivamPR21]

Docker Engine Version: 18.09.6
Kernel Version: Linux 4403d707f13a 4.15.0-1077-aws #81-Ubuntu SMP Wed Jun 24 16:48:15 UTC 2020 x86_64 Linux
Starting container rosplanning/navigation2:master.debug
  image cache not found on this host, downloading rosplanning/navigation2:master.debug

Error response from daemon: manifest for rosplanning/navigation2:master.debug not found``` 
I think a "-" is missing.

[AlexeyMerzlyakov]

Apologies for long time no being. I will also be connected and review the commit shortly, as possible.

[SteveMacenski]

Lets put a pause on this until Alexey takes a look. Theres a bunch of potential issues here and new messages that I don't 100% buy into being necessary.

I'm not sure its worth putting any more time into it @ShivamPR21 until we get a plan. I value your time and interest in getting this done, but this PR's changes is not very well structured and it would be difficult to maintain or ever modify in the future. We should slow down and come up with a design to implement and/or take a step back and use the lessons learned from this PR moving forward. Lets focus not on just capability but also code quality and maintainability.

[AlexeyMerzlyakov]

Here is a big change trying to preserve current Map Server's concept adding there 3D library which seems to be consecutive. I've started a review today and tomorrow will continue it trying to better understand the details and implementation of the intention by adding more comments about the change.

But the main items appear to put an attention I can highlight today:

1. Files/modules structure and naming.
   If you are making a new nav2_map_server_3d namespace for a new library, it is better to separate sources and includes by: general files, like map_server or map_saver; map_io library for 2D case and map_io_3d library for 3D? It will be better for understanding rather than mixing them into one place, or separating only 3D part.

2. Making the united code to be more simple.
   The map_server.cpp/map_saver.cpp code having both 2D/3D working callbacks should be simplified, e.g. as Steve mentioned - by using templates. In current implementation, there are a lot of boilerplate code, came from 2d part almost without changes which will make further code maintenance to be really difficult.

3. Mixing origin and orientation. This is two different concepts: origin has its own position and orientation. So, it is better to use geometry_msgs/Pose or something similar (origin.position and origin.orientation) or 7-items origin vector for it. In my opinion - geometry_msgs/Pose is the best choice.

4. There are a lot of new messages added for 3D part. Do we really need them?

I also think, we need to work out the design first, in order to make it simple as we can in order to not get confused in the details in the future. We need to carefully think over it and about required messages from HLD point of view which is required to complete this item.

[AlexeyMerzlyakov]

Regarding overall design and messages/service, I suppose the following structure as attached below:

This is very similar to what @ShivamPR21 made, however a new Map3D.msg is introduced optionally, as well as number of new services and service messages is being discussed: if we do not need to have similar services as 2D map-server has, we can skip get_map_3d and/or load_map_3d services at all.

[ShivamPR21]

@AlexeyMerzlyakov here is a detailed structure of what is there to support 3D structure including the name of services.

[SteveMacenski]

So looking over that, so the map_server would then have both sets of publishers/service servers? Ideally I'd want to create network interfaces based on the type of inputs (e.g. if the yaml was a PCD or PNG, then only create one set of them). If we could make the IO library APIs similar (don't need to be exact) we might be able to template this - what do you think? For common functions we have just the MapT but we can have specific smaller functions with specific overrides for each type, hopefully only making up a very small amount of the code. If it becomes too much, it would be cleaner to have 2 separate servers overall.

I'm starting to come around to the idea of having seperate 2D and 3D servers unless we can work through those issues. Each would have some base class server with the basic functionality required and then only the 2D vs 3D specifics would be in their respective derived classes.

The idea is to reduce the amount of overhead of adding new features or changes in the future. Templating is always preferred over inheritance, so I'd like to give that a stab first, but would need to be thoughtfully implemented to refactor the code to make that work.

[AlexeyMerzlyakov]

I believe, we can get along with one map_server and one map_saver having a templated code structure with two different implementations of main API methods depending map: 2D or 3D type. Could be made for publishers in map_server as well? I think why not. Regarding MapIO: from first sight, MapIO and MapIO_3D have less common in the code to have templated API. Additionally, MapIO was initially designed to work only per one type of maps. So, I consider to have two separate libraries: MapIO and MapIO_3D and one common map_server/map_saver components. I also agree with @SteveMacenski that templating is preferred over inheritance.

Regarding sets of services: unfortunately yes. In current services structure GetMap and LoadMap service messages have a map (be it 2D or 3D) inside a message. I do not know whether we can a possibility to specify a temaplated type direct in a MSG/SRV-file. So it will be required to have additional 3D version of these services. Only one service: SaveMap could be re-used for 3D, because it has a std::string map reference which could be considered dynamically which kind of a map do we have at the moment of request.
That is why I've marked SaveMap3D.srv to be optional service message in the HLD above.

So, the main question today is - how can we make a templated structure of MapServer/MapSaver API in order to reduce amount of duplicating code and simplify whole structure. @ShivamPR21, how do you think to handle over it?

[SteveMacenski]

I think that's a good analysis. For the services, the base server would need to have both callbacks in the class but we can just create the one that the template uses, we can have an if/else based on std::is_same<T, animal>::value or have 2 specialized function versions for each template type.

Or we modify the services themselves to support both... but that doesn't seem ideal, that would have alot of waste in each message.

[ShivamPR21]

So, we have 2 main requirements,

1. Code should be easy to handle (less amount of duplicated/redundant structure and easy to review)
2. The overall framework should be upgradable (most important I think, in order to reduce the hassle which we are facing right now)

@AlexeyMerzlyakov @SteveMacenski as you have suggested for this I also think the template(metaprogramming) is a better option, actually, there could be a much straightforward option if all custom messages are inherited from a unified class but that's not the case at this moment so we should go with metaprogramming. I have tried it to do this in the past while developing this in the initial phase, yet to retain the current implementations chosen the simple way, but as of now here is the solution based on templates.

The solution to the problem:-

I think here is a simple solution which would make us achieve both the targets mentioned above.

Use of simple class modules:-

1. Reduces meta-parameters required
2. Eases the upgrade to other services and data types

// The base class restricting the structure of module 
// base_class____.hpp
class base_class{
    virtual void configure();
    virtual void clean();
    virtual void activate();
    // ..................... more methods   
};

// Defining module class and overriding methods to be used
// modeule_class____.hpp
class module_class : public base_class {
    
    // service and message list
    // .......................

    void configure() override {
        // do something whatever system needs 
        // this function will be called during configure
    } 

    // ......................... implement more methods
    void clean() override {
        // do something whatever system needs 
        // this function will be called during cleanup
    }
};

Once defined these would be used as template parameters in map_server

template<class T> class map_server{
    // ................. existing structure
    void on_confiure(){   // For example only
        services.configure();
        
    }

    T services;
};

#include "module_class___.hpp"
#include "map_server.hpp"
// usage
int main(){
    map_server<module_class> a; // dummy usage actual may be different
}

For upgrading the existing capabilities one has to only make his own module_class inherited from the base_calss and use it as template parameter input.

@SteveMacenski I think that this structure would lead to better maintainability as the new capabilities won't need to be directly added to the library instead of just create another class and use it.

@AlexeyMerzlyakov what's your say on this??

Note:- The presented structure is only a dummy not in its actual form as it would be in the code.

[ShivamPR21]

@SteveMacenski @AlexeyMerzlyakov in map_saver we have 2 services initialized simultaneously for both image and pcd (with 3D suffix in service name), with the structure proposed above we have the freedom to do templating to map_saver as well but I think the saver doesn't need to host any kind of data as a server do so yes, I think it would be better to make a single server instance for both 2D and 3D ones.

Yet, I'm a bit hopping b/w the two ideas for map_saver as the expansion of map_saver would lead to overburden to the existing library, and contrary to this if we use the template then it would also be modular and overall library will have a lesser amount of code.

What do you suggest on this??

Now with the end of this comment I'm a bit leaned towards the templated version? map_saver, so the map_saver would be designed in the same way as map_server in the previous comment.

[ShivamPR21]

@AlexeyMerzlyakov the method proposed above I think has the capability to release the hold on extension name for files as well as you pointed out

Do we really need it? Cutting-off the input by file extension not always works, e.g. if developer intentionally rename map.pgm -> to say my.map. As I understand, Magick++ has its own module identifying file by its header rather than extension, so I am not sure about this part.

Till now I didn't see anything in the PCL source that restricts the extension of the file even if it is so the method would be able to handle this by decoupling all the io libraries from directly interacting with map_server|map_saver.
@SteveMacenski this would give us/user ability to make his/her own io library and then use it in to create callbacks and other utilities for their specific module_class.

Overall the method would decouple both io and different services form the map_saver|map_server so will reduce the requirement of any other kind of io to be directly integrated into the library instead, one would be able to make his own as per the requirements. From the nav2 side, we will be providing support for common type of io like image|pcd v7/v6

[AlexeyMerzlyakov]

Okay, @ShivamPR21 it looks like we have caught the right direction. MapServer class' main API will be templated but I can not understand, why do we need to have so overloaded structure with base_class and module_class. Could we just template MapServer class directly by <class MapT> where MapT could be either nav_msgs::msg::OccupancyGrid or sensor_msgs::msg::PointCloud2 type? Depending on this type as Steve mentioned by using std::is_same<MapT, double>::value, there could be enabled one or another logic.
Please refer to the following example to get what I mean:

#include <iostream>

template <class MapT>
class MapServer
{
  public:
    MapServer(MapT map) {
      map_ = map; 
    }
    void on_configure() {
      if (std::is_same<MapT, int>::value) {
        std::cout << "publishing int value: " << map_ << "\n";
      } else if (std::is_same<MapT, double>::value) {
        std::cout << "publishing double value: " << map_ << "\n";
      } else {
        std::cout << "Error here\n";
      }
    }
  private:
    MapT map_;
};

int main () {
  MapServer<int> map_i(10);
  MapServer<double> map_d(5.5);
  map_i.on_configure();
  map_d.on_configure();

  return 0;
}

There are many ways how to simple this. An example above - is just an idea how to handle it. In the map_server.cpp code, you can write new methods which will be responsible for publishing or make one if-statement inside existing logic. It is up to you. But the main goal is to make the code to be a) simple and readable b) easy to maintain in the future, e.g. to add here new types of maps.

Regarding MapSaver, I also think it could be templated by MapT type in the same way as MapServer, even if it does not host map data.

[SteveMacenski]

Closing from inactivity

We can readdress this if we can get another PR going with these issues fixed, but closing out inactive PRs for the end of the year to help start 2020 with a cleaner slate.