transform and publish footprint

cmraaron · Apr 24, 2023 · 02c1fce · 02c1fce
1 parent 460329b
commit 02c1fce
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp
@@ -120,6 +120,11 @@ int main(int argc, char * argv[])
         }
       }
       broadcast_tf();
+      geometry_msgs::msg::PolygonStamped oriented_footprint;
+      nav2_costmap_2d::transformFootprint(
+        pos_active.x, pos_active.y, pos_active.theta, footprint,
+        oriented_footprint);
+      local_footprint_pub->publish(oriented_footprint);
     });
 
   const auto nav_to_pose_action_service = rclcpp_action::create_server<NavigateToPose>(