Repository navigation
feat(market_sim): add a Mobile FR3 Duo supermarket sim with AMCL and Nav2 - #1046
sanchit-badamikar wants to merge 7 commits into
Conversation
|
Important Draft PR not reviewedDraft PRs are not automatically reviewed by default.
To automatically review draft PRs, update your CodeRabbit configuration: reviews:
auto_review:
drafts: true
Tip 🎯 Customize your risk assessments
✅ Automatic approvals, with your rules
|
|
Consider whether the change should land upstream in Overlapping files
|
1d28343 to
2fa8e14
Compare
2fa8e14 to
9d154f8
Compare
bkanator
left a comment
bkanator
left a comment
There was a problem hiding this comment.
[written by AI]
Requesting changes, mostly for duplication: much of this package is copied from moveit_pro_franka_ws or hangar_sim; details inline. I built it and ran its 34 tests locally (all pass); I did not run it live.
| `market_sim` is a standalone configuration. Its robot description, planning groups, controllers, | ||
| odometry bridge and scan and base-velocity nodes are copied from the Franka Mobile FR3 Duo | ||
| configuration in `moveit_pro_franka_ws` (`mobile_fr3_duo_mock` and `mobile_fr3_duo_sim`, at commit |
There was a problem hiding this comment.
[written by AI] The SRDF, MoveIt and ros2_control config, scripts, tests and several Objectives are verbatim copies of moveit_pro_franka_ws, and nothing here tracks that upstream, so upstream fixes will never show up as drift. Please vendor it under src/external_dependencies/ with an UPSTREAM.yaml and overlay market_sim on it, as the earlier split did. If a standalone copy is deliberate, please say why in the PR description.
There was a problem hiding this comment.
you don't have to do this, this is mainly just questioning on how you want to handle upstream changes. Keeping separate copies are fine as long as maybe you state that decision somewhere
There was a problem hiding this comment.
[written by AI]
Stated rather than vendored, in e3d6592: README.md now says the copy is deliberate, names the upstream commit it was taken at, and says this package does not track that source so an upstream fix has to be copied here by hand. That follows your follow-up that keeping separate copies is fine as long as the decision is written down somewhere.
| ID="GetLatestTransform" | ||
| name="Read the refined pose" | ||
| target_frame_id="map" | ||
| source_frame_id="base_link" |
There was a problem hiding this comment.
[written by AI] This subtree and localize_robot, localize_robot_if_needed and refine_localization_in_place are hangar_sim's (objectives) with only the base frame, scan topics, max_range and one default changed. Adding ports for those would let one copy serve both packages. A shared location is fine as a follow-up; it shouldn't block this PR.
There was a problem hiding this comment.
[written by AI]
Left as the follow-up you allowed. e3d6592 added xy_variance and yaw_variance ports, but the base frame, the two scan topics, the map topic and max_range are still fixed here, so one copy does not yet serve both packages. Sharing it is a cross-package move and this package is the only consumer today, so it is not folded into this PR.
| Both simulated SICK TIM571s are <camera> sensors with DEPTH_TYPE=THREE_D_LIDAR, so | ||
| picknik_mujoco_ros publishes them as an organized PointCloud2 rather than as a LaserScan. | ||
| Everything downstream of /scan_front and /scan_rear -- the two laser_filters chains, then | ||
| dual_laser_merger, then beluga_amcl on /scan_merged and both Nav2 costmap obstacle layers |
There was a problem hiding this comment.
[written by AI] This package has no dual_laser_merger and no /scan_merged; it uses scan_merger.py and publishes /scan.
| <param name="lidar_publish_rate">10</param> | ||
| <!-- The URDF root odom is not a MuJoCo body, so the base body is named explicitly. --> | ||
| <param name="base_link_name">${base_link_name}</param> | ||
| <!-- world -> mj_world comes from launch/additional_driver_nodes.launch.py, so the plugin must not publish it. --> |
There was a problem hiding this comment.
[written by AI] Stale: world -> mj_world is published by launch/sim/robot_drivers_to_persist_sim.launch.py (line 147); additional_driver_nodes.launch.py doesn't exist here.
There was a problem hiding this comment.
[written by AI]
Fixed in e3d6592. That line now names launch/sim/robot_drivers_to_persist_sim.launch.py, which is the file that publishes world -> mj_world, and the additional_driver_nodes.launch.py reference is gone.
| install( | ||
| DIRECTORY | ||
| config | ||
| description | ||
| launch | ||
| maps | ||
| mjcf | ||
| objectives | ||
| params | ||
| waypoints | ||
| DESTINATION | ||
| share/${PROJECT_NAME} |
There was a problem hiding this comment.
[written by AI] 15 meshes in mjcf/assets/ are byte-identical to franka_description's, and it is already an exec_depend here. Please install-copy them from its share directory at build time (add a build_depend and find_package), as factory_sim/CMakeLists.txt:19-31 does, instead of committing copies.
There was a problem hiding this comment.
[written by AI]
In part: #1087 goes one step further for kitchen_sim and dual_arm_sim: the meshes that are byte-identical to franka_description are loaded from the vendored copy with package://franka_description/... paths, the copies are removed, and nothing is copied at build time. The reworked version of this PR will do the same for market_sim's 14 meshes, so market_sim needs no install-copy rule.
There was a problem hiding this comment.
[written by AI]
Gone further than asked, in e3d6592: the byte-identical meshes are no longer committed at all. The MJCF loads them at run time from the vendored franka_description over package://franka_description/..., so there is no install-copy rule to add and no copies to drift.
There was a problem hiding this comment.
[written by AI]
Follow-up now that the mesh handling has moved on, in ca5ea7d and c8953dd on this branch. The package still commits no franka_description copies. It now also loads 29 convex collision parts and the 13 real visual meshes from a vendored franka_meshes over package://, so the MuJoCo view shows the visual geometry rather than the collision stand-ins, and the five concave links stop colliding as their filled-in hulls. Still no install-copy rule and still nothing duplicated.
| lib/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) |
There was a problem hiding this comment.
[written by AI] Unlike the other sims, this has no ament_lint_auto and no objectives_integration_test. Please add lint at least.
There was a problem hiding this comment.
[written by AI]
Fixed in e3d6592: CMakeLists.txt now has ament_lint_auto with ament_lint_auto_find_test_dependencies(), the matching lint test_depends are in package.xml, and objectives_integration_test is registered alongside the offline pytest targets.
df18f0f to
65ef498
Compare
Adds market_sim: a Franka Mobile FR3 Duo in a MuJoCo supermarket, localizing with Beluga AMCL and navigating with Nav2. The store is generated rather than drawn. One text floor plan gives the rows, the checkouts and the walls, and the generator writes the scene, both maps and the per-aisle navigation Objectives from it, so the world and the goals cannot drift apart. Also brings in the shelf and robot mesh assets the generator builds from.
mjcf/mobile_fr3_duo.xml is copied from moveit_pro_franka_ws (940aa42) and built from franka_description 2.9.0 (FR3 v2, spine, head and TMR base kinematics, inertials and dynamics), MuJoCo Menagerie franka_fr3 (actuator settings) and Menagerie franka_emika_panda (finger coupling from hand.xml). - mjcf/mobile_fr3_duo.xml says so in a comment at its top and that it is modified, as Apache-2.0 section 4(b) asks. - mjcf/NOTICE lists those sources; it sits in mjcf/ so the mesh attribution for mjcf/assets/ can be added separately. - mjcf/LICENSE-franka_description and mjcf/LICENSE-mujoco_menagerie_franka_fr3 are the upstream LICENSE files (Apache-2.0, each with its BSD section). - package.xml declares Apache-2.0 alongside BSD-3-Clause, and the README names the moveit_pro_franka_ws commit the files were copied from. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
…e lidar plane Review rework of the supermarket config, plus the localization fix the first simulator runs turned up. The robot's meshes that are byte-identical to franka_description now load through package:// instead of being copied, and the base's visual mesh is rebuilt as per-material parts with UVs. Licensing collapses to one NOTICE at the package root alongside the upstream licence texts. The store ships empty, so the first change is navigation only. Every base drive goes through one "Drive Base using Nav2" SubTree with the planner as a port, rather than each Objective repeating the call. Both lidar scans reach AMCL through two relays. Comments and docstrings were corrected throughout, and the package now has lint and an Objectives integration test. The localization fix matters most. Both lidars scan a plane just above the bottom deck, so in an empty store that plane passes over the deck and strikes the back panel. The map marked the whole gondola footprint solid, which let a scan point score well anywhere inside it and let the estimate slide by the deck's depth with no penalty. The generator now also writes a map of only what the lidars can see at their height, AMCL and the in-place refinement read that one, and the costmaps keep the full map so the base still never plans onto a deck. Two tests cover it, one of which ray-casts the lidar plane in the simulator and fails against the old map.
…e drives Work from running every localization and navigation Objective against the simulator, which is what turned up the faults below. The store was built on 1.114 m gondolas with 1.6 m aisles, leaving the stowed robot 4 mm of lateral clearance once both shelf faces are inflated by the footprint's inscribed radius. A centre-point planner never notices; one that checks the whole footprint cannot plan in it. Gondola depth is a layout choice rather than a property of the shelf asset, so the asset now stretches in depth to fill it, and the generator recomputes the world, both maps and the aisle goals together. Navigate with SMAC had never worked. The SubTree named its Nav2 tree by a bare filename and leaned on bt_navigator's working directory; the directory was right and Nav2 opened the file, but BehaviorTree.CPP refused the relative path. The driver launch file now writes the stock navigate-to-pose tree with the Smac planner selected to an absolute path, and the SubTree names it. Generating it at startup keeps it from drifting from the stock tree, and the selector is checked so the build fails if that tree stops defaulting to NavFn. The store was also bare: no ceiling, no lights, nothing loose on the floor, and a planning scene holding only the racks, so an arm could plan through a wall, a checkout or the floor. The generator now writes the enclosure and lighting and two stocking carts, and the planning scene gains a box per rack, wall, register and cart plus a finite floor slab, each taken from the geom the simulator collides with. The carts are physical but absent from both maps on purpose, so the costmaps must see them through the lidars. Collision stays simple boxes. Three Objective faults besides. Localize Robot relabelled the marker's frame instead of transforming it, and asserted an input frame the UI need not use. The in-place refinement seeded the filter without a variance, so it discarded the estimate it was refining. The Rotation Shim restarted its heading turn on every replan, making the base stutter. The package also declared none of the Nav2 nodes its launch file starts, resolving them only through a bringup package it never launches that drags a simulator stack into the image. Four nodes caught the wrong exception on SIGINT and could not shut down cleanly. The Objectives test raced the controller spawners, and its drive budget predated the wider store. The floor and walls are textured with layered PBR materials rather than a procedural checker and a flat colour, so the rendered store reads as a shop. Every asset the scene references is installed, and a test resolves each reference against the installed tree rather than the source tree, where a missing install rule looks fine. The base's planar travel limits are widened to match the larger store, and every aisle drive gets the same budget.
65ef498 to
72a124c
Compare
Collapses the 2026-11-03 work answering bkanator's review. Most of the review was already satisfied by the earlier rework; these are the four comments that still needed a change. The lidar flattener described hangar_sim's scanner, not this one. Its docstring and parameter comments named description/ur5e_ridgeback.xml and params/laser_filter_params.yaml, neither of which exists in this package, and carried TIM571 and <rangefinder> history plus a column-to-azimuth mapping measured on an 811-beam sweep when these cameras are 1651-beam nanoScan3s. The provenance line also claimed only the docstring differs from hangar_sim's copy, which the added ExternalShutdownException guard contradicts. The comments now point at mjcf/mobile_fr3_duo.xml's <camera> elements and the launch parameters, and the claims that cannot be checked here are gone. The QoS comment no longer cites a laser_filters chain this package does not have. The package shipped the Apache licence text twice: LICENSE-mujoco_menagerie_franka_fr3 was byte-identical to LICENSE-franka_description. Keep the one named for the file it actually is, franka_description's own licence, and let NOTICE name every source it covers. NOTICE now also points at the store textures' own notice, which are third-party under different terms. Drive Base using Nav2 did not say who reads its tree path. bt_navigator does, and the launch file that writes the Smac copy is the one that starts it, so the file is always where the reader is. README said CMakeLists.txt wrote that copy at build time; the launch file writes it at startup. Two stale claims found while checking: AMCL's scan_topic comment called the two relayed scans merged into one 360 deg scan, and NOTICE said the MJCF records its widened planar travel in a comment it did not have. AGENTS.md records that the other configs keep Nav2's default server timeout, so this config's raise does not read as a workspace-wide setting.
72a124c to
10f4239
Compare
|
|
…franka_meshes MuJoCo collides a mesh as its convex hull, so a link whose collision mesh is far from convex collides as the filled-in shape. The model now collides already convex parts instead, loaded by package:// from the newly vendored franka_meshes, which is the same package and the same pinned commit dual_arm_sim uses. The five source meshes and their part counts are franka_head (3), fr3v2/link5 (6), fr3_duo_mount (4), fr3v2/link3 (8) and fr3v2/link4 (8); the effective-volume figures are in that package's own README and are its measurements, not re-measured here. market_sim is the same Franka variant as dual_arm_sim, a two-arm Mobile FR3 Duo on fr3v2 arms with the fr3_duo_mount_v0_3 mount and the franka_head_v0_2 head, so all five source meshes exist here. link3, link4 and link5 appear once per arm and the mount and head once each, which is eight collision geoms replaced by 51 parts. Checked without a simulator, before against after: bodies 86 to 86, joints 37 to 37, actuators 20 to 20, sites 7 to 7, cameras 6 to 6, bodies with an explicit inertial 42 to 42, and the sum of those masses 178.153827 kg unchanged. Mass cannot move here because every affected body declares its own inertial and the compiler sets no inertiafromgeom, so geom density is not consulted. Mesh assets rise by 29 and colliding geoms by 43, which is the 51 parts less the 8 geoms they replace. All 25 objective XML files and all 41 XML, xacro and urdf files in the package still parse. The parts never reach a URDF, because MoveIt collides the mesh itself, so franka_description's shared robots/common/utils.xacro is untouched, as is everything else under the franka_config snapshot. The visual geoms keep their source mesh, their group and density="0". The model has not yet been loaded through the real package resource path, which needs a built workspace; that proof is outstanding.
The MJCF rendered franka_description's simplified collision STLs as its visual geoms, so the MuJoCo view of the arms, spine, mount, cover, head and hands was coarser than the robot MoveIt Pro draws. Each group-1 geom now carries the matching visual mesh from the vendored franka_meshes instead, over package://, which is the same source and the same wiring dual_arm_sim uses. franka_meshes' snapshot is taken verbatim from the dual-arm branch so the shared dependency is byte-identical in both lanes. That snapshot un-prunes the 13 visual meshes it had dropped, which also makes its own test true as written: VISUAL_COUNT is 13 and 13 are now present, so the assertion was corrected by restoring the files rather than by weakening the number. Its generator also resolves franka_description in the vendored layout, not only the upstream one. 22 group-1 geoms are repointed across those 13 meshes: the spine, mount, cover and head once each, and link0 to link7 and the hand once per arm. The finger meshes and the five tmrv0_2 base meshes are left alone, because those are already real visual meshes rather than collision stand-ins. Collision is provably untouched, before against after: the collision fields of all 82 colliding geoms, taken together as an ordered fingerprint of name, mesh, type, contype, conaffinity, condim, friction, margin, solref, solimp, pos and quat, are unchanged. So are bodies 86, joints 37, actuators 20, sites 7, cameras 6, bodies with an explicit inertial 42, and the sum of those masses at 178.153827 kg. Mesh assets rise by 13 and nothing else moves. All 25 objective XML files and all 41 XML, xacro and urdf files still parse, and all 42 franka_meshes references resolve on disk. The model has not yet been loaded through the real package resource path; that proof is outstanding.
|
[written by AI]
Motivation
The workspace has no mobile manipulator example at store scale. This PR adds
market_sim: a Franka Mobile FR3 Duo in a MuJoCo supermarket, with Beluga AMCL localization and Nav2 navigation.Part of the split of #1031 (see it for the history). Based on #1033 (its assets), which sits on #1045 (kitchen_sim and dual_arm_sim) and #1032 (franka_description 2.9.0). Replaces the closed drafts #1034 and #1035.
Brief description
franka_description. No other config package is needed.scripts/generate_market.pybuilds the store, the occupancy map and the Navigate to Aisle Objectives from a text floor plan. A test fails if the committed files differ from its output.default,loose_a_d,loose_e_i) move the loose shelf units between aisles.scripts/reset_relocalizer.pyre-seeds AMCL at the true pose after a reset.How it was tested
picknikciuser/moveit-pro:main-jazzy: the build and 34 tests pass, the config loads with no parent, the expanded URDF matches the live one, and the scene compiles with its keyframes.bin/validate_workspace_dependencies.py --verify-upstreamand pre-commit pass.Notes for reviewers
main.Release notes
market_simexample: a Franka Mobile FR3 Duo in a simulated supermarket with AMCL localization and Nav2 navigation.