    [map_server-1] 	map_server lifecycle node launched.
    [map_server-1] 	Waiting on external lifecycle transitions to activate
    [map_server-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [map_server-1] [INFO] [1686316127.473589303] [map_server]: Creating
    [planner_server-6] [INFO] [1686316127.475723769] [planner_server]:
    [planner_server-6] 	planner_server lifecycle node launched.
    [planner_server-6] 	Waiting on external lifecycle transitions to activate
    [planner_server-6] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [planner_server-6] [INFO] [1686316127.483253780] [planner_server]: Creating
    [lifecycle_manager-3] [INFO] [1686316127.486173802] [lifecycle_manager_localization]: Starting managed nodes bringup...
    [lifecycle_manager-3] [INFO] [1686316127.486978141] [lifecycle_manager_localization]: Configuring map_server
    [planner_server-6] [INFO] [1686316127.504156159] [global_costmap.global_costmap]:
    [planner_server-6] 	global_costmap lifecycle node launched.
    [planner_server-6] 	Waiting on external lifecycle transitions to activate
    [planner_server-6] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [planner_server-6] [INFO] [1686316127.506386794] [global_costmap.global_costmap]: Creating Costmap
    [map_server-1] [INFO] [1686316127.524907822] [map_server]: Configuring
    [map_server-1] [INFO] [map_io]: Loading yaml file: /home/bot/Downloads/map/map.yaml
    [map_server-1] [DEBUG] [map_io]: resolution: 0.05
    [map_server-1] [DEBUG] [map_io]: origin[0]: -26.8
    [map_server-1] [DEBUG] [map_io]: origin[1]: -3.56
    [map_server-1] [DEBUG] [map_io]: origin[2]: 0
    [map_server-1] [DEBUG] [map_io]: free_thresh: 0.25
    [map_server-1] [DEBUG] [map_io]: occupied_thresh: 0.65
    [map_server-1] [DEBUG] [map_io]: mode: trinary
    [map_server-1] [DEBUG] [map_io]: negate: 0
    [map_server-1] [INFO] [map_io]: Loading image_file: /home/bot/Downloads/map/maphallway.pgm
    [behavior_server-7] [INFO] [1686316127.576239915] [behavior_server]:
    [behavior_server-7] 	behavior_server lifecycle node launched.
    [behavior_server-7] 	Waiting on external lifecycle transitions to activate
    [behavior_server-7] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [lifecycle_manager-11] [INFO] [1686316127.577538824] [lifecycle_manager_navigation]: Creating
    [waypoint_follower-9] [INFO] [1686316127.580612526] [waypoint_follower]:
    [bt_navigator-8] [INFO] [1686316127.580790134] [bt_navigator]:
    [bt_navigator-8] 	bt_navigator lifecycle node launched.
    [bt_navigator-8] 	Waiting on external lifecycle transitions to activate
    [bt_navigator-8] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [bt_navigator-8] [INFO] [1686316127.580902129] [bt_navigator]: Creating
    [lifecycle_manager-11] [INFO] [1686316127.581034352] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
    [velocity_smoother-10] [INFO] [1686316127.582633174] [velocity_smoother]:


edited


* [ The ReadME Project  GitHub community articles  ](/readme)


> Closed is whether to connect the start/end points in the polygon, but
> doesn't say if we want to fill the interior or not.


None yet


We read every piece of feedback, and take your input very seriously.


We use multiple robots in a shared environment. There are some cases in which
the onboard-sensors of robot A cannot fully see robot B. Thus, robots share
their location (and planned path) with each other. Now, to help with local and
global planning, we want to put the footprint of robot B on the costmap of
robot A.


* Compute complete coverage path


Sorry, something went wrong.


[ Sign in ](/login?return_to=https%3A%2F%2Fgithub.com%2Fros-
planning%2Fnavigation2%2Fissues%2F3017)


> I think a unit8 would be fine, 32 bits seems excessive.


      * Smoothers
      * Waypoint Following
    * State Estimation
      * Standards
      * Global Positioning: Localization and SLAM
      * Odometry
    * Environmental Representation
      * Costmaps and Layers
      * Costmap Filters
      * Other Forms
    * Nav2 Academic Overview
  * [ First-Time Robot Setup Guide ](../setup_guides/index.html)
    * [ Setting Up Transformations ](../setup_guides/transformation/setup_transforms.html)
      * [ Transforms Introduction ](../setup_guides/transformation/setup_transforms.html#transforms-introduction)
      * [ Static Transform Publisher Demo ](../setup_guides/transformation/setup_transforms.html#static-transform-publisher-demo)
      * [ Transforms in Navigation2 ](../setup_guides/transformation/setup_transforms.html#transforms-in-navigation2)
      * [ Conclusion ](../setup_guides/transformation/setup_transforms.html#conclusion)
    * [ Setting Up The URDF ](../setup_guides/urdf/setup_urdf.html)
      * [ URDF and the Robot State Publisher ](../setup_guides/urdf/setup_urdf.html#urdf-and-the-robot-state-publisher)
      * [ Setting Up the Environment ](../setup_guides/urdf/setup_urdf.html#setting-up-the-environment)
      * [ Writing the URDF ](../setup_guides/urdf/setup_urdf.html#writing-the-urdf)
      * [ Build and Launch ](../setup_guides/urdf/setup_urdf.html#build-and-launch)
      * [ Visualization using RVIZ ](../setup_guides/urdf/setup_urdf.html#visualization-using-rviz)
      * [ Adding Physical Properties ](../setup_guides/urdf/setup_urdf.html#adding-physical-properties)
      * [ Conclusion ](../setup_guides/urdf/setup_urdf.html#conclusion)
    * [ Setting Up Odometry ](../setup_guides/odom/setup_odom.html)
      * [ Odometry Introduction ](../setup_guides/odom/setup_odom.html#odometry-introduction)
      * [ Setting Up Odometry on your Robot ](../setup_guides/odom/setup_odom.html#setting-up-odometry-on-your-robot)
      * [ Simulating an Odometry System using Gazebo ](../setup_guides/odom/setup_odom.html#simulating-an-odometry-system-using-gazebo)
        * [ Setup and Prerequisites ](../setup_guides/odom/setup_odom.html#setup-and-prerequisites)
        * [ Adding Gazebo Plugins to a URDF ](../setup_guides/odom/setup_odom.html#adding-gazebo-plugins-to-a-urdf)
        * [ Launch and Build Files ](../setup_guides/odom/setup_odom.html#launch-and-build-files)
        * [ Build, Run and Verification ](../setup_guides/odom/setup_odom.html#build-run-and-verification)
      * [ Robot Localization Demo ](../setup_guides/odom/setup_odom.html#robot-localization-demo)
        * [ Configuring Robot Localization ](../setup_guides/odom/setup_odom.html#configuring-robot-localization)
        * [ Launch and Build Files ](../setup_guides/odom/setup_odom.html#id3)
        * [ Build, Run and Verification ](../setup_guides/odom/setup_odom.html#id4)
      * [ Conclusion ](../setup_guides/odom/setup_odom.html#conclusion)
    * [ Setting Up Sensors ](../setup_guides/sensors/setup_sensors.html)
      * [ Sensor Introduction ](../setup_guides/sensors/setup_sensors.html#sensor-introduction)


All reactions


**[ tonynajjar ](/tonynajjar) ** commented  Jun 15, 2022  •


> ` priority ` could be removed with a bool for ` use_max ` or similar to
> indicate if we should use the maximum or minimum value for adding the shape
> to the map.


Yes, but as I understand costmaps are using ` OccupancyGrid ` -s to produce `
Costmap2D ` maps by it. Everything here is tied to costmap update rate.
By this cons, I mean the case, if we want to add vector objects on `
OccupancyGrid ` map generally. w/o referencing costmaps at all. An example
might be produced by AMCL map with some missed by lidar objects (like glass
wall); the map we want to save to a file (by map server) for further re-use.
Costmaps are not designed for that.
But anyway, w/o thinking about examples; having something operating with
OccGrids (and Costmaps through a StaticLayer) separately, is conceptually have
a wider ability of applications, than costmap-based layer only. That was the
meaning of the cons for D2.


Action servers are similar to a canonical service server. A client will
request some task to be completed, except, this task may take a long time. An
example would be moving the shovel up from a bulldozer or ask a robot to
travel 10 meters to the right.


**[ SteveMacenski ](/SteveMacenski) ** commented  Jan 31, 2023  •


I work on the same project as [ @tonynajjar ](https://github.com/tonynajjar)
and can give some context on our use:


* Follow a path


[
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=40&u=0925d37e9d3569c71d930114b66e6394c2efb3c6&v=4)
](/SteveMacenski) [ SteveMacenski ](/SteveMacenski) added the [ question
](/ros-planning/navigation2/labels/question) Further information is requested
label  Jun 9, 2023


> StaticLayer


[ ALL ](/questions/scope:all/sort:activity-desc/page:1/) [ UNANSWERED
](/questions/scope:unanswered/sort:answers-asc/page:1/)


I am currently writing my own custom costmap plugin for use with the
navigation2 stack. I'm trying to have the costmap layer take the navigation
goal into account when updating costs near it. I've been trying to do this by
adding a subscriber to the custom costmap layer that will pull the position of
the navigation goal when it is set.


Pick a username


In the second, the ` nav2_waypoint_follower  ` is a nice sample application /
proof of concept, but you really need your waypoint following / autonomy
system on the robot to carry more weight in making a robust solution. In this
case, you should use the ` nav2_behavior_tree  ` package to create a custom
application-level behavior tree using navigation to complete the task. This
can include subtrees like checking for the charge status mid-task for
returning to dock or handling more than 1 unit of work in a more complex task.
Soon, there will be a ` nav2_bt_waypoint_follower  ` (name subject to
adjustment) that will allow you to create this application more easily. In
this school of thought, the waypoint following application is more closely
tied to the system autonomy, or in many cases, is the system autonomy.


Password


  * Proc. Design versatility, allowing not only to add vector objects on costmaps, but everywhere, incl. Nav2 maps and Costmap Filters
  * Cons1. Performance. The whole map is being stored, updated with VO and then trasferred to the costmaps. VO-Server has no knowledge about rolling Costmap window, so it could operate only with whole maps. This might be a real issue for huge outdoor/warehouses maps and high costmap update frequencies.
  * Cons2. Concept issue: initially we are talking about static/dynamic VO affecting only costmaps. OccupancyGrid maps - are intentionally raster maps, used in Nav2 and in Costmap Filters as well. So, there is no clear reason (from the use-cases), why do we need to change these concepts?


     ros2 launch tiago_2dnav tiago_nav_bringup.launch.py  map:=/home/bot/Downloads/map/map.yaml use_sim_time:=true
    [INFO] [launch]: All log files can be found below /home/bot/.ros/log/2023-06-09-15-08-46-053379-bot-37395
    [INFO] [launch]: Default logging verbosity is set to INFO
    [INFO] [map_server-1]: process started with pid [37407]
    [INFO] [amcl-2]: process started with pid [37409]
    [INFO] [lifecycle_manager-3]: process started with pid [37411]
    [INFO] [controller_server-4]: process started with pid [37413]
    [INFO] [smoother_server-5]: process started with pid [37415]
    [INFO] [planner_server-6]: process started with pid [37417]
    [INFO] [behavior_server-7]: process started with pid [37419]
    [INFO] [bt_navigator-8]: process started with pid [37421]
    [INFO] [waypoint_follower-9]: process started with pid [37423]
    [INFO] [velocity_smoother-10]: process started with pid [37425]
    [INFO] [lifecycle_manager-11]: process started with pid [37427]
    [INFO] [rviz2-12]: process started with pid [37429]
    [lifecycle_manager-3] [INFO] [1686316127.394525752] [lifecycle_manager_localization]: Creating
    [amcl-2] [INFO] [1686316127.396533449] [amcl]:
    [amcl-2] 	amcl lifecycle node launched.
    [amcl-2] 	Waiting on external lifecycle transitions to activate
    [amcl-2] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [amcl-2] [INFO] [1686316127.398174565] [amcl]: Creating
    [smoother_server-5] [INFO] [1686316127.402320182] [smoother_server]:
    [smoother_server-5] 	smoother_server lifecycle node launched.
    [smoother_server-5] 	Waiting on external lifecycle transitions to activate
    [smoother_server-5] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [lifecycle_manager-3] [INFO] [1686316127.406322281] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
    [controller_server-4] [INFO] [1686316127.411335524] [controller_server]:
    [controller_server-4] 	controller_server lifecycle node launched.
    [controller_server-4] 	Waiting on external lifecycle transitions to activate
    [controller_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [smoother_server-5] [INFO] [1686316127.411661080] [smoother_server]: Creating smoother server
    [controller_server-4] [INFO] [1686316127.431395574] [controller_server]: Creating controller server
    [controller_server-4] [INFO] [1686316127.454978669] [local_costmap.local_costmap]:
    [controller_server-4] 	local_costmap lifecycle node launched.
    [controller_server-4] 	Waiting on external lifecycle transitions to activate
    [controller_server-4] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
    [controller_server-4] [INFO] [1686316127.456992135] [local_costmap.local_costmap]: Creating Costmap
    [map_server-1] [INFO] [1686316127.471440644] [map_server]:


##  1  Answer


Sorry, something went wrong.


**Have a question about this project?** Sign up for a free GitHub account to
open an issue and contact its maintainers and the community.


[ ![@dharanish-
nh](https://avatars.githubusercontent.com/u/15521002?s=80&u=ef939dba8902fa59a5ab3a6fceb06a2486ead81a&v=4)
](/dharanish-nh)


Isn't idea with centralized VO-server, publisging whole rasterized OccGrid map
for the polygons (D1 architecture), will have a performance pitfalls comparing
to separate VO-layer (w/o VO-server architecture D2) in the costmap?

---

All reactions


  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);


Copy link


> Assuming the CPU or latency of regularly publishing that out isn't too high,
> then that occupancy grid is then subscribed to by the Costmap Filter plugins
> to interpret the polygons files' shape values to perform some action (e.g.
> speed, keepout, higher cost, lanes, whatever).


I don't think any external server is required, but is certainly an option. We
could have the layer simply take in a file itself which populates the
information. A centralized server for the maps are helpful because there are
many consumers of the map (costmaps, localization, collision checkers,
visualization, etc). If we think there will be many consumers of the virtual
obstacle information, then a centralized server is sensible. Else, loading
from file is fine. The layer should be the one managing the set of polygons
and operating upon them, the only role of a server should be to serve that
information up to the layer from a file. Think of the obstacle layer: sensor
data is sent to populate in the layer itself (I think the idea of having more
processing in the external server was why you mentioned the ` OccupancyGrid `
/ ` Costmap ` message stuff?).


These methods may also produce other output including position topics, maps,
or other metadata, but they must provide that transformation to be valid.
Multiple positioning methods can be fused together using robot localization,
discussed more below.


* [ ros2-humble ](/questions/scope:all/sort:activity-desc/tags:ros2-humble/page:1/)


These three servers then expose an action interface corresponding to its task.
When the behavior tree ticks the corresponding BT node, it will call the
action server to process its task. The action server callback inside the
server will call the chosen algorithm by its name (e.g. ` FollowPath  ` ) that
maps to a specific algorithm. This allows a user to abstract the algorithm
used in the behavior tree to classes of algorithms. For instance, you can have
` N  ` plugin controllers to follow paths, dock with charger, avoid dynamic
obstacles, or interface with a tool. Having all of these plugins in the same
server allows the user to make use of a single environmental representation
object, which is costly to duplicate.


**[ AlexeyMerzlyakov ](/AlexeyMerzlyakov) ** commented  Feb 28, 2023  •


[ **answered 2023-06-15 15:37:42 -0500  ** ](/answers/416541/revisions/)


> [ @tonynajjar ](https://github.com/tonynajjar) so what's your plan here?
> Remake something using that as a reference or wait for it to be ported and
> we can go through some design reviews to patch it up?


> I think services would add too much latency here, in case robot B is moving.


However, this only fair if we have right model of usage/usecases for that.
So, it depends on what do we want:


###  Global Positioning: Localization and SLAM  ¶


##  ROS 2  ¶


Sort by »  [ oldest  ](/question/414211/adding-subscriber-to-custom-
costmap/?sort=oldest#sort-top) [ newest  ](/question/414211/adding-subscriber-
to-custom-costmap/?sort=latest#sort-top) [ most voted
](/question/414211/adding-subscriber-to-custom-costmap/?sort=votes#sort-top)


I figured this was because the Lifecycle node needed to be in the process of
being configured to create a subscriber. So the next thing I attempted to do
was define an on_configure callback to the node within the Layer class. I
attempted to do this via the register_on_configure method. I could not get my
types to match though. When I made a func function the method always
complained that it needed a type std::function<func>.


It is important to note that the behavior server can hold any behavior to
share access to expensive resources like costmaps or TF buffers, not just
recovery behaviors. Each may have their own API.


[ Multi Robot simulation on gazebo ](/question/360480/multi-robot-simulation-
on-gazebo/)


edited


Sorry, something went wrong.


I remember there were some points why each shape is better to describe in its
own file, were on the call. But I can not recall exactly is there any specific
point why we should do it, if we are using one VO-server per specific costmap
layer/filter?

---

All reactions


Thanks a lot for your insights [ @SteveMacenski
](https://github.com/SteveMacenski) . I have so far mostly taken inspiration
from the [ costmap_converter ](http://wiki.ros.org/costmap_converter) often
used with the TEB planner, which converts a costmap into polygons and
publishes them as a topic - and thought of what we want to do here as the
opposite direction.


As was expected, KeepoutFilter process time was not dependent on mask size, as
we are processing area inside rolling window.
When we have different mask and global frames, same mask and global frame, the
average mask processing time differs on the ~ similar values:


[ ![@jplapp](https://avatars.githubusercontent.com/u/17068999?s=80&v=4)
](/jplapp)


double wx, wy;


Closed


Labels


One reason this library is used is its ability to load subtrees. This means
that the Nav2 behavior tree can be loaded into another higher-level BT to use
this project as node plugin. An example would be in soccer play, using the
Nav2 behavior tree as the “go to ball” node with a ball detection as part of a
larger task. Additionally, we supply a ` NavigateToPoseAction  ` plugin (among
others) for BT so the Nav2 stack can be called from a client application
through the usual action interface.


Other systems could be used to design complex autonomous behavior, namely
Hierarchical FSMs (HFSM). Behavior Trees were selected due to popularity
across the robotics and related industries and by largely user demand.
However, due to the independent task server nature of Nav2, it is not
difficult to offer a ` nav2_hfsm_navigator  ` package in the future, pending
interest and contribution.


For


Both mask publishing and receiving times are directly proportional to mask
size. However, map publish time is ~40-100x times higher than map receive
time, so let's focus on it.
This linear dependency is being presented at the picture attached below:
[ ![map_frame](https://user-
images.githubusercontent.com/60094858/220634824-42ce0518-94b1-43ff-
bf7e-e534223ba330.png) ](https://user-
images.githubusercontent.com/60094858/220634824-42ce0518-94b1-43ff-
bf7e-e534223ba330.png)
Effectively, maximum mask size for having suitable dynamic update rate at
**~30Hz** \- is **20-25 Mpixels** (which is 5000x5000 pixels mask size, or
250x250 meters with ` 0.05 m resolution ` )


I've tried to use the existing Lifecycle node within the
nav2_costmap_2d::Layer class, but no matter what I do, I simply cannot get the
data types to match. When I first tried to create the subscriber within the
onInitialize method I wasn't able to build the plugin due to colcon
complaining that it could not find the subscriber (error looked like this):


PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)


* 👍  1 reaction


Collaborator


  * Load vector objects from YAML-file (e.g. as ROS-parameters)
  * Waiting for service requests and load vector objects from incoming services data
  * Adding/updating vector object from topics, as the [ discussion ](https://github.com/ros-planning/navigation2/issues/3017#issuecomment-1162526689) result:


First time here? Check out the FAQ!


Just as in ROS, action servers are a common way to control long running tasks
like navigation. This stack makes more extensive use of actions, and in some
cases, without an easy topic interface. It is more important to understand
action servers as a developer in ROS 2. Some simple CLI examples can be found
in the [ ROS 2 documentation
](https://docs.ros.org/en/rolling/Tutorials/Understanding-ROS2-Actions.html) .


* Open Source


Author


latest


However, it could be used for more than just a sample application. There are 2
schools of thoughts for fleet managers / dispatchers. \- Dumb robot; smart
centralized dispatcher \- Smart robot; dumb centralized dispatcher


* gradient maps, which are similar to costmaps but represent surface gradients to check traversibility over


> Add RPC instead of published msgs in order to have the possibility of
> deleting obstacles with uuid identification


#  Search code, repositories, users, issues, pull requests...


Sorry, something went wrong.


    * [ Topics ](/topics)
    * [ Trending ](/trending)
    * [ Collections ](/collections)


[
![@GMahmoud](https://avatars.githubusercontent.com/u/29754310?s=80&u=1493f619be29310ca5f6e336f7f72f4d945ab80f&v=4)
](/GMahmoud)


Performance estimation results of using centralized VO-server (duplicating
from Slack, just for future use):


#  Saved searches


  * mask preparation time on VO-server
  * OccupancyGrid mask publish time on VO-server side
  * OccupancyGrid mask receive time on KeepoutFilter side (with  [ Improve KeepoutFilter mask receiving performance  #3420  ](https://github.com/ros-planning/navigation2/pull/3420) enabled)
  * KeepoutFilter process time


[ Robot Localization ](https://github.com/cra-ros-pkg/robot_localization/) is
typically used for this fusion. It will take in ` N  ` sensors of various
types and provide a continuous and smooth odometry to TF and to a topic. A
typical mobile robotics setup may have odometry from wheel encoders, IMUs, and
vision fused in this manor.


[
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=80&u=0925d37e9d3569c71d930114b66e6394c2efb3c6&v=4)
](/SteveMacenski)


These action servers are used to host a map of algorithm plugins to complete
various tasks. They also host the environmental representation used by the
algorithm plugins to compute their outputs.


* Compute shortest path


Four of the action servers in this project are the planner, behavior, smoother
and controller servers.


        * [ 4- Run Your Custom plugin ](../plugin_tutorials/docs/writing_new_bt_plugin.html#run-your-custom-plugin)
    * [ Writing a New Behavior Plugin ](../plugin_tutorials/docs/writing_new_behavior_plugin.html)
      * [ Overview ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#overview)
      * [ Requirements ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#requirements)
      * [ Tutorial Steps ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#tutorial-steps)
        * [ 1- Creating a new Behavior Plugin ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#creating-a-new-behavior-plugin)
        * [ 2- Exporting the Behavior Plugin ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#exporting-the-behavior-plugin)
        * [ 3- Pass the plugin name through params file ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#pass-the-plugin-name-through-params-file)
        * [ 4- Run Behavior Plugin ](../plugin_tutorials/docs/writing_new_behavior_plugin.html#run-behavior-plugin)
    * [ Writing a New Navigator Plugin ](../plugin_tutorials/docs/writing_new_navigator_plugin.html)
      * [ Overview ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#overview)
      * [ Requirements ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#requirements)
      * [ Tutorial Steps ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#tutorial-steps)
        * [ 1- Create a new Navigator Plugin ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#create-a-new-navigator-plugin)
        * [ 2- Exporting the navigator plugin ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#exporting-the-navigator-plugin)
        * [ 3- Pass the plugin name through the params file ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#pass-the-plugin-name-through-the-params-file)
        * [ 4- Run plugin ](../plugin_tutorials/docs/writing_new_navigator_plugin.html#run-plugin)
  * [ Configuration Guide ](../configuration/index.html)
    * [ Behavior-Tree Navigator ](../configuration/packages/configuring-bt-navigator.html)
      * [ Parameters ](../configuration/packages/configuring-bt-navigator.html#parameters)
      * [ Example ](../configuration/packages/configuring-bt-navigator.html#example)
    * [ Behavior Tree XML Nodes ](../configuration/packages/configuring-bt-xml.html)
      * [ Action Plugins ](../configuration/packages/configuring-bt-xml.html#action-plugins)
        * [ Wait ](../configuration/packages/bt-plugins/actions/Wait.html)
        * [ Spin ](../configuration/packages/bt-plugins/actions/Spin.html)
        * [ BackUp ](../configuration/packages/bt-plugins/actions/BackUp.html)
        * [ DriveOnHeading ](../configuration/packages/bt-plugins/actions/DriveOnHeading.html)
        * [ AssistedTeleop ](../configuration/packages/bt-plugins/actions/AssistedTeleop.html)
        * [ ComputePathToPose ](../configuration/packages/bt-plugins/actions/ComputePathToPose.html)
        * [ FollowPath ](../configuration/packages/bt-plugins/actions/FollowPath.html)
        * [ NavigateToPose ](../configuration/packages/bt-plugins/actions/NavigateToPose.html)
        * [ ClearEntireCostmap ](../configuration/packages/bt-plugins/actions/ClearEntireCostmap.html)
        * [ ClearCostmapExceptRegion ](../configuration/packages/bt-plugins/actions/ClearCostmapExceptRegion.html)
        * [ ClearCostmapAroundRobot ](../configuration/packages/bt-plugins/actions/ClearCostmapAroundRobot.html)
        * [ ReinitializeGlobalLocalization ](../configuration/packages/bt-plugins/actions/ReinitializeGlobalLocalization.html)
        * [ TruncatePath ](../configuration/packages/bt-plugins/actions/TruncatePath.html)
        * [ TruncatePathLocal ](../configuration/packages/bt-plugins/actions/TruncatePathLocal.html)
        * [ PlannerSelector ](../configuration/packages/bt-plugins/actions/PlannerSelector.html)
        * [ ControllerSelector ](../configuration/packages/bt-plugins/actions/ControllerSelector.html)
        * [ SmootherSelector ](../configuration/packages/bt-plugins/actions/SmootherSelector.html)


    vo_server:
      ros__parameters:
        frame_id: "map"
        map_topic_name: "vectorized_map"
        use_max: True
        circles: ["CircleA"]
        polygons: ["PolyA", "PolyB"]
        CircleA:
          center: [0.1, 0.1]
          radius: 0.2
          priority: 1
          fill: true
          value: 0 # FREE_SPACE
          UUID:  78985aa8-a353-4164-9fb1-0219795ec176
        ...
        PolyA:
          points: [1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0]
          priority: 2
          closed: true
          fill: true
          value: 127 # somewhere in the middle
          UUID: c785690c-286b-4030-b9c4-cc4474b840f0
        PolyB:
          points: [0.5, 0.5, 0.5, 1.5, 1.5, 1.5, 1.5, 0.5]
          priority: 2
          closed: true
          fill: false
          value: 254 # LETHAL_OBSTACLE
          UUID: 3ad2b866-bd7f-4e21-af35-bb79d1e9c507


    for (int i = min_i; i < max_i; ++i) {
      for (int j = min_j; j < max_j; ++j) {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf2::Vector3 p(wx, wy, 0);
        p = tf2_transform * p;
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my)) {
          if (!use_maximum_) {
            master_grid.setCost(i, j, getCost(mx, my));
          } else {
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
          }
        }
      }
    }
  }
  current_ = true;
}


Regarding the production use [ @SteveMacenski
](https://github.com/SteveMacenski) , I think people are using this plugin
differently. Personally, I am using it (not with demo project) to define ` no-
access-zone ` or to help the robot to avoid some ` undetectable ` obstacles
via a web application.


            map_subscribe_transient_local: True
          inflation_layer:
            plugin: "nav2_costmap_2d::InflationLayer"
            cost_scaling_factor: 2.0
            inflation_radius: 0.45
          social_layer:
            plugin: "nav2_social_costmap_plugin::SocialLayer"
            enabled: True
            cutoff: 10.0
            amplitude: 255.0
            publish_occgrid: False
            use_passing: True
            use_vel_factor: True
            speed_factor_multiplier: 5.0
            covariance_front_height: 0.4
            covariance_front_width: 0.25
            covariance_rear_height: 0.25
            covariance_rear_width: 0.25
            covariance_right_height: 0.3
            covariance_right_width: 0.2
            covariance_when_still: 0.25
          always_send_full_costmap: True

---

The text was updated successfully, but these errors were encountered:


  rclcpp::QoS map_qos(10);  // initialize to default
  if (map_subscribe_transient_local_) {
    map_qos.transient_local();
    map_qos.reliable();
    map_qos.keep_last(1);
  }


  * This has analogs elsewhere in the stack so it fits into the general workflow established (e.g. inflation, obstacle)
  * It doesn't result in smaller, kb sized messages from file resulting in larger mb sized messages communicated at a more regular frequency for updates at runtime. Large messages are a bit of an issue in ROS 2 still in out-of-the-box DDS configs (and even after fixing, much higher CPU requirements) and likely in the foreseeable future, so as much as that can be minimized is good. Running a service call to update or remove a shape is a relatively small operation to other things happening in costmap layers like transforming dense pointclouds or performing hundreds of raycasting runs on laser scans.
  * I'm not 100% sure how the server would know what resolution of occupancy grid to send over to the costmap layer for use. What would happen if the global/local costmaps used different resolutions?


Sorry, something went wrong.


Copy link


[
![@tonynajjar](https://avatars.githubusercontent.com/u/14879660?s=80&u=e29d50db1bac036eae32b30fec612615ccd3791a&v=4)
](/tonynajjar)


#  Provide feedback


    * [ Collision Monitor ](../configuration/packages/configuring-collision-monitor.html)
      * [ Features ](../configuration/packages/configuring-collision-monitor.html#features)
      * [ Parameters ](../configuration/packages/configuring-collision-monitor.html#parameters)
        * [ Polygons parameters ](../configuration/packages/configuring-collision-monitor.html#polygons-parameters)
        * [ Observation sources parameters ](../configuration/packages/configuring-collision-monitor.html#observation-sources-parameters)
      * [ Example ](../configuration/packages/configuring-collision-monitor.html#example)
    * [ Waypoint Follower ](../configuration/packages/configuring-waypoint-follower.html)
      * [ Parameters ](../configuration/packages/configuring-waypoint-follower.html#parameters)
      * [ Provided Plugins ](../configuration/packages/configuring-waypoint-follower.html#provided-plugins)
        * [ WaitAtWaypoint ](../configuration/packages/nav2_waypoint_follower-plugins/wait_at_waypoint.html)
        * [ PhotoAtWaypoint ](../configuration/packages/nav2_waypoint_follower-plugins/photo_at_waypoint.html)
        * [ InputAtWaypoint ](../configuration/packages/nav2_waypoint_follower-plugins/input_at_waypoint.html)
      * [ Default Plugin ](../configuration/packages/configuring-waypoint-follower.html#default-plugin)
      * [ Example ](../configuration/packages/configuring-waypoint-follower.html#example)
  * [ Tuning Guide ](../tuning/index.html)
    * [ Inflation Potential Fields ](../tuning/index.html#inflation-potential-fields)
    * [ Robot Footprint vs Radius ](../tuning/index.html#robot-footprint-vs-radius)
    * [ Rotate in Place Behavior ](../tuning/index.html#rotate-in-place-behavior)
    * [ Planner Plugin Selection ](../tuning/index.html#planner-plugin-selection)
    * [ Controller Plugin Selection ](../tuning/index.html#controller-plugin-selection)
    * [ Caching Obstacle Heuristic in Smac Planners ](../tuning/index.html#caching-obstacle-heuristic-in-smac-planners)
    * [ Nav2 Launch Options ](../tuning/index.html#nav2-launch-options)
    * [ Other Pages We’d Love To Offer ](../tuning/index.html#other-pages-we-d-love-to-offer)
  * [ Nav2 Behavior Trees ](../behavior_trees/index.html)
    * [ Introduction To Nav2 Specific Nodes ](../behavior_trees/overview/nav2_specific_nodes.html)
      * [ Action Nodes ](../behavior_trees/overview/nav2_specific_nodes.html#action-nodes)
      * [ Condition Nodes ](../behavior_trees/overview/nav2_specific_nodes.html#condition-nodes)
      * [ Decorator Nodes ](../behavior_trees/overview/nav2_specific_nodes.html#decorator-nodes)
      * [ Control: PipelineSequence ](../behavior_trees/overview/nav2_specific_nodes.html#control-pipelinesequence)
      * [ Control: Recovery ](../behavior_trees/overview/nav2_specific_nodes.html#control-recovery)
      * [ Control: RoundRobin ](../behavior_trees/overview/nav2_specific_nodes.html#control-roundrobin)
    * [ Detailed Behavior Tree Walkthrough ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html)
      * [ Overview ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html#overview)
      * [ Prerequisites ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html#prerequisites)
      * [ Navigate To Pose With Replanning and Recovery ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html#navigate-to-pose-with-replanning-and-recovery)
      * [ Navigation Subtree ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html#navigation-subtree)
      * [ Recovery Subtree ](../behavior_trees/overview/detailed_behavior_tree_walkthrough.html#recovery-subtree)
    * [ Navigate To Pose ](../behavior_trees/trees/nav_to_pose_recovery.html)
    * [ Navigate Through Poses ](../behavior_trees/trees/nav_through_poses_recovery.html)
    * [ Navigate To Pose and Pause Near Goal-Obstacle ](../behavior_trees/trees/nav_to_pose_and_pause_near_goal_obstacle.html)


        * [ StoppedGoalChecker ](../configuration/packages/nav2_controller-plugins/stopped_goal_checker.html)
      * [ Default Plugins ](../configuration/packages/configuring-controller-server.html#default-plugins)
      * [ Example ](../configuration/packages/configuring-controller-server.html#example)
    * [ DWB Controller ](../configuration/packages/configuring-dwb-controller.html)
      * [ Controller ](../configuration/packages/configuring-dwb-controller.html#controller)
        * [ DWB Controller ](../configuration/packages/dwb-params/controller.html)
        * [ XYTheta Iterator ](../configuration/packages/dwb-params/iterator.html)
        * [ Kinematic Parameters ](../configuration/packages/dwb-params/kinematic.html)
        * [ Publisher ](../configuration/packages/dwb-params/visualization.html)
      * [ Plugins ](../configuration/packages/configuring-dwb-controller.html#plugins)
        * [ LimitedAccelGenerator ](../configuration/packages/dwb-plugins/limited_accel_generator.html)
        * [ StandardTrajectoryGenerator ](../configuration/packages/dwb-plugins/standard_traj_generator.html)
      * [ Trajectory Critics ](../configuration/packages/configuring-dwb-controller.html#trajectory-critics)
        * [ BaseObstacleCritic ](../configuration/packages/trajectory_critics/base_obstacle.html)
        * [ GoalAlignCritic ](../configuration/packages/trajectory_critics/goal_align.html)
        * [ GoalDistCritic ](../configuration/packages/trajectory_critics/goal_dist.html)
        * [ ObstacleFootprintCritic ](../configuration/packages/trajectory_critics/obstacle_footprint.html)
        * [ OscillationCritic ](../configuration/packages/trajectory_critics/oscillation.html)
        * [ PathAlignCritic ](../configuration/packages/trajectory_critics/path_align.html)
        * [ PathDistCritic ](../configuration/packages/trajectory_critics/path_dist.html)
        * [ PreferForwardCritic ](../configuration/packages/trajectory_critics/prefer_forward.html)
        * [ RotateToGoalCritic ](../configuration/packages/trajectory_critics/rotate_to_goal.html)
        * [ TwirlingCritic ](../configuration/packages/trajectory_critics/twirling.html)
      * [ Example ](../configuration/packages/configuring-dwb-controller.html#example)
    * [ Regulated Pure Pursuit ](../configuration/packages/configuring-regulated-pp.html)
      * [ Regulated Pure Pursuit Parameters ](../configuration/packages/configuring-regulated-pp.html#regulated-pure-pursuit-parameters)
      * [ Example ](../configuration/packages/configuring-regulated-pp.html#example)
    * [ Model Predictive Path Integral Controller ](../configuration/packages/configuring-mppic.html)
      * [ MPPI Parameters ](../configuration/packages/configuring-mppic.html#mppi-parameters)
        * [ Trajectory Visualization ](../configuration/packages/configuring-mppic.html#trajectory-visualization)
        * [ Path Handler ](../configuration/packages/configuring-mppic.html#path-handler)
        * [ Ackermann Motion Model ](../configuration/packages/configuring-mppic.html#ackermann-motion-model)
        * [ Constraint Critic ](../configuration/packages/configuring-mppic.html#constraint-critic)
        * [ Goal Angle Critic ](../configuration/packages/configuring-mppic.html#goal-angle-critic)
        * [ Goal Critic ](../configuration/packages/configuring-mppic.html#goal-critic)
        * [ Obstacles Critic ](../configuration/packages/configuring-mppic.html#obstacles-critic)
        * [ Path Align Critic ](../configuration/packages/configuring-mppic.html#path-align-critic)
        * [ Path Angle Critic ](../configuration/packages/configuring-mppic.html#path-angle-critic)
        * [ Path Follow Critic ](../configuration/packages/configuring-mppic.html#path-follow-critic)
        * [ Prefer Forward Critic ](../configuration/packages/configuring-mppic.html#prefer-forward-critic)


[ ](https://github.com/)


Not sure, why do we need to put each shape into separate file? If everything
will be described as one ROS-param YAML, having all-shapes in one list (like
we have for [ Collision Monitor
](https://navigation.ros.org/configuration/packages/configuring-collision-
monitor.html?highlight=collision%20monitor#example) ), could it be an option?


  * [ Terms ](https://docs.github.com/site-policy/github-terms/github-terms-of-service)
  * [ Privacy ](https://docs.github.com/site-policy/privacy-policies/github-privacy-statement)
  * [ Security ](/security)
  * [ Status ](https://www.githubstatus.com/)
  * [ Docs ](https://docs.github.com/)
  * [ Contact ](https://support.github.com?tags=dotcom-footer)
  * Manage cookies
  * Do not share my personal information


**[ jplapp ](/jplapp) ** commented  Jun 17, 2022


The ` nav2_waypoint_follower  ` contains a waypoint following program with a
plugin interface for specific task executors. This is useful if you need to go
to a given location and complete a specific task like take a picture, pick up
a box, or wait for user input. It is a nice demo application for how to use
Nav2 in a sample application.


[ enhancement  ](/ros-planning/navigation2/labels/enhancement) New feature or
request


{{ message }}


I'd like to avoid doubling the work with [ @GMahmoud
](https://github.com/GMahmoud) . To decide on a way forward [ @GMahmoud
](https://github.com/GMahmoud) can you let us know:


Thanks [ @tonynajjar ](https://github.com/tonynajjar) .


Assignees


[ Sign in ](/login?return_to=https%3A%2F%2Fgithub.com%2Fros-
planning%2Fnavigation2%2Fissues%2F3613)


|  [ Ask Your Question ](/questions/ask/)
---|---|---|---

0


[ Search syntax tips ](https://docs.github.com/search-github/github-code-
search/understanding-github-code-search-syntax)


I think that's out of scope of what we define as what a "costmap filter" is in
the documentation. Its a rastered map of cost. The vectorized information is
not a rastered map and there isn't the same kind of ` costmap filter info `
conversion to be done to convert costs into actions. But we could adjust that
definition if required.


**[ SteveMacenski ](/SteveMacenski) ** commented  Feb 28, 2023  •


##  Comments


Do you see an issue when you don't use that costmap layer?

---

All reactions


* Mesh maps, which are similar to gradient maps but with surface meshes at many angles


Sorry, something went wrong.


I might recommend just adding a "add" and "remove" service and part of the
request includes some string or int index for the objects. Not using pub/sub.


edited


Separate of this topic, but I also wonder if it would be nice to have a ` fill
` option, I think in some cases it wouldn't really matter to fill in the
polygon with cost, rather just make a solid costed outline so that the
planners / controllers cannot pass.

---

All reactions


[
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=80&u=0925d37e9d3569c71d930114b66e6394c2efb3c6&v=4)
](/SteveMacenski)


  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);


  * Support of non convex obstacles.
  * Add RPC instead of published msgs in order to have the possibility of deleting obstacles with id identification,
  * Add additional parameters to obstacles (lifetime, cost, random position, ...).
  * Better documentation and samples.
  * ROS2 compatibility.


* Keep-out/safety zones where robots will never enter.


* Product


[ Error when initializing trac_ik on Noetic ](/question/409387/error-when-
initializing-trac_ik-on-noetic/)


In the first, the ` nav2_waypoint_follower  ` is fully sufficient to create a
production-grade on-robot solution. Since the autonomy system / dispatcher is
taking into account things like the robot’s pose, battery level, current task,
and more when assigning tasks, the application on the robot just needs to
worry about the task at hand and not the other complexities of the system
complete the requested task. In this situation, you should think of a request
to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1
security patrole loop, 1 aisle, etc) to do a task and then return to the
dispatcher for the next task or request to recharge. In this school of
thought, the waypoint following application is just one step above navigation
and below the system autonomy application.


The major reasons why I'm thinking this design:


[ tonynajjar ](/tonynajjar) opened this issue  Jun 15, 2022  · 40 comments


[ Edit ](https://github.com/ros-planning/navigation.ros.org)


    * [ Navigate To Pose With Consistent Replanning And If Path Becomes Invalid ](../behavior_trees/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.html)
    * [ Follow Dynamic Point ](../behavior_trees/trees/follow_point.html)
    * [ Odometry Calibration ](../behavior_trees/trees/odometry_calibration.html)
  * [ Navigation Plugins ](../plugins/index.html)
    * [ Behavior-Tree Navigators ](../plugins/index.html#behavior-tree-navigators)
    * [ Costmap Layers ](../plugins/index.html#costmap-layers)
    * [ Costmap Filters ](../plugins/index.html#costmap-filters)
    * [ Controllers ](../plugins/index.html#controllers)
    * [ Planners ](../plugins/index.html#planners)
    * [ Smoothers ](../plugins/index.html#smoothers)
    * [ Behaviors ](../plugins/index.html#behaviors)
    * [ Waypoint Task Executors ](../plugins/index.html#waypoint-task-executors)
    * [ Goal Checkers ](../plugins/index.html#goal-checkers)
    * [ Progress Checkers ](../plugins/index.html#progress-checkers)
    * [ Behavior Tree Nodes ](../plugins/index.html#behavior-tree-nodes)
  * [ Migration Guides ](../migration/index.html)
    * [ Dashing to Eloquent ](../migration/Dashing.html)
      * [ New Packages ](../migration/Dashing.html#new-packages)
      * [ New Plugins ](../migration/Dashing.html#new-plugins)
      * [ Navigation2 Architectural Changes ](../migration/Dashing.html#navigation2-architectural-changes)
    * [ Eloquent to Foxy ](../migration/Eloquent.html)
      * [ General ](../migration/Eloquent.html#general)
      * [ Server Updates ](../migration/Eloquent.html#server-updates)
      * [ New Plugins ](../migration/Eloquent.html#new-plugins)
      * [ Map Server Re-Work ](../migration/Eloquent.html#map-server-re-work)
      * [ New Particle Filter Messages ](../migration/Eloquent.html#new-particle-filter-messages)
      * [ Selection of Behavior Tree in each navigation action ](../migration/Eloquent.html#selection-of-behavior-tree-in-each-navigation-action)
      * [ FollowPoint Capability ](../migration/Eloquent.html#followpoint-capability)
      * [ New Costmap Layer ](../migration/Eloquent.html#new-costmap-layer)
    * [ Foxy to Galactic ](../migration/Foxy.html)
      * [ NavigateToPose Action Feedback updates ](../migration/Foxy.html#navigatetopose-action-feedback-updates)
      * [ NavigateToPose BT-node Interface Changes ](../migration/Foxy.html#navigatetopose-bt-node-interface-changes)
      * [ NavigateThroughPoses and ComputePathThroughPoses Actions Added ](../migration/Foxy.html#navigatethroughposes-and-computepaththroughposes-actions-added)
      * [ ComputePathToPose BT-node Interface Changes ](../migration/Foxy.html#computepathtopose-bt-node-interface-changes)
      * [ ComputePathToPose Action Interface Changes ](../migration/Foxy.html#computepathtopose-action-interface-changes)
      * [ BackUp BT-node Interface Changes ](../migration/Foxy.html#backup-bt-node-interface-changes)
      * [ BackUp Recovery Interface Changes ](../migration/Foxy.html#backup-recovery-interface-changes)
      * [ Nav2 Controllers and Goal Checker Plugin Interface Changes ](../migration/Foxy.html#nav2-controllers-and-goal-checker-plugin-interface-changes)
      * [ FollowPath goal_checker_id attribute ](../migration/Foxy.html#followpath-goal-checker-id-attribute)
      * [ Groot Support ](../migration/Foxy.html#groot-support)


void
StaticLayer::onInitialize()
{
  global_frame_ = layered_costmap_->getGlobalFrameID();


Additional navigation options


std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());


  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }


###  Controllers  ¶


Copy link


[ ](https://github.com/)


There is **no** requirement on using a LIDAR on your robot to use the
navigation system. There is no requirement to use lidar-based collision
avoidance, localization, or slam. However, we do provide instructions and
support tried and true implementations of these things using lidars. You can
be equally as successful using a vision or depth based positioning system and
using other sensors for collision avoidance. The only requirement is that you
follow the standards below with your choice of implementation.


Open


Sorry, something went wrong.


> like intercepting maps going form Map Server and dynamically adding static
> of moving objects on it, e.g. for testing purposes


I don't think any priority is needed. If you have intersecting shapes, `
use_max ` or so would tell you the policy to deconflict (use the max or min
values). I don't see how any other policy would be sensible logically. If you
have 3 shapes overlapping of values 1, 2, and 3: what's a situation where
you'd want ` 2 ` to be the "right" answer? It wouldn't be for speed limits
(you'd want minimum) or keepout (you'd want maximum) or anything else we've
previously discussed having as a costmap filter / vector layer. I don't think
there are generally priorities in shapes. If I as a user draw 20 shapes on a
map to correspond to different zones, none have more priority or interest over
the others. They're all just keepout zones or speed restricted zones. If
however there are overlaps the values themselves correspond to what the
deconflicting policy should be: for keepout, use maximum value; for speed, use
minimum value.


When a node is started, it is in the unconfigured state, only processing the
node’s constructor which should **not** contain any ROS networking setup or
parameter reading. By the launch system, or the supplied lifecycle manager,
the nodes need to be transitioned to inactive by configuring. After, it is
possible to activate the node by transitioning through the activating stage.


  // resize costmap if size, resolution or origin do not match
  Costmap2D * master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
    master->getSizeInCellsY() != size_y ||
    master->getResolution() != new_map.info.resolution ||
    master->getOriginX() != new_map.info.origin.position.x ||
    master->getOriginY() != new_map.info.origin.position.y ||
    !layered_costmap_->isSizeLocked()))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
    layered_costmap_->resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x,
      new_map.info.origin.position.y,
      true);
  } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
    resolution_ != new_map.info.resolution ||
    origin_x_ != new_map.info.origin.position.x ||
    origin_y_ != new_map.info.origin.position.y)
  {
    // only update the size of the costmap stored locally in this layer
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
    resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x, new_map.info.origin.position.y);
  }


    [rviz2-12] Start navigation
    [rviz2-12] [INFO] [1686316135.218599374] [_]: NavigateToPose will be called using the BT Navigator's default behavior tree.
    [bt_navigator-8] [INFO] [1686316135.219099244] [bt_navigator]: Begin navigating from current location to (0.96, 0.10)
    [rviz2-12] Start navigation
    [rviz2-12] [INFO] [1686316212.467698125] [_]: NavigateToPose will be called using the BT Navigator's default behavior tree.
    [bt_navigator-8] [INFO] [1686316212.469291806] [bt_navigator]: Received goal preemption request
    [bt_navigator-8] [INFO] [1686316212.469554086] [bt_navigator]: Begin navigating from current location to (0.82, 0.78)


[
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=80&u=0925d37e9d3569c71d930114b66e6394c2efb3c6&v=4)
](/SteveMacenski)


No branches or pull requests


[ ![YAML_example](https://user-
images.githubusercontent.com/60094858/221828273-ecf7599e-d7d7-44bb-9ffc-246dab65d435.png)
](https://user-
images.githubusercontent.com/60094858/221828273-ecf7599e-d7d7-44bb-9ffc-246dab65d435.png)


But isn't that what the costmap filters are for? I don't think that's a con,
that's just a feature that already exists.


[
![@AlexeyMerzlyakov](https://avatars.githubusercontent.com/u/60094858?s=80&u=bb82ce480f0854e3758a8c4f07129af932258cb1&v=4)
](/AlexeyMerzlyakov)


Edit: Ahhh, I think I understand a little bit better now your thinking, You
want to use the default costmap filter plugin layers, don't you? You're seeing
this as doing the pre-work for the layers so that the same speed and keepout
zone costmap filters can be used, but the rastered costmaps are coming from
this external server? That was unclear since the presentation shows a new `
VectorLayer ` . OK. Doing all this pre-work in a server makes alot of sense
**if** we're not making any new layers. If we're making new layers, lets do it
in the layer.

---

All reactions


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  Copyright (c) 2015, Fetch Robotics, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/


None yet


##  Footer


I suppose we could support both services and topics, though not my ideal
outcome. I would want clear warnings to users informing them that topics are
not recommended, but may be useful in certain circumstances.


  * How the polygons/shapes are being filled in, and how the cost value is chosen / specified
  * How the polygons / shapes are being specified (service, topic, etc) and how to add / remove them
  * How it handles rolling costmaps or costmap updates efficiently


The robot should be able to navigate when the goal is sent and should plan its
path according to the costs in cost map.


void
StaticLayer::reset()
{
  has_updated_data_ = true;
  current_ = false;
}


[ ![ROS Answers logo](/m/ros/media/images/logoros.png?v=28) ](/questions/)


[ ![@tonynajjar](https://avatars.githubusercontent.com/u/14879660?s=52&v=4)
](/tonynajjar) [
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=52&v=4)
](/SteveMacenski) [
![@jplapp](https://avatars.githubusercontent.com/u/17068999?s=52&v=4)
](/jplapp) [
![@GMahmoud](https://avatars.githubusercontent.com/u/29754310?s=52&v=4)
](/GMahmoud) [
![@AlexeyMerzlyakov](https://avatars.githubusercontent.com/u/60094858?s=52&v=4)
](/AlexeyMerzlyakov) [
![@PranavShevkar](https://avatars.githubusercontent.com/u/71570841?s=52&v=4)
](/PranavShevkar)


Pick a username


Repositories


> For this, robot A would publish the received footprint of robot B on a
> topic, and this new plugin would then add it to the costmap. I think
> services would add too much latency here, in case robot B is moving.


edited


##  Related questions


####  Steps to reproduce issue


[ Last incompatible policy: RELIABILITY_QOS_POLICY - Ros2 bag problem
](/question/416740/last-incompatible-policy-reliability_qos_policy-ros2-bag-
problem/)


I will let you know when I'll get some free time to upgrade the virtual layer
by adding some other features that might help:


Introspection of services is on it way soon actually [
https://discourse.ros.org/t/rep-2012-service-introspection/26079/4
](https://discourse.ros.org/t/rep-2012-service-introspection/26079/4) . But
like I said above, you could at least log failures with services that you
couldn't with topics. Also, if you don't finely handle your QoS on the
polygons topic, you could end up backfilling and having a huge amount of
memory overhead and/or instant network traffic flooding the pipes when a
connection is re-established for old messages trying to pass through - since I
imagine you would want to have some kind of transient-local properties if just
streaming a topic on separate wireless devices of this nature.


So, it could be used a ` KeepoutFilter ` . However, currently ` KeepoutFilter
` updates the [ maximum ](https://github.com/ros-
planning/navigation2/blob/63b690a27a4ba1bde24ebd8e6c592c9894eec169/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp#L286-L287)
from ` mask_value ` and ` master_costmap ` . So, to take everything from VO-
server, we need to add ` use_maximum ` option support to KeepoutFilter (which
is trivial but anyway).

---

All reactions


[ ![JSLbotbuilder gravatar image](/m/default/media/images/nophoto.png?v=28)
](/users/145125/jslbotbuilder/)


You can’t perform that action at this time.


---

👍  1  AlexeyMerzlyakov reacted with thumbs up emoji


**[ SteveMacenski ](/SteveMacenski) ** commented  Feb 6, 2023  •


###  Odometry  ¶


I will surely adapt my integration to nav2 use cases 😺, if you need to use
only a simplified polygon.


Using services will allow the sender (robot B) to know that the communication
is broken. However, it might not suffice to prevent the dangerous situation:
If communication is broken, what can robot B do with this information? It
can't tell robot A to stop.


Explore


[
![@GMahmoud](https://avatars.githubusercontent.com/u/29754310?s=80&u=1493f619be29310ca5f6e336f7f72f4d945ab80f&v=4)
](/GMahmoud)


Finally, in the case of a total failure, a recovery may be implemented to call
an operators attention for help. This can be done with email, SMS, Slack,
Matrix, etc.


Assignees


[ Sign up for free ](/join?source=comment-repo) **to join this conversation on
GitHub** . Already have an account? [ Sign in to comment
](/login?return_to=https%3A%2F%2Fgithub.com%2Fros-
planning%2Fnavigation2%2Fissues%2F3017)


Understood. I will check the NPVL package to make some changes. if I have to
generalize my question it will be " How can I debug costmap plugin code when
there is a problem between costmap plugin and bt_navigator?". I also don't see
any warning or error messages. I receive all the costmap layers well and there
is no problem with it. So I was looking forward to understanding what could be
causing this issue in the NAV2 stack.
It is just a graduation project and not a commercial project. I will continue
searching for other 3rd party examples to find the difference.
Thank you for replying.

---

All reactions


All reactions


It is the role of the odometry system to provide the ` odom  ` -> ` base_link
` transformation. Odometry can come from many sources including LIDAR, RADAR,
wheel encoders, VIO, and IMUs. The goal of the odometry is to provide a smooth
and continuous local frame based on robot motion. The global positioning
system will update the transformation relative to the global frame to account
for the odometric drift.


Copy link


The last branch [ @GMahmoud ](https://github.com/GMahmoud) posted looked
overall pretty good to me from a quick scan. I can't speak to the efficiency
and certainly the parsing from file we discussed having done potentially by an
outside server, but that's overall a good starting point. I wouldn't reinvent
what's done well for us, if after some analysis you agree there are lessons to
be taken from it.

---

All reactions


> logging with rosbags


Sorry, something went wrong.


* 👍  1 reaction


I think services are the right choice for the interface design.

---

All reactions


[
![@AlexeyMerzlyakov](https://avatars.githubusercontent.com/u/60094858?s=80&u=bb82ce480f0854e3758a8c4f07129af932258cb1&v=4)
](/AlexeyMerzlyakov)


Seen: **198 times**


  *[
              2023-04-10 14:45:27 -0500
             ]: 2023-04-10 14:45:27 -0500
  *[
              2023-06-15 15:37:42 -0500
             ]: 2023-06-15 15:37:42 -0500
  *[
        2023-04-10 14:45:27 -0500
       ]: 2023-04-10 14:45:27 -0500
  *[
        2024-04-14 00:57:56 -0500
       ]: 2024-04-14 00:57:56 -0500


[ Nav2 how to param DWBLocalPlanner to stay on Path
](/question/415638/nav2-how-to-param-dwblocalplanner-to-stay-on-path/)


I have developed also some navigation plugins (global and local planning) that
I want to make them compatible with ROS2 and Nav2 🤖 🚀. I will let you know in
slack or in another Github issue/thread about this work.

---

All reactions


[ Sign up
](/signup?ref_cta=Sign+up&ref_loc=header+logged+out&ref_page=%2F%3Cuser-
name%3E%2F%3Crepo-
name%3E%2Fvoltron%2Fissues_fragments%2Fissue_layout&source=header-
repo&source_repo=ros-planning%2Fnavigation2)


[ Sign up for free ](/join?source=comment-repo) **to join this conversation on
GitHub** . Already have an account? [ Sign in to comment
](/login?return_to=https%3A%2F%2Fgithub.com%2Fros-
planning%2Fnavigation2%2Fissues%2F3613)


Collaborator


* ❤  1 reaction


**[ SteveMacenski ](/SteveMacenski) ** commented  Jun 10, 2023


D1 is more simple conceptually, but as I recall we've discussed, that this
approach might change the concept/meaning/understanding of ` OccupancyGrid `
map itself, as a raster map.

---

All reactions


which is insignificant in our case


Copy link


To see all available qualifiers, see our [ documentation
](https://docs.github.com/search-github/github-code-search/understanding-
github-code-search-syntax) .


**[ AlexeyMerzlyakov ](/AlexeyMerzlyakov) ** commented  Feb 3, 2023


Labels


void
StaticLayer::deactivate()
{
  dyn_params_handler_.reset();
}


> Hm... that is really interesting. I thought, it actively used in Costmap2D-s


Sorry, something went wrong.


**[ AlexeyMerzlyakov ](/AlexeyMerzlyakov) ** commented  Sep 21, 2023


####  Actual behavior


Another example would be if the robot was stuck due to dynamic obstacles or
poor control. Backing up or spinning in place, if permissible, allow the robot
to move from a poor location into free space it may navigate successfully.


##  State Estimation  ¶


These appear to be real problems you should deal with.


Also, ` closed ` is valid only for ` Polygon ` types of shapes. For circles we
do no need this parameter, unless we won't introduce the arcs with `
begin_angle ` and ` end_angle ` . However, I think it is over-complicating the
design in our case: we could use circle + polygons to do that.

---

All reactions


By Solution


edited


###  Lifecycle Nodes and Bond  ¶


* Solutions


Collaborator


D2. Vector Object layer on Costmap2D operating only with costmaps


Member


    * [ All features ](/features)
    * [ Documentation  ](https://docs.github.com)
    * [ GitHub Skills  ](https://skills.github.com/)
    * [ Blog  ](https://github.blog)


But the general principle seems good.

---

All reactions


I've selected the second one, to have an ability to expand the number of types
of vector objects supported. If we do not need it (we'd like to leave only
polygons and circles as it was made for Collision Monitor, or even only
polygons w/o circles), the best option is O1.


[ question  ](/ros-planning/navigation2/labels/question) Further information
is requested


Thanks for pointing this out! ` fill ` would be also a useful attribute for
the vector object.


Contributor  Author


Skip to content


Copy link


> If you were to define a Polygon as a series of connected line segments, I'm
> not entirely sure why the difference between convex/concave is important (or
> using so many external polygon libraries). Obviously I'm not in the middle
> of this development, so I'm sure you have good reasons!


[
![@SteveMacenski](https://avatars.githubusercontent.com/u/14944147?s=80&u=0925d37e9d3569c71d930114b66e6394c2efb3c6&v=4)
](/SteveMacenski)


Search


Please note: ROS Answers requires javascript to work properly, please enable
javascript in your browser, [ here is how
](https://www.google.com/support/bin/answer.py?answer=23852)


The lifecycle node framework is used extensively through out this project and
all servers utilize it. It is best convention for all ROS systems to use
lifecycle nodes if it is possible.


[ tonynajjar ](/tonynajjar) opened this issue  Jun 15, 2022  · 40 comments


##  Navigation Servers  ¶


Later, I will prepare design PPT update. with more details and attach there.

---

All reactions


    if (param_name == name_ + "." + "map_subscribe_transient_local" ||
      param_name == name_ + "." + "map_topic" ||
      param_name == name_ + "." + "subscribe_to_updates")
    {
      RCLCPP_WARN(
        logger_, "%s is not a dynamic parameter "
        "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
    } else if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name == name_ + "." + "transform_tolerance") {
        transform_tolerance_ = tf2::durationFromSec(parameter.as_double());
      }
    } else if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
        enabled_ = parameter.as_bool();


##  Use saved searches to filter your results more quickly


Cancel  Submit feedback


Member


I've created some kind of simple VO-Server node in Nav2 preparing required
size of mask and publishing it regularly. Then enabling KeepoutFilter in local
costmap with published mask.
There were measured the following items:


Yes, I was not entirely accurate. The main performance pitfall we may get when
we have a large ` OccGrid ` map with large vector polygon (occupying almost
all the space) on it, and small costmap rolling window. If we on each costmap
update cycle are calculating only inside small window ` (int min_i, int min_j,
int max_i, int max_j) ` , it will be much better (in terms of performance),
than re-calculate whole ` OccupancyGrid ` map by VO-server. The explanation
behind it is: separate VO-server have no knowledge about dynamic costmap
window.


The planner, smoother and controller servers will be configured at runtime
with the names (aliases) and types of algorithms to use. These types are the
pluginlib names that have been registered and the names are the aliases for
the task. An example would be the DWB controller used with name ` FollowPath
` , as it follows a reference path. In this case, then all parameters for DWB
would be placed in that namespace, e.g. ` FollowPath.<param> ` .


###  Footer navigation


Skip to content


  * StaticLayer reads input OccupancyGrid map once, so we could not implement dynamic updates on it
  * StaticLayer converts whole incoming OccupancyGrid map by-item to Costmap2D, which will cause significant performance slowdown (KeepoutFilter won't do this since  [ Improve KeepoutFilter mask receiving performance  #3420  ](https://github.com/ros-planning/navigation2/pull/3420) )
  * StaticLayer works only with the same as main map OccupancyGrids, and thus, VO-server needs to read the input map from map-server to define the parameters (origin, resolution and size), which unnecessary complicating the architecture.


Working on the feature is in active progress. There is ~90% of main algorithms
code done. After combing-up stage, I plan to open draft PR on it.

---

All reactions


6 participants


      * [ Fix CostmapLayer clearArea invert param logic ](../migration/Galactic.html#fix-costmaplayer-cleararea-invert-param-logic)
      * [ Dynamic Composition ](../migration/Galactic.html#dynamic-composition)
      * [ BT Cancel Node ](../migration/Galactic.html#bt-cancel-node)
      * [ BT PathLongerOnApproach Node ](../migration/Galactic.html#bt-pathlongeronapproach-node)
      * [ BT TruncatePathLocal Node ](../migration/Galactic.html#bt-truncatepathlocal-node)
      * [ Constrained Smoother ](../migration/Galactic.html#constrained-smoother)
      * [ Replanning at a Constant Rate and if the Path is Invalid ](../migration/Galactic.html#replanning-at-a-constant-rate-and-if-the-path-is-invalid)
      * [ Euclidean Distance 2D ](../migration/Galactic.html#euclidean-distance-2d)
      * [ Recovery To Behavior ](../migration/Galactic.html#recovery-to-behavior)
      * [ Respawn Support in Launch and Lifecycle Manager ](../migration/Galactic.html#respawn-support-in-launch-and-lifecycle-manager)
      * [ New Nav2 Velocity Smoother ](../migration/Galactic.html#new-nav2-velocity-smoother)
      * [ Goal Checker API Changed ](../migration/Galactic.html#goal-checker-api-changed)
      * [ Added Assisted Teleop ](../migration/Galactic.html#added-assisted-teleop)
    * [ Humble to Iron ](../migration/Humble.html)
      * [ New Behavior-Tree Navigator Plugins ](../migration/Humble.html#new-behavior-tree-navigator-plugins)
      * [ Added Collision Monitor ](../migration/Humble.html#added-collision-monitor)
      * [ Removed use_sim_time from yaml ](../migration/Humble.html#removed-use-sim-time-from-yaml)
      * [ Run-time Speed up of Smac Planner ](../migration/Humble.html#run-time-speed-up-of-smac-planner)
      * [ Recursive Refinement of Smac and Simple Smoothers ](../migration/Humble.html#recursive-refinement-of-smac-and-simple-smoothers)
      * [ Simple Commander Python API ](../migration/Humble.html#simple-commander-python-api)
      * [ Smac Planner Start Pose Included in Path ](../migration/Humble.html#smac-planner-start-pose-included-in-path)
      * [ Parameterizable Collision Checking in RPP ](../migration/Humble.html#parameterizable-collision-checking-in-rpp)
      * [ Expanded Planner Benchmark Tests ](../migration/Humble.html#expanded-planner-benchmark-tests)
      * [ Smac Planner Path Tolerances ](../migration/Humble.html#smac-planner-path-tolerances)
      * [ costmap_2d_node default constructor ](../migration/Humble.html#costmap-2d-node-default-constructor)
      * [ Feedback for Navigation Failures ](../migration/Humble.html#feedback-for-navigation-failures)
      * [ Costmap Filters ](../migration/Humble.html#costmap-filters)
      * [ Savitzky-Golay Smoother ](../migration/Humble.html#savitzky-golay-smoother)
      * [ Changes to Map yaml file path for map_server node in Launch ](../migration/Humble.html#changes-to-map-yaml-file-path-for-map-server-node-in-launch)
      * [ SmootherSelector BT Node ](../migration/Humble.html#smootherselector-bt-node)
      * [ Publish Costmap Layers ](../migration/Humble.html#publish-costmap-layers)
      * [ Give Behavior Server Access to Both Costmaps ](../migration/Humble.html#give-behavior-server-access-to-both-costmaps)
      * [ New Model Predictive Path Integral Controller ](../migration/Humble.html#new-model-predictive-path-integral-controller)
      * [ Behavior Tree Uses Error Codes ](../migration/Humble.html#behavior-tree-uses-error-codes)
      * [ Load, Save and Loop Waypoints from the Nav2 Panel in RViz ](../migration/Humble.html#load-save-and-loop-waypoints-from-the-nav2-panel-in-rviz)
      * [ DWB Forward vs Reverse Pruning ](../migration/Humble.html#dwb-forward-vs-reverse-pruning)
      * [ More stable regulation on curves for long lookahead distances ](../migration/Humble.html#more-stable-regulation-on-curves-for-long-lookahead-distances)
      * [ Publish Collision Monitor State ](../migration/Humble.html#publish-collision-monitor-state)
      * [ Renamed ROS-parameter in Collision Monitor ](../migration/Humble.html#renamed-ros-parameter-in-collision-monitor)
      * [ New safety behavior model “limit” in Collision Monitor ](../migration/Humble.html#new-safety-behavior-model-limit-in-collision-monitor)


[ @tonynajjar ](https://github.com/tonynajjar) so what's your plan here?
Remake something using that as a reference or wait for it to be ported and we
can go through some design reviews to patch it up? Or should we start design
reviews here now with the intention of including the final results into Nav2
itself?


Hey, in our use case, we would like to use [
https://github.com/GMahmoud/virtual_costmap_layer
](https://github.com/GMahmoud/virtual_costmap_layer) with Nav2. Would you
consider it a generic costmap layer that could be part of Nav2? Could it also
be an extension of the Obstacle Layer?
We could work on contributing it.


* [ The ReadME Project  GitHub community articles  ](/readme)


I'd want to know


For such constantly updating data as in case 2), other obstacle layers also
receive their (critical) input via topics, so what would be different here?
There are some usability advantages of topics, such as logging with rosbags,
that would be quite helpful for us.

---

All reactions


  * O1. Support each new shape in the separate object and having each service for each shape:
` Add(Remove)Polygon.srv ` , ` Add(Remove)Circle.srv ` , ` AddEllipse.srv ` ,
` AddArc.srv ` , etc...


[
![@AlexeyMerzlyakov](https://avatars.githubusercontent.com/u/60094858?s=80&u=bb82ce480f0854e3758a8c4f07129af932258cb1&v=4)
](/AlexeyMerzlyakov)


[ ![ gravatar image]() ]() [ ]() (  2024-04-14 00:57:56 -0500  )  edit


Sorry, something went wrong.


You have the option in a costmap layer to "override" cost instead of taking
the maximum. So you can have the detector layer of your pallet that you'd be
using to clear the costmap as the highest layer and it would wipe out all the
sensor measurements as long as you're detecting the pallet there (or whatever
your logic). That's probably the easiest way and only ~200 lines of code.

---

All reactions


experiment. This is especially concerning
for the propellers possibly causing shortterm injuries to eyes.
Minor Possible 5 Procedure,
PPE
Only necessary personnel should be
allowed within the motion capture
facilities and operating vicinity. Safety
glasses may also be worn within the
motion capture facilities.
Mechanical Hair or loose fitting clothing may become
caught or tangled in the motors and
propellers of the Crazyflie.
Minor Possible 5 PPE Hair must be tied up and loose clothing
must be sufficiently restricted.
Mechanical If the Crazyflie crashes, parts can be
dislodged and act as shrapnel.
Minor Rare 3 PPE Safety glasses may be worn within the
motion capture facilities.
Electrical Minor electric shocks may be experienced
from handling the Crazyflie and
Raspberry Pi, but the voltage and current
of these shocks are very minimal.
Minor Unlikely 4 Procedure In manufacturing and grounding, it is
unlikely for electrical shocks but the
Crazyflie and Raspberry Pi must still be
handled with care such that they are not
damaged and become hazardous.
Chemical The LiPo battery may erupt (fire,
explosions, and toxic smoke) if exposed
to high temperatures or penetrated, where
the lithium will be exposed which is
highly flammable and potentially
explosive when mixed with air and may
cause chemical burns. This may occur
during use, charging, or storage.
Major Possible 7 Modification The battery must be housed correctly
within the frame of the Crazyflie so
exposure is minimised in the event that it
unexpectedly erupts. The battery must be
check before and after each use. A good


Or check it out in the app stores


There is an exception for experimental support to change the parameter from
within [ firmwareâs app layer ](/documentation/repository/crazyflie-
firmware/master/userguides/app_layer/#internal-log-and-param-system) . However
do mind that this functionality is not according to the design of the
parameters framework so that the host might not be updated correctly on the
parameter change.


  * [ ![](/advisor/images/footer-github-icon.svg) ](https://github.com/snyk/ "GitHub")
  * [ ![](/advisor/images/footer-npm-icon.svg) ](https://www.npmjs.com/~snyk-admin "npm")


    lg.addVariable(LogVariable("stabilizer.pitch", "float"))
            lg.addVariable(LogVariable("stabilizer.yaw", "float"))
            lg.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))

            self.log = self.helper.cf.log.create_log_packet(lg)
            if (self.log is not None):
                self.log.dataReceived.add_callback(self._imu_data_signal.emit)
                self.log.error.add_callback(self.loggingError)
                self.log.start()
            else:
                logger.warning("Could not setup logconfiguration after "
                               "connection!")

            # MOTOR
            lg = LogConfig("Motors", 50)
            lg.addVariable(LogVariable("motor.m1", "uint32_t"))
            lg.addVariable(LogVariable("motor.m2", "uint32_t"))
            lg.addVariable(LogVariable("motor.m3", "uint32_t"))
            lg.addVariable(LogVariable("motor.m4", "uint32_t"))

            self.log = self.helper.cf.log.create_log_packet(lg)
            if (self.log is not None):
                self.log.dataReceived.add_callback(self._motor_data_signal.emit)
                self.log.error.add_callback(self.loggingError)
                self.log.start()
            else:
                logger.warning("Could not setup logconfiguration after "
                               "connection!")

            # BARO
            lg = LogConfig("Baro", 50)


Version:  [  master  , [ 0.1.25.1 ](/documentation/repository/crazyflie-lib-
python/0.1.25.1/) , [ 0.1.24 ](/documentation/repository/crazyflie-lib-
python/0.1.24/) , [ 0.1.23 ](/documentation/repository/crazyflie-lib-
python/0.1.23/) , [ 0.1.22 ](/documentation/repository/crazyflie-lib-
python/0.1.22/) , [ 0.1.21 ](/documentation/repository/crazyflie-lib-
python/0.1.21/) , [ 0.1.20.1 ](/documentation/repository/crazyflie-lib-
python/0.1.20.1/) , [ 0.1.19 ](/documentation/repository/crazyflie-lib-
python/0.1.19/) ]


All names of parameters and log variables use the same structure: ` group.name
` .


####  Resources


  * _radio://0/10/2M_ : Radio interface, USB dongle number 0, radio channel 10 and radio speed 2 Mbit/s: radio://0/10/2M
  * _debug://0/1_ : Debug interface, id 0, channel 1
  * _usb://0_ : USB cable to microusb port, id 0
  * _serial://ttyAMA0_ : Serial port, id ttyAMA0
  * _tcp://aideck-AABBCCDD.local:5000_ : TCP network connection, Name: aideck-AABBCCDD.local, port 5000


The parameters should be used in the following way:


if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    # Scan for Crazyflies and use the first one found
    print('Scanning interfaces for Crazyflies...')
    available = cflib.crtp.scan_interfaces()
    print('Crazyflies found:')
    for i in available:
        print(i[0])


        with SyncCrazyflie(URI) as scf:
            with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc:
                # Go to the coordinate (0, 0, 1)
                pc.go_to(0.0, 0.0, 1.0)
                # The Crazyflie lands when leaving this "with" section
            # When leaving this "with" section, the connection is automatically closed


####  Track our development


##  To help you get started, we’ve selected a few cfclient examples, based on
popular ways it is used in public projects.


Then you can use the following to close the link again:


283 uri = "radio://0/80/2M/E7E7E7E7E8"
284 logging.basicConfig (level = logging.ERROR)
285 crazyflie = Crazyflie (rw_cache = "./cache")
286
287 # Set the variables for the initial setup before tracking.
288 altitude = 0.4
289 yaw = 0
290 #velocity_max = 0.4
291 focal_length = 113.5
77
292
293 # Find basic requirements from the user.
294 record = input ("Must the source be recorded as a video? [Y/N] ")
295 if record == "Y":
296 version = input ("Must basic [B] or combined [C] output be recorded? [B/C] ")
297 if version != "B" and version != "C":
298 print ("! The input for the recording version is invalid.")
299 raise SystemExit
300 elif record != "N":
301 print ("! The input for the recording of the source is invalid.")
302 raise SystemExit
303
304 # --------------------------------------------------------------------------------
305 # Perform Checks
306 # --------------------------------------------------------------------------------
307
308 # Identify the source for use and recording if desired.
309 print ("Identifying the source for use and recording if desired.")
310 source = cv.VideoCapture (0)
311 source.set (cv.CAP_PROP_FRAME_WIDTH, res_x) # Alternate identifier: 3.
312 source.set (cv.CAP_PROP_FRAME_HEIGHT, res_y) # Alternate identifier: 4.
313 if record == "Y":
314 codec = cv.VideoWriter_fourcc (*"XVID")
315 if version == "B":
316 video = cv.VideoWriter ("Out.avi", codec, fps, (res_x, res_y), True)
317 elif version == "C":
318 video = cv.VideoWriter ("Out.avi", codec, fps, (res_x * 2, res_y * 2), True)
319
320 # Ensure the source is open and available.
321 active = True


  * [ JavaScript Security ](https://snyk.io/learn/javascript-security/)
  * [ Kubernetes Security ](https://snyk.io/learn/kubernetes-security/)
  * [ Container Security ](https://snyk.io/learn/container-security/)
  * [ Open Source Security ](https://snyk.io/open-source-security/)
  * [ Secure SDLC ](https://snyk.io/learn/secure-sdlc/)


**Top 71%** [ Rank by size
](https://www.reddit.com/best/communities/908/#t5_296ksl/)


The logging cannot be started until your are connected to a Crazyflie:


Expand user menu  Open settings menu


  * [ Vulnerability DB ](https://security.snyk.io)
  * [ Blog ](https://snyk.io/blog)
  * [ Learn ](https://snyk.io/learn/)
  * [ Documentation ](https://support.snyk.io/hc/en-us)
  * [ Snyk API ](https://snyk.docs.apiary.io)
  * [ Research ](https://snyk.io/research)
  * [ FAQs ](https://support.snyk.io/)
  * [ npm packages list ](/advisor/npm-packages-list)
  * [ Python Code Examples ](/advisor/code/python/popular)
  * [ Browse all Python packages ](/advisor/packages/python/popular)


        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            self._lg_stab.start()
        except KeyError as e:
            print('Could not start log configuration,'
                  '{} not found in TOC'.format(str(e)))
        except AttributeError:
            print('Could not add Stabilizer log config, bad configuration.')


• Develop and implement target detection and tracking through end-point open-loop visual servoing using the pinhole camera model and computer vision techniques for image processing on
low-end hardware with marginal computational effort, where a quadrotor must remotely track
the position of the target in a mirroring fashion while varying its altitude to match the camera.
The tracking will be performed for three degrees of translational freedom and one degree of rotational freedom as yaw orientation. (The Raspberry Pi Camera Module 2.1 and Raspberry Pi
Zero W 1.1 will be used along with OpenCV and a Bitcraze Crazyflie 2.1 quadrotor).
• Test the effectiveness of the target detection and tracking using an external motion capture system
with multiple cameras, where the relative deviation between the desired position from the target
and actual position of the quadrotor is measured throughout the experimentation. (The motion
capture system will comprise of four Qualisys Miqus M1 cameras).
4 APPARATUS
The available apparatus consists of various parts for different systems, as broadly outlined in Figure 16. In these interactions, the quadrotor receives control setpoints from the central processing
based on the visual servoing from the camera, while the motion of the target and quadrotor are tracked
by the motion capture system (to be used only as validation for the effectiveness of the actual target
tracking of the target by the quadrotor) and information about the current state is reported to a ground
control laptop for monitoring. Moreover, the basic details of the utilised software need to be briefly
explained, because it will form an intrinsic part in implementing the methodology presented in Section 5. Further resources from the manufacturers of the parts are included in Appendix A.
Camera Processing Quadrotor
Ground Laptop
Moving Target
Motion Capture
Arduino Python
Direct Connection
Indirect Dependence
Figure 16: Outline and interactions of the apparatus from a broad overview.
4.1 QUADROTOR PARTS
For the quadrotor, the open-source Bitcraze Crazyflie 2.1 (subsequently referred to as Crazyflie) will
be used. This quadrotor consists of a central control board with various on-board sensors (also acting
as a lightweight frame), four rotor motors, four propellers, four stands, and battery - these components
are seen in Figure 17 showing the complete assembly. Additionally, expansion decks can be connected
18
to the control board for further on-board functionality and sensing, and a 3D printed propeller guard
may be mounted with three 12mm motion capture markers - this leads to an additional payload of 7g
which is below the supported additional payload of 14g. (Unfortunately, the motion capture markers
could not be placed asymmetrically elsewhere as instability arose with unpredictable drift, but the
chosen placement is acceptable since the yaw measurements will be accurate and are more important
than the roll and pitch measurements which may be adversely affected).
Top Isometric View Bottom Isommetric View
Flow
Deck
Contorl


Carbon
Fibre
Calibration
Kit With
Motion
Capture
Markers
Wand-Tool
L-Tool
300.8mm
Figure 25: Photographs showing a Qualisys Miqus M1 (top-left), basic layout of the motion capture
facilities (top-middle), carbon fibre calibration kit for the initial calibration of the motion capture
volume (top-right), and arrangement of the apparatus within the motion capture facilities (bottom).
26
4.5 GROUND CONTROL LAPTOP
A laptop will be used to act as a ground control station for remotely accessing the Raspberry Pi, monitoring the target detection and tracking, and running the Qualisys Track Manager (QTM) software
for the motion capture system. To facilitate connection to the Raspbian operating system on the Raspberry Pi, a local wireless LAN network will be created by the ground control laptop. In this case, an
HP Notebook 14-an013nr will be used with an AMD E3-7110 processor incorporating four threads
and four cores running at 1.8GHz, but any modern laptop would likely be a sufficient substitute.
4.6 SOFTWARE
The software to be used for visual servoing, controlling the quadrotor, and controlling the target is
open-source with free availability and primarily includes Python, OpenCV, Raspbian, and Arduino.
The logos associated with these software packages are illustrated in Figure 26 for visual distinction.
Figure 26: Open-source software used for visual servoing, quadrotor control, and target control.
4.6.1 CRAZYFLIE
The low-level firmware of the Crazyflie is written in C and C++, but there is a Crazyflie Python application programming interface (API) library cflib with high-level bindings which can send setpoints.
With access through Python 3.7.3, the following classes and functions will be used:
• Crazyflie: Contains the intrinsic commands and callbacks used by the higher-level functions
in other classes, where the same design as in the firmware is used for a one-to-one mapping.
• SyncCrazyflie: Wrapper around the Crazyflie class to handle its asynchronous nature and
convert it for use as blocking functions in scripts performing tasks as sequences of events.
• SyncLogger: Provide synchronous access to log current variables. The variables which can be
logged include estimated position, velocity, acceleration, angular orientation, and other information associated with the current state of the quadrotor as measured from the on-board sensors.
• LogConfig: Create a configuration in which to log variables.
– add_variable: Add a variable to be logged with its group and name.
– start: Begin the logging of the added variables.
– stop: End the logging of the added variables.
27
• Commander: Send control setpoints to the Crazyflie. The setpoints need to be sent as often as
possible, where the Crazyflie has a continuous backend or watchdog timer which will reset the


of estimating the altitude is uncommon and does not directly appear in literature.
As mentioned, a complete motion capture system could be used to estimate the absolute position of
the target and quadrotor, where multiple external cameras detect reflective infra-red markers attached
14
Ground
Camera View
Camera DOFs
Target DOFs
Ground
Quadrotor mirrors
the motion of the
target and altitude
of the camera.
Command Setpoints
z Quadrotor
Translational
DOFs
z
x y
x y
Map / Mirror
Position From
Target Centroid
Figure 13: Arrangement for end-point open-loop control through detection and tracking of a target.
to the target and quadrotor, and use triangulation to estimate the position of each marker in threedimensions. To capture three translational degrees of freedom, it is only necessary for at least one
marker to be used on a rigid body, but to capture three translational degrees of freedom and three
rotational degrees of freedom, it is necessary for three or more markers to be used on a rigid body,
preferably in a non-ambiguous asymmetric arrangement [2]. However, since the aim of the research is
focussed on using a single inexpensive camera with a lightweight algorithm, a motion capture system
will not be considered for target detection or tracking purposes - although a motion capture system is
available and may be used to quantify and validate the effectiveness of the developed system.
2.7 CONTROL PRINCIPLES
The general goal in control theory is to generate a desired setpoint or reference state to achieve a
matching output. The control may operate through an open-loop or closed-loop. For open-loop
control, a desired setpoint is input to a controller as a reference state which generates a corresponding
signal for actuators to manipulate the system appropriately, where it is then assumed that the desired
setpoint is satisfied in the final output state - in other words, there is no sensor to decide whether the
desired setpoint has actually been satisfied [15]. This process is seen in Figure 14. To attain successful
open-loop control, the actuation must be repeatable and reliable with the ability to mitigate effects
from disturbances and noise, such that these effects are predictable or become negligible [15]. If the


72 / 100


####  Find us online


and 4.46o
respectively. An altitude could
also be maintained fairly successfully, where the Crazyflie only drifted by 3.75% downwards or
3.98% upwards on average before returning to the correct altitude.
9 RECOMMENDATIONS
The following points are recommended for further research and development:
• Although it did not affect the results and conclusions, it may be helpful to revise the take off
function in the control script for smoother behaviour if further testing is to be performed.
• The tests should be repeated in a larger environment to gauge the capability for expansion. This
will involve calibrating the camera for different altitudes and possibly using a larger target.
59
• Additional tests should also be performed in a variety of uncontrolled environments which contain minor obstacles, such as outdoors with effects on the flight control from inconsistent wind
and effects on the target detection from a greater variation in background colours and textures.
• The control script can be adapted to function with multiple targets and quadrotors, where each
target is uniquely identified and assigned to a specific quadrotor to follow the motion.
• The target could move at higher speeds to highlight the effect this will have on the lag and
whether the computational effort of the Raspberry Pi has a direct influence in this situation.
• For faster execution, the commands for the Crazyflie and image processing in OpenCV could be
rewritten and performed natively in C and C++, which are generally considered to require less
computational time than Python since C and C++ are compiled while Python is interpreted.
• An infra-red camera, as is traditional for motion capture systems, can be investigated for use
in darker environments or when there is noise from changing lighting. The Raspberry Pi NoIR
Camera Module 2.1 is a suitable infra-red camera and is compatible with the current setup.
• Using a larger quadrotor, the camera can be mounted and end-point closed-loop could be performed with the quadrotor directly tracking the target. This was partially developed to complement the completed research, as documented in Appendix E, but it was not possible to get the
quadrotor to fly safely after several attempts and a reasonable amount of effort.
• Finally, there is an opportunity to research artificial intelligence and deep learning algorithms,
such as a convolutional neural network, for more controlled and accurate results compared to the
pinhole camera model, although this will likely require greater computational effort.
60
REFERENCES
[1] M. Rabah et al. ‘Autonomous Moving Target-Tracking For A UAV Quadcopter Based On
Fuzzy-PI’. In: Institute of Electrical and Electronics Engineers (IEEE) Access Vol. 7 (Apr.
2019), pp. 38407–38419. ISSN: 2169-3536. DOI: 10.1109/ACCESS.2019.2906345. URL:
https://ieeexplore.ieee.org/document/8672092.
[2] O. Dunkley. Visual Inertial Control Of A Nano-Quadrotor. Technical University Munich, Sept.
2014. URL: https://vision.in.tum.de/_media/spezial/bib/dunkley14msc.pdf.
[3] R. Mahony, V. Kumar and P. Corke. ‘Multirotor Aerial Vehicles: Modelling, Estimation, And
Control’. In: Robotics And Automation Magazine, Institute of Electrical and Electronics Engineers (IEEE) Vol. 19. (3) (Sept. 2012), pp. 20–32. ISSN: 1070-9932. DOI: 10.1109/MRA.2
012.2206474. URL: https://ieeexplore.ieee.org/document/6289431.
[4] F. Poirier et al. Control Of Crazyflie Quadcopter. University of Manchester, Manchester, Sept.


  * Communication protocol for [ logging ](/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or [ parameters ](/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/)
  * [ Automated documentation for Python API ](/documentation/repository/crazyflie-lib-python/master/api/cflib/)
  * Examples: See the [ example folder of the repository ](https://github.com/bitcraze/crazyflie-lib-python/tree/master/examples) .


Plant
Element
Sensing / Feedback
Element
Disturbance
Element
+ +
s(t) r(t) e(t) f(t) m(t) n(t) c(t)
d(t)
b(t)
Forward Direction
s(t) = Setpoint Input
r(t) = Reference Signal
e(t) = Error Signal
f(t) = Control Signal
m(t) = Manipulated Signal
n(t) = Resultant Signal
d(t) = Disturbance Input
c(t) = Plant Output
b(t) = Feedback Signal
Feedback
Closed-Loop Control
Figure 15: General schematic of a closed-loop control system with feedback.
For a quadrotor, the controller will output a velocity command to the rotors in an attempt to correct
the state of the system. To obtain satisfactory results, the controller will usually operate based on
a proportional-integral-derivative (PID) control method. The proportional (P) component reacts directly to the magnitude of the current error, where an increased gain will allow for faster response but
may exhibit unwanted oscillations [2, 6, 15]. The integral (I) component reacts relative to the accumulation of past errors over time, where there is an increasing effect over time to reach a steady-state
[2, 6, 15]. The derivative (D) component reacts relative to the rate of change of the error signal with
time for a prediction of future errors, where there is a reduction of oscillations by regulating the speed
of the response and increasing stability [6, 15]. These components utilise Equation 12 for the error
between the setpoint and actual signals with the implementation of PID control in the analytical form
16
of Equation 13 or numerical form of Equation 14, where constant parameters or gains are used to
distribute the effects of the respective components. There are alternate methods of control, such as
non-linear sliding mode control or adaptive backstepping control, but PID control has been proven to
be robust and reliable when knowledge of the underlying processes are not known [1, 2, 5]. (It should
be noted that, in other situations, it may only be necessary for a subset of the control components,
such as only P, PD, or PI controllers - for 90% of these cases, a PI controller will be sufficient [15]).
e(t) = r(t) − b(t) (12)
f(t) = KPe(t) + KI


  * Connect to the Crazyflie (log configurations needs a TOC to work)
  * Create a log configuration that contains a number of variables to log and a period at which they should be logged
  * Add the log configuration. This will also validate that the log configuration is sane (i.e uses a supported period and all variables are in the TOC)
  * After checking that the configuration is valid, set up callbacks for the data in your application and start the log configuration
  * Each time the firmware sends data back to the host, the callback will be called with a time-stamp and the data