Merger not subscribing to gazebo generated laser topics

[schultza]

Hello,
while trying to set up my system in Gazebo, I was noticing that the merger doesn't subscribe to the laser topics generated by 3 hokuyo lidars in Gazebo.
My config is the following:
3 lasers mounted at robot -> publishing topics: /scan_hokuyo1 /scan_hokuyo2 and /scan_ibeo1
See my output of rostopic list:

rostopic list
/scan_hokuyo1
/scan_hokuyo2
/scan_ibeo1
/scan_multi_merged


You also see the topic `/scan_multi_merged` which is published by the laser merger node as you can see here:

rosnode info /laserscan_multi_merger

Node [/laserscan_multi_merger]
Publications:

• /laserscan_multi_merger/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
• /merged_cloud [sensor_msgs/PointCloud2]
• /rosout [rosgraph_msgs/Log]
• /laserscan_multi_merger/parameter_updates [dynamic_reconfigure/Config]
• /scan_multi_merged [sensor_msgs/LaserScan]

Subscriptions:

• /tf [tf2_msgs/TFMessage]
• /tf_static [tf2_msgs/TFMessage]
• /clock [rosgraph_msgs/Clock]

Services:

• /laserscan_multi_merger/set_parameters
• /laserscan_multi_merger/set_logger_level
• /laserscan_multi_merger/get_loggers

contacting node http://aschultz-ThinkPad-T450s:42260/ ...
Pid: 21244
Connections:

• topic: /rosout
  • to: /rosout
  • direction: outbound
  • transport: TCPROS
• topic: /clock
  • to: /gazebo (http://aschultz-ThinkPad-T450s:33525/)
  • direction: inbound
  • transport: TCPROS
• topic: /tf
  • to: /vehicle_state_publisher (http://aschultz-ThinkPad-T450s:55570/)
  • direction: inbound
  • transport: TCPROS
• topic: /tf_static
  • to: /vehicle_state_publisher (http://aschultz-ThinkPad-T450s:55570/)
  • direction: inbound
  • transport: TCPROS

You can see the node isn't subscribing to the topics mentioned above. I started the node with the following part of my launch file:

<node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="screen">
     <param name="destination_frame" value="/ibeo_frame"/>
     <param name="cloud_destination_topic" value="/merged_cloud"/>
     <param name="scan_destination_topic" value="/scan_multi_merged"/>
     <param name="laserscan_topics" value ="/scan_hokuyo1 /scan_hokuyo2 /scan_ibeo1" /> 
</node>

If I try to echo the topic of the laser merger the following output appears and also ROS_DEBUG messages are published:

rostopic echo /scan_multi_merged 
WARNING: no messages received and simulated time is active.
Is /clock being published?

[DEBUG] [1453887963.432518126, 100.157000000]: Incoming queue full for topic "/clock". Discarding oldest message (current queue size [0])

So I don't think that the subscription to topics is done correctly.
Thanks in advance

[cyborg-x1]

I guess the problem here is that the node checks if the topic is already there and then subscribes it.
Well if you launch it in a launchfile probably the node of the scan merger is launched in the beginning... topics not there ... topics not subscribed

change the topic parser function like this, and it should work, as far as it does ...

void LaserscanMerger::laserscan_topic_parser()
{
    // LaserScan topics to subscribe
    ros::master::V_TopicInfo topics;
    ros::master::getTopics(topics);
    istringstream iss(laserscan_topics);
    vector<string> tokens;
    copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));
    vector<string> tmp_input_topics;
    for(int i=0;i<tokens.size();++i)
    {
//          for(int j=0;j<topics.size();++j)
//      {
//          if( (tokens[i].compare(topics[j].name) == 0) && (topics[j].datatype.compare("sensor_msgs/LaserScan") == 0) )
//          {
                tmp_input_topics.push_back(tokens[i]);
//          }
//      }
    }

[EwingKang]

change the topic parser function like this, and it should work, as far as it does ...

I've bump into the same problem, and thanks to @cyborg-x1, the fix worked. Guess this behavior is more desirable considering the ROS philosophy....

[Fellfalla]

try this fix: https://answers.ros.org/question/225183/ira_laser_tools-merger-doesnt-subscribe-to-topics-from-laser-in-gazebo/

Basically he adds a success return value and loops over laserscan_topic_parser() until the topics are published.

[ryanduringhours]

try this fix: https://answers.ros.org/question/225183/ira_laser_tools-merger-doesnt-subscribe-to-topics-from-laser-in-gazebo/

Basically he adds a success return value and loops over laserscan_topic_parser() until the topics are published.

I saw that thread and tried to make that fix. I feel like I'm doing it wrong. Could you post the modified source code or help me find where exactly im supposed to add this ros::Rate loop_rate(0.5); //-- 0.5 [Hz] -> 2.0 [s] while( !this->laserscan_topic_parser() ) { ROS_DEBUG("LaserscanMerger: Retrying to detect the topics to subscribe to..."); loop_rate.sleep(); }
Thanks

[SmitRmb]

// LaserScan topics to subscribe
	ros::master::V_TopicInfo topics;
	ros::master::getTopics(topics);

    istringstream iss(laserscan_topics);
	vector<string> tokens;
	copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));

	vector<string> tmp_input_topics;
	while(tokens.size() != tmp_input_topics.size())
	{	
		ros::master::getTopics(topics);
		
		for(int i=0;i<tokens.size();++i)
		{
			for(int j=0;j<topics.size();++j)
			{
				if( (tokens[i].compare(topics[j].name) == 0) && (topics[j].datatype.compare("sensor_msgs/LaserScan") == 0) )
				{
					tmp_input_topics.push_back(topics[j].name);
				}
			}
		}

		sort(tmp_input_topics.begin(),tmp_input_topics.end());
		std::vector<string>::iterator last = std::unique(tmp_input_topics.begin(), tmp_input_topics.end());
		tmp_input_topics.erase(last, tmp_input_topics.end());
		ROS_INFO_STREAM_THROTTLE(2.0, "Laserscan merger waiting for specified input topics. " <<  tmp_input_topics.size() << "/" << tokens.size() << " input topics found");
		if(tokens.size() == tmp_input_topics.size()) //Do not sleep if all input topics are found
		{
			break; 
		}
		ros::Duration(1.0).sleep(); 		
	}


	// Do not re-subscribe if the topics are the same
	if( (tmp_input_topics.size() != input_topics.size()) || !equal(tmp_input_topics.begin(),tmp_input_topics.end(),input_topics.begin()))
	{
....

This fix works for me. Waits until all input topics are available, and outputs a throttled ROS_INFO to let a user know how many of its selected input topics are available and how many are still to come.