laser_assembler not gathering clouds
I've been trying to use laser_assembler package to gather every single cloud that is publish by another node. For this, I've created a client node to define the service needed from the laser_assembler package(included in launch file). However, I still couldn't be able to have the clouds gathered.
The client code:
void AssemblerClient::useAssembler(const sensor_msgs::LaserScanConstPtr& scan_ptr)
{
ROS_INFO("Assembler service is being used");
ros::service::waitForService("assemble_scans");
ros::ServiceClient client = n.serviceClient<AssembleScans>("assemble_scans");
AssembleScans srv;
srv.request.begin = ros::Time(0);
srv.request.end = ros::Time::now();
if (client.call(srv))
ROS_INFO("Got cloud with %u points\n", srv.response.cloud.points.size());
else
ROS_INFO("Service call failed\n");
}
In the launch file:
<node type="point_cloud_assembler" pkg="laser_assembler" name="my_assembler">
<remap from="cloud" to="cloud"/>
<param name="max_clouds" type="int" value="4000" />
<param name="fixed_frame" type="string" value="world" />
</node>
<node pkg="pcd_bot" type="assembler_client_node" name="assembler_client_node">
</node>
Am I missing something?
Thanks in advance.
EDIT
No error at all I receive but in the rviz, I even cannot choose/tick for "assemble_scans" topic of type PointCloud.
I've added the following 2 lines at the bottom of the above code snippet(but still doesn't work):
gathered_cloud = srv.response.cloud;
gathered_cloud_publisher_.publish(gathered_cloud);
What is the error exactly?'
What is the error exactly?
No error at all but in the rxconsole I receive this info "Got cloud with 0 points" and in the rviz of course, it displays only one scan line. In the rviz, I even cannot choose/tick for "assemble_scans" topic.
@DimitriProsser: Do I need to subscribe explicitly to the "assemble_scans" topic that has been provided by laser_assembler package?
Supposedly, will the laser_assembler publish a topic on "assemble_scans"? When I rostopic list, I didn't see the "assemble_scans" listed.
@alfa_80 assemble_scans is not a topic it's a service, and you can't subscribe to it to see the assembled cloud. In your code you correctly printing out the cloud size, you can choose to publish this cloud on some topic. But it looks like the resulting cloud has 0 points, so there's some other issue
For example, pr2_laser_snapshotter (which uses the assemble_scans service) publishes assembled clouds when the tilting laser does a full up-down sweep.
@arebgun & @DimitriProsser: At the end, I got this in the rxconsole "Got cloud with 89280 points ", It just needs a little bit time to get that. But then, why I cannot choose "assemble_scans" in rviz of type PointCloud? Any idea on how to resolve this?