Merge branch 'main' into arjo/fix/mapgen_error

Author: Yadunund

rmf_building_map_tools/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package rmf\_building\_map\_tools
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.8.2 (2023-12-15)
+------------------
+* Export door information to nav graphs (`#479 <https://github.com/open-rmf/rmf_traffic_editor/pull/479>`_)
+* Export lift information to nav graphs (`#478 <https://github.com/open-rmf/rmf_traffic_editor/pull/478>`_)
+
 1.8.1 (2023-08-10)
 ------------------
 * Fix gz classic model download (`#470 <https://github.com/open-rmf/rmf_traffic_editor/pull/470>`_)

rmf_building_map_tools/building_map/building.py

@@ -352,6 +352,8 @@ def generate_nav_graphs(self):
             g = {}
             g['building_name'] = self.name
             g['levels'] = {}
+            g['lifts'] = {}
+            g['doors'] = {}
 
             if self.coordinate_system == CoordinateSystem.web_mercator:
                 g['crs_name'] = self.global_transform.crs_name
@@ -372,6 +374,22 @@ def generate_nav_graphs(self):
                 g['levels'][level_name] = level_graph
                 if level_graph['lanes']:
                     empty = False
+
+                for door_edge in level.doors:
+                    door_edge.calc_statistics(level.transformed_vertices)
+                    g['doors'][door_edge.params['name'].value] = {
+                        'endpoints': [
+                            [door_edge.x1, door_edge.y1],
+                            [door_edge.x2, door_edge.y2]
+                        ],
+                        'map': level_name
+                    }
+
+            for lift_name, lift in self.lifts.items():
+                g['lifts'][lift_name] = {
+                    'position': [lift.x, lift.y, lift.yaw],
+                    'dims': [lift.width, lift.depth]
+                }
             if not empty:
                 nav_graphs[f'{i}'] = g
         return nav_graphs

rmf_building_map_tools/building_map/level.py

@@ -211,6 +211,10 @@ def add_edge_from_coords(self, edge_type, p1, p2, props):
                 ParamValue.DOUBLE,
                 props.get('speed_limit', 0.0)
             ])
+            edge.params['mutex'] = ParamValue([
+                ParamValue.STRING,
+                props.get('mutex', "")
+            ])
             self.lanes.append(edge)
         else:
             raise ValueError('unknown edge type!')
@@ -486,6 +490,10 @@ def generate_nav_graph(self, graph_idx, always_unidirectional=True):
                     l.params['speed_limit'].value > 0.0:
                 p['speed_limit'] = l.params['speed_limit'].value
 
+            if 'mutex' in l.params and \
+                    l.params['mutex'].value:
+                p['mutex'] = l.params['mutex'].value
+
             if 'demo_mock_floor_name' in l.params and \
                     l.params['demo_mock_floor_name'].value:
                 p['demo_mock_floor_name'] = \
@@ -503,9 +511,6 @@ def generate_nav_graph(self, graph_idx, always_unidirectional=True):
             if 'dock_name' in v1.params:  # lane segment begins at dock
                 beginning_dock = v1.params['dock_name'].value
 
-            if 'speed_limit' in l.params:
-                p['speed_limit'] = l.params['speed_limit'].value
-
             if always_unidirectional and l.is_bidirectional():
                 # now flip things around and make the second link
                 forward_params = copy.deepcopy(p)

rmf_building_map_tools/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rmf_building_map_tools</name>
-  <version>1.8.1</version>
+  <version>1.8.2</version>
   <description>RMF Building map tools</description>
   <maintainer email="[email protected]">Morgan Quigley</maintainer>
   <maintainer email="[email protected]">Marco A. Gutiérrez</maintainer>

rmf_traffic_editor/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package rmf\_traffic\_editor
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.8.2 (2023-12-15)
+------------------
+* Merge radius property (`#480 <https://github.com/open-rmf/rmf_traffic_editor/pull/480>`_)
+* Mutex group property (`#477 <https://github.com/open-rmf/rmf_traffic_editor/pull/477>`_)
+
 1.8.1 (2023-08-10)
 ------------------
 

rmf_traffic_editor/gui/building_dialog.cpp

@@ -87,8 +87,16 @@ void BuildingDialog::ok_button_clicked()
   _building.reference_level_name =
     _reference_floor_combo_box->currentText().toStdString();
 
-  _building.params["generate_crs"] =
-    Param(generate_crs_line_edit->text().toStdString());
+  if (!generate_crs_line_edit->text().isEmpty())
+  {
+    _building.params["generate_crs"] = Param(
+      generate_crs_line_edit->text().toStdString());
+  }
+  else
+  {
+    if (_building.params.find("generate_crs") != _building.params.end())
+      _building.params.erase("generate_crs");
+  }
 
   accept();
 }

rmf_traffic_editor/gui/edge.cpp

@@ -132,6 +132,7 @@ void Edge::create_required_parameters()
       std::string());
     create_param_if_needed("demo_mock_lift_name", Param::STRING, std::string());
     create_param_if_needed("speed_limit", Param::DOUBLE, 0.0);
+    create_param_if_needed("mutex", Param::STRING, std::string());
   }
   else if (type == DOOR)
   {

rmf_traffic_editor/gui/level.cpp

@@ -1184,6 +1184,7 @@ void Level::draw(
     v.draw(
       scene,
       vertex_radius / drawing_meters_per_pixel,
+      drawing_meters_per_pixel,
       vertex_name_font,
       coordinate_system);
 

rmf_traffic_editor/gui/vertex.cpp

@@ -39,6 +39,8 @@ const vector<pair<string, Param::Type>> Vertex::allowed_params
   { "is_holding_point", Param::Type::BOOL },
   { "is_passthrough_point", Param::Type::BOOL },
   { "human_goal_set_name", Param::Type::STRING },
+  { "mutex", Param::Type::STRING },
+  { "merge_radius", Param::Type::DOUBLE },
 };
 
 
@@ -131,6 +133,7 @@ YAML::Node Vertex::to_yaml(const CoordinateSystem& coordinate_system) const
 void Vertex::draw(
   QGraphicsScene* scene,
   const double radius,
+  const double drawing_meters_per_pixel,
   const QFont& font,
   const CoordinateSystem& coordinate_system) const
 {
@@ -163,6 +166,19 @@ void Vertex::draw(
   const double icon_ring_radius = radius * 2.5;
   const double icon_scale = 2.0 * radius / 128.0;
 
+  if (const auto r_merge_opt = merge_radius())
+  {
+    const double r_merge = *r_merge_opt / drawing_meters_per_pixel;
+    const QPen radius_pen(selected ? selected_color : Qt::black);
+    auto* radius_item = scene->addEllipse(
+      x - r_merge,
+      y - r_merge,
+      2 * r_merge,
+      2 * r_merge,
+      radius_pen);
+    radius_item->setZValue(19.0);
+  }
+
   if (is_holding_point())
   {
     const double icon_bearing = -135.0 * M_PI / 180.0;
@@ -342,6 +358,15 @@ bool Vertex::is_charger() const
   return it->second.value_bool;
 }
 
+std::optional<double> Vertex::merge_radius() const
+{
+  const auto it = params.find("merge_radius");
+  if (it == params.end())
+    return std::nullopt;
+
+  return it->second.value_double;
+}
+
 bool Vertex::is_cleaning_zone() const
 {
   const auto it = params.find("is_cleaning_zone");

rmf_traffic_editor/gui/vertex.h

@@ -22,6 +22,7 @@
 #include <map>
 #include <string>
 #include <vector>
+#include <optional>
 #include <yaml-cpp/yaml.h>
 
 #include <QColor>
@@ -58,13 +59,15 @@ class Vertex
   void draw(
     QGraphicsScene* scene,
     const double radius,
+    const double drawing_meters_per_pixel,
     const QFont& font,
     const CoordinateSystem& coordinate_system) const;
 
   bool is_parking_point() const;
   bool is_holding_point() const;
   bool is_cleaning_zone() const;
   bool is_charger() const;
+  std::optional<double> merge_radius() const;
 
   std::string dropoff_ingestor() const;
   std::string pickup_dispenser() const;

rmf_traffic_editor/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rmf_traffic_editor</name>
-  <version>1.8.1</version>
+  <version>1.8.2</version>
   <description>traffic editor</description>
   <maintainer email="[email protected]">Morgan Quigley</maintainer>
   <maintainer email="[email protected]">Marco A. Gutiérrez</maintainer>

rmf_traffic_editor_assets/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package rmf\_traffic\_editor\_assets
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.8.2 (2023-12-15)
+------------------
+
 1.8.1 (2023-08-10)
 ------------------
 

rmf_traffic_editor_assets/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rmf_traffic_editor_assets</name>
-  <version>1.8.1</version>
+  <version>1.8.2</version>
   <description>Assets for use with traffic_editor.</description>
 
   <maintainer email="[email protected]">Brandon Ong</maintainer>

rmf_traffic_editor_test_maps/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package rmf\_traffic\_editor\_test\_maps
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.8.2 (2023-12-15)
+------------------
+
 1.8.1 (2023-08-10)
 ------------------
 * Fix gz classic model download (`#470 <https://github.com/open-rmf/rmf_traffic_editor/pull/470>`_)

rmf_traffic_editor_test_maps/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rmf_traffic_editor_test_maps</name>
-  <version>1.8.1</version>
+  <version>1.8.2</version>
   <description>
     Some test maps for traffic_editor and rmf_building_map_tools.
   </description>