diff --git a/rmf_building_map_tools/CHANGELOG.rst b/rmf_building_map_tools/CHANGELOG.rst index 74c42725..b3395255 100644 --- a/rmf_building_map_tools/CHANGELOG.rst +++ b/rmf_building_map_tools/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf\_building\_map\_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.11.0 (2024-11-27) +------------------- +* Fix startup error when waypoint's name is pure numbers (`#508 `_) +* Contributors: Gary Bey + 1.10.0 (2024-06-12) ------------------- diff --git a/rmf_building_map_tools/package.xml b/rmf_building_map_tools/package.xml index b738fa76..d28a0313 100644 --- a/rmf_building_map_tools/package.xml +++ b/rmf_building_map_tools/package.xml @@ -2,7 +2,7 @@ rmf_building_map_tools - 1.10.0 + 1.11.0 RMF Building map tools Morgan Quigley Marco A. Gutiérrez diff --git a/rmf_building_map_tools/setup.py b/rmf_building_map_tools/setup.py index 6a623c38..316b42be 100644 --- a/rmf_building_map_tools/setup.py +++ b/rmf_building_map_tools/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version='1.10.0', + version='1.11.0', packages=[ 'building_crowdsim', 'building_crowdsim.navmesh', diff --git a/rmf_traffic_editor/CHANGELOG.rst b/rmf_traffic_editor/CHANGELOG.rst index 92c394cd..511dee52 100644 --- a/rmf_traffic_editor/CHANGELOG.rst +++ b/rmf_traffic_editor/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rmf\_traffic\_editor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.11.0 (2024-11-27) +------------------- +* Fix lift cabin graphics on rotated floor plans (`#513 `_) +* Fix crash when no features are available to calculate a layer transform (`#460 `_) +* Addition of delete button for lift table (`#511 `_) +* Fix reference file name being kept when loading new building (`#501 `_) +* Contributors: Luca Della Vedova, Tan Jun Kiat + 1.10.0 (2024-06-12) ------------------- diff --git a/rmf_traffic_editor/gui/building.cpp b/rmf_traffic_editor/gui/building.cpp index e38b7a0c..64887bfb 100644 --- a/rmf_traffic_editor/gui/building.cpp +++ b/rmf_traffic_editor/gui/building.cpp @@ -429,7 +429,8 @@ void Building::draw_lifts(QGraphicsScene* scene, const int level_idx) true, t.scale, t.dx, - t.dy); + t.dy, + t.rotation); } } @@ -499,6 +500,7 @@ Building::Transform Building::compute_transform( t.scale = 1.0; t.dx = 0.0; t.dy = 0.0; + t.rotation = 0.0; return t; } @@ -520,6 +522,25 @@ Building::Transform Building::compute_transform( } } + // calculate the rotation between each pair of fiducials + vector> rotations; + // we take the first fiducial as the reference point + std::pair ref_rotation = make_pair(fiducials[0].first, + fiducials[0].second); + for (std::size_t f_idx = 1; f_idx < fiducials.size(); f_idx++) + { + rotations.push_back( + make_pair( + fiducials[f_idx].first.rotation(ref_rotation.first), + fiducials[f_idx].second.rotation(ref_rotation.second))); + } + + // it is a crude method for now, but we will just compute the mean of the angles of all the fiducial pairs + double relative_rotation_sum = 0; + for (std::size_t i = 0; i < rotations.size(); i++) + relative_rotation_sum += rotations[i].first - rotations[i].second; + const double rotation = relative_rotation_sum / rotations.size(); + // calculate the distances between each fiducial on their levels vector> distances; for (std::size_t f0_idx = 0; f0_idx < fiducials.size(); f0_idx++) @@ -549,8 +570,12 @@ Building::Transform Building::compute_transform( double trans_y_sum = 0; for (const auto& fiducial : fiducials) { - trans_x_sum += fiducial.second.x - fiducial.first.x * scale; - trans_y_sum += fiducial.second.y - fiducial.first.y * scale; + trans_x_sum += fiducial.second.x - + (std::cos(rotation) * fiducial.first.x + std::sin(rotation) * + fiducial.first.y) * scale; + trans_y_sum += fiducial.second.y - + (-std::sin(rotation) * fiducial.first.x + std::cos(rotation) * + fiducial.first.y) * scale; } const double trans_x = trans_x_sum / fiducials.size(); const double trans_y = trans_y_sum / fiducials.size(); @@ -559,13 +584,16 @@ Building::Transform Building::compute_transform( t.scale = scale; t.dx = trans_x; t.dy = trans_y; + t.rotation = rotation; - printf("transform %d->%d: scale = %.5f translation = (%.2f, %.2f)\n", + printf( + "transform %d->%d: scale = %.5f translation = (%.2f, %.2f) rotation = %.5f\n", from_level_idx, to_level_idx, t.scale, t.dx, - t.dy); + t.dy, + t.rotation); return t; } @@ -772,6 +800,16 @@ Polygon::EdgeDragPolygon Building::polygon_edge_drag_press( return levels[level_idx].polygon_edge_drag_press(polygon, x, y); } +Lift Building::get_lift(const std::string& name) const +{ + for (const auto& lift : lifts) + { + if (lift.name == name) + return lift; + } + return Lift(); +} + void Building::add_constraint( const int level_idx, const QUuid& a, diff --git a/rmf_traffic_editor/gui/building.h b/rmf_traffic_editor/gui/building.h index d361155d..2ab9c773 100644 --- a/rmf_traffic_editor/gui/building.h +++ b/rmf_traffic_editor/gui/building.h @@ -147,6 +147,7 @@ class Building double scale = 1.0; double dx = 0.0; double dy = 0.0; + double rotation = 0.0; }; typedef std::map TransformMap; TransformMap transforms; @@ -195,6 +196,8 @@ class Building const double x, const double y); + Lift get_lift(const std::string& name) const; + private: std::string filename; }; diff --git a/rmf_traffic_editor/gui/editor.cpp b/rmf_traffic_editor/gui/editor.cpp index 4b80f05c..ede3ff9c 100644 --- a/rmf_traffic_editor/gui/editor.cpp +++ b/rmf_traffic_editor/gui/editor.cpp @@ -2009,11 +2009,57 @@ void Editor::mouse_move( } else if (mouse_vertex_idx >= 0) { + if (!building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin() + .empty()) + { + auto lift_name = + building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin(); + auto lift = building.get_lift(lift_name); + auto lift_ref_level = lift.reference_floor_name; + if (lift_ref_level != building.levels[level_idx].name) + { + QMessageBox::critical( + this, + "Could not move vertex", + "Vertex is not on the reference level."); + return; + } + } + // we're dragging a vertex Vertex& pt = building.levels[level_idx].vertices[mouse_vertex_idx]; pt.x = p.x(); pt.y = p.y(); + + // update the other levels if a lift cabin vertex is moved + if (!building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin() + .empty()) + { + for (size_t i = 0; i < building.levels.size(); i++) + { + if (i == static_cast(level_idx)) + continue; + + auto& level = building.levels[i]; + auto& vertices = level.vertices; + auto t = building.get_transform(level_idx, i); + + for (auto& vertex : level.vertices) + { + if (vertex.lift_cabin() == pt.lift_cabin()) + { + auto rotated_x = std::cos(-t.rotation)*p.x() - + std::sin(-t.rotation)*p.y(); + auto rotated_y = std::sin(-t.rotation)*p.x() + + std::cos(-t.rotation)*p.y(); + vertex.x = rotated_x * t.scale + t.dx; + vertex.y = rotated_y * t.scale + t.dy; + } + } + } + } + latest_move_vertex->set_final_destination(p.x(), p.y()); create_scene(); } diff --git a/rmf_traffic_editor/gui/fiducial.cpp b/rmf_traffic_editor/gui/fiducial.cpp index 2137e9e6..4e1002f2 100644 --- a/rmf_traffic_editor/gui/fiducial.cpp +++ b/rmf_traffic_editor/gui/fiducial.cpp @@ -90,3 +90,10 @@ double Fiducial::distance(const Fiducial& f) const double dy = f.y - y; return std::sqrt(dx*dx + dy*dy); } + +double Fiducial::rotation(const Fiducial& f) +{ + const double dx = x - f.x; + const double dy = y - f.y; + return std::atan2(dy, dx); +} diff --git a/rmf_traffic_editor/gui/fiducial.h b/rmf_traffic_editor/gui/fiducial.h index 9f9728a7..63dab5d9 100644 --- a/rmf_traffic_editor/gui/fiducial.h +++ b/rmf_traffic_editor/gui/fiducial.h @@ -46,6 +46,7 @@ class Fiducial void draw(QGraphicsScene*, const double meters_per_pixel) const; double distance(const Fiducial& f); + double rotation(const Fiducial& f); }; #endif diff --git a/rmf_traffic_editor/gui/level.cpp b/rmf_traffic_editor/gui/level.cpp index 02d41d03..acc5d8df 100644 --- a/rmf_traffic_editor/gui/level.cpp +++ b/rmf_traffic_editor/gui/level.cpp @@ -1796,6 +1796,7 @@ void Level::optimize_layer_transforms() layers[i].transform.translation().x(), layers[i].transform.translation().y() }; + int num_constraints_found = 0; for (const Constraint& constraint : constraints) { @@ -1865,8 +1866,15 @@ void Level::optimize_layer_transforms() &scale, &translation[0]); + ++num_constraints_found; } + // Make sure there were features for this layer, otherwise ceres will fail + if (num_constraints_found == 0) + { + printf("No constraints found for layer, skipping optimization\n"); + continue; + } problem.SetParameterLowerBound(&scale, 0, 0.01); ceres::Solver::Options options; diff --git a/rmf_traffic_editor/gui/lift.cpp b/rmf_traffic_editor/gui/lift.cpp index a6fed06f..85683ab5 100644 --- a/rmf_traffic_editor/gui/lift.cpp +++ b/rmf_traffic_editor/gui/lift.cpp @@ -145,7 +145,8 @@ void Lift::draw( const bool apply_transformation, const double scale, const double translate_x, - const double translate_y) const + const double translate_y, + const double rotation) const { if (elevation > highest_elevation || elevation < lowest_elevation) return; @@ -216,8 +217,11 @@ void Lift::draw( if (apply_transformation) { - group->setRotation(-180.0 / 3.1415926 * yaw); - group->setPos(x * scale + translate_x, y * scale + translate_y); + auto rotated_x = std::cos(-rotation)*x - std::sin(-rotation)*y; + auto rotated_y = std::sin(-rotation)*x + std::cos(-rotation)*y; + group->setRotation(-180.0 / 3.1415926 * (yaw + rotation)); + group->setPos(rotated_x * scale + translate_x, + rotated_y * scale + translate_y); } } diff --git a/rmf_traffic_editor/gui/lift.h b/rmf_traffic_editor/gui/lift.h index 737063bb..75f5569b 100644 --- a/rmf_traffic_editor/gui/lift.h +++ b/rmf_traffic_editor/gui/lift.h @@ -92,7 +92,8 @@ class Lift const bool apply_transformation = true, const double scale = 1.0, const double translate_x = 0.0, - const double translate_y = 0.0) const; + const double translate_y = 0.0, + const double rotation = 0.0) const; bool level_door_opens( const std::string& level_name, diff --git a/rmf_traffic_editor/package.xml b/rmf_traffic_editor/package.xml index 194bed98..8a15d8c3 100644 --- a/rmf_traffic_editor/package.xml +++ b/rmf_traffic_editor/package.xml @@ -2,7 +2,7 @@ rmf_traffic_editor - 1.10.0 + 1.11.0 traffic editor Morgan Quigley Marco A. Gutiérrez diff --git a/rmf_traffic_editor_assets/CHANGELOG.rst b/rmf_traffic_editor_assets/CHANGELOG.rst index 19ad8966..6f8a3cdc 100644 --- a/rmf_traffic_editor_assets/CHANGELOG.rst +++ b/rmf_traffic_editor_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf\_traffic\_editor\_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.11.0 (2024-11-27) +------------------- + 1.10.0 (2024-06-12) ------------------- diff --git a/rmf_traffic_editor_assets/package.xml b/rmf_traffic_editor_assets/package.xml index f94717fa..1aa3767d 100644 --- a/rmf_traffic_editor_assets/package.xml +++ b/rmf_traffic_editor_assets/package.xml @@ -2,7 +2,7 @@ rmf_traffic_editor_assets - 1.10.0 + 1.11.0 Assets for use with traffic_editor. Brandon Ong diff --git a/rmf_traffic_editor_test_maps/CHANGELOG.rst b/rmf_traffic_editor_test_maps/CHANGELOG.rst index b761f4ed..68795f4f 100644 --- a/rmf_traffic_editor_test_maps/CHANGELOG.rst +++ b/rmf_traffic_editor_test_maps/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf\_traffic\_editor\_test\_maps ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.11.0 (2024-11-27) +------------------- + 1.10.0 (2024-06-12) ------------------- diff --git a/rmf_traffic_editor_test_maps/package.xml b/rmf_traffic_editor_test_maps/package.xml index 908ca805..48a8f071 100644 --- a/rmf_traffic_editor_test_maps/package.xml +++ b/rmf_traffic_editor_test_maps/package.xml @@ -2,7 +2,7 @@ rmf_traffic_editor_test_maps - 1.10.0 + 1.11.0 Some test maps for traffic_editor and rmf_building_map_tools.