laser_assembler not reading any point clouds

asked 2022-11-16 14:08:46 -0500

jojonmarijon gravatar image

So I am currently working with a PrimeSense RGB-D camera and getting PointCloud2 values from the camera, I was hoping to use laser_assembler to combine all the point clouds I am getting. Since the cloud assemblers only work with PointCloud, I convert my PointCloud2 to PointCloud and publish it to another topic which I am subscribing to using my laser_assembler, although this does not seem to work for me, the problem is there is no error indicators for me to know why this is not reading.

I can confirm the conversion to PointCloud is successful because I can view the new topic on RVIZ.. so I cannot quite understand what the issue is.. just a note I am running this on ubuntu 18.04 with ROS Melodic

Thank you in advance!

For context: This is the launch file to launch the laser_assembler service

<launch> <node type="point_cloud_assembler" pkg="laser_assembler" name="my_assembler"> <remap from="cloud" to="cloud"/>
</node> </launch>

This is the node that converts the PointCloud2 to PointCloud:

using namespace laser_assembler;

class depth_subscriber { ros::ServiceClient client; ros::Subscriber sub; ros::Publisher pubPointCloud; ros::NodeHandle n; AssembleScans srv;

public:
depth_subscriber() {
    sub = n.subscribe("/camera/depth/points", 1000, &depth_subscriber::depthCallback, this);

    ros::service::waitForService("assemble_scans");

    client = n.serviceClient<AssembleScans>("assemble_scans");
    pubPointCloud = n.advertise<sensor_msgs::PointCloud>("cloud", 1000);
}

void depthCallback(const sensor_msgs::PointCloud2ConstPtr& msg) {

    sensor_msgs::PointCloud outputPointCloud;
    sensor_msgs::PointCloud2 inputPointCloud2 = *msg;

    if (sensor_msgs::convertPointCloud2ToPointCloud (inputPointCloud2, outputPointCloud)) {
        pubPointCloud.publish(outputPointCloud);
    }

    srv.request.begin = ros::Time(0,0);
    srv.request.end   = ros::Time::now();

    if (client.call(srv)){
        if (srv.response.cloud.points.size() > 0) {
            printf("Got cloud with %u points\n", srv.response.cloud.points.size());
        }
    }

    else {
        printf("Service call failed\n");
    }
}

};

int main(int argc, char **argv) { ros::init(argc, argv, "depth_subscriber"); depth_subscriber depth; ros::spin();

return 0;

}

edit retag flag offensive close merge delete