ira_laser_tools merger doesn't subscribe to topics from laser in gazebo

asked 2016-01-27 04:18:40 -0600

schultza
232 ●23 ●31 ●40

updated 2016-01-28 01:56:09 -0600

Hey together,

please see edit #1 and #2. The node for the laser scanner won't subscribe to the topics given in its parameters in the launch file. But the scan topics are published correctly.

I am trying to merge 3 laserscans together to one. For this I am trying to use the ira_laser_tools packaged and in this the laser_multi_merge. My setup is following: 3 simulated Lasers in gazebo mounted to my robot, publishing 3 different topics. I am able to view the scan topics in rviz with no problem. After launching the laser_multi_merger with following launch file part:

<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>

rostopic list comfirms that the topics /scan_hokuyo1 /scan_hokuyo2 and /scan_ibeo1 are being published. It also shows the topic /scan_multi_merged, but if I subscribe to it whit rostopic echo /scan_multi_merged the following output appears:

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

If i enable via rqt the debug level of the node it also says that something is wrong with the /clock topic. Output of ROS_DEBUG message:

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

Can someone of you lead me to the right way and show me my faults I did?

Thanks in advance :)

Edit #1 Output of rosnode info /laserscan_multi_merger

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

Edit #2: Changed launch file because Laser merger is not subscribing to topics

<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>

Output of rostopic info /scan_hokuyo1 /scan_hokuyo2 /scan_ibeo1

rostopic info /scan_hokuyo1
Type: sensor_msgs/LaserScan

Publishers: 
 * /gazebo (http://aschultz-ThinkPad-T450s:51633/)

Subscribers: 
 * /rviz (http://aschultz-ThinkPad-T450s:55316/)


rostopic info /scan_hokuyo2
Type: sensor_msgs/LaserScan

Publishers: 
 * /gazebo (http://aschultz-ThinkPad-T450s:51633/)

Subscribers: 
 * /rviz (http://aschultz-ThinkPad-T450s:55316/)


rostopic info /scan_ibeo1
Type: sensor_msgs/LaserScan

Publishers: 
 * /gazebo (http://aschultz-ThinkPad-T450s:51633/)

Subscribers: 
 * /rviz (http://aschultz-ThinkPad-T450s:55316/)

edit retag flag offensive close merge delete

Comments

Do you see your topics if you do a "rosnode info laserscan_multi_merger" ?

F.Brosseau ( 2016-01-27 05:10:26 -0600 )edit

I just see the following

rosnode list /laserscan_multi_merger 
/laserscan_multi_merger

while

rostopic list
/scan_hokuyo1
/scan_hokuyo2
/scan_ibeo1
/scan_multi_merged

gives me this output. So if I understood you correctly I can't see the topics with the command

schultza ( 2016-01-27 06:01:05 -0600 )edit

If you do a rosnode info /laserscan_multi_merger you will see which topics are published and subscribed by the node. It will allow you to check if your topics are connected to your node.

F.Brosseau ( 2016-01-27 06:24:33 -0600 )edit

Okay. Sorry i had a copy paste error and pastet the output of rosnode list /laserscan_multi_merger which is complete nonsense :D sry. See my edit of my question for the output of rosnode info /laser_scan_merger

schultza ( 2016-01-27 06:35:45 -0600 )edit

Seems like your node is not subscribing to your topics /scan_hokuyo1, /scan_hokuyo2 and /scan_ibeo1. Do these topics are sensor_msgs::LaserScan type?

F.Brosseau ( 2016-01-27 06:49:34 -0600 )edit

Yeah thats correct. Do you have an idea why or should i open an new topic for that

schultza ( 2016-01-27 07:22:14 -0600 )edit

I edited the question with the output of all rostopic info

schultza ( 2016-01-27 07:26:51 -0600 )edit

I have no idea why, sorry. I hope that somebody else will be able to help you.

F.Brosseau ( 2016-01-27 09:11:58 -0600 )edit

see more comments

1

answered 2016-03-16 02:52:43 -0600

schultza
232 ●23 ●31 ●40

Thanks for all your answers! As ahendrix said, the node really needs the topics to be published before starting the node. However it is not possible to start nodes in a specific order in a launch file. For now we found a solution in letting the node sleep 5 seconds before trying to get the topics (with ros::Duration(5).sleep(); directly in the constructor). This works in simulation as well at our real robot. But it is not a very clean solution. So I will try to get the PR from the link ahendrix posted and look at it again. Thanks so for :)

edit flag offensive delete link

add a comment

3

answered 2016-12-20 13:37:25 -0600

Sergio MP
182 ●6 ●11 ●18

updated 2016-12-20 13:38:28 -0600

Hello, I was just having this problem and thanks to your comments I got a decent solution, so I thought I should share it here. I modified the laserscan_topic_parser to retry to read the topics listed after a delay, so, here's my modification:

bool LaserscanMerger::laserscan_topic_parser(){

bool success = false;
// 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(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());


// 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()))
{

    // Unsubscribe from previous topics
    for(int i=0; i<scan_subscribers.size(); ++i)
        scan_subscribers[i].shutdown();

    input_topics = tmp_input_topics;
    if(input_topics.size() > 0)
    {
        scan_subscribers.resize(input_topics.size());
        clouds_modified.resize(input_topics.size());
        clouds.resize(input_topics.size());
        ROS_INFO("Subscribing to topics\t%ld", scan_subscribers.size());
        for(int i=0; i<input_topics.size(); ++i)
        {
            scan_subscribers[i] = node_.subscribe<sensor_msgs::LaserScan> (input_topics[i].c_str(), 1, boost::bind(&LaserscanMerger::scanCallback,this, _1, input_topics[i]));
            clouds_modified[i] = false;
            cout << input_topics[i] << " ";
        }
        success = true;
    }
    else
        ROS_INFO("Not subscribed to any topic.");
}
return success;}

So, basically, I only changed the return type from void to bool and created a local variable (called "success") to determine if any topic was found. Then I added a while loop on the LaserscanMerger class constructor to retry every X seconds:

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();
}

Hope this helps someone =).

edit flag offensive delete link

add a comment

2

answered 2016-03-16 00:16:59 -0600

DavidN
544 ●5 ●13

I think the reason is that laser_scan_merger node was launched before gazebo started publishing the laser scan topics. If so, the laser merger would not subscribe to those laser topics.

If the laser scan merger has ever successfully subscribed to laser scan topics, you should be seeing: "Subscribing to topics x" in rosout.

One quick work around is to launch laser scan merger manually after gazebo has done launching.

edit flag offensive delete link

Comments

ROS topic setup does not depend on whether the subscriber or the publisher is created first.

ahendrix ( 2016-03-16 01:06:48 -0600 )edit

1

Nevermind; this node appears to be filtering input topics based on their type, so it DOES require topics to be published before if can subscribe to them. How broken.

ahendrix ( 2016-03-16 01:14:00 -0600 )edit

This PR appears to fix the topic subscription order problem: https://github.com/iralabdisco/ira_la...

ahendrix ( 2016-03-16 01:16:22 -0600 )edit

Exactly as ahendrix mentioned, the node actually checks if the msg type of the topic is a laser scan msg, thus ignoring the topics if it is not being published. I had a similar problem with real robot (not gazebo) with multiple lasers (more than 3)

DavidN ( 2016-03-16 01:54:50 -0600 )edit

add a comment

ira_laser_tools merger doesn't subscribe to topics from laser in gazebo

Comments

3 Answers

Comments

Question Tools

Stats

Related questions

ira_laser_tools merger doesn't subscribe to topics from laser in gazebo edit

Comments

3 Answers

Comments

Question Tools

Stats

Related questions

ira_laser_tools merger doesn't subscribe to topics from laser in gazebo