Feature/multicast sdk

[ShepelIlya]

Added multicast data receiver. To use this you need to set udp_dest on ouster configuration page to something like "239.xxx.xxx.xxx" and also set parameter mtp_dest in launch file to IP address of host interface where you want to receive multicast data (for adding this interface to multicast group).

Tested on three Jetson Xavier and two Ouster OS-1 Rev D with FW 2.4

Link to corresponding PR in ros driver

[ShepelIlya]

I understood the logic with the sensor_config structure and moved the mtp_dest parameter to the OusterSensor class.

[MattC11]

I've attached a file I used in examples to test this:

#include <iostream>

#include "ouster/client.h"

using namespace ouster;

const size_t UDP_BUF_SIZE = 65536;

int main(int argc, char* argv[]) {
    if (argc != 3) {
        std::cerr << "\n\nUsage: <sensor_hostname> <main secondary>"
                  << std::endl;

        return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE;
    }

    const std::string sensor_hostname = argv[1];
    // sensor::sensor_config config;

    bool main = false;

    if (std::string(argv[2]) == "main") {
        main = true;
    } else if (std::string(argv[2]) == "secondary") {
        main = false;
    } else {
        std::cerr << "Invalid second argument: " << argv[2]
                  << " only values of main or secondary are valid" << std::endl;
        return EXIT_FAILURE;
    }

    sensor::sensor_config config;
    if (!sensor::get_config(sensor_hostname, config)) {
        std::cerr << "Failed to get sensor config" << std::endl;
        return EXIT_FAILURE;
    }
    config.udp_dest = "239.201.201.201";
    if (sensor::in_multicast(config.udp_dest.value().c_str())) {
        std::cerr << "In multicast" << std::endl;
    } else {
        std::cerr << "Not a multicast address" << std::endl;
        return -1;
    }
    std::shared_ptr<ouster::sensor::client> cli;
    if (main) {
        // This assumes a change to subscribe to IPADD_ANY if not specified.
        // Othewise you need to specify your own IP address such as
        // cli = sensor::mtp_init_client_main(sensor_hostname, config,
        // "192.168.1.11");
        cli = sensor::mtp_init_client_main(sensor_hostname, config);
    } else {
        cli = sensor::mtp_init_client_secondary(sensor_hostname, config);
    }
    if (!cli) {
        std::cerr << "Failed to initialize sensor" << std::endl;
        return EXIT_FAILURE;
    }

    auto metadata = sensor::get_metadata(*cli);
    sensor::sensor_info info = sensor::parse_metadata(metadata);
    sensor::packet_format pf = sensor::get_format(info);
    auto packet_buf = std::make_unique<uint8_t[]>(UDP_BUF_SIZE);

    while (true) {
        sensor::client_state st = sensor::poll_client(*cli);

        if (st == sensor::TIMEOUT) {
            std::cerr << "Timed out" << std::endl;
            continue;
        }
        if (st & sensor::LIDAR_DATA) {
            if (sensor::read_lidar_packet(*cli, packet_buf.get(), pf)) {
                std::cerr << "Read Lidar Packet" << std::endl;
            }
        }

        if (st & sensor::IMU_DATA) {
            if (sensor::read_imu_packet(*cli, packet_buf.get(), pf)) {
                std::cerr << "Read IMU packet" << std::endl;
            }
        }
    }
}

[Samahu]

@ShepelIlya Sorry I got busy with many other stuff :tired_face: I'll try to catch on this PR tomorrow.

Here are few things that got my eyes on:

1. I think we need to hide the mtp_init_client method, as I perceived the method is provided solely so it would be invoked by mtp_init_client_main and mtp_init_client_secondary. We don't want to confuse users, there are two versions of mtp_ that the client provides to users (_main and _secondary), that's what the API provides.
2. In response to the thread I will test this again tomorrow.
3. In response to the thread with regard to the new CONFIG_GET_ACTIVE flag, I am not 100% sure, but I still think it is not required and you can still achieve the same results for secondary mtp inits without having the flag added. Again, I won't be sure until I have a closer look and maybe prototype it to see. Since I may need to prototype it anyway till I find out, if I was successful at this without making the code more complicated I will probably push the code to a branch and open a merge request against this branch and let you review it (just to speed things up).

Finally, I will double check that the code cross-compiles across all platforms that we support.

I plan to have this PR merged by the end of this week. Thanks for your help.

[ShepelIlya]

@ShepelIlya Sorry I got busy with many other stuff tired_face I'll try to catch on this PR tomorrow.

Here are few things that got my eyes on:

1. I think we need to hide the `mtp_init_client` method, as I perceived the method is provided solely so it would be invoked by `mtp_init_client_main` and `mtp_init_client_secondary`. We don't want to confuse users, there are two versions of mtp_ that the client provides to users (_main and _secondary), that's what the API provides.

2. In response to [the thread](https://github.com/ouster-lidar/ouster_example/pull/480#discussion_r1105798908) I will test this again tomorrow.

3. In response to [the thread](https://github.com/ouster-lidar/ouster_example/pull/480#discussion_r1106318115) with regard to the new `CONFIG_GET_ACTIVE` flag, I am not 100% sure, but I still think it is not required and you can still achieve the same results for secondary mtp inits without having the flag added. Again, I won't be sure until I have a closer look and maybe prototype it to see. Since I may need to prototype it anyway till I find out, if I was successful at this without making the code more complicated I will probably push the code to a branch and open a merge request against this branch and let you review it (just to speed things up).

Finally, I will double check that the code cross-compiles across all platforms that we support.

I plan to have this PR merged by the end of this week. Thanks for your help.

How do you want to hide mtp_init_client?? - I can do it tomorrow.

[Samahu]

@ShepelIlya Sorry I got busy with many other stuff tired_face I'll try to catch on this PR tomorrow. Here are few things that got my eyes on:

1. I think we need to hide the `mtp_init_client` method, as I perceived the method is provided solely so it would be invoked by `mtp_init_client_main` and `mtp_init_client_secondary`. We don't want to confuse users, there are two versions of mtp_ that the client provides to users (_main and _secondary), that's what the API provides.

2. In response to [the thread](https://github.com/ouster-lidar/ouster_example/pull/480#discussion_r1105798908) I will test this again tomorrow.

3. In response to [the thread](https://github.com/ouster-lidar/ouster_example/pull/480#discussion_r1106318115) with regard to the new `CONFIG_GET_ACTIVE` flag, I am not 100% sure, but I still think it is not required and you can still achieve the same results for secondary mtp inits without having the flag added. Again, I won't be sure until I have a closer look and maybe prototype it to see. Since I may need to prototype it anyway till I find out, if I was successful at this without making the code more complicated I will probably push the code to a branch and open a merge request against this branch and let you review it (just to speed things up).

Finally, I will double check that the code cross-compiles across all platforms that we support. I plan to have this PR merged by the end of this week. Thanks for your help.

How do you want to hide mtp_init_client?? - I can do it tomorrow.

Remove it from the header and keep the definition in the cpp file.

[Samahu]

@ShepelIlya I discussed this with @MattC11 who is a member of our team, he suggested that it would be simpler to have one mtp_init_client method with a boolean flag that indicates whether this is a main or a secondary rather than providing two separate APIs, after reviewing the ouster-ros code and the changes you had to make I think I lean towards his opinion.

So the current suggestion is to maintain mtp_init_client method you had defined in client.h and add a new parameter main to it as follows:

/**
 * Connect to sensor and start listening for data via multicast.
 *
 * @param[in] hostname hostname or ip of the sensor. 
 * @param[in] config sensor config.
 * @param[in] mtp_dest_host multicast ip address where the sensor should send data.
 * @param[in] main a flag that indicates this is the main connection to the sensor in an mtp setup
 *  when this flag is set the config object will be used to configure the sensor, otherwise only the
 *  port values within the config object will be used and the rest will be ignored.
 *
 * @return pointer owning the resources associated with the connection.
 */
std::shared_ptr<client> mtp_init_client(const std::string& hostname,
                                        const sensor_config& config,
                                        const std::string& mtp_dest_host
                                        bool main,
                                        int timeout_sec = 60);

This method would accomplish what the two method mtp_init_client_main and mtp_init_client_secondary are doing so we can remove them.

What we can do is pass this flag directly from the ouster-ros launch files, so we would have something like:

roslaunch ouster_ros sensor_mtp.launch \
   sensor_hostname:=$SENSOR_IP \ # always required
   udp_dest=$MULTICAST_IP \           # required arg for sensor_mtp.launch
   mtp_dest:=$CLIENT_IP \             # required arg for sensor_mtp.launch
   mtp_main:=true ..                  # we can set mtp_main default value in the launch file to false or leave it as a required argument

I think with that you don't need the CONFIG_GET_ACTIVE which based on my review it is only needed within the ouster-ros.

Sorry for the back and forth but it is a design issue, the whole idea is to avoid sensor init/reinit on every new secondary connection to the sensor.

Sorry for the back and forth but this is a design issue and the problem only surfaced to me after you added the CONFIG_GET_ACTIVE, I think if the launch command tells you readily that we are requesting a main vs secondary connection then you wouldn't need to do a lot work trying to guess what mode the user wants to run this session with.

Let me know what do you think?

[Samahu]

Within ouster-ros/os_sensor_nodelet if mtp_main flag was set to false then we can bypass the configure_sensor method and proceed directly with create_sensor_client() { auto cli = sensor::mtp_init_client(host, config, mtp_dest_host=mtp_host, mtp_main=false); ... return cli; }.

[Samahu]

I did run your code by our cross-platform compilation checks and it didn't report any problems.

[ShepelIlya]

@ShepelIlya I discussed this with @MattC11 who is a member of our team, he suggested that it would be simpler to have one mtp_init_client method with a boolean flag that indicates whether this is a main or a secondary rather than providing two separate APIs, after reviewing the ouster-ros code and the changes you had to make I think I lean towards his opinion.

So the current suggestion is to maintain mtp_init_client method you had defined in client.h and add a new parameter main to it as follows:

/**
 * Connect to sensor and start listening for data via multicast.
 *
 * @param[in] hostname hostname or ip of the sensor. 
 * @param[in] config sensor config.
 * @param[in] mtp_dest_host multicast ip address where the sensor should send data.
 * @param[in] main a flag that indicates this is the main connection to the sensor in an mtp setup
 *  when this flag is set the config object will be used to configure the sensor, otherwise only the
 *  port values within the config object will be used and the rest will be ignored.
 *
 * @return pointer owning the resources associated with the connection.
 */
std::shared_ptr<client> mtp_init_client(const std::string& hostname,
                                        const sensor_config& config,
                                        const std::string& mtp_dest_host
                                        bool main,
                                        int timeout_sec = 60);

This method would accomplish what the two method mtp_init_client_main and mtp_init_client_secondary are doing so we can remove them.

What we can do is pass this flag directly from the ouster-ros launch files, so we would have something like:

roslaunch ouster_ros sensor_mtp.launch \
   sensor_hostname:=$SENSOR_IP \ # always required
   udp_dest=$MULTICAST_IP \           # required arg for sensor_mtp.launch
   mtp_dest:=$CLIENT_IP \             # required arg for sensor_mtp.launch
   mtp_main:=true ..                  # we can set mtp_main default value in the launch file to false or leave it as a required argument

I think with that you don't need the CONFIG_GET_ACTIVE which based on my review it is only needed within the ouster-ros.

Sorry for the back and forth but it is a design issue, the whole idea is to avoid sensor init/reinit on every new secondary connection to the sensor.

Sorry for the back and forth but this is a design issue and the problem only surfaced to me after you added the CONFIG_GET_ACTIVE, I think if the launch command tells you readily that we are requesting a main vs secondary connection then you wouldn't need to do a lot work trying to guess what mode the user wants to run this session with.

Let me know what do you think?

Then I can add mtp_main flag directly - its ok for me, my biggest motivation for CONFIG_GET ACTIVE was to keep user from configuring based on mtp settings. If its ok - to give him choice, than adding flag to launch is simpler.

I'll do it later today than

[ShepelIlya]

@Samahu i think i did it right now:

sdk:

1. CONFIG_GET_ACTIVE deleted
2. mtp_init_client_main and mtp_init_client_secondary merged to mtp_initi_client with flag main

driver:

1. corresponding mtp_init_client update
2. added bool mtp_main flag
3. deleted sensor_mtp_slave.launch
4. flag mtp_main added to sensor_mtp.launch

Check it please.

[Samahu]

taking a look right now

[MattC11]

@ShepelIlya Just to say thanks for all your work on this. We realize there have been a lot of requests for changes.

[ShepelIlya]

Sorry for formatting and name mismatch in last commit, i was in a rush ;(
I added all your suggestions for now. After merge of this PR it will be much easier for me to keep our fork (with our custom features) actual. Looking forward to discussing them (may be i'll create some issues, just to know if you are intrested in)

[Samahu]

Sorry for formatting and name mismatch in last commit, i was in a rush ;( I added all your suggestions for now. After merge of this PR it will be much easier for me to keep our fork (with our custom features) actual. Looking forward to discussing them (may be i'll create some issues, just to know if you are intrested in)

No worries, I am doing final tests, we can discuss your suggestions later on.

[Samahu]

I would rather try to test it, unfortunately I don’t have a windows machine ready but I can try to setup one later today..

[kairenw]

:tada: :tada: :tada: :tada: :tada: :exclamation: