How to update ROS costmap position periodically?

user_13635682 0
Viewed 2495

I'm stuck with the following problem:

I have an occupancy grid based on information about the environment received from a camera plugged on my robot. Since the robot is supposed to move around in unknown territory, there is no "global costmap" in its original context. We use the information received from the camera as the "global costmap". Currently, the global costmap is configured as a static_layer that is linked to the base_link-frame (I need it to be a static_layer as it has multiple cost-values and only the static_layer with its "trinary_costmap"-parameter has the option to map intermediate cost_values range 0-255). Due to it is linked to the base_link-frame, it permanently moves along with the frame. But since the costmap is updated with a frequency of 2 occupancy grids per second, this permanent movement falsifies the costmap-information (since it's supposed to be a static map until the new occupancy grid gets published).

My idea was to create a new frame according to the ros tutorial for adding a new dynamic frame, which takes the current position of base_link and updates its position with a specific rate.
Therefore, my source-code looks like this:

int main(int argc, char** argv) {
        ros::init(argc, argv, "path_segmentation_client");     // initialise ROS
        ros::NodeHandle nh;                                   // create NodeHandle for the node
        ros::ServiceClient client = nh.serviceClient<path_detection_cnn::segmentation_to_occgrid>("segmentation_to_occgrid");    //create client for segmentation_to_occgrid-service
        path_detection_cnn::segmentation_to_occgrid srv;    //create service-instance
        double rate;    //Variable to store loop rate
        nh.getParam("/path_segmentation_client/looprate", rate);    //get loop-rate from parameters
        ros::Rate r(rate);    //apply loop-rate to ros
        ros::Publisher map_pub = nh.advertise<nav_msgs::OccupancyGrid>("prediction_occ_grid",1000);    //declare publisher for publishing occupancy grid

        static tf2_ros::StaticTransformBroadcaster brs;
        geometry_msgs::TransformStamped global_costmap_link;
        global_costmap_link.header.stamp = ros::Time::now();
        global_costmap_link.header.frame_id = "base_link";
        global_costmap_link.child_frame_id = "global_costmap_link";
        global_costmap_link.transform.translation.x = 0; 
        global_costmap_link.transform.translation.y = 0;
        global_costmap_link.transform.translation.z = 0.0;
        global_costmap_link.transform.rotation.x = 0;
        global_costmap_link.transform.rotation.y = 0;
        global_costmap_link.transform.rotation.z = 0;
        global_costmap_link.transform.rotation.w = 1;
        while(nh.ok()){
            client.waitForExistence();
            if (client.call(srv))
            {
                nav_msgs::OccupancyGrid occGrid = get_occupancy_grid(srv.response.topViewMsg);    //convert topView to Occupancy Grid
                map_pub.publish(occGrid);    // publish occupancyGrid
                brs.sendTransform(global_costmap_link); //update location of costmap_origin
                r.sleep();    //sleep to keep loop rate
            }
            else
            {
                ROS_ERROR("Failed to call service segmentation_to_occgrid.");
            }
        }

        return 0;
}

As you can see, I use a client-service-system to obtain data and convert it to an occupancy grid, creating a frame named "global_costmap_link" as a child of base_link and publishing the frame and the occupancy grid within the same rate. Doing so, the new "global_costmap_link"-frame unfortunately also keeps moving along with the base_link-frame instead of remaining at the position it was published and only move to the new position of base_link when it is re-published according to the rate.

Does anyone know how to fix this? Or does anyone have another approach on how to configure my costmap in a way that it moves along with the base_link-frame but delayed according to the given rate?

For more information, here is my global_costmap_param.yaml:

global_costmap:
  global_frame: base_link
  map_topic: "prediction_occ_grid"
  update_frequency: 2.0
  publish_frequency: 2.0
  rolling_window: false
  always_send_full_costmap: true
  /move_base/global_costmap/static_layer/trinary_costmap: false
  height: 26.9
  width: 36.4
  mode: scale
  plugins: 
     - {name: static_layer, type: "costmap_2d::StaticLayer"}

I use the costmap2d-library on ROS Melodic.

ros navigation
2 Answers

Classically, there's always a global costmap: if there's no fixed world coordinate or a static map reference to act as the global origin, it's just referred to as map frame, and the origin is initialized to your initial position. Then you use an ekf or such (robot_localization/amcl) to (automatically) compute the transform from base_link to map. Then you would have a local costmap that is centered on the base_link, which only translates.

You could just launch the costmap node itself, which supports different publishing & updating rates. This should also be handled by move_base, if you're using it, since it wraps/calls costmap with your laserscan/pointcloud inputs.

As long as you have the transform from base_link to map, you don't need to make the costmap tied to your base_link, and it therefore wouldn't move.

user_60695860

Since in my case it's better to update the whole costmap than only updating the new discovered area, I solved it by publishing a new frame in the same code (and in the same rate) where I publish my occupancy grid. Then just set the frame for the global costmap to the new published frame and it works. See my code blow:

Script where I publish my occupancy grid:

...
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
...
int main(int argc, char** argv) {
        ...
        double rate;    //Variable to store loop rate
        nh.getParam("/path_segmentation_client/looprate", rate);    //get loop-rate from parameters
        ros::Rate r(rate);    //apply loop-rate to ros
        ros::Publisher map_pub = nh.advertise<nav_msgs::OccupancyGrid>("prediction_occ_grid",1000);    //declare publisher for publishing occupancy grid

        static tf2_ros::StaticTransformBroadcaster brs;
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
        geometry_msgs::TransformStamped global_costmap_link;
        global_costmap_link.header.stamp = ros::Time::now();
        global_costmap_link.header.frame_id = "map";
        global_costmap_link.child_frame_id = "global_costmap_link";

        while(nh.ok()){
            client.waitForExistence();
            if (client.call(srv))
            {
                nav_msgs::OccupancyGrid occGrid = get_occupancy_grid(srv.response.topViewMsg);    //convert topView to Occupancy Grid
                map_pub.publish(occGrid);    // publish occupancyGrid 
                geometry_msgs::TransformStamped listenerStamped;
                try{
                    listenerStamped = tfBuffer.lookupTransform("map", "d435_link", ros::Time::now());
                }
                catch(tf2::TransformException &ex){
                    ROS_WARN("Error in path_segmentation: %s", ex.what());
                    continue;
                }
                global_costmap_link.transform.translation.x = listenerStamped.transform.translation.x; 
                global_costmap_link.transform.translation.y = listenerStamped.transform.translation.y;
                global_costmap_link.transform.translation.z = listenerStamped.transform.translation.z;
                global_costmap_link.transform.rotation.x = listenerStamped.transform.rotation.x;
                global_costmap_link.transform.rotation.y = listenerStamped.transform.rotation.y;
                global_costmap_link.transform.rotation.z = listenerStamped.transform.rotation.z;
                global_costmap_link.transform.rotation.w = listenerStamped.transform.rotation.w;
                brs.sendTransform(global_costmap_link); //update location of costmap_origin
                r.sleep();    //sleep to keep loop rate
            }
            else
            {
                ROS_ERROR("Failed to call service segmentation_to_occgrid.");
            }
        }

        return 0;
}

In global_costmap_params.yaml:

global_frame: global_costmap_link
user_136356820
Related