[WIP] World Model refactor and new features

nav2_bringup/README.md

@@ -54,6 +54,7 @@ Set the World Model and the two costmap nodes to use simulation time
 ```
 ros2 param set /world_model use_sim_time True
 ros2 param set /global_costmap/global_costmap use_sim_time True
+ros2 param set /robot_centric_costmap/robot_centric_costmap use_sim_time True
 ros2 param set /local_costmap/local_costmap use_sim_time True
 ```
 
@@ -113,7 +114,10 @@ ros2 launch nav2_bringup nav2_bringup_launch.py map:=<full/path/to/map.yaml>
 If running in simulation:
 ```
 ros2 launch nav2_bringup nav2_bringup_launch.py map:=<full/path/to/map.yaml> use_sim_time:=True
-ros2 param set /world_model use_sim_time True; ros2 param set /global_costmap/global_costmap use_sim_time True; ros2 param set /local_costmap/local_costmap use_sim_time True
+ros2 param set /world_model use_sim_time True
+ros2 param set /global_costmap/global_costmap use_sim_time True
+ros2 param set /robot_centric_costmap/robot_centric_costmap use_sim_time True
+ros2 param set /local_costmap/local_costmap use_sim_time True
 ```
 
 ## Future Work

nav2_bringup/config/nav2_default_view.rviz

@@ -9,9 +9,10 @@ Panels:
         - /TF1/Frames1
         - /TF1/Tree1
         - /Global Planner1
-        - /Global Planner1/Global Costmap1
-      Splitter Ratio: 0.5833333134651184
-    Tree Height: 574
+        - /Local Planner1
+        - /World Model1
+      Splitter Ratio: 0.583333313
+    Tree Height: 640
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -76,10 +77,18 @@ Visualization Manager:
           Value: true
         base_scan:
           Value: false
+        caster_back_link:
+          Value: false
+        imu_link:
+          Value: false
         map:
           Value: true
         odom:
           Value: true
+        wheel_left_link:
+          Value: false
+        wheel_right_link:
+          Value: false
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -89,7 +98,17 @@ Visualization Manager:
         map:
           odom:
             base_footprint:
-              {}
+              base_link:
+                base_scan:
+                  {}
+                caster_back_link:
+                  {}
+                imu_link:
+                  {}
+                wheel_left_link:
+                  {}
+                wheel_right_link:
+                  {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -180,16 +199,6 @@ Visualization Manager:
       Value: true
     - Class: rviz_common/Group
       Displays:
-        - Alpha: 0.699999988
-          Class: rviz_default_plugins/Map
-          Color Scheme: map
-          Draw Behind: true
-          Enabled: false
-          Name: Global Costmap
-          Topic: /global_costmap/costmap
-          Unreliable: false
-          Use Timestamp: false
-          Value: false
         - Class: rviz_default_plugins/Marker
           Enabled: true
           Name: PathEndPoints
@@ -198,7 +207,7 @@ Visualization Manager:
           Queue Size: 10
           Topic: /endpoints
           Unreliable: false
-          Value: false
+          Value: true
         - Alpha: 1
           Buffer Length: 1
           Class: rviz_default_plugins/Path
@@ -225,18 +234,18 @@ Visualization Manager:
         - Alpha: 1
           Class: rviz_default_plugins/Polygon
           Color: 25; 255; 0
-          Enabled: false
+          Enabled: true
           Name: Polygon
           Topic: /global_costmap/footprint
           Unreliable: false
-          Value: false
+          Value: true
       Enabled: true
       Name: Global Planner
     - Class: rviz_common/Group
       Displays:
-        - Alpha: 0.699999988
+        - Alpha: 0.5
           Class: rviz_default_plugins/Map
-          Color Scheme: costmap
+          Color Scheme: map
           Draw Behind: false
           Enabled: true
           Name: Local Costmap
@@ -286,6 +295,47 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Local Planner
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.5
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Global Costmap
+          Topic: /global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 0.5
+          Class: rviz_default_plugins/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Robot Centric Costmap
+          Topic: /robot_centric_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Polygon
+          Topic: /robot_centric_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Class: rviz_default_plugins/Marker
+          Enabled: true
+          Name: Query
+          Namespaces:
+            {}
+          Queue Size: 10
+          Topic: /query_region
+          Unreliable: false
+          Value: true
+      Enabled: true
+      Name: World Model
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -311,7 +361,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -1.57000005
+      Angle: -1.56999958
       Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
@@ -321,25 +371,25 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 134.638428
+      Scale: 150.21405
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
-      X: -0.0326152146
-      Y: -0.0801941454
+      X: 0.0214617159
+      Y: -0.0895999148
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 914
+  Height: 995
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034c000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000034cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000034c000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003840000034c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000018f00000384fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000384000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003f5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003f5000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004640000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1545
-  X: 2085
-  Y: 124
+  Width: 1529
+  X: 632
+  Y: 206

nav2_bringup/launch/nav2_params.yaml

@@ -47,3 +47,22 @@ global_costmap:
         max_obstacle_height: 2.0
         clearing: True
         marking: True
+robot_centric_costmap:
+  robot_centric_costmap:
+    ros__parameters:
+      width: 5
+      height: 5
+      origin_x: -2.5
+      origin_y: -2.5
+      plugin_names: ["obstacle_layer", "inflation_layer"]
+      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+      global_frame: "base_link"
+      obstacle_layer:
+        enabled: True
+      always_send_full_costmap: True
+      observation_sources: scan
+      scan:
+        topic: /scan
+        max_obstacle_height: 2.0
+        clearing: True
+        marking: True

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -55,6 +55,9 @@ namespace nav2_costmap_2d
 // convenient for storing x/y point pairs
 struct MapLocation
 {
+  MapLocation(const unsigned int x, const unsigned int y)
+  : x(x), y(y) {}
+
   unsigned int x;
   unsigned int y;
 };
@@ -483,7 +486,7 @@ class Costmap2D
     // just push the relevant cells back onto the list
     inline void operator()(unsigned int offset)
     {
-      MapLocation loc;
+      MapLocation loc{0, 0};
       costmap_.indexToCells(offset, loc.x, loc.y);
       cells_.push_back(loc);
     }

nav2_costmap_2d/src/costmap_2d.cpp

@@ -327,7 +327,7 @@ bool Costmap2D::setConvexPolygonCost(
   // we need to transform it to map coordinates
   std::vector<MapLocation> map_polygon;
   for (unsigned int i = 0; i < polygon.size(); ++i) {
-    MapLocation loc;
+    MapLocation loc{0, 0};
     if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
       // ("Polygon lies outside map bounds, so we can't fill it");
       return false;
@@ -377,7 +377,7 @@ void Costmap2D::convexFillCells(
   polygonOutlineCells(polygon, polygon_cells);
 
   // quick bubble sort to sort points by x
-  MapLocation swap;
+  MapLocation swap{0, 0};
   unsigned int i = 0;
   while (i < polygon_cells.size() - 1) {
     if (polygon_cells[i].x > polygon_cells[i + 1].x) {
@@ -394,8 +394,8 @@ void Costmap2D::convexFillCells(
   }
 
   i = 0;
-  MapLocation min_pt;
-  MapLocation max_pt;
+  MapLocation min_pt{0, 0};
+  MapLocation max_pt{0, 0};
   unsigned int min_x = polygon_cells[0].x;
   unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;
 
@@ -423,7 +423,7 @@ void Costmap2D::convexFillCells(
       ++i;
     }
 
-    MapLocation pt;
+    MapLocation pt{0, 0};
     // loop though cells in the column
     for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
       pt.x = x;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -441,6 +441,12 @@ bool Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) c
   robot_pose.header.stamp = rclcpp::Time();
 
   rclcpp::Time current_time = node_->now();  // save time for checking tf delay later
+
+  if (robot_base_frame_ == global_frame_) {
+    // Robot is at the origin
+    return true;
+  }
+
   // get the global pose of the robot
   try {
     tf_.transform(robot_pose, global_pose, global_frame_);

nav2_motion_primitives/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(nav2_tasks REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_robot REQUIRED)
+find_package(nav2_world_model REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -30,6 +31,7 @@ set(dependencies
   nav2_tasks
   nav2_msgs
   nav2_robot
+  nav2_world_model
   nav_msgs
   tf2
   tf2_geometry_msgs

nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp

@@ -34,10 +34,10 @@ class BackUp : public MotionPrimitive<nav2_tasks::BackUpCommand, nav2_tasks::Bac
 
   nav2_tasks::TaskStatus onCycleUpdate(nav2_tasks::BackUpResult & result) override;
 
+  bool pathIsClear() override;
+
 protected:
-  double min_linear_vel_;
-  double max_linear_vel_;
-  double linear_acc_lim_;
+  geometry_msgs::msg::Twist default_vel_;
 
   nav_msgs::msg::Odometry::SharedPtr initial_pose_;
   double command_x_;