0
$\begingroup$

When I execute this node I receive the error mentioned in the title and I had already tried many things but anything worked, what can I do?

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/distances.h>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <stdint.h>
#include <boost/make_shared.hpp>
#include <pcl/search/impl/kdtree.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <vector>
#include <iostream>
#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;
nav_msgs::OccupancyGrid costmap_p;
pcl::PointCloud<pcl::PointXYZ> nubeDePuntos_p;
pcl::PointCloud<pcl::PointXYZ> output_cloud;
// output_cloud.width = 0;  // Inicializa el ancho como 0
// output_cloud.height = 0; // Inicializa la altura como 0
// output_cloud.points.resize(0); // Inicializa el vector de puntos con tamano 0
nav_msgs::OccupancyGrid dev;
costmap_2d::Costmap2D tf_costmap;



// Funcion para convertir un nav_msgs::OccupancyGrid a un costmap_2d::Costmap2D
costmap_2d::Costmap2D convertirACostmap2D( nav_msgs::OccupancyGrid& occupancy_grid, tf::TransformListener& tf) {
    //auto costmap = std::make_shared<costmap_2d::Costmap2DROS>("mi_costmap", tf);
    costmap_2d::Costmap2D costmap_data(occupancy_grid.info.width, occupancy_grid.info.height, 
                                        occupancy_grid.info.resolution, 
                                        occupancy_grid.info.origin.position.x, 
                                        occupancy_grid.info.origin.position.y);

    // Copiar los datos de ocupacion del mensaje OccupancyGrid al Costmap2D
    for(unsigned int x = 0; x < occupancy_grid.info.width; ++x) {
        for(unsigned int y = 0; y < occupancy_grid.info.height; ++y) {
            int8_t occupancy_value = occupancy_grid.data[x + y * occupancy_grid.info.width];
            unsigned char cost = costmap_2d::FREE_SPACE; // Por defecto, se considera celda libre

            if (occupancy_value == 100) {
                cost = costmap_2d::LETHAL_OBSTACLE; // Si es 100, se considera un obstaculo letal
            } else if (occupancy_value > 0) {
                cost = costmap_2d::INSCRIBED_INFLATED_OBSTACLE; // Si es mayor que 0, se considera un obstaculo inscrito
            }

            costmap_data.setCost(x, y, cost);
        }
    }

    //costmap->updateMap();
    return costmap_data;
}



nav_msgs::OccupancyGrid filterSlopesFn (  pcl::PointCloud<pcl::PointXYZ> input_cloud,
                                          nav_msgs::OccupancyGrid costmap_data )
{
    int i = 0;
    //pcl::PointCloud<pcl::PointXYZ> output_cloud;
    if(input_cloud.size()>0){
        using namespace pcl;
        //pcl::removeNaNFromPointCloud (const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, Indices &index)
        // Parametro de umbral de inclinacion (ajusta segun sea necesario)
        double slope_threshold = 0.1;
        output_cloud.clear();
        std::cout<<input_cloud.size()<<std::endl;
        ROS_INFO("publi");
        pub.publish(costmap_data);

        // Construir un kdtree para buscar vecinos
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(input_cloud.makeShared());
        
        costmap_2d::Costmap2DROS* costmap_ros;
        tf2_ros::TransformBroadcaster broadcaster;         
        tf2::Quaternion quaternion_combined;

        // output_cloud.height = input_cloud.height;
        // output_cloud.width = input_cloud.width;
        // output_cloud.resize(100);
        // Iterar a traves de los puntos de la nube
        //#pragma omp parallel for
        for (auto& point : input_cloud.points)
        {
            ros::NodeHandle nh;
            // Convertir pcl::PointCloud a sensor_msgs::PointCloud2

            quaternion_combined.setRPY(M_PI / 2, 0, M_PI / 2); // Roll (X), Pitch (Y), Yaw (Z)

            // Crear un mensaje de transformacion para la nueva transformacion estatica
            geometry_msgs::TransformStamped transform_msg;
            transform_msg.header.stamp = ros::Time::now();
            transform_msg.header.frame_id = "robot_base_link"; // Marco de referencia origen
            transform_msg.child_frame_id = "frame_transformado"; // Nuevo marco de referencia
            transform_msg.transform.translation.x = 0.0; // Ajusta segun sea necesario
            transform_msg.transform.translation.y = 0.0; // Ajusta segun sea necesario
            transform_msg.transform.translation.z = 0.0; // Ajusta segun sea necesario
            transform_msg.transform.rotation.x = quaternion_combined.x(); // Ajusta segun sea necesario
            transform_msg.transform.rotation.y = quaternion_combined.y(); // Ajusta segun sea necesario
            transform_msg.transform.rotation.z = quaternion_combined.z(); // Ajusta segun sea necesario
            transform_msg.transform.rotation.w = quaternion_combined.w(); // Ajusta segun sea necesario

            // Publicar la transformacion estatica
            broadcaster.sendTransform(transform_msg);

            sensor_msgs::PointCloud2 cloud_msg;
            pcl::toROSMsg(output_cloud, cloud_msg);

            // Configurar el marco de referencia
            cloud_msg.header.frame_id = "frame_transformado"; // Reemplaza "base_link" con el marco de referencia adecuado

            // Publicar el mensaje
            ros::Publisher filtered_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/robot/filtered_point_cloud", 1);
            filtered_cloud_pub.publish(cloud_msg);
            // ros::Publisher filtered_cloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/robot/filtered_point_cloud", 1);
            // filtered_cloud_pub.publish(output_cloud);

            i = i+1;
            if(pcl_isfinite(point.x) && pcl_isfinite(point.y) && pcl_isfinite(point.z)){
                //ROS_INFO("hola");
                // Buscar vecinos del punto
                std::vector<int> point_indices;
                std::vector<float> point_distances;
                kdtree.radiusSearch(point, 1.0, point_indices, point_distances);
                //std::cout<< i <<std::endl;
                // Verificar si hay vecinos suficientes para calcular la pendiente
                if (point_indices.size() >= 3 && point_indices[1] < input_cloud.points.size())
                {
                    // Calcular la inclinacion local (pendiente en el eje Z)
                    double slope = (input_cloud.points[point_indices[1]].z - point.z) /
                                sqrt(pow(input_cloud.points[point_indices[1]].x - point.x, 2) +
                                        pow(input_cloud.points[point_indices[1]].y - point.y, 2));

                    // Agregar el punto al nuevo cloud si cumple con los criterios
                    if (fabs(slope) < slope_threshold)
                    {
                        //std::cout << "Type of point: " << typeid(point).name() << std::endl;
                        //std::cout<<point<<std::endl;
                        //ROS_INFO("hola");
                        // try {
                        //     output_cloud.points.push_back(point);
                        // } catch (const std::exception& e) {
                        //     std::cerr << "Exception caught: " << e.what() << std::endl;
                        // }
                        output_cloud.push_back(point);
                        //std::cout<<"Tamano de nube:" << output_cloud.size()<<std::endl;
                    }
                }
                
                if(i == input_cloud.size() - 1){
                    //std::cout<<"Tamano de nube:" << output_cloud.size()<<std::endl;
                    // for (auto& point : output_cloud.points)
                    // {
                    //     std::cout << "X: " << point.x << ", Y: " << point.y << ", Z: " << point.z << std::endl;
                    // }
                }
            }


        }
    }


    // ros::NodeHandle nh;
    // ros::Publisher filtered_cloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/robot/filtered_point_cloud", 1);
    // filtered_cloud_pub.publish(output_cloud);

    for ( auto& point : output_cloud.points) {
        //ROS_INFO("si");
        double costmap_x = point.x;
        double costmap_y = point.y;
        // ROS_INFO("x: %lf, y: %lf, z: %lf ", costmap_x, costmap_y);
        
        // *Modificar costos en el costmap local*
        // Supongamos que costmap es un objeto costmap_2d::Costmap2DROS ya inicializado
        //for ( auto& point : output_cloud.points) {
            
            //costmap_data.data[point.y * costmap_data.info.width + point.x] = costmap_2d::FREE_SPACE;

        int width = costmap_data.info.width;
        int height = costmap_data.info.height;

        // Calcula el indice correspondiente a la posicion (costmap_x, costmap_y)
        int index = costmap_x + costmap_y * width;

        // Asegurate de que el indice este dentro de los limites del array de datos
        tf::TransformListener tf_listener;
        //tf::TransformListener& tf_listener;
        //costmap_2d::Costmap2D tf_costmap;
        if(pcl_isfinite(point.x) && pcl_isfinite(point.y) && pcl_isfinite(point.z)){
            tf_costmap = convertirACostmap2D(costmap_data, tf_listener);
            // Accede y modifica el valor del mapa en la posicion (costmap_x, costmap_y)
            tf_costmap.setCost(costmap_x, costmap_y, 0);
            //std::cout<<tf_costmap<<std::endl;
            //costmap_2d::Costmap2DROS costmap_final("costmapfinal", tf_listener);
        }

    }
        // Copiar los datos del costmap a los datos de la grid de ocupacion
        costmap_data.data.resize(tf_costmap.getSizeInCellsX() * tf_costmap.getSizeInCellsY());
        for (unsigned int y = 0; y < tf_costmap.getSizeInCellsY(); ++y) {
            for (unsigned int x = 0; x < tf_costmap.getSizeInCellsX(); ++x) {
                ROS_INFO("paso");
                unsigned int costmap_index = tf_costmap.getIndex(x, y);
                unsigned char cost = tf_costmap.getCost(x, y);
                if (cost == costmap_2d::LETHAL_OBSTACLE) {
                    costmap_data.data[y * tf_costmap.getSizeInCellsX() + x] = 100; // Celda ocupada
                } else if (cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
                        cost == costmap_2d::NO_INFORMATION) {
                    costmap_data.data[y * tf_costmap.getSizeInCellsX() + x] = 100; // Celda desconocida
                } else {
                    costmap_data.data[y * tf_costmap.getSizeInCellsX() + x] = 100; // Celda libre
                }
            }
        }
        ROS_INFO("no");
        pub.publish(dev);

    
    //return costmap_data;
    
}

nav_msgs::OccupancyGrid filterSlopes(  pcl::PointCloud<pcl::PointXYZ> input_cloud,
                                            nav_msgs::OccupancyGrid costmap_data )
{
    //ros::Rate;

    ros::spinOnce();

    // Filtro de muestreo para reducir la densidad
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(input_cloud.makeShared());
    sor.setLeafSize(0.1, 0.1, 0.1);  // Ajusta el tamano del voxel segun tus necesidades
    sor.filter(input_cloud);
    return filterSlopesFn(input_cloud, costmap_data);
}

void cloudCallback(  pcl::PointCloud<pcl::PointXYZ> dato1)
{
    // Procesar la nube de puntos aqui para distinguir pendientes
    //pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Filtrar pendientes
    nubeDePuntos_p = dato1;

    // Publicar la nube de puntos filtrada si es necesario
    filterSlopes(nubeDePuntos_p, costmap_p);
}

void costmapCallback(nav_msgs::OccupancyGrid dato2)
{
    // Procesar la nube de puntos aqui para distinguir pendientes
    // pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Filtrar pendientes
    // costmap_p = boost::make_shared<nav_msgs::OccupancyGrid>(*dato2);
    costmap_p = dato2;

    // Publicar la nube de puntos filtrada si es necesario
    dev = filterSlopes(nubeDePuntos_p, costmap_p);
    //std::cout<<dev<<std::endl;
    ros::NodeHandle nh;
    //pub = nh.advertise<nav_msgs::OccupancyGrid>("/robot/move_base/local_costmap/costmap", 1000);
    //ROS_INFO("publi");
    // pub.publish(dev);
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "slope_filter_node");
    ros::NodeHandle nh;

    //costmap_2d::Costmap2DROS tf_costmap("tf_costmap", tf);

    // Sustituye "point_cloud_topic" con el nombre del topico de la nube de puntos de tu camara RGBD
    ros::Subscriber sub = nh.subscribe<const pcl::PointCloud<pcl::PointXYZ>>("/robot/front_rgbd_camera/depth/points", 1000, cloudCallback);
    ros::Subscriber sub2 = nh.subscribe<nav_msgs::OccupancyGrid>("/robot/move_base/local_costmap/costmap", 1000, costmapCallback);
    
    pub = nh.advertise<nav_msgs::OccupancyGrid>("/robot/nube_filtrada", 1000);
    pub.publish(dev);

    // Sustituye "filtered_point_cloud_topic" con el nombre del nuevo topico para la nube de puntos filtrada
    // pub = nh.advertise<nav_msgs::OccupancyGrid>("/robot/nube_filtrada", 1);
    
    ros::spin();

    return 0;
}
$\endgroup$
1
  • $\begingroup$ Please edit the question to limit it to a specific problem with enough detail to identify an adequate answer. $\endgroup$
    – Community Bot
    Commented Apr 29 at 0:18

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.