search

iralabdisco / ira_laser_tools

All laser type assemblers and manipulators.
BSD 3-Clause "New" or "Revised" License
197 stars 215 forks source link

Merger not subscribing to gazebo generated laser topics #5

Open schultza opened 8 years ago

schultza commented 8 years ago

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:

Subscriptions:

Services:

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

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 commented 8 years ago

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 commented 5 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago 
// 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.