tutorial4.launch /robot_0/Mapper node connects robot_0\odom frame to robot_0\map but /robot_1/Mapper doesn't connect robot_1\odom frame to robot_1\map

[s-arora1987]

I launched tutorial4.launch, and generated frames.pdf using 'rosrun tf view_frames'. /robot_0/Mapper node connects robot_0\odom frame to robot_0\map (via robot_0\offset) but /robot_1/Mapper doesn't connect robot_1\odom frame to robot_1\map. Could you please guide me about how to troubleshoot this issue? frames.pdf

[skasperski]

This is the expected behavior, because the second mapper cannot start mapping before it is localized. You have to complete the localization process as described here: http://wiki.ros.org/nav2d/Tutorials/DistributedMapping

[s-arora1987]

I believe you're refering to this part "Start the launch file to see Stage with two robots standing next to each other and RVIZ with two maps. Only the left robot (id=1) is localized. The second one only knows about the map that it received from the first. Hold button 1 to move the first robot around, until it has covered a reasonable area around its starting point. After a fixed amount of received measurements (parameter min_map_size of mapper node), the second robot will initialize a particle filter to localize itself within the map. (See http://wiki.ros.org/amcl for a description of the localization approach)" of tutorial. I am confused about following aspects:

Q1 : If robots are mapping, how can any of them localize before completely knowing the map? AFAIK, map should be completely known for localization. Q2 : How can I modify the usage of code to a scenario where both robots can individually explore (yet share measurements) from the beginning of mapping regardless of localization? Q3 : If completing localization is only alternative, how can I finish tutorial4 without a joystick?

[skasperski]

Q1: You also localize within an incomplete map. What defines a "complete" map anyway? ;) Of course the explored map should be "large enough" to localize in it. What "enough" means is a bit of an experimental value.

Q2: This is indeed a difficult problem and theoretical solutions to this exist in the literature. The most common approach is to collect data in parallel until a match between both maps can be established, which is actually very similar to the localization approach in nav2d. The place to solve this would be the nav2d_karto package.

Q3: You would have to start Operator and Navigator on robot_0 as well in order to have it run autonomously. This was left out for the tutorial, because the first robot could be controlled with a joystick.

[s-arora1987]

Thanks for directions. You mentioned "Q2: This is indeed a difficult problem and theoretical solutions to this exist in the literature. The most co mmon approach is to collect data in parallel until a match between both maps can be established, which is actually very similar to the localization approach in nav2d. The place to solve this would be the nav2d_karto package."

I may need some pointers about integrating this package I came across http://wiki.ros.org/multirobot_map_merge The technique given section 3.1 of link above (merging with known initial positions) needs input about initial poses of both robots. I think localizer in your tutorial can do that. Please correct me if I am wrong. Is there a way I can use the output map from nav2d_karto as inputs to Subscribed Topics (/map,/map_updates) of multirobot_map_merge package?

[skasperski]

I only had a quick look at the map-merge package, but I think integration should be straight forward. The package takes nav_msgs/OccupancyGrid as input, which is exactly what the mapper produces, so this should be no problem.

[s-arora1987]

I really appreciate your guidance, may be I can avoid the need for additional package for merging map. I tried tutorial 4 without joystick by adding {operator, navigator,getmap,setgoal,explore} to robot 0 group as you suggested:

<group ns="robot_1">
    <param name="robot_id" value="2" />
    <param name="tf_prefix" type="string" value="robot_1"/>
    <rosparam file="$(find nav2d_tutorials)/param/ros.yaml"/>

    <!-- Start the Operator to control the simulated robot -->
    <node name="Operator" pkg="nav2d_operator" type="operator" >
        <remap from="scan" to="base_scan"/>
        <rosparam file="$(find nav2d_tutorials)/param/operator.yaml"/>
        <rosparam file="$(find nav2d_tutorials)/param/costmap.yaml" ns="local_map" />
    </node>

    <!-- Start the Navigator to move the robot autonomously -->
    <node name="Navigator" pkg="nav2d_navigator" type="navigator">
        <rosparam file="$(find nav2d_tutorials)/param/navigator.yaml"/>
    </node>

    <node name="GetMap" pkg="nav2d_navigator" type="get_map_client" />
    <node name="SetGoal" pkg="nav2d_navigator" type="set_goal_client" />

    <node name="Localize" pkg="nav2d_navigator" type="localize_client" />
    <node name="Explore" pkg="nav2d_navigator" type="explore_client" />

    <node name="Mapper" pkg="nav2d_karto" type="mapper">
        <remap from="scan" to="base_scan"/>
        <remap from="karto_in" to="/shared_scans_r1"/>
        <remap from="karto_out" to="/shared_scans_r2"/>
        <rosparam file="$(find nav2d_tutorials)/param/mapper.yaml"/>
    </node>

</group>

Localization is only for robot 1. I started services is this order: robot_1/startlocalization robot_0/startmapping robot_0 moved a little and then stopped. robot_0/startexploration robot_0 started exploring. After few scans from robot_0, robot_1 started to move on its own and both robots started exploration. But robot_1 stopped after some time. calling robot_1/startexploration made it start again

Then they both stopped after the extent of exploration shown in attached image

Q: Could you please explain what and where in code are the criteria (including completely known map) that stops the exploration? Q: Also what reason can make robot 1 stop right after starting?

[skasperski]

The exploration stops, when no new frontiers can be detected by the loaded exploration algorithm. (See RobotNavigator.cpp)

robot_0 started exploring. After few scans from robot_0, robot_1 started to move on its own and both robots started exploration. But robot_1 stopped after some time.

No, robot_0 was not exploring, only localizing. You did not call "StartExploration" on robot_0. You should start localization of robot_1 only after robot_0 has finished "StartMapping".

The map in your image is broken. This is because robot_1 was not correctly localized when it started the exploration.

[s-arora1987]

Thanks. I got it now.

I have added a distance based connectivity constraint in mapping by modifying readlaserscan method in nav2d_karto and receiveexploregoal in navigator. The two robots can merge their scans only if they are within allowed displacement from each other. If their next frontier cross that threshold, they find another frontier.

I want to introduce a capability that if a robot crosses connectivity threshold distance, they can still build separate individual maps (instead of finding new frontier) and those maps can be merged later when connectivity is established again. How should I modifying readlaserscan method for that? Is it only about changing location of sendmap() calls in nav2d_karto?

[skasperski]

I am not sure if I understand what you are trying to achieve. If connection is lost between robots, they can continue to map individually. No modification is required for that. What you need to do is buffer the calls to sendLocalizedScan, which sends the acquired scans to the other robots. Once the connection es available, all added scans can be send to the other robots and will be merged into the map.

[s-arora1987]

I looked into the steps inside receivedLaserScan method. Are these the steps I am supposed to follow? Create a dynamic array scans of sensor_msgs::LaserScan::ConstPtr objects, and another array localizedscans of karto::LocalizedLaserScanPtr objects. When robots get connected, send all of them sequentially as shown below. if(robots connected) { For each <sensor_msgs::LaserScan::ConstPtr& scan’,karto::LocalizedLaserScanPtr laserScan’> in <scans, localizedscans> arrays: sendLocalizedScan(scan, laserScan->GetOdometricPose()); }

Question: Don’t I need to buffer the following sendmap calls too? if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate) { sendMap(); } Question: Instead of saving both scan and laserscan, is there a way to retrieve scan msg from karto::LocalizedLaserScanPtr ?

[skasperski]

if(robots connected) { For each <sensor_msgs::LaserScan::ConstPtr& scan’,karto::LocalizedLaserScanPtr laserScan’> in <scans, localizedscans> arrays: sendLocalizedScan(scan, laserScan->GetOdometricPose()); }

Basically yes, but you only need to buffer the karto::LocalizedLaserScan's. (These are generated from the sensor_msgs::LaserScan's that come from the sensor.)

Question: Don’t I need to buffer the following sendmap calls too? if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate) { sendMap(); }

No, the map is generated locally on each robot, so only the scans that are added to karto need to be buffered. Keep in mind that karto is a graph-based SLAM, so it holds all the scans that are used to build the map in a graph. The resulting OccupancyGrid is rendered from the actual scans on demand.

Question: Instead of saving both scan and laserscan, is there a way to retrieve scan msg from karto::LocalizedLaserScanPtr ?

The method createFromRosMessage is used to convert from sensor_msgs::LaserScan to karto::LocalizedRangeScan. When sending these via ros-topic you should use the nav2d_msgs::LocalizedScan message, which is done with sendLocalizedScan.

[s-arora1987]

I am sorry for bothering; I know that this a lot to ask but I am trying to understand the code better.

if(robots connected) {
For each <sensor_msgs::LaserScan::ConstPtr& scan’, karto::LocalizedLaserScanPtr laserScan’> in <scans, localizedscans> arrays:
sendLocalizedScan(scan, laserScan->GetOdometricPose());
}

Basically yes, but you only need to buffer the karto::LocalizedLaserScan's.

Let's call the sensor_msgs::LaserScan::ConstPtr& received when robots were disconnected' an old-scan and thesensor_msgs::LaserScan::ConstPtr& received when robots just got connected' a new-scan. If I don't buffer old-scan, then I believe I am calling sendLocalizedScan() with a new-scan and an OdometricPose corresponding to karto::LocalizedLaserScan computed for an old-scan. Then method MultiMapper::sendLocalizedScan will publish nav2d_msgs::LocalizedScan with ( x,y,yaw computed for old-scan ) but ( scan->ranges ) for new scan. Please correct me if my interpretation so far is incorrect. If it is correct, will that effect accuracy of merging of maps explored so far?

[skasperski]

There seems to be some misunderstanding regarding the involved types. There are two ROS-Message types: sensor_msgs::LaserScan and nav2d::LocalizedScan. The first one is typically created by LaserScanner devices in the ROS ecosystem. This incoming scan (from a robot's sensor) is then converted to karto::LocalizedRangeScan (which is not an ROS type but karto's internal scan type). These karto-scans can then be added to the mapper.

The nav2d::LocalizedScan message is only needed in a multi-robot setup to exchange karto-scans between different robots using the karto_in and karto_out topics. In its current state, each scan is send immediatly via the karto_out topic after it was added to the karto mapper. (Only scans from the own sensor are received as sensor_msgs::LaserScan, scans from other robots are received as nav2d::LocalizedScan via the karto_in topic.)

So if you have information on the connection status, you could do something like this in MultiMapper::receiveLaserScan:

if(robots_connected)
{
    sendScansFromBuffer();
    sendLocalizedScan(scan, laserScan->GetOdometricPose());
}else
{
    addLocalizedScanToBuffer(scan, laserScan->GetOdometricPose());
}

[s-arora1987]

... else { addLocalizedScanToBuffer(scan, laserScan->GetOdometricPose()); }

Each call to this method must create a buffer-entry of two things: a const sensor_msgs::LaserScan::ConstPtr& scan, and a const karto::Pose2& pose. Therefore, I need two dynamic arrays to implement the buffer: one array for scan and one array for pose. But I am facing an issue doing that. I can't make std::vector dynamic array of pointers. Is there an alternative way to have a dynamic size storage/container for these types?

[skasperski]

I think it would be easier to create one buffer (std::vector) of nav2d::LocalizedRangeScan instead. (A scan and a pose together form a localized Scan). This one is currently created in sendLocalizedScan directly. So you could write a conversion method instead and call sendLocalizedScan and the newly created addLocalizedScanToBuffer directly with a nav2d::LocalizedScan.

[s-arora1987]

Thanks, that solution helps create buffer. In order to reduce combined efforts of exploration, I need to make robots assure that they connect back at some random interval. Going always towards frontier will not achieve that because it may make them go farther away. Therefore, if robots are disconnected for very long time, I will make one robot stop and second robot go to nearest possible position where connectivity can be re-established. Can I do this by sending a goal position to "goal" topic for the subscriber in send_goal_client.cpp?

[s-arora1987]

There is an update. I modified the code for buffering of localized scans and sharing them when robots got connected.

void MultiMapper::receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan) { ///// everything above this same as before////////// // create localized laser scan karto::LocalizedLaserScanPtr laserScan = createFromRosMessage(*scan, mLaser->GetIdentifier()); laserScan->SetOdometricPose(kartoPose); laserScan->SetCorrectedPose(kartoPose);

    bool success;
    try
    {
        success = mMapper->Process(laserScan);
    }
    catch(karto::Exception e)
    {
        ROS_ERROR("%s", e.GetErrorMessage().ToCString());
        success = false;
    }

    if(success)
    {

        // Compute the map->odom transform
        karto::Pose2 corrected_pose = laserScan->GetCorrectedPose();// kartoPose
        tf::Pose map_in_robot(tf::createQuaternionFromYaw(corrected_pose.GetHeading()), tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0));
        map_in_robot = map_in_robot.inverse();
        tf::Stamped<tf::Pose> map_in_odom;
        bool ok = true;
        try
        {
            mTransformListener.transformPose(mOffsetFrame, tf::Stamped<tf::Pose>(map_in_robot, ros::Time(0) /*scan->header.stamp*/, mLaserFrame), map_in_odom);
        }
        catch(tf::TransformException e)
        {
            ROS_WARN("Transform from %s to %s failed! (%s)", mLaserFrame.c_str(), mOffsetFrame.c_str(), e.what());
            ok = false;
        }
        if(ok)
        {
            mMapToOdometry = tf::Transform(tf::Quaternion( map_in_odom.getRotation() ), tf::Point(map_in_odom.getOrigin() ) ).inverse();
            tf::Vector3 v = mMapToOdometry.getOrigin();
            v.setZ(0);
            mMapToOdometry.setOrigin(v);
        }
        mNodesAdded++;
        mMapChanged = true;

        ros::WallDuration d = ros::WallTime::now() - mLastMapUpdate;

        if(mMapUpdateRate > 0 && d.toSec() > mMapUpdateRate)
        {
            sendMap();
        }

        nav2d_msgs::LocalizedScan convertedScan;
        ROS_ERROR("commsafe_GT1 %d",commsafe_GT1);
        if (commsafe_GT1) {

            // if buffer not empty, empty that first
            if (!scans.empty()) {
                size_t sizebuff = scans.size();
                for( size_t i = 0; i < sizebuff; ++i ) {
                    convertedScan = scans[i];
                    mScanPublisher.publish(convertedScan);
                }
            }
            //Reset buffer
            scans.clear();

            // Send the latest scan to the other robots via com-layer (DDS)
            ROS_DEBUG("Robot %d: Sending scan (uniqueID: %d, Sensor: %s, stateID: %d)", mRobotID, laserScan->GetUniqueId(), laserScan->GetSensorIdentifier().ToString().ToCString(), laserScan->GetStateId());
            convertedScan = convertoLocalizedScan(scan, laserScan->GetOdometricPose());
            mScanPublisher.publish(convertedScan);

// sendLocalizedScan(scan, laserScan->GetOdometricPose()); } else { //buffer scans convertedScan = convertoLocalizedScan(scan, laserScan->GetOdometricPose()); scans.push_back(convertedScan);

        }

        // Publish via extra topic
        nav2d_msgs::RobotPose other;
        other.header.stamp = ros::Time::now();
        other.header.frame_id = mMapFrame;
        other.robot_id = mRobotID;
        other.pose.x = laserScan->GetCorrectedPose().GetX();
        other.pose.y = laserScan->GetCorrectedPose().GetY();
        other.pose.theta = laserScan->GetCorrectedPose().GetHeading();
        mOtherRobotsPublisher.publish(other);
    }
}

When robots got connected back after disconnecting, the updates in maps were not correct. Due to inaccurate positions of objects, robot can’t move to next frontier, and exploration failed. What can be the reasons for this issue?

[skasperski]

Is the local map also broken or only the one on the other robot after the reconnect? I can't guess on the reason without details on what went wrong.

[s-arora1987]

The problem of robot not being able to move to next frontier has been present before I introduced the connectivity related code. That's why I have created a separate issue to resolve that. Could you please reply to the issue with title "Autonomous Exploration with Initial Poses Known". I have also attached the code in that post.