如何定期更新 ROS 代价地图位置？

Question

我遇到了以下问题：

我有一个占用网格，该网格基于从我的机器人上插入的摄像头接收到的环境信息。由于机器人应该在未知区域四处移动，因此其原始上下文中没有 "global costmap"。我们使用从相机接收到的信息作为 "global costmap"。目前，全局成本图配置为链接到 base_link-frame 的 static_layer（我需要它是 static_layer，因为它有多个成本值，只有 static_layer 及其 "trinary_costmap" 参数可以选择映射中间 cost_values 范围 0-255）。由于它链接到 base_link-frame，它会永久地与框架一起移动。但是由于成本地图以每秒 2 个占用网格的频率更新，这种永久移动会伪造成本地图信息（因为在新的占用网格发布之前它应该是静态地图）。

我的想法是根据 the ros tutorial for adding a new dynamic frame 创建一个新框架，它采用 base_link 的当前位置并以特定速率更新其位置。
因此，我的源代码如下所示：

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

如您所见，我使用客户端服务系统获取数据并将其转换为占用网格，创建一个名为 "global_costmap_link" 的框架作为 base_link 的子框架并发布frame和内格的占用率相同。这样做，不幸的是，新的 "global_costmap_link"-frame 也继续与 base_link-frame 一起移动，而不是停留在它发布的位置，并且只在重新发布时移动到 base_link 的新位置- 按比例发布。

有谁知道如何解决这个问题？或者有人有其他方法 如何配置我的成本图，使其与 base_link-frame 一起移动但根据给定速率延迟？

更多信息，这里是我的 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"}

我在 ROS Melodic 上使用了 costmap2d 库。

Answer 1

传统上，总是有一个全局代价地图：如果没有固定的世界坐标或静态地图参考作为全局原点，它就被称为map frame，原点被初始化为你的初始值位置。然后你使用 ekf 或类似的 (robot_localization/amcl) 来（自动）计算来自 base_link to map 的变换。然后你会有一个以 base_link 为中心的本地代价地图，它只翻译。

您可以只启动 costmap node 本身，它支持不同的发布和更新率。如果您正在使用它，这也应该由 move_base 处理，因为它 wraps/calls 成本图与您的 laserscan/pointcloud 输入。

只要你有来自 base_link to map 的转换，你就不需要将代价地图绑定到你的 base_link，因此它不会移动。

Answer 2

由于在我的情况下更新整个代价地图比只更新新发现的区域更好，我通过在我发布占用网格的相同代码（并且以相同的速率）中发布一个新帧来解决它。然后只需将全局成本图的框架设置为新发布的框架即可。看我的代码打击：

我发布占用网格的脚本：

...
#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;
}

在global_costmap_params.yaml中：

global_frame: global_costmap_link

如何定期更新 ROS 代价地图位置？

How to update ROS costmap position periodically?

navigation

ros