This PR moves our system from fixed workspace walls to dynamically-computed workspace walls. Specifically, the workspace walls are computed as the tightest axis-aligned box that: (a) contains all collision objects that were specified as being within the workspace walls; (b) contains all robot arm configurations that are specified are specified as being within the workspace walls; and (c) has a parameter-specified margin to the above objects/configurations. Additionally, this PR introduces an action server that can be called to recompute the workspace walls (e.g., if the robot arm configurations change).
In the process of creating this PR, it was discovered that ada_planning_scene is becoming too long of a node to easily contain in a single file. Thus, this PR also refactors ada_planning_scene into its own package, and divides functionality into their own classes.
Testing procedure
[x] Tests in sim:
[x] Launch just the ada_planning_scene code, verify that initialization times out and terminates: ros2 launch ada_planning_scene ada_planning_scene_launch.xml
[x] Launch just the moveit code and then the ada_planning_scene code, verify that ada_planning_scene adds the static objects but terminates when trying to add the workspace walls: ros2 launch ada_moveit demo.launch.py sim:=mock
[x] Change the config file by deleting get_urdf_parameter_service_name, urdf_parameter_name, get_robot_configurations_parameter_service_name, namespace_to_use_parameter_name, and robot_configurations_parameter_names. Re-build and launch ada_planning_scene. Verify that initialization succeeds, with workspace walls.
[x] Undo the above changes, set use_workspace_walls to False, re-build and re-run moveit and ada_planning_scene, and verify that initialization succeeds with no workspace walls.
[x] Undo the above changes, terminate all the running code, and re-build
[x] Launch the code in sim: python3 src/ada_feeding/start.py --sim mock
[x] Verify that initialization succeeds and all objects are added to the planning scene as expected (NOTE: on my slow VM it timed out in 40 seconds; should test on the non-VM computers to tune the timeout.).
[x] In the web app, customize the staging configuration by rotating it right until it comes out of the workspace walls.
[x] Using the web app, attempt to move above plate and verify that it fails.
[x] Using the command line, invoke the action to re-compute workspace walls: ros2 action send_goal /recompute_workspace_walls ada_feeding_msgs/action/Trigger "{}" --feedback Verify that the workspace walls get changed in the moveit planning scene to encompass the new staging configuration.
[x] Using the web app, attempt to move above plate and verify it succeeds.
[x] NOTE: dummy face detection will likely move the face into a weird configuration, so you may need to use RVIZ to remove the face from the planning scene for this to succeed.
[x] Launch the code in real: python3 src/ada_feeding/start.py
[x] Verify that initialization succeeds and all objects are added to the planning scene as expected.
[x] In the web app, customize the staging configuration by rotating it right until it comes out of the workspace walls.
[x] Using the web app, attempt to move above plate and verify that it fails.
[x] Using the command line, invoke the action to re-compute workspace walls: ros2 action send_goal /recompute_workspace_walls ada_feeding_msgs/action/Trigger "{}" --feedback Verify that the workspace walls get changed in the moveit planning scene to encompass the new staging configuration.
[x] Using the web app, attempt to move above plate and verify it succeeds.
[x] Terminate moveit. Teleop the arm to a configuration out of the workspace walls. Set the param workspace_walls_contain_current_robot_config to False. Launch the code, try to MoveAbovePlate, and verify it failes. then, terminate moveit again, change that param back to True, launch the code, and verify that MoveAbovePlate succeeds.
Future Work
ada_planning_scene should perhaps save modified table and head parameters, to prevent wrongful collisions when the code restarts (e.g., if the user customized the staging configuration such that the arm collides with the default table but not their actual table).
Before opening a pull request
[x] Format your code using black formatterpython3 -m black .
[x] Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..
Description
This PR moves our system from fixed workspace walls to dynamically-computed workspace walls. Specifically, the workspace walls are computed as the tightest axis-aligned box that: (a) contains all collision objects that were specified as being within the workspace walls; (b) contains all robot arm configurations that are specified are specified as being within the workspace walls; and (c) has a parameter-specified margin to the above objects/configurations. Additionally, this PR introduces an action server that can be called to recompute the workspace walls (e.g., if the robot arm configurations change).
In the process of creating this PR, it was discovered that
ada_planning_sceneis becoming too long of a node to easily contain in a single file. Thus, this PR also refactorsada_planning_sceneinto its own package, and divides functionality into their own classes.Testing procedure
ada_planning_scenecode, verify that initialization times out and terminates:ros2 launch ada_planning_scene ada_planning_scene_launch.xmlmoveitcode and then theada_planning_scenecode, verify thatada_planning_sceneadds the static objects but terminates when trying to add the workspace walls:ros2 launch ada_moveit demo.launch.py sim:=mockget_urdf_parameter_service_name,urdf_parameter_name,get_robot_configurations_parameter_service_name,namespace_to_use_parameter_name, androbot_configurations_parameter_names. Re-build and launchada_planning_scene. Verify that initialization succeeds, with workspace walls.use_workspace_wallsto False, re-build and re-runmoveitandada_planning_scene, and verify that initialization succeeds with no workspace walls.python3 src/ada_feeding/start.py --sim mockros2 action send_goal /recompute_workspace_walls ada_feeding_msgs/action/Trigger "{}" --feedbackVerify that the workspace walls get changed in the moveit planning scene to encompass the new staging configuration.python3 src/ada_feeding/start.pyros2 action send_goal /recompute_workspace_walls ada_feeding_msgs/action/Trigger "{}" --feedbackVerify that the workspace walls get changed in the moveit planning scene to encompass the new staging configuration.workspace_walls_contain_current_robot_configto False. Launch the code, try to MoveAbovePlate, and verify it failes. then, terminate moveit again, change that param back to True, launch the code, and verify that MoveAbovePlate succeeds.Future Work
ada_planning_sceneshould perhaps save modified table and head parameters, to prevent wrongful collisions when the code restarts (e.g., if the user customized the staging configuration such that the arm collides with the default table but not their actual table).Before opening a pull request
python3 -m black .ada_feedingdirectory, run:pylint --recursive=y --rcfile=.pylintrc ..Before Merging
Squash & Merge