laser_assembler not reading any point clouds
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;
}