ROS2 porting of the project

[Maxcode19]

Hi, thanks for sharing your code. I am working with ROS2 eloquent and I need a tracker to essentially track pedestrians. Your project looked suitable to my purpose and so I decided to adapt your code to ROS2. At the moment I have succeeded in building the new code but I have bumped into this issue:

• when I run the code, it enters correctly in cloud_cb(), it enters in firstFrame() branch and reaches ec.extract(cluster_indices). At this point it seems the code gets stuck. It looks like it does not exit from the function extract() any more and it does not display any message or error from that moment on. When I try to close the node (Ctrl + C), it does not respond and I have to force the closure of the terminal. The impression is that it works in background but it is not able to finish.

I am wondering if you have experienced something similar during the development of your code. I am pretty sure that the problem is in the function extract() (so in PCL 1.8.0) but I don't know how to deal with it since no error or message is displayed.

Here is the part of code it executes:

   void Tracker::cloud_cb (const sensor_msgs::msg::PointCloud2::SharedPtr input)
   {
   cout<<"IF firstFrame="<<firstFrame<<"\n";   // Debug

   /* If this is the first frame, initialize kalman filters for the clustered objects */
   if (firstFrame)
   {   
    /* Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. 
    Could be made generic by creating a Kalman Filter only when a new object is detected */ 

        float dvx=0.01f;   //1.0
        float dvy=0.01f;   //1.0
        float dx=1.0f;
        float dy=1.0f;
        KF0.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
        KF1.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
        KF2.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
        KF3.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
        KF4.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);
        KF5.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0,   0,dy,0,1,  0,0,dvx,0,  0,0,0,dvy);

        cv::setIdentity(KF0.measurementMatrix);
        cv::setIdentity(KF1.measurementMatrix);
        cv::setIdentity(KF2.measurementMatrix);
        cv::setIdentity(KF3.measurementMatrix);
        cv::setIdentity(KF4.measurementMatrix);
        cv::setIdentity(KF5.measurementMatrix);

        /* Process Noise Covariance Matrix Q */
        // [ Ex 0  0    0 0    0 ]
        // [ 0  Ey 0    0 0    0 ]
        // [ 0  0  Ev_x 0 0    0 ]
        // [ 0  0  0    1 Ev_y 0 ]
        //// [ 0  0  0    0 1    Ew ]
        //// [ 0  0  0    0 0    Eh ]
        float sigmaP=0.01;
        float sigmaQ=0.1;
        setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP));
        setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP));
        setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP));
        setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP));
        setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP));
        setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP));

    /* Meas noise cov matrix R */
        cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ));   //1e-1
        cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
        cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
        cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
        cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
        cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));

    /* Process the point cloud */
    pcl::PointCloud<pcl::PointXYZ>::Ptr not_filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);   // it may have NaN elements
        pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);   // it does not have NaN elements
    pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);

        /* Creating the KdTree from input point cloud*/
    std::cout << "Before definition of tree" << "\n";   // Debug
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    pcl::fromROSMsg (*input, *not_filtered_cloud);

    /////////////////////////////////////////////////////////////////////////////////////////////////////
    /* NaN filter.
    It filters out the NaN elements of 'not_filtered_cloud' */

    std::cout << "Before Nan filter" << "\n";   // Debug

    std::shared_ptr<std::vector<int>> indeces_for_filtering(new std::vector<int>);
    pcl::removeNaNFromPointCloud(*not_filtered_cloud, *input_cloud, *indeces_for_filtering);

    std::cout << "After Nan filter" << "\n";   // Debug

    /////////////////////////////////////////////////////////////////////////////////////////////////////

    std::cout << "Before setInputCloud of tree" << "\n";   // Debug
    tree->setInputCloud (input_cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (0.08); 
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (600);

    ec.setSearchMethod (tree);
    std::cout << "After setSearchMethod of tree" << "\n";   // Debug
    ec.setInputCloud (input_cloud);
    std::cout << "After setInputCloud of input_cloud" << "\n";   // Debug

    /* Extract the clusters out of pc and save indices in cluster_indices.*/
    ec.extract (cluster_indices);

    std::cout << "After extract of ec" << "\n";   // Debug
        ...
        }

The main differences with respect to your code are:

• the presence of a NaN filter since I have experienced the issue #29 (now should be ok). Anyway, the stuck of the code happens also without the filter
• some different data types (adapted to ROS2)
• some additional std::cout() for debug

The terminal shows the following:

user@user-Lenovo-B50-80:~$ ros2 run obj_tracker obj_tracker
IF firstFrame=1
Before definition of tree
Before Nan filter
After Nan filter
Before setInputCloud of tree
After setSearchMethod of tree
After setInputCloud of input_cloud

As you can see, it never exits from extract(). Just for completeness, the PointCloud2 comes from a simulated camera in Gazebo 9, but I hope it does not affect the final result.

Thank you in advance for any feedback you will give me

[praveen-palanisamy]

Hi @Maxcode19 , Nice to hear about your work on porting this ROS node to ROS2. The likely reasons for why the ec.extract(...) call isn't returning causing a non responsive process is either because there are a huge number of points in the point cloud or the EuclideanClusterExtraction is indeed having a hard time extracting clusters in the input point cloud with the given parameters. This can be due to a bug in the PCL lib. Listed below are some things that you can try to avoid this situation:

1. Test with a different input source for the point clouds ( to eliminate issues arising from the input source)
2. Reduce the number of points in the input point cloud and or make sure there are indeed clusters present in the simulated camera you are using as your point cloud source.
3. Tune the cluster tolerance to a suitable value based on your input point cloud data. You can also try adjusting the min and max size values of the clusters so that valid cluster(s) can be found.

   ec.setClusterTolerance (0.08); 
   ec.setMinClusterSize (10);
   ec.setMaxClusterSize (600);

Hope you have the info you need to get past this hurdle. Once you get your ROS2 porting, it will be great if you can send a Pull Request.

[Maxcode19]

Hi @praveen-palanisamy . Thank you very much for your response. You are completely right, the problem is the number of points in input that makes extremely long the computation (the critical point was in file pcl/segmentation/impl/extract_clusters.hpp). For what regards the pull request, I will do it for sure if I succeed in making things work. For now I see in Rviz the cubes but they are not right yet. I think that I have at least 2 issues which depend on my system setup (one is the number of cubes I see, so I will try to tune the parameters you suggested me, the other is the position they are represented on the map so I will modify frame and coordinates). If you just need need your code adapted to ROS 2, I could send a pull request in the next days. In this case, however, I can't guarantee you that everything works correctly (probably with your setup and your adjustments it will). Let me know which one you are interested in. I think I can close this issue. Thank you very much again

[DaniGarciaLopez]

I would also be interested in have it running in ROS2 but I don't see any PR or fork. Did you manage to port it? Thanks!

[aldras]

I have ported this to ROS 2 Foxy Fitzroy, but do note some of the issues that still need to be resolved. Please see https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/issues/52 https://github.com/aldras/multiple_object_tracking_lidar_ros2.git