feat: update map origin (#24)

RyuYamamoto · Jun 18, 2024 · bd39297 · bd39297
1 parent a3e4cc0
commit bd39297
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 11 deletions.
diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp
@@ -42,6 +42,8 @@ class DynamicLayer : public Layer
     min_laser_range_ = node_->declare_parameter<double>("dynamic_layer.min_laser_range");
     max_laser_range_ = node_->declare_parameter<double>("dynamic_layer.max_laser_range");
 
+    update_map_origin_ = node_->declare_parameter<bool>("dynamic_layer.update_map_origin", false);
+
     global_frame_ = node_->declare_parameter<std::string>("dynamic_layer.global_frame");
     const std::string scan_topic =
       node_->declare_parameter<std::string>("dynamic_layer.scan_topic");
@@ -74,19 +76,29 @@ class DynamicLayer : public Layer
     projection_.projectLaser(*scan_, cloud_msg);
 
     // get robot pose
-    geometry_msgs::msg::TransformStamped map_to_sensor_frame;
-    if (!get_transform(global_frame_, scan_->header.frame_id, map_to_sensor_frame)) {
+    geometry_msgs::msg::TransformStamped transform_frame;
+    if (!get_transform(global_frame_, scan_->header.frame_id, transform_frame)) {
       RCLCPP_ERROR_STREAM(node_->get_logger(), "Can not get frame.");
       return false;
     }
 
+    // update map origin
+    if (update_map_origin_) {
+      const double size_x = master_costmap.info.width * master_costmap.info.resolution;
+      const double size_y = master_costmap.info.height * master_costmap.info.resolution;
+      master_costmap.info.origin.position.x =
+        transform_frame.transform.translation.x - size_x / 2.0;
+      master_costmap.info.origin.position.y =
+        transform_frame.transform.translation.y - size_y / 2.0;
+    }
+
     pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::fromROSMsg(cloud_msg, *laser_cloud);
 
     // transform laser cloud
     if (scan_->header.frame_id != global_frame_) {
-      const Eigen::Affine3d affine = tf2::transformToEigen(map_to_sensor_frame);
+      const Eigen::Affine3d affine = tf2::transformToEigen(transform_frame);
       const Eigen::Matrix4f matrix = affine.matrix().cast<float>();
       pcl::transformPointCloud(*laser_cloud, *transform_cloud, matrix);
     } else {
@@ -141,6 +153,8 @@ class DynamicLayer : public Layer
 
   double min_laser_range_;
   double max_laser_range_;
+
+  bool update_map_origin_;
 };
 
 #endif
diff --git a/navyu_navigation/config/navyu_params.yaml b/navyu_navigation/config/navyu_params.yaml
@@ -16,8 +16,8 @@ navyu_global_costmap_node:
 
 navyu_local_costmap_node:
   ros__parameters:
-    update_frequency: 1.0
-    global_frame_id: base_link
+    update_frequency: 5.0
+    global_frame_id: odom
     width: 5
     height: 5
     resolution: 0.05
@@ -27,7 +27,8 @@ navyu_local_costmap_node:
       robot_radius: 0.22
     dynamic_layer:
       scan_topic: scan
-      global_frame: base_link
+      global_frame: odom
+      update_map_origin: true
       min_laser_range: 0.0
       max_laser_range: 5.0
 

diff --git a/navyu_navigation/rviz/navyu.rviz b/navyu_navigation/rviz/navyu.rviz
@@ -6,7 +6,6 @@ Panels:
       Expanded:
         - /Global Options1
         - /Grid1
-        - /Local Costmap1/Status1
         - /Local Costmap1/Topic1
       Splitter Ratio: 0.5
     Tree Height: 1079
@@ -250,7 +249,7 @@ Visualization Manager:
     - Alpha: 0.699999988079071
       Class: rviz_default_plugins/Map
       Color Scheme: costmap
-      Draw Behind: false
+      Draw Behind: true
       Enabled: true
       Name: Global Costmap
       Topic:
@@ -458,11 +457,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 124.54267883300781
+      Scale: 156.22634887695312
       Target Frame: base_link
       Value: TopDownOrtho (rviz_default_plugins)
-      X: 1.049617052078247
-      Y: -0.3355798125267029
+      X: 1.2739816904067993
+      Y: -0.17563483119010925
     Saved: ~
 Window Geometry:
   Displays: