Multiple nodes providing same service, which one will get through (many to one services) [closed]
I am trying a very simple task. I have a 2D laser scanner and 3D velodyne laser on my robot and I want to create an assembled point cloud which includes data from both lasers in one point cloud. I am trying to have laser_assembler package do all the work with laser_scan_assembler
and point_cloud2_assembler
nodes, which both provide services of same name /assembled_point_cloud
which is of the same type sensor_msgs::PointCloud.
The problem I'm facing is that my temp_service_call_node
only subscribes to one service (the one provided by point_cloud2_assembler), and gets no data from laser_scan_assembler. Also, if I kill the velodyne node, so that no point cloud2 data is published for point_cloud2_assembler, it doesn't change anything. /assembled_point_cloud
which instead of showing only 2D laser scan data when no point cloud2 data is available, keeps subscribing to the point_cloud2_assembler
which doesn't give any data.
Is there anything I'm missing or a different approach that I can try?
Please see code for reference-
Here's my launch file:
<launch>
<node type="laser_scan_assembler" pkg="laser_assembler" name="laser_scan_assembler">
<remap from="scan" to="/laser/scan"/>
<remap from="assemble_scans" to="assembled_point_cloud"/>
<param name="max_scans" type="int" value="1" />
<param name="fixed_frame" type="string" value="base_link" />
</node>
<node type="point_cloud_assembler" pkg="laser_assembler" name="point_cloud_assembler">
<remap from="cloud" to="/laser/point_cloud"/>
<remap from="assemble_scans" to="assembled_point_cloud"/>
<param name="max_clouds" type="int" value="1" />
<param name="fixed_frame" type="string" value="base_link" />
</node>
<node type="point_cloud2_assembler" pkg="laser_assembler" name="point_cloud2_assembler">
<remap from="cloud" to="/laser/point_cloud2"/>
<remap from="assemble_scans" to="assembled_point_cloud"/>
<param name="max_clouds" type="int" value="1" />
<param name="fixed_frame" type="string" value="base_link" />
</node>
</launch>
temp_service_call.cpp file:
void publishPointCloud2()
{
//Wait for service to start
ros::service::waitForService("/assembled_point_cloud");
ros::Rate r(updateRate);
while(ros::ok())
{
assemblerService.request.begin = assemblerService.request.end;
assemblerService.request.end = ros::Time::now();
if (assemblerClient.call(assemblerService))
{
pointCloud = assemblerService.response.cloud;
sensor_msgs::convertPointCloudToPointCloud2(pointCloud, pointCloud2);
ROS_INFO("Got cloud with %u points\n", assemblerService.response.cloud.points.size());
}
else
{
ROS_ERROR("Service call for getting assembled laser scan failed\n");
}
if (pointCloud2.data.size() > 0)
pointCloud2Publisher.publish(pointCloud2);
ros::spinOnce();
r.sleep();
}
}