├── manymove_bringup ├── resource │ └── manymove_bringup ├── test │ ├── __pycache__ │ │ └── test_black.cpython-310-pytest-6.2.5.pyc │ ├── test_copyright.py │ └── test_pep257.py ├── config │ └── ur │ │ ├── pilz_cartesian_limits.yaml │ │ ├── pilz_industrial_motion_planner_planning.yaml │ │ ├── controllers_fake_minimal.yaml │ │ ├── chomp_planning.yaml │ │ ├── ompl_planning.yaml │ │ ├── ompl_planning.legacy.yaml │ │ └── moveit_cpp.yaml ├── CMakeLists.txt ├── docker │ ├── scripts │ │ └── auto_source_overlay.sh │ └── Dockerfile.manymove_xarm ├── LICENSE ├── manymove_bringup │ ├── __init__.py │ └── ros_compat.py ├── package.xml └── srdf │ └── ur_with_robotiq.srdf.xacro ├── manymove_planner ├── config │ ├── isaac_ros │ │ ├── .isaac_ros_common-config │ │ ├── lite6.usd │ │ ├── lite6_grf.usd │ │ ├── charuco_12x9.png │ │ ├── lite6_ros2.usd │ │ ├── lite6_grf_ros2.usd │ │ ├── example_scene_lite6_grf_ros2.usd │ │ ├── Dockerfile.manymove │ │ ├── build-manymove.sh │ │ ├── lite6_grf.xrdf │ │ ├── lite6.xrdf │ │ ├── cumotion_startup.md │ │ └── lite6_gr.xrdf │ ├── isaac_ros_cumotion_planning.yaml │ ├── moveit_cpp_real_L_ufactory.yaml │ ├── moveit_cpp_real_M_ufactory.yaml │ ├── moveit_cpp_real_R_ufactory.yaml │ ├── moveit_cpp_real_ufactory.yaml │ ├── moveit_cpp_cumotion.yaml │ ├── moveit_cpp.yaml │ └── sensors_3d.yaml ├── LICENSE ├── include │ └── manymove_planner │ │ └── compat │ │ ├── tf2_linear_compat.hpp │ │ └── cartesian_interpolator_compat.hpp ├── package.xml ├── README.md └── src │ └── action_server_node.cpp ├── media ├── manymove_example.gif └── manymove_structure.png ├── manymove_cpp_trees ├── tutorials │ ├── image.png │ └── media │ │ ├── empty_scene.png │ │ ├── tutorial_01.gif │ │ ├── init_ground_obj.png │ │ ├── SpawnFixedObjects.png │ │ ├── tutorial_01_section_1.png │ │ ├── tutorial_01_section_2.png │ │ └── complete_tree_tutorial_01.png ├── package.xml ├── LICENSE ├── include │ └── manymove_cpp_trees │ │ ├── compat │ │ └── tf2_linear_compat.hpp │ │ ├── blackboard_utils.hpp │ │ ├── hmi_utils.hpp │ │ ├── hmi_service_node.hpp │ │ └── main_imports_helper.hpp ├── test │ ├── test_bt_converters.cpp │ └── test_tf_node.cpp ├── launch │ ├── bt_lite_fake.launch.py │ ├── bt_lite_real.launch.py │ ├── bt_panda_fake.launch.py │ ├── bt_lite_fake_dual.launch.py │ └── bt_lite_fake_dual_app.launch.py └── README.md ├── manymove_object_manager ├── meshes │ ├── unit_tube.stl │ ├── custom_scene │ │ ├── machine.stl │ │ ├── slider.stl │ │ └── end_plate.stl │ └── custom_end_tools │ │ ├── tube_holder.stl │ │ └── pneumatic_lite.stl ├── package.xml ├── LICENSE ├── config │ └── objects.yaml ├── include │ └── manymove_object_manager │ │ └── compat │ │ ├── tf2_linear_compat.hpp │ │ └── rclcpp_client_compat.hpp ├── src │ └── object_manager_main.cpp ├── launch │ ├── object_manager.launch.py │ └── collision_spawner.launch.py ├── README.md ├── test │ ├── fake_planning_scene_server.hpp │ └── fake_planning_scene_server.cpp └── CMakeLists.txt ├── manymove_msgs ├── action │ ├── ResetRobotState.action │ ├── CheckRobotState.action │ ├── LoadTrajController.action │ ├── UnloadTrajController.action │ ├── PlanManipulator.action │ ├── SetOutput.action │ ├── GetInput.action │ ├── CheckObjectExists.action │ ├── GetObjectPose.action │ ├── RemoveCollisionObject.action │ ├── MoveManipulator.action │ ├── AttachDetachObject.action │ └── AddCollisionObject.action ├── srv │ └── SetBlackboardValues.srv ├── msg │ ├── MoveManipulatorGoal.msg │ └── MovementConfig.msg ├── package.xml ├── LICENSE ├── CMakeLists.txt └── README.md ├── .clang-format ├── manymove_hmi ├── package.xml ├── LICENSE ├── include │ └── manymove_hmi │ │ ├── default_app_hmi.hpp │ │ ├── ros2_worker.hpp │ │ └── hmi_gui.hpp ├── test │ └── test_hmi_gui.cpp └── CMakeLists.txt ├── .pre-commit-config.yaml ├── LICENSE ├── pyproject.toml └── .github └── workflows └── ci.yml /manymove_bringup/resource/manymove_bringup: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/.isaac_ros_common-config: -------------------------------------------------------------------------------- 1 | CONFIG_IMAGE_KEY=ros2_humble.realsense.manymove 2 | -------------------------------------------------------------------------------- /media/manymove_example.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/media/manymove_example.gif -------------------------------------------------------------------------------- /media/manymove_structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/media/manymove_structure.png -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/image.png -------------------------------------------------------------------------------- /manymove_object_manager/meshes/unit_tube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/unit_tube.stl -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/lite6.usd -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6_grf.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/lite6_grf.usd -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/empty_scene.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/empty_scene.png -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/tutorial_01.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/tutorial_01.gif -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/charuco_12x9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/charuco_12x9.png -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6_ros2.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/lite6_ros2.usd -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6_grf_ros2.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/lite6_grf_ros2.usd -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/init_ground_obj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/init_ground_obj.png -------------------------------------------------------------------------------- /manymove_object_manager/meshes/custom_scene/machine.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/custom_scene/machine.stl -------------------------------------------------------------------------------- /manymove_object_manager/meshes/custom_scene/slider.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/custom_scene/slider.stl -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/SpawnFixedObjects.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/SpawnFixedObjects.png -------------------------------------------------------------------------------- /manymove_msgs/action/ResetRobotState.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | --- 3 | 4 | # Result 5 | bool success 6 | string message 7 | 8 | --- 9 | # Feedback 10 | string status 11 | -------------------------------------------------------------------------------- /manymove_object_manager/meshes/custom_scene/end_plate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/custom_scene/end_plate.stl -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/tutorial_01_section_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/tutorial_01_section_1.png -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/tutorial_01_section_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/tutorial_01_section_2.png -------------------------------------------------------------------------------- /manymove_cpp_trees/tutorials/media/complete_tree_tutorial_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_cpp_trees/tutorials/media/complete_tree_tutorial_01.png -------------------------------------------------------------------------------- /manymove_object_manager/meshes/custom_end_tools/tube_holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/custom_end_tools/tube_holder.stl -------------------------------------------------------------------------------- /manymove_object_manager/meshes/custom_end_tools/pneumatic_lite.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_object_manager/meshes/custom_end_tools/pneumatic_lite.stl -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/example_scene_lite6_grf_ros2.usd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_planner/config/isaac_ros/example_scene_lite6_grf_ros2.usd -------------------------------------------------------------------------------- /manymove_msgs/action/CheckRobotState.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | --- 3 | 4 | # Result 5 | bool ready 6 | int32 err 7 | int32 mode 8 | int32 state 9 | string message 10 | 11 | --- 12 | # Feedback 13 | -------------------------------------------------------------------------------- /manymove_bringup/test/__pycache__/test_black.cpython-310-pytest-6.2.5.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pastoriomarco/manymove/HEAD/manymove_bringup/test/__pycache__/test_black.cpython-310-pytest-6.2.5.pyc -------------------------------------------------------------------------------- /manymove_msgs/action/LoadTrajController.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string controller_name 3 | 4 | --- 5 | # Result 6 | bool success 7 | string message 8 | 9 | --- 10 | # Feedback 11 | float32 progress 12 | -------------------------------------------------------------------------------- /manymove_msgs/action/UnloadTrajController.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string controller_name 3 | 4 | --- 5 | # Result 6 | bool success 7 | string message 8 | 9 | --- 10 | # Feedback 11 | float32 progress 12 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Cartesian limits for Pilz planners 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -2.25 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /manymove_msgs/srv/SetBlackboardValues.srv: -------------------------------------------------------------------------------- 1 | string[] key 2 | string[] value_type # e.g. ["bool", "double", "string", "double_array", "pose", ...] 3 | string[] value_data 4 | --- 5 | bool success 6 | string message 7 | -------------------------------------------------------------------------------- /manymove_msgs/action/PlanManipulator.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | manymove_msgs/MoveManipulatorGoal goal 3 | --- 4 | # Result 5 | bool success 6 | trajectory_msgs/JointTrajectory trajectory 7 | --- 8 | # Feedback 9 | float32 progress 10 | -------------------------------------------------------------------------------- /manymove_msgs/action/SetOutput.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string io_type # "tool" or "controller" 3 | int16 ionum 4 | int16 value # 1 for ON, 0 for OFF 5 | 6 | --- 7 | # Result 8 | bool success 9 | string message 10 | 11 | --- 12 | # Feedback 13 | string status 14 | -------------------------------------------------------------------------------- /manymove_msgs/action/GetInput.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string io_type # "tool" or "controller" 3 | int16 ionum 4 | 5 | --- 6 | # Result 7 | int16 value # 1 for ON, 0 for OFF 8 | bool success 9 | string message 10 | 11 | --- 12 | # Feedback 13 | string status 14 | -------------------------------------------------------------------------------- /manymove_msgs/msg/MoveManipulatorGoal.msg: -------------------------------------------------------------------------------- 1 | string movement_type # "pose", "joint", "named", "cartesian" 2 | geometry_msgs/Pose pose_target 3 | string named_target 4 | float64[] joint_values 5 | float64[] start_joint_values 6 | manymove_msgs/MovementConfig config 7 | -------------------------------------------------------------------------------- /manymove_msgs/action/CheckObjectExists.action: -------------------------------------------------------------------------------- 1 | # Goal definition 2 | string object_id 3 | 4 | --- 5 | # Result definition 6 | bool exists 7 | bool is_attached 8 | string link_name 9 | string message 10 | 11 | --- 12 | # Feedback definition 13 | # (No feedback needed for this simple action) 14 | -------------------------------------------------------------------------------- /manymove_msgs/action/GetObjectPose.action: -------------------------------------------------------------------------------- 1 | # Goal definition 2 | string object_id 3 | float64[] pre_transform_xyz_rpy 4 | float64[] post_transform_xyz_rpy 5 | string link_name 6 | --- 7 | # Result definition 8 | geometry_msgs/Pose pose 9 | bool success 10 | string message 11 | --- 12 | # Feedback definition 13 | string status 14 | -------------------------------------------------------------------------------- /manymove_msgs/action/RemoveCollisionObject.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string id # Unique identifier of the object to remove 3 | 4 | --- 5 | # Result 6 | bool success # Indicate if the object was removed successfully 7 | string message # Additional information or error messages 8 | 9 | --- 10 | # Feedback 11 | string status # Current status message 12 | -------------------------------------------------------------------------------- /manymove_msgs/action/MoveManipulator.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | manymove_msgs/MoveManipulatorGoal plan_request 3 | trajectory_msgs/JointTrajectory existing_trajectory 4 | 5 | --- 6 | # Result 7 | bool success 8 | string message 9 | int32 error_code # 0 = success, 1 = aborted, 2 = canceled, 3 = collision 10 | trajectory_msgs/JointTrajectory final_trajectory 11 | 12 | --- 13 | # Feedback 14 | float32 progress 15 | bool in_collision 16 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros_cumotion_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_plugin: isaac_ros_cumotion_moveit/CumotionPlanner 2 | request_adapters: >- 3 | default_planner_request_adapters/FixWorkspaceBounds 4 | default_planner_request_adapters/FixStartStateBounds 5 | default_planner_request_adapters/FixStartStateCollision 6 | default_planner_request_adapters/FixStartStatePathConstraints 7 | start_state_max_bounds_error: 0.1 8 | num_steps: 32 9 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: AlwaysBreak 7 | BraceWrapping: 8 | AfterClass: true 9 | AfterFunction: true 10 | AfterNamespace: true 11 | AfterStruct: true 12 | BreakBeforeBraces: Custom 13 | ColumnLimit: 100 14 | ConstructorInitializerIndentWidth: 0 15 | ContinuationIndentWidth: 2 16 | DerivePointerAlignment: false 17 | PointerAlignment: Middle 18 | ReflowComments: false 19 | ... 20 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/pilz_industrial_motion_planner_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_plugin: pilz_industrial_motion_planner/CommandPlanner 2 | request_adapters: >- 3 | default_planner_request_adapters/FixWorkspaceBounds 4 | default_planner_request_adapters/FixStartStateBounds 5 | default_planner_request_adapters/FixStartStateCollision 6 | default_planner_request_adapters/FixStartStatePathConstraints 7 | default_planner_config: PTP 8 | capabilities: >- 9 | pilz_industrial_motion_planner/MoveGroupSequenceAction 10 | pilz_industrial_motion_planner/MoveGroupSequenceService 11 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/controllers_fake_minimal.yaml: -------------------------------------------------------------------------------- 1 | controller_names: 2 | - joint_trajectory_controller 3 | - robotiq_gripper_controller 4 | 5 | joint_trajectory_controller: 6 | type: FollowJointTrajectory 7 | action_ns: '' 8 | default: true 9 | joints: 10 | - shoulder_pan_joint 11 | - shoulder_lift_joint 12 | - elbow_joint 13 | - wrist_1_joint 14 | - wrist_2_joint 15 | - wrist_3_joint 16 | 17 | robotiq_gripper_controller: 18 | type: GripperCommand 19 | action_ns: gripper_cmd 20 | default: true 21 | joints: 22 | - robotiq_85_left_knuckle_joint 23 | -------------------------------------------------------------------------------- /manymove_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(manymove_bringup) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(ament_cmake_python REQUIRED) 6 | 7 | ament_python_install_package(${PROJECT_NAME}) 8 | 9 | install(FILES package.xml DESTINATION share/${PROJECT_NAME}) 10 | 11 | install( 12 | DIRECTORY 13 | config 14 | launch 15 | urdf 16 | srdf 17 | DESTINATION share/${PROJECT_NAME} 18 | ) 19 | 20 | install( 21 | DIRECTORY resource/ 22 | DESTINATION share/${PROJECT_NAME}/resource 23 | FILES_MATCHING PATTERN "*" 24 | ) 25 | 26 | ament_package() 27 | -------------------------------------------------------------------------------- /manymove_bringup/docker/scripts/auto_source_overlay.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Abort if shell is not interactive 4 | if [[ "$-" != *i* ]]; then 5 | return 0 2>/dev/null || exit 0 6 | fi 7 | 8 | WORKSPACE_ROOT="${MANYMOVE_WS:-/opt/manymove_ws}" 9 | OVERLAY="${WORKSPACE_ROOT}/install/setup.bash" 10 | 11 | # Avoid re-sourcing within the same shell session 12 | if [[ -n "${MANYMOVE_OVERLAY_SOURCED:-}" ]]; then 13 | return 0 2>/dev/null || exit 0 14 | fi 15 | 16 | if [[ -f "${OVERLAY}" ]]; then 17 | # shellcheck disable=SC1090 18 | source "${OVERLAY}" 19 | export MANYMOVE_OVERLAY_SOURCED=1 20 | fi 21 | 22 | return 0 2>/dev/null || exit 0 23 | -------------------------------------------------------------------------------- /manymove_msgs/action/AttachDetachObject.action: -------------------------------------------------------------------------------- 1 | # Goal definition 2 | string object_id # Unique identifier of the object 3 | string link_name # Name of the robot link to attach/detach the object 4 | bool attach # true to attach, false to detach 5 | string[] touch_links # (Optional) List of robot links to exclude from collision checking 6 | 7 | --- 8 | # Result definition 9 | bool success # Indicate if the operation was successful 10 | string message # Additional information or error messages 11 | 12 | --- 13 | # Feedback definition 14 | string status # Current status message 15 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp_real_L_ufactory.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/L_ufactory/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ["ompl"] 12 | 13 | plan_request_params: 14 | planning_attempts: 1 15 | planning_pipeline: ompl 16 | planner_id: RRTConnect 17 | max_velocity_scaling_factor: 1.0 18 | max_acceleration_scaling_factor: 1.0 19 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp_real_M_ufactory.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/M_ufactory/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ["ompl"] 12 | 13 | plan_request_params: 14 | planning_attempts: 1 15 | planning_pipeline: ompl 16 | planner_id: RRTConnect 17 | max_velocity_scaling_factor: 1.0 18 | max_acceleration_scaling_factor: 1.0 19 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp_real_R_ufactory.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/R_ufactory/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ["ompl"] 12 | 13 | plan_request_params: 14 | planning_attempts: 1 15 | planning_pipeline: ompl 16 | planner_id: RRTConnect 17 | max_velocity_scaling_factor: 1.0 18 | max_acceleration_scaling_factor: 1.0 19 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp_real_ufactory.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/ufactory/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ["ompl"] 12 | 13 | plan_request_params: 14 | planning_attempts: 1 15 | planning_pipeline: ompl 16 | planner_id: RRTConnect 17 | max_velocity_scaling_factor: 1.0 18 | max_acceleration_scaling_factor: 1.0 19 | -------------------------------------------------------------------------------- /manymove_msgs/action/AddCollisionObject.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | string id # Unique identifier for the object 3 | string shape # Shape type: box, cylinder, sphere 4 | string mesh_file # Path to mesh file (used if shape == "mesh") 5 | float64[] dimensions # Dimensions corresponding to the shape 6 | geometry_msgs/Pose pose # Pose of the object 7 | float64[] scale_mesh # Value to scale the mesh object in X, Y and Z axis 8 | 9 | --- 10 | # Result 11 | bool success # Indicate if the object was added successfully 12 | string message # Additional information or error messages 13 | 14 | --- 15 | # Feedback 16 | string status # Current status message 17 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp_cumotion.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ['isaac_ros_cumotion'] 12 | 13 | default_planning_pipeline: 'isaac_ros_cumotion' 14 | 15 | plan_request_params: 16 | planning_attempts: 1 17 | planning_pipeline: isaac_ros_cumotion 18 | planner_id: cuMotion 19 | max_velocity_scaling_factor: 1.0 20 | max_acceleration_scaling_factor: 1.0 21 | -------------------------------------------------------------------------------- /manymove_bringup/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Validate that project files include the required copyright headers.""" 16 | 17 | import pytest 18 | from ament_copyright.main import main 19 | 20 | 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | """Run the ament-copyright linter.""" 25 | rc = main(argv=['.', 'test']) 26 | assert rc == 0, 'Found errors' 27 | -------------------------------------------------------------------------------- /manymove_planner/config/moveit_cpp.yaml: -------------------------------------------------------------------------------- 1 | planning_scene_monitor_options: 2 | name: "planning_scene_monitor" 3 | robot_description: "robot_description" 4 | joint_state_topic: "/joint_states" 5 | attached_collision_object_topic: "/attached_collision_object" 6 | publish_planning_scene_topic: "/planning_scene" 7 | monitored_planning_scene_topic: "/monitored_planning_scene" 8 | wait_for_initial_state_timeout: 10.0 9 | 10 | planning_pipelines: 11 | pipeline_names: ["ompl", "chomp", "pilz_industrial_motion_planner"] 12 | # in Jazzy, you can leverage STOMP too: update the above line adding "stomp", then uncomment the STOMP-related definition in move.hpp in src/manymove/manymove_cpp_trees/include/manymove_cpp_trees/move.hpp 13 | # You also need the _planning.yaml in your moveit config, but for github.com/pastoriomarco/xarm_ros2 they are already there 14 | 15 | plan_request_params: 16 | planning_attempts: 1 17 | planning_pipeline: ompl 18 | planner_id: RRTConnect 19 | max_velocity_scaling_factor: 1.0 20 | max_acceleration_scaling_factor: 1.0 21 | -------------------------------------------------------------------------------- /manymove_bringup/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Validate that the package follows the PEP 257 docstring conventions.""" 16 | 17 | import pytest 18 | from ament_pep257.main import main 19 | 20 | 21 | @pytest.mark.linter 22 | @pytest.mark.pep257 23 | def test_pep257(): 24 | """Run ament_pep257 using the shared project configuration.""" 25 | rc = main(argv=['.', 'test']) 26 | assert rc == 0, 'Found code style errors / warnings' 27 | -------------------------------------------------------------------------------- /manymove_hmi/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_hmi 5 | 0.3.2 6 | Interface to control execution of manymove behaviortree packages 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | 13 | rclcpp 14 | std_msgs 15 | std_srvs 16 | manymove_msgs 17 | 18 | qtbase5-dev 19 | qtbase5-dev 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | ament_cmake_uncrustify 24 | ament_uncrustify 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | # Generic whitespace and file hooks 3 | - repo: https://github.com/pre-commit/pre-commit-hooks 4 | rev: v5.0.0 5 | hooks: 6 | - id: end-of-file-fixer 7 | - id: trailing-whitespace 8 | - id: check-added-large-files 9 | 10 | # Import sorting – runs before Ruff so Ruff doesn't undo the ordering 11 | - repo: https://github.com/pre-commit/mirrors-isort 12 | rev: v5.9.3 13 | hooks: 14 | - id: isort 15 | args: 16 | - "--profile=black" 17 | - "--line-length=100" 18 | - "--force-single-line-imports" 19 | 20 | # Python linting/formatting via Ruff 21 | - repo: https://github.com/astral-sh/ruff-pre-commit 22 | rev: v0.4.4 23 | hooks: 24 | - id: ruff 25 | args: 26 | - "--config=pyproject.toml" 27 | - "--fix" # Automatically apply fixes where possible 28 | additional_dependencies: 29 | - "flake8-quotes" # Ensures Ruff has the quotes plugin available 30 | 31 | # Optional Black hook (disabled here). Ruff already formats code 32 | # - repo: https://github.com/psf/black 33 | # rev: 23.9.1 34 | # hooks: 35 | # - id: black 36 | # args: 37 | # - "--config=pyproject.toml" 38 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_plugin: chomp_interface/CHOMPPlanner 2 | request_adapters: >- 3 | default_planner_request_adapters/AddTimeOptimalParameterization 4 | default_planner_request_adapters/FixWorkspaceBounds 5 | default_planner_request_adapters/FixStartStateBounds 6 | default_planner_request_adapters/FixStartStateCollision 7 | default_planner_request_adapters/FixStartStatePathConstraints 8 | start_state_max_bounds_error: 0.1 9 | planning_time_limit: 10.0 10 | max_iterations: 200 11 | max_iterations_after_collision_free: 5 12 | smoothness_cost_weight: 0.1 13 | obstacle_cost_weight: 1.0 14 | learning_rate: 0.01 15 | animate_path: true 16 | add_randomness: false 17 | smoothness_cost_velocity: 0.0 18 | smoothness_cost_acceleration: 1.0 19 | smoothness_cost_jerk: 0.0 20 | hmc_discretization: 0.01 21 | hmc_stochasticity: 0.01 22 | hmc_annealing_factor: 0.99 23 | use_hamiltonian_monte_carlo: false 24 | ridge_factor: 0.0 25 | use_pseudo_inverse: false 26 | pseudo_inverse_ridge_factor: 1e-4 27 | animate_endeffector: false 28 | animate_endeffector_segment: "panda_rightfinger" 29 | joint_update_limit: 0.1 30 | collision_clearance: 0.2 31 | collision_threshold: 0.07 32 | random_jump_amount: 1.0 33 | use_stochastic_descent: true 34 | enable_failure_recovery: false 35 | max_recovery_attempts: 5 36 | trajectory_initialization_method: "quintic-spline" 37 | -------------------------------------------------------------------------------- /manymove_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_msgs 5 | 0.3.2 6 | package with all the interfaces for manymove 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | rosidl_default_generators 13 | 14 | rclcpp 15 | rclcpp_action 16 | geometry_msgs 17 | shape_msgs 18 | action_msgs 19 | trajectory_msgs 20 | tf2 21 | tf2_ros 22 | tf2_geometry_msgs 23 | std_msgs 24 | std_srvs 25 | control_msgs 26 | controller_manager_msgs 27 | rosidl_default_generators 28 | 29 | rosidl_default_runtime 30 | 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | rosidl_interface_packages 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /manymove_cpp_trees/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_cpp_trees 5 | 0.3.2 6 | Package to generate behaviortrees for manymove_planner using BehaviorTree.CPP 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | 13 | rclcpp 14 | geometry_msgs 15 | behaviortree_cpp_v3 16 | rclcpp_action 17 | manymove_planner 18 | manymove_object_manager 19 | trajectory_msgs 20 | tf2 21 | tf2_ros 22 | tf2_geometry_msgs 23 | std_srvs 24 | manymove_msgs 25 | control_msgs 26 | std_msgs 27 | topic_based_ros2_control 28 | simulation_interfaces 29 | vision_msgs 30 | rcpputils 31 | 32 | ament_lint_auto 33 | ament_lint_common 34 | ament_cmake_uncrustify 35 | ament_uncrustify 36 | 37 | 38 | ament_cmake 39 | 40 | 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_object_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_object_manager 5 | 0.3.2 6 | Package to manage graspable objects and obstacles for manymove_planner 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | 13 | rclcpp 14 | geometry_msgs 15 | moveit_msgs 16 | std_srvs 17 | tf2 18 | tf2_ros 19 | tf2_geometry_msgs 20 | yaml_cpp_vendor 21 | geometric_shapes 22 | manymove_msgs 23 | 24 | ament_cmake_test 25 | ament_cmake_gtest 26 | ament_lint_auto 27 | ament_lint_common 28 | ament_cmake_uncrustify 29 | ament_cmake_cppcheck 30 | ament_cmake_cpplint 31 | ament_cmake_flake8 32 | ament_cmake_pep257 33 | ament_cmake_xmllint 34 | ament_uncrustify 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /manymove_hmi/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_msgs/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_bringup/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_cpp_trees/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_planner/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_object_manager/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024-2025, Flexin Group SRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /manymove_planner/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | octomap_resolution: 0.02 # Sets the octomap resolution 2 | 3 | sensors: 4 | - ros # Specifies that the sensor configuration named 'ros' will be used 5 | 6 | ros: 7 | sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater # Plugin to use 8 | point_cloud_topic: /camera/camera/depth/color/points # Topic to subscribe to for point clouds 9 | max_range: 2.0 # Maximum range for the sensor 10 | point_subsample: 1 # Subsampling rate for point clouds 11 | padding_offset: 0.1 # Padding offset for the octomap 12 | padding_scale: 1.0 # Padding scale for the octomap 13 | max_update_rate: 1.0 # Maximum update rate for the octomap 14 | filtered_cloud_topic: filtered_cloud # Topic to publish the filtered cloud 15 | 16 | ## To use this with a ufactory robot with the camera mounted on the 6th axis, paste this file inside src/xarm_ros2/xarm_moveit_config/config/ 17 | ## If the robot the camera is attached to has a non-empty prefix, create a modified file that has the prefix in the camera name: 18 | # point_cloud_topic: /camera/camera/depth/color/points 19 | ## Also, launch the realsense node adding the prefix to the camera name: 20 | # ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true pointcloud.stream_filter:=0 camera_name:=L_camera 21 | -------------------------------------------------------------------------------- /manymove_bringup/manymove_bringup/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Utility helpers for launching the Manymove bringup stack.""" 30 | -------------------------------------------------------------------------------- /manymove_msgs/msg/MovementConfig.msg: -------------------------------------------------------------------------------- 1 | # manymove parameters 2 | float64 max_cartesian_speed # max cartesian speed for the move, referred to TCP 3 | int32 plan_number_target # number of valid plans to find before declaring success 4 | int32 plan_number_limit # max planning tries before fail 5 | string smoothing_type # "time-optimal" / "ruckig" 6 | string tcp_frame # End-effector (TCP) frame for this request 7 | float64 linear_precision # Linear precision for end-point trajectory position check 8 | float64 rotational_precision # Rotational precision for end-point trajectory position check 9 | float64 deceleration_time # Time to decelerate the robot to arrive at a full stop 10 | float64 min_stop_time # Minimum time to perform a controlled stop, if less time remains the traj will just end 11 | # moveit planner parameters 12 | float64 velocity_scaling_factor # 0.0 to 1.0 13 | float64 acceleration_scaling_factor # 0.0 to 1.0 14 | string planning_pipeline # e.g. "ompl", "chomp", "pilz_industrial_motion_planner", ... 15 | string planner_id # e.g. "RRTConnect", "PTP", "LIN", ... 16 | float64 planning_time # overall time budget in seconds 17 | int32 planning_attempts # e.g. 1, 5, etc. 18 | # CartesianInterpolator parameters 19 | float64 step_size 20 | float64 jump_threshold # for humble only 21 | float64 cartesian_precision_translational # from jazzy 22 | float64 cartesian_precision_rotational # from jazzy 23 | float64 cartesian_precision_max_resolution # from jazzy 24 | -------------------------------------------------------------------------------- /manymove_object_manager/config/objects.yaml: -------------------------------------------------------------------------------- 1 | objects: 2 | - name: "obstacle_ground" 3 | type: "box" 4 | dimensions: [0.8, 0.8, 0.1] 5 | pose: 6 | position: {x: 0.0, y: 0.0, z: -0.05} 7 | orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0} 8 | 9 | - name: "obstacle_wall" 10 | type: "box" 11 | dimensions: [0.8, 0.02, 0.8] 12 | pose: 13 | position: {x: 0.0, y: 0.4, z: 0.3} 14 | orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0} 15 | 16 | - name: "graspable_cylinder" 17 | type: "cylinder" 18 | dimensions: [0.1, 0.005] 19 | pose: 20 | position: {x: 0.0, y: 0.2, z: 0.005} 21 | orientation: {roll: 0.0, pitch: 1.57, yaw: 0.0} 22 | 23 | # example for mesh object 24 | - name: "mesh_object" 25 | type: "mesh" 26 | # this example mesh represents a tube of diameter 1m and length 1m, with a wall 0.1m thick 27 | mesh_file: "package://manymove_object_manager/meshes/unit_tube.stl" 28 | # we'll scale it to a tube of diameter 10mm and length 100mm 29 | scale: {x: 0.01, y: 0.01, z: 0.1} 30 | pose: 31 | position: {x: 0.0, y: -0.2, z: 0.005} 32 | orientation: {roll: 0.0, pitch: 1.57, yaw: 0.0} 33 | 34 | # # example for mesh object 35 | # - name: "mesh_object" 36 | # type: "mesh" 37 | # mesh_file: "package://manymove_object_manager/meshes/pneumatic_lite.stl" 38 | # scale: {x: 1.0, y: 1.0, z: 1.0} 39 | # pose: 40 | # position: {x: 0.15, y: -0.05, z: 0.0} 41 | # orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0} 42 | 43 | # # example for random position object 44 | # - name: "graspable_random_cylinder" 45 | # type: "cylinder" 46 | # dimensions: [0.1, 0.005] 47 | # # No pose provided; will be placed randomly 48 | 49 | # # example for sphere object: 50 | # - name: "custom_sphere" 51 | # type: "sphere" 52 | # dimensions: [0.05] 53 | # pose: 54 | # position: {x: 0.2, y: 0.2, z: 0.05} 55 | # orientation: {roll: 0.0, pitch: 0.0, yaw: 1.57} 56 | -------------------------------------------------------------------------------- /manymove_planner/include/manymove_planner/compat/tf2_linear_compat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #if __has_include() 34 | #include 35 | #else 36 | #include 37 | #endif 38 | 39 | #include 40 | -------------------------------------------------------------------------------- /manymove_cpp_trees/include/manymove_cpp_trees/compat/tf2_linear_compat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #if __has_include() 34 | #include 35 | #else 36 | #include 37 | #endif 38 | 39 | #include 40 | -------------------------------------------------------------------------------- /manymove_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_bringup 5 | 0.3.2 6 | Bringup for manymove repo 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | 13 | ament_cmake_python 14 | 15 | launch 16 | launch_ros 17 | python3-numpy 18 | controller_manager 19 | joint_state_broadcaster 20 | joint_state_publisher 21 | manymove_cpp_trees 22 | manymove_hmi 23 | manymove_object_manager 24 | manymove_planner 25 | moveit_configs_utils 26 | moveit_resources_panda_moveit_config 27 | moveit_ros_move_group 28 | robot_state_publisher 29 | rviz2 30 | tf2_ros 31 | xacro 32 | ur_robot_driver 33 | ur_description 34 | ur_moveit_config 35 | robotiq_description 36 | robotiq_controllers 37 | 38 | ament_lint_auto 39 | ament_pep257 40 | ament_copyright 41 | python3-pytest 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | -------------------------------------------------------------------------------- /manymove_object_manager/include/manymove_object_manager/compat/tf2_linear_compat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #if __has_include() 34 | #include 35 | #else 36 | #include 37 | #endif 38 | 39 | #include 40 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=42", "wheel"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | # ------------------------------------------------------------------- 6 | # Python formatting and linting according to ROS 2 code-style guide 7 | # ------------------------------------------------------------------- 8 | 9 | [tool.ruff] 10 | # General settings 11 | line-length = 100 12 | 13 | # Linting rules 14 | lint.select = ["E", "F", "Q"] 15 | lint.ignore = [ 16 | "E203", # Whitespace before ':' (conflicts with Black-style slicing) 17 | ] 18 | lint.fixable = ["ALL"] 19 | 20 | # Quote and formatting style 21 | format.quote-style = "single" 22 | lint.flake8-quotes.inline-quotes = "single" 23 | lint.flake8-quotes.docstring-quotes = "double" 24 | 25 | # ------------------------------------------------------------------- 26 | # isort-compatible import sorting 27 | # ------------------------------------------------------------------- 28 | [tool.isort] 29 | profile = "black" 30 | line_length = 100 31 | force_single_line = true 32 | multi_line_output = 3 33 | include_trailing_comma = true 34 | 35 | # Make ROS 2 packages appear before yaml 36 | known_first_party = ["manymove"] 37 | known_third_party = [ 38 | "ament_index_python", 39 | "rclpy", 40 | "launch", 41 | "launch_ros", 42 | "manymove_bringup", 43 | "manymove_cpp_trees", 44 | "manymove_planner", 45 | "uf_ros_lib" 46 | ] 47 | 48 | # Ensure sorting sections are explicit (so yaml ends up after ament_index_python) 49 | sections = ["FUTURE", "STDLIB", "THIRDPARTY", "FIRSTPARTY", "LOCALFOLDER"] 50 | default_section = "FIRSTPARTY" 51 | 52 | # ------------------------------------------------------------------- 53 | # Ruff’s built-in isort mirror (keeps Ruff and isort in sync) 54 | # ------------------------------------------------------------------- 55 | [tool.ruff.isort] 56 | known-first-party = ["manymove"] 57 | known-third-party = [ 58 | "ament_index_python", 59 | "rclpy", 60 | "launch", 61 | "launch_ros", 62 | "manymove_bringup", 63 | "manymove_cpp_trees", 64 | "manymove_planner", 65 | "uf_ros_lib" 66 | ] 67 | -------------------------------------------------------------------------------- /manymove_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14.4) 2 | project(manymove_msgs) 3 | 4 | # Set common compiler flags 5 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 6 | add_compile_options(-Wall -Wextra -Wpedantic) 7 | endif() 8 | 9 | # Find required packages 10 | find_package(ament_cmake REQUIRED) 11 | find_package(rosidl_default_generators REQUIRED) 12 | 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(shape_msgs REQUIRED) 15 | find_package(action_msgs REQUIRED) 16 | find_package(trajectory_msgs REQUIRED) 17 | find_package(tf2 REQUIRED) 18 | find_package(tf2_ros REQUIRED) 19 | find_package(tf2_geometry_msgs REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(std_srvs REQUIRED) 22 | find_package(control_msgs REQUIRED) 23 | find_package(controller_manager_msgs REQUIRED) 24 | 25 | # Generate messages, actions, and (if any) services. 26 | rosidl_generate_interfaces(${PROJECT_NAME} 27 | # Messages: 28 | "msg/MovementConfig.msg" 29 | "msg/MoveManipulatorGoal.msg" 30 | 31 | # Services: 32 | "srv/SetBlackboardValues.srv" 33 | 34 | # Actions: 35 | "action/AddCollisionObject.action" 36 | "action/RemoveCollisionObject.action" 37 | "action/CheckObjectExists.action" 38 | "action/AttachDetachObject.action" 39 | "action/GetObjectPose.action" 40 | 41 | "action/PlanManipulator.action" 42 | "action/MoveManipulator.action" 43 | "action/LoadTrajController.action" 44 | "action/UnloadTrajController.action" 45 | 46 | "action/SetOutput.action" 47 | "action/GetInput.action" 48 | "action/CheckRobotState.action" 49 | "action/ResetRobotState.action" 50 | 51 | DEPENDENCIES 52 | geometry_msgs 53 | shape_msgs 54 | action_msgs 55 | trajectory_msgs 56 | std_msgs 57 | std_srvs 58 | control_msgs 59 | controller_manager_msgs 60 | ) 61 | 62 | # Ensure generated headers are properly exported 63 | install( 64 | DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp/ 65 | DESTINATION include/ 66 | ) 67 | 68 | ament_export_dependencies(rosidl_default_runtime) 69 | ament_export_include_directories(${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp) 70 | 71 | ament_package() 72 | -------------------------------------------------------------------------------- /manymove_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manymove_planner 5 | 0.3.2 6 | Package publishing action servers to plan and execute moves with move_group and moveitcpp 7 | Marco Pastorio 8 | BSD-3-Clause 9 | Marco Pastorio 10 | 11 | ament_cmake 12 | 13 | rclcpp 14 | moveit_ros_planning_interface 15 | moveit_core 16 | tf2_geometry_msgs 17 | geometry_msgs 18 | trajectory_msgs 19 | moveit_msgs 20 | action_msgs 21 | control_msgs 22 | rclcpp_action 23 | rosidl_default_generators 24 | rosidl_default_runtime 25 | controller_manager_msgs 26 | manymove_msgs 27 | rcpputils 28 | 29 | controller_manager 30 | joint_state_broadcaster 31 | joint_state_publisher 32 | manymove_hmi 33 | manymove_object_manager 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | robot_state_publisher 37 | ros2_controllers 38 | rviz2 39 | tf2_ros 40 | xacro 41 | gripper_controllers 42 | 43 | ament_lint_auto 44 | ament_lint_common 45 | ament_cmake_uncrustify 46 | ament_uncrustify 47 | ament_cmake_gtest 48 | 49 | 50 | ament_cmake 51 | 52 | 53 | -------------------------------------------------------------------------------- /manymove_object_manager/src/object_manager_main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "manymove_object_manager/object_manager.hpp" 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | int main(int argc, char ** argv) 37 | { 38 | rclcpp::init(argc, argv); 39 | 40 | auto node = std::make_shared(); 41 | 42 | rclcpp::executors::MultiThreadedExecutor executor; 43 | executor.add_node(node); 44 | 45 | executor.spin(); 46 | 47 | rclcpp::shutdown(); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /manymove_bringup/manymove_bringup/ros_compat.py: -------------------------------------------------------------------------------- 1 | """ROS distro compatibility helpers for launch-time tweaks.""" 2 | 3 | from __future__ import annotations 4 | 5 | import os 6 | from typing import Iterable 7 | 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | LEGACY_MOVEIT_ADAPTER_DISTROS = { 11 | # Distros that still expect request/response adapters as a single string 12 | 'foxy', 13 | 'galactic', 14 | 'humble', 15 | 'iron', 16 | } 17 | 18 | _LEGACY_OVERRIDE_ENV = 'MANYMOVE_FORCE_LEGACY_MOVEIT_ADAPTER_FORMAT' 19 | 20 | 21 | def use_legacy_moveit_adapter_format() -> bool: 22 | """Return True if MoveIt expects adapter plugins encoded as a single string.""" 23 | override = os.getenv(_LEGACY_OVERRIDE_ENV, '').strip().lower() 24 | if override in ('1', 'true', 'yes', 'on'): 25 | return True 26 | if override in ('0', 'false', 'no', 'off'): 27 | return False 28 | 29 | ros_distro = os.getenv('ROS_DISTRO', '').strip().lower() 30 | if not ros_distro: 31 | # Default to the modern behaviour if the distro is unknown; this matches ROS Jazzy+. 32 | return False 33 | return ros_distro in LEGACY_MOVEIT_ADAPTER_DISTROS 34 | 35 | 36 | def resolve_package_file(package_name: str, candidates: Iterable[str]) -> str: 37 | """Return the first relative path under a package share directory that exists.""" 38 | share_dir = get_package_share_directory(package_name) 39 | for relative in candidates: 40 | candidate_path = os.path.join(share_dir, relative) 41 | if os.path.exists(candidate_path): 42 | return relative 43 | candidate_list = ', '.join(candidates) 44 | raise FileNotFoundError( 45 | f"None of the candidate files [{candidate_list}] exist in package '{package_name}'." 46 | ) 47 | 48 | 49 | def resolve_moveit_controller_config(package_name: str) -> str: 50 | """Pick the correct MoveIt controller config file for the given package.""" 51 | return resolve_package_file( 52 | package_name, 53 | ( 54 | os.path.join('config', 'controllers.yaml'), 55 | os.path.join('config', 'ros2_controllers.yaml'), 56 | os.path.join('config', 'moveit_controllers.yaml'), 57 | ), 58 | ) 59 | -------------------------------------------------------------------------------- /manymove_object_manager/launch/object_manager.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the object manager scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch_ros.actions import Node 33 | 34 | 35 | def generate_launch_description(): 36 | """Create the launch description entry point.""" 37 | return LaunchDescription( 38 | [ 39 | Node( 40 | package='manymove_object_manager', 41 | executable='object_manager_node', 42 | name='object_manager_node', 43 | output='screen', 44 | parameters=[{'frame_id': 'world'}], 45 | ) 46 | ] 47 | ) 48 | -------------------------------------------------------------------------------- /manymove_hmi/include/manymove_hmi/default_app_hmi.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_HMI__DEFAULT_APP_HMI_HPP_ 30 | #define MANYMOVE_HMI__DEFAULT_APP_HMI_HPP_ 31 | 32 | #include 33 | 34 | #include "manymove_hmi/app_module.hpp" 35 | 36 | /* Application-specific module with the *original* 10 keys ---------- 37 | * Nothing here except the list-builder. */ 38 | class DefaultAppModule : public AppModule 39 | { 40 | public: 41 | explicit DefaultAppModule(QWidget * parent = nullptr) 42 | : AppModule(buildKeys(), parent) {} 43 | 44 | private: 45 | static std::vector buildKeys(); 46 | }; 47 | 48 | #endif // MANYMOVE_HMI__DEFAULT_APP_HMI_HPP_ 49 | -------------------------------------------------------------------------------- /manymove_object_manager/README.md: -------------------------------------------------------------------------------- 1 | # ManyMove Object Manager 2 | 3 | ## Overview 4 | ManyMove Object Manager provides a ROS 2 node that manages collision objects in a MoveIt planning scene for the ManyMove stack. It exposes action servers to add, remove, attach, detach, and query objects, confirming every change through the MoveIt `GetPlanningScene` service. The node publishes `moveit_msgs/msg/CollisionObject` and `moveit_msgs/msg/AttachedCollisionObject` messages so downstream planners stay synchronized. 5 | 6 | ## Actions 7 | - `AddCollisionObject` – Adds a collision object by publishing to the planning scene topics and waits until the new object is reported by `GetPlanningScene`. 8 | - `RemoveCollisionObject` – Removes a collision object and checks the planning scene until the object disappears. 9 | - `AttachDetachObject` – Attaches or detaches an object to a link, keeping the attached collision object topic in sync. 10 | - `CheckObjectExists` – Queries the planning scene to confirm whether an object is present or attached. 11 | - `GetObjectPose` – Retrieves an object's pose, optionally aligning it to a target frame using TF2 transforms. 12 | 13 | Every action uses the MoveIt `GetPlanningScene` service for verification and leads to collision or attached object updates being republished. 14 | 15 | ## Launch 16 | - For workspace setup, dependencies, and launch instructions, follow the top-level [ManyMove README](../README.md). 17 | 18 | ## Interactions and Dependencies 19 | Action definitions come from `manymove_msgs`, and message types are provided by `moveit_msgs`, `geometry_msgs`, `shape_msgs`, and the TF2 stack. The node is designed to cooperate with `manymove_planner`, and `manymove_cpp_trees`, giving these planners a consistent interface for collision management without direct planning scene manipulation. 20 | 21 | ## Configuration 22 | The file `config/objects.yaml` contains example obstacles and graspable objects that can be pre-loaded into the planning scene by companion tools such as a collision spawner. Customize this list to describe application-specific geometry, including mesh resources under `meshes/`, before invoking the object management actions. 23 | 24 | ## License and Maintainers 25 | This package is licensed under BSD-3-Clause. Maintainer: Marco Pastorio . 26 | See main [ManyMove README](../README.md) for `CONTRIBUTION` details. 27 | -------------------------------------------------------------------------------- /manymove_object_manager/launch/collision_spawner.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the collision spawner scenario.""" 30 | 31 | import os 32 | 33 | from ament_index_python.packages import get_package_share_directory 34 | from launch import LaunchDescription 35 | from launch_ros.actions import Node 36 | 37 | 38 | def generate_launch_description(): 39 | """Create the launch description entry point.""" 40 | config_dir = os.path.join(get_package_share_directory('manymove_object_manager'), 'config') 41 | objects_yaml = os.path.join(config_dir, 'objects.yaml') 42 | 43 | return LaunchDescription( 44 | [ 45 | Node( 46 | package='manymove_object_manager', 47 | executable='collision_spawner', 48 | name='collision_spawner', 49 | output='screen', 50 | parameters=[{'frame_id': 'world'}, {'config_file': objects_yaml}], 51 | ) 52 | ] 53 | ) 54 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_plugin: ompl_interface/OMPLPlanner 2 | start_state_max_bounds_error: 0.1 3 | jiggle_fraction: 0.05 4 | request_adapters: >- 5 | default_planning_request_adapters/ResolveConstraintFrames 6 | default_planning_request_adapters/ValidateWorkspaceBounds 7 | default_planning_request_adapters/CheckStartStateBounds 8 | default_planning_request_adapters/CheckStartStateCollision 9 | default_planning_request_adapters/CheckForStackedConstraints 10 | response_adapters: >- 11 | default_planning_response_adapters/AddTimeOptimalParameterization 12 | default_planning_response_adapters/ValidateSolution 13 | default_planning_response_adapters/DisplayMotionPath 14 | 15 | planner_configs: 16 | SBLkConfigDefault: 17 | type: geometric::SBL 18 | range: 0.0 19 | ESTkConfigDefault: 20 | type: geometric::EST 21 | range: 0.0 22 | goal_bias: 0.05 23 | LBKPIECEkConfigDefault: 24 | type: geometric::LBKPIECE 25 | range: 0.0 26 | border_fraction: 0.9 27 | min_valid_path_fraction: 0.5 28 | BKPIECEkConfigDefault: 29 | type: geometric::BKPIECE 30 | range: 0.0 31 | border_fraction: 0.9 32 | failed_expansion_score_factor: 0.5 33 | min_valid_path_fraction: 0.5 34 | KPIECEkConfigDefault: 35 | type: geometric::KPIECE 36 | range: 0.0 37 | goal_bias: 0.05 38 | border_fraction: 0.9 39 | failed_expansion_score_factor: 0.5 40 | min_valid_path_fraction: 0.5 41 | RRTkConfigDefault: 42 | type: geometric::RRT 43 | range: 0.0 44 | goal_bias: 0.05 45 | RRTConnectkConfigDefault: 46 | type: geometric::RRTConnect 47 | range: 0.0 48 | RRTConnect: 49 | type: geometric::RRTConnect 50 | range: 0.0 51 | RRTstarkConfigDefault: 52 | type: geometric::RRTstar 53 | range: 0.0 54 | goal_bias: 0.05 55 | delay_collision_checking: 1 56 | TRRTkConfigDefault: 57 | type: geometric::TRRT 58 | range: 0.0 59 | goal_bias: 0.05 60 | max_states_failed: 10 61 | temp_change_factor: 2.0 62 | min_temperature: 1.0e-09 63 | init_temperature: 1.0e-05 64 | frountier_threshold: 0.0 65 | frountierNodeRatio: 0.1 66 | k_constant: 0.0 67 | PRMkConfigDefault: 68 | type: geometric::PRM 69 | max_nearest_neighbors: 10 70 | PRMstarkConfigDefault: 71 | type: geometric::PRMstar 72 | 73 | ur_manipulator: 74 | planner_configs: 75 | - SBLkConfigDefault 76 | - ESTkConfigDefault 77 | - LBKPIECEkConfigDefault 78 | - BKPIECEkConfigDefault 79 | - KPIECEkConfigDefault 80 | - RRTkConfigDefault 81 | - RRTConnectkConfigDefault 82 | - RRTConnect 83 | - RRTstarkConfigDefault 84 | - TRRTkConfigDefault 85 | - PRMkConfigDefault 86 | - PRMstarkConfigDefault 87 | longest_valid_segment_fraction: 0.01 88 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/ompl_planning.legacy.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt OMPL planning configuration tailored for legacy (Humble and older) 2 | # distros that still expose the default_planner_request_adapters/Fix* plugin 3 | # identifiers. 4 | 5 | planning_plugin: ompl_interface/OMPLPlanner 6 | start_state_max_bounds_error: 0.1 7 | jiggle_fraction: 0.05 8 | request_adapters: >- 9 | default_planner_request_adapters/ResolveConstraintFrames 10 | default_planner_request_adapters/FixWorkspaceBounds 11 | default_planner_request_adapters/FixStartStateBounds 12 | default_planner_request_adapters/FixStartStateCollision 13 | response_adapters: >- 14 | default_planning_response_adapters/AddTimeOptimalParameterization 15 | default_planning_response_adapters/ValidateSolution 16 | default_planning_response_adapters/DisplayMotionPath 17 | 18 | planner_configs: 19 | SBLkConfigDefault: 20 | type: geometric::SBL 21 | range: 0.0 22 | ESTkConfigDefault: 23 | type: geometric::EST 24 | range: 0.0 25 | goal_bias: 0.05 26 | LBKPIECEkConfigDefault: 27 | type: geometric::LBKPIECE 28 | range: 0.0 29 | border_fraction: 0.9 30 | min_valid_path_fraction: 0.5 31 | BKPIECEkConfigDefault: 32 | type: geometric::BKPIECE 33 | range: 0.0 34 | border_fraction: 0.9 35 | failed_expansion_score_factor: 0.5 36 | min_valid_path_fraction: 0.5 37 | KPIECEkConfigDefault: 38 | type: geometric::KPIECE 39 | range: 0.0 40 | goal_bias: 0.05 41 | border_fraction: 0.9 42 | failed_expansion_score_factor: 0.5 43 | min_valid_path_fraction: 0.5 44 | RRTkConfigDefault: 45 | type: geometric::RRT 46 | range: 0.0 47 | goal_bias: 0.05 48 | RRTConnectkConfigDefault: 49 | type: geometric::RRTConnect 50 | range: 0.0 51 | RRTConnect: 52 | type: geometric::RRTConnect 53 | range: 0.0 54 | RRTstarkConfigDefault: 55 | type: geometric::RRTstar 56 | range: 0.0 57 | goal_bias: 0.05 58 | delay_collision_checking: 1 59 | TRRTkConfigDefault: 60 | type: geometric::TRRT 61 | range: 0.0 62 | goal_bias: 0.05 63 | max_states_failed: 10 64 | temp_change_factor: 2.0 65 | min_temperature: 1.0e-09 66 | init_temperature: 1.0e-05 67 | frountier_threshold: 0.0 68 | frountierNodeRatio: 0.1 69 | k_constant: 0.0 70 | PRMkConfigDefault: 71 | type: geometric::PRM 72 | max_nearest_neighbors: 10 73 | PRMstarkConfigDefault: 74 | type: geometric::PRMstar 75 | 76 | ur_manipulator: 77 | planner_configs: 78 | - SBLkConfigDefault 79 | - ESTkConfigDefault 80 | - LBKPIECEkConfigDefault 81 | - BKPIECEkConfigDefault 82 | - KPIECEkConfigDefault 83 | - RRTkConfigDefault 84 | - RRTConnectkConfigDefault 85 | - RRTConnect 86 | - RRTstarkConfigDefault 87 | - TRRTkConfigDefault 88 | - PRMkConfigDefault 89 | - PRMstarkConfigDefault 90 | longest_valid_segment_fraction: 0.01 91 | -------------------------------------------------------------------------------- /manymove_cpp_trees/include/manymove_cpp_trees/blackboard_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_CPP_TREES__BLACKBOARD_UTILS_HPP_ 30 | #define MANYMOVE_CPP_TREES__BLACKBOARD_UTILS_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace manymove_cpp_trees 40 | { 41 | // Structure representing a blackboard key. 42 | struct BlackboardEntry 43 | { 44 | std::string key; 45 | std::string value_type; // e.g. "bool", "double", "string", "pose", etc. 46 | }; 47 | 48 | template 49 | void defineVariableKey( 50 | const rclcpp::Node::SharedPtr & node_ptr, BT::Blackboard::Ptr blackboard, 51 | std::vector & keys, const std::string & key_name, const std::string & key_type, 52 | const T & value) 53 | { 54 | // Set the blackboard key with initial value 55 | blackboard->set(key_name, value); 56 | 57 | // These keys need to be published for the HMI, adding them here: 58 | keys.push_back({key_name, key_type}); 59 | 60 | RCLCPP_INFO( 61 | node_ptr->get_logger(), "%s key added to 'blackboard_status' service.", key_name.c_str()); 62 | } 63 | } // namespace manymove_cpp_trees 64 | 65 | #endif // MANYMOVE_CPP_TREES__BLACKBOARD_UTILS_HPP_ 66 | -------------------------------------------------------------------------------- /manymove_hmi/include/manymove_hmi/ros2_worker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_HMI__ROS2_WORKER_HPP_ 30 | #define MANYMOVE_HMI__ROS2_WORKER_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "manymove_hmi/app_module.hpp" 42 | #include "manymove_hmi/hmi_gui.hpp" 43 | 44 | // Forward declarations 45 | class HmiGui; 46 | class AppModule; 47 | 48 | class Ros2Worker : public rclcpp::Node 49 | { 50 | public: 51 | Ros2Worker(const std::string & node_name, HmiGui * gui, const std::string & robot_prefix = ""); 52 | 53 | void callStartExecution(); 54 | void callStopExecution(); 55 | void callResetProgram(); 56 | 57 | const std::string & getRobotPrefix() const {return robot_prefix_;} 58 | 59 | private: 60 | void statusCallback(const std_msgs::msg::String::SharedPtr msg); 61 | 62 | HmiGui * gui_; 63 | rclcpp::Subscription::SharedPtr subscription_; 64 | rclcpp::Client::SharedPtr update_blackboard_client_; 65 | 66 | std::string robot_prefix_; 67 | }; 68 | 69 | #endif // MANYMOVE_HMI__ROS2_WORKER_HPP_ 70 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/Dockerfile.manymove: -------------------------------------------------------------------------------- 1 | # -*- Dockerfile for ManyMove dev image -*- 2 | ARG BASE_IMAGE=ros2_humble-image 3 | FROM ${BASE_IMAGE} 4 | 5 | # where run_dev.sh will mount your host workspace 6 | ENV ISAAC_ROS_WS=/workspaces/isaac_ros-dev \ 7 | ROS_DISTRO=humble \ 8 | DEBIAN_FRONTEND=noninteractive 9 | 10 | # 0) Update ros keyring (currently the keyring in the image is expired) 11 | RUN sudo rm /usr/share/keyrings/ros-archive-keyring.gpg && \ 12 | sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 13 | 14 | # 1) install all system & ROS binaries needed before colcon build 15 | RUN apt-get -o Acquire::AllowReleaseInfoChange::Origin=true \ 16 | -o Acquire::AllowReleaseInfoChange::Label=true update && \ 17 | apt-get install -y --no-install-recommends \ 18 | build-essential git curl ninja-build python3-pip \ 19 | python3-colcon-common-extensions \ 20 | liburdfdom-headers-dev \ 21 | ros-${ROS_DISTRO}-rclcpp \ 22 | ros-${ROS_DISTRO}-nvblox-msgs \ 23 | ros-${ROS_DISTRO}-isaac-manipulator-ros-python-utils \ 24 | ros-${ROS_DISTRO}-moveit-common \ 25 | ros-${ROS_DISTRO}-joint-state-publisher-gui \ 26 | ros-${ROS_DISTRO}-xacro \ 27 | ros-${ROS_DISTRO}-tf-transformations \ 28 | ros-${ROS_DISTRO}-find-object-2d \ 29 | ros-${ROS_DISTRO}-topic-based-ros2-control \ 30 | ros-${ROS_DISTRO}-simulation-interfaces \ 31 | ros-${ROS_DISTRO}-isaac-ros-dnn-image-encoder \ 32 | ros-${ROS_DISTRO}-isaac-ros-tensor-rt \ 33 | ros-${ROS_DISTRO}-isaac-ros-nitros-tensor-list-type \ 34 | ros-${ROS_DISTRO}-isaac-ros-managed-nitros \ 35 | ros-${ROS_DISTRO}-isaac-ros-nitros \ 36 | ros-${ROS_DISTRO}-isaac-ros-triton \ 37 | ros-${ROS_DISTRO}-isaac-ros-nitros-detection2-d-array-type \ 38 | ros-${ROS_DISTRO}-isaac-ros-nitros-detection3-d-array-type \ 39 | ros-${ROS_DISTRO}-isaac-ros-nitros-point-cloud-type \ 40 | ros-${ROS_DISTRO}-isaac-ros-stereo-image-proc \ 41 | ros-${ROS_DISTRO}-isaac-ros-depth-image-proc \ 42 | ros-${ROS_DISTRO}-isaac-ros-nitros-topic-tools \ 43 | ros-${ROS_DISTRO}-isaac-ros-h264-decoder \ 44 | ros-${ROS_DISTRO}-isaac-ros-visual-slam \ 45 | ros-${ROS_DISTRO}-nova-carter-bringup \ 46 | ros-${ROS_DISTRO}-usb-cam \ 47 | && rm -rf /var/lib/apt/lists/* 48 | 49 | # 2) copy bootstrap into the base image’s entrypoint_additions 50 | # (workspace-entrypoint.sh will run *all* scripts in this folder) 51 | COPY scripts/build-manymove.sh \ 52 | /usr/local/bin/scripts/entrypoint_additions/00-build-manymove.sh 53 | RUN chmod +x /usr/local/bin/scripts/entrypoint_additions/00-build-manymove.sh 54 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/build-manymove.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -eo pipefail 4 | 5 | # The host workspace is mounted here: 6 | WORKDIR="${ISAAC_ROS_WS}" 7 | SRC="${WORKDIR}/src" 8 | INSTALL_SETUP="${WORKDIR}/install/setup.bash" 9 | 10 | echo "--- [build-manymove] starting ---" 11 | 12 | # Ensure src dir exists 13 | mkdir -p "${SRC}" 14 | 15 | cd "${WORKDIR}" 16 | 17 | # If we already have an overlay, do nothing 18 | if [ -f "${INSTALL_SETUP}" ]; then 19 | echo "[build-manymove] install/ already present, skipping build." 20 | source /opt/ros/${ROS_DISTRO}/setup.bash 21 | source ${INSTALL_SETUP} 22 | # This script is sourced by the container entrypoint; use return, not exit 23 | return 0 24 | fi 25 | 26 | # echo "[build-manymove] applying mesh/config tweaks…" 27 | # mkdir -p src/xarm_ros2/xarm_description/meshes/other 28 | # cp -r src/manymove/manymove_object_manager/meshes/custom_end_tools/* \ 29 | # src/xarm_ros2/xarm_description/meshes/other/ 30 | # cp src/manymove/manymove_planner/config/xarm_user_params.yaml \ 31 | # src/xarm_ros2/xarm_api/config/ || true 32 | 33 | echo "[build-manymove] installing rosdep dependencies…" 34 | 35 | # Resolve dependencies across the whole workspace once (safer for cross-tree deps like xarm_msgs) 36 | apt update 37 | rosdep update || true 38 | rosdep install --from-paths "${SRC}" --ignore-src -y -r || true 39 | 40 | echo "[build-manymove] building via colcon as normal user…" 41 | 42 | # Source the distro first; then drop privileges *only* for colcon. 43 | source /opt/ros/${ROS_DISTRO}/setup.bash 44 | 45 | # if [[ "$(id -u)" -eq 0 && -n "${USERNAME}" ]]; then 46 | # gosu "${USERNAME}" bash -c " 47 | # set -eo pipefail 48 | # source /opt/ros/${ROS_DISTRO}/setup.bash 49 | # colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release 50 | # " 51 | if [[ "$(id -u)" -eq 0 && -n "${USERNAME}" ]]; then 52 | gosu "${USERNAME}" bash -c " 53 | set -eo pipefail 54 | source /opt/ros/${ROS_DISTRO}/setup.bash 55 | cd ${ISAAC_ROS_WS} 56 | colcon build \ 57 | --symlink-install \ 58 | --packages-up-to \ 59 | manymove_cpp_trees \ 60 | manymove_hmi \ 61 | manymove_bringup \ 62 | isaac_ros_cumotion \ 63 | isaac_ros_cumotion_examples \ 64 | isaac_ros_yolov8 \ 65 | isaac_ros_foundationpose \ 66 | isaac_ros_custom_bringup \ 67 | xarm_msgs \ 68 | xarm_moveit_config 69 | " 70 | else 71 | # Fallback: we are already the normal user (or USERNAME unset) 72 | # colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release 73 | cd ${ISAAC_ROS_WS} 74 | colcon build \ 75 | --symlink-install \ 76 | --packages-up-to \ 77 | manymove_cpp_trees \ 78 | manymove_hmi \ 79 | manymove_bringup \ 80 | isaac_ros_cumotion \ 81 | isaac_ros_cumotion_examples \ 82 | isaac_ros_yolov8 \ 83 | isaac_ros_foundationpose \ 84 | isaac_ros_custom_bringup \ 85 | xarm_msgs \ 86 | xarm_moveit_config 87 | fi 88 | 89 | source ${INSTALL_SETUP} 90 | 91 | echo "[build-manymove] build complete — install/ now available" 92 | -------------------------------------------------------------------------------- /manymove_cpp_trees/include/manymove_cpp_trees/hmi_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_CPP_TREES__HMI_UTILS_HPP_ 30 | #define MANYMOVE_CPP_TREES__HMI_UTILS_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | namespace manymove_cpp_trees 39 | { 40 | inline void setHMIMessage( 41 | const BT::Blackboard::Ptr & blackboard, const std::string & key, const std::string & value, 42 | const std::string & color, double rate_hz = 5.0) 43 | { 44 | using clock = std::chrono::steady_clock; 45 | struct State 46 | { 47 | clock::time_point last_time{}; 48 | std::string last_value; 49 | std::string last_color; 50 | }; 51 | 52 | static std::unordered_map states; 53 | auto now = clock::now(); 54 | auto period = std::chrono::duration(1.0 / rate_hz); 55 | 56 | State & st = states[key]; 57 | bool should_update = false; 58 | if (st.last_value != value || st.last_color != color) { 59 | should_update = true; 60 | } else if (now - st.last_time >= period) { 61 | should_update = true; 62 | } 63 | 64 | if (should_update) { 65 | blackboard->set(key + "message", value); 66 | blackboard->set(key + "message_color", color); 67 | st.last_time = now; 68 | st.last_value = value; 69 | st.last_color = color; 70 | } 71 | } 72 | } // namespace manymove_cpp_trees 73 | 74 | #endif // MANYMOVE_CPP_TREES__HMI_UTILS_HPP_ 75 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6_grf.xrdf: -------------------------------------------------------------------------------- 1 | format: "xrdf" 2 | 3 | format_version: 1.0 4 | 5 | default_joint_positions: 6 | joint1: -1e-04 7 | joint2: 0.0 8 | joint3: 1.57 9 | joint4: 0.0 10 | joint5: -0.0 11 | joint6: 0.0 12 | left_finger_joint: 0.0 13 | right_finger_joint: 0.0 14 | 15 | cspace: 16 | joint_names: [] 17 | acceleration_limits: [] 18 | jerk_limits: [] 19 | 20 | collision: 21 | geometry: "auto_generated_collision_sphere_group" 22 | 23 | self_collision: 24 | geometry: "auto_generated_collision_sphere_group" 25 | ignore: 26 | world: 27 | - "link_base" 28 | link_base: 29 | - "link1" 30 | link1: 31 | - "link2" 32 | link2: 33 | - "link3" 34 | link3: 35 | - "link4" 36 | link4: 37 | - "link5" 38 | link5: 39 | - "link6" 40 | link6: 41 | - "link_eef" 42 | link_eef: 43 | - "lite_gripper_base_link" 44 | lite_gripper_base_link: 45 | - "link_tcp" 46 | - "lite_gripper_left_finger" 47 | - "lite_gripper_right_finger" 48 | link_tcp: 49 | - "lite_gripper_left_finger" 50 | - "lite_gripper_right_finger" 51 | lite_gripper_left_finger: 52 | - "lite_gripper_right_finger" 53 | 54 | geometry: 55 | auto_generated_collision_sphere_group: 56 | spheres: 57 | link_base: 58 | - center: [-0.006, 0.003, 0.044] 59 | radius: 0.0838 60 | - center: [-0.001, -0.0, 0.125] 61 | radius: 0.0782 62 | - center: [-0.036, -0.022, 0.037] 63 | radius: 0.0772 64 | link1: 65 | - center: [-0.001, -0.006, -0.002] 66 | radius: 0.0762 67 | - center: [0.001, -0.009, -0.035] 68 | radius: 0.0712 69 | link2: 70 | - center: [0.189, -0.0, 0.095] 71 | radius: 0.0683 72 | - center: [0.016, -0.001, 0.104] 73 | radius: 0.0606 74 | - center: [0.125, -0.005, 0.113] 75 | radius: 0.0535 76 | - center: [0.07, -0.0, 0.113] 77 | radius: 0.0535 78 | link3: 79 | - center: [-0.002, -0.002, -0.008] 80 | radius: 0.0666 81 | - center: [0.082, 0.0, 0.001] 82 | radius: 0.065 83 | link4: 84 | - center: [0.0, -0.062, -0.06] 85 | radius: 0.069 86 | - center: [0.002, 0.006, -0.165] 87 | radius: 0.0546 88 | - center: [-0.002, -0.052, -0.009] 89 | radius: 0.0508 90 | - center: [-0.003, -0.002, -0.102] 91 | radius: 0.0546 92 | - center: [-0.002, -0.042, -0.099] 93 | radius: 0.0546 94 | link5: 95 | - center: [0.001, 0.006, 0.001] 96 | radius: 0.0654 97 | link6: 98 | - center: [-0.0, -0.0, 0.064] 99 | radius: 0.0409 100 | - center: [0.002, -0.001, 0.036] 101 | radius: 0.0409 102 | - center: [-0.0, -0.001, 0.009] 103 | radius: 0.0409 104 | -------------------------------------------------------------------------------- /manymove_object_manager/include/manymove_object_manager/compat/rclcpp_client_compat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace manymove_object_manager_compat 37 | { 38 | 39 | template 40 | auto create_service_client( 41 | rclcpp::Node * node, const std::string & service_name, 42 | const rclcpp::CallbackGroup::SharedPtr & callback_group, int) 43 | -> decltype(node->create_client(service_name, rclcpp::ServicesQoS(), callback_group)) 44 | { 45 | auto qos = rclcpp::ServicesQoS(); 46 | return node->create_client(service_name, qos, callback_group); 47 | } 48 | 49 | template 50 | auto create_service_client( 51 | rclcpp::Node * node, const std::string & service_name, 52 | const rclcpp::CallbackGroup::SharedPtr & callback_group, std::int64_t) 53 | -> decltype(node->create_client( 54 | service_name, rmw_qos_profile_services_default, callback_group)) 55 | { 56 | return node->create_client( 57 | service_name, rmw_qos_profile_services_default, callback_group); 58 | } 59 | 60 | template 61 | auto create_service_client( 62 | rclcpp::Node * node, const std::string & service_name, 63 | const rclcpp::CallbackGroup::SharedPtr & callback_group) 64 | { 65 | return create_service_client(node, service_name, callback_group, 0); 66 | } 67 | 68 | } // namespace manymove_object_manager_compat 69 | -------------------------------------------------------------------------------- /manymove_bringup/docker/Dockerfile.manymove_xarm: -------------------------------------------------------------------------------- 1 | # syntax=docker/dockerfile:1 2 | ARG ROS_DISTRO=humble 3 | FROM manymove:${ROS_DISTRO} 4 | 5 | ARG MANYMOVE_COLCON_WORKERS 6 | ARG XARM_REPO=https://github.com/pastoriomarco/xarm_ros2.git 7 | ARG XARM_BRANCH=${ROS_DISTRO} 8 | ARG XARM_COMMIT="" 9 | 10 | USER root 11 | 12 | ENV MANYMOVE_COLCON_WORKERS=${MANYMOVE_COLCON_WORKERS} 13 | 14 | SHELL ["/bin/bash", "-o", "pipefail", "-c"] 15 | 16 | RUN apt-get update && \ 17 | apt-get install -y --no-install-recommends \ 18 | ros-${ROS_DISTRO}-joint-state-publisher-gui \ 19 | ros-${ROS_DISTRO}-tf-transformations \ 20 | ros-${ROS_DISTRO}-xacro \ 21 | ros-${ROS_DISTRO}-control-toolbox \ 22 | ros-${ROS_DISTRO}-image-view \ 23 | ros-${ROS_DISTRO}-find-object-2d \ 24 | ros-${ROS_DISTRO}-joy \ 25 | ros-${ROS_DISTRO}-moveit-servo \ 26 | ros-${ROS_DISTRO}-joint-state-broadcaster \ 27 | ros-${ROS_DISTRO}-joint-trajectory-controller \ 28 | ros-${ROS_DISTRO}-diff-drive-controller \ 29 | ros-${ROS_DISTRO}-hardware-interface \ 30 | ros-${ROS_DISTRO}-camera-info-manager && \ 31 | if [ "${ROS_DISTRO}" = "humble" ]; then \ 32 | apt-get install -y --no-install-recommends \ 33 | ros-${ROS_DISTRO}-gazebo-ros \ 34 | ros-${ROS_DISTRO}-gazebo-plugins \ 35 | ros-${ROS_DISTRO}-gazebo-ros2-control; \ 36 | else \ 37 | apt-get install -y --no-install-recommends \ 38 | ros-${ROS_DISTRO}-ros-gz \ 39 | ros-${ROS_DISTRO}-ros-gz-bridge \ 40 | ros-${ROS_DISTRO}-gz-ros2-control \ 41 | ros-${ROS_DISTRO}-ros-gz-sim; \ 42 | fi && \ 43 | rm -rf /var/lib/apt/lists/* 44 | 45 | RUN gosu "${USERNAME}" bash -lc "set -euo pipefail; \ 46 | cd \"${MANYMOVE_WS}/src\" && \ 47 | rm -rf xarm_ros2 && \ 48 | git clone --recursive --branch ${XARM_BRANCH} ${XARM_REPO} xarm_ros2 && \ 49 | cd xarm_ros2 && \ 50 | git submodule update --init --recursive && \ 51 | if [ -n \"${XARM_COMMIT}\" ]; then \ 52 | git fetch --depth 1 origin ${XARM_COMMIT} && \ 53 | git checkout --detach ${XARM_COMMIT} && \ 54 | git submodule update --init --recursive; \ 55 | fi && \ 56 | git rev-parse HEAD > \"${MANYMOVE_WS}/.xarm_ros2_commit\"" 57 | 58 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ 59 | cd ${MANYMOVE_WS} && \ 60 | apt-get update && \ 61 | rosdep update && \ 62 | rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y && \ 63 | apt-get clean && rm -rf /var/lib/apt/lists/* 64 | 65 | RUN gosu "${USERNAME}" bash -lc "set -eo pipefail; \ 66 | source /opt/ros/${ROS_DISTRO}/setup.bash; \ 67 | cd ${MANYMOVE_WS}; \ 68 | parallel_args=\"\"; \ 69 | if [[ -n \"${MANYMOVE_COLCON_WORKERS}\" && \"${MANYMOVE_COLCON_WORKERS}\" =~ ^[0-9]+$ && \"${MANYMOVE_COLCON_WORKERS}\" -gt 0 ]]; then \ 70 | parallel_args=\"--parallel-workers ${MANYMOVE_COLCON_WORKERS}\"; \ 71 | export CMAKE_BUILD_PARALLEL_LEVEL=${MANYMOVE_COLCON_WORKERS}; \ 72 | fi; \ 73 | colcon build --symlink-install \${parallel_args}" 74 | 75 | USER ${USERNAME} 76 | -------------------------------------------------------------------------------- /manymove_bringup/config/ur/moveit_cpp.yaml: -------------------------------------------------------------------------------- 1 | # manymove_planner/config/moveit_cpp.yaml 2 | 3 | planning_scene_monitor_options: 4 | name: "planning_scene_monitor" 5 | robot_description: "robot_description" 6 | joint_state_topic: "/joint_states" 7 | attached_collision_object_topic: "/attached_collision_object" 8 | publish_planning_scene_topic: "/planning_scene" 9 | monitored_planning_scene_topic: "/monitored_planning_scene" 10 | wait_for_initial_state_timeout: 10.0 11 | 12 | # Refactored planning pipeline configuration (MoveIt ≥ March 2024). 13 | planning_pipelines: 14 | # Pipelines available to MoveIt. Each must have a corresponding section. 15 | pipeline_names: ["ompl", "chomp", "pilz_industrial_motion_planner"] 16 | 17 | # General planning request parameters. Select the default pipeline here. 18 | plan_request_params: 19 | planning_attempts: 1 20 | planning_pipeline: ompl 21 | planner_id: RRTConnect 22 | max_velocity_scaling_factor: 1.0 23 | max_acceleration_scaling_factor: 1.0 24 | 25 | # OMPL pipeline (sampling‑based planners such as RRTConnect). 26 | ompl: 27 | planning_plugins: 28 | - ompl_interface/OMPLPlanner 29 | request_adapters: 30 | - default_planning_request_adapters/ResolveConstraintFrames 31 | - default_planning_request_adapters/ValidateWorkspaceBounds 32 | - default_planning_request_adapters/CheckStartStateBounds 33 | - default_planning_request_adapters/CheckStartStateCollision 34 | response_adapters: 35 | - default_planning_response_adapters/AddTimeOptimalParameterization 36 | - default_planning_response_adapters/ValidateSolution 37 | - default_planning_response_adapters/DisplayMotionPath 38 | start_state_max_bounds_error: 0.1 39 | 40 | # CHOMP pipeline (gradient‑based optimisation). 41 | chomp: 42 | planning_plugins: 43 | - chomp_interface/CHOMPPlanner 44 | request_adapters: 45 | - default_planning_request_adapters/ResolveConstraintFrames 46 | - default_planning_request_adapters/ValidateWorkspaceBounds 47 | - default_planning_request_adapters/CheckStartStateBounds 48 | - default_planning_request_adapters/CheckStartStateCollision 49 | response_adapters: 50 | - default_planning_response_adapters/AddTimeOptimalParameterization 51 | - default_planning_response_adapters/ValidateSolution 52 | - default_planning_response_adapters/DisplayMotionPath 53 | 54 | # Pilz industrial motion planner (PTP/LIN/CIRC Cartesian motions). 55 | pilz_industrial_motion_planner: 56 | planning_plugins: 57 | - pilz_industrial_motion_planner/CommandPlanner 58 | request_adapters: 59 | # Pilz planners operate on joint commands only; no need for ResolveConstraintFrames. 60 | - default_planning_request_adapters/ValidateWorkspaceBounds 61 | - default_planning_request_adapters/CheckStartStateBounds 62 | - default_planning_request_adapters/CheckStartStateCollision 63 | response_adapters: 64 | - default_planning_response_adapters/ValidateSolution 65 | - default_planning_response_adapters/AddTimeOptimalParameterization 66 | - default_planning_response_adapters/DisplayMotionPath 67 | # Choose the default motion type for Pilz planners. Options include PTP, LIN and CIRC. 68 | default_planner_config: PTP 69 | # Expose Pilz sequence planning capabilities (optional). 70 | capabilities: > 71 | pilz_industrial_motion_planner/MoveGroupSequenceAction 72 | pilz_industrial_motion_planner/MoveGroupSequenceService 73 | -------------------------------------------------------------------------------- /manymove_cpp_trees/test/test_bt_converters.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include "manymove_cpp_trees/bt_converters.hpp" 38 | 39 | TEST(BTConverters, ParsePoseFromString) 40 | { 41 | const std::string s = 42 | "position: {x: 0.15, y: -0.2, z: 0.3}, orientation: {x: 0.1, y: 0.2, z: 0.3, w: 0.9}"; 43 | geometry_msgs::msg::Pose p = BT::convertFromString(s); 44 | EXPECT_NEAR(p.position.x, 0.15, 1e-9); 45 | EXPECT_NEAR(p.position.y, -0.2, 1e-9); 46 | EXPECT_NEAR(p.position.z, 0.3, 1e-9); 47 | EXPECT_NEAR(p.orientation.x, 0.1, 1e-9); 48 | EXPECT_NEAR(p.orientation.y, 0.2, 1e-9); 49 | EXPECT_NEAR(p.orientation.z, 0.3, 1e-9); 50 | EXPECT_NEAR(p.orientation.w, 0.9, 1e-9); 51 | } 52 | 53 | TEST(BTConverters, ParseVectorDouble) 54 | { 55 | const std::string s = "[1,2,3.5]"; 56 | auto v = BT::convertFromString>(s); 57 | ASSERT_EQ(v.size(), 3u); 58 | EXPECT_DOUBLE_EQ(v[0], 1.0); 59 | EXPECT_DOUBLE_EQ(v[1], 2.0); 60 | EXPECT_DOUBLE_EQ(v[2], 3.5); 61 | EXPECT_EQ(BT::convertToString(v), s); 62 | } 63 | 64 | TEST(BTConverters, ParseVectorString) 65 | { 66 | const std::string s = "[foo,bar,baz]"; 67 | auto v = BT::convertFromString>(s); 68 | ASSERT_EQ(v.size(), 3u); 69 | EXPECT_EQ(v[0], "foo"); 70 | EXPECT_EQ(v[1], "bar"); 71 | EXPECT_EQ(v[2], "baz"); 72 | EXPECT_EQ(BT::convertToString(v), s); 73 | } 74 | 75 | TEST(BTConverters, RejectMalformedVector) 76 | { 77 | EXPECT_THROW(BT::convertFromString>("1,2,3]"), BT::RuntimeError); 78 | EXPECT_THROW(BT::convertFromString>("foo,bar]"), BT::RuntimeError); 79 | } 80 | -------------------------------------------------------------------------------- /manymove_object_manager/test/fake_planning_scene_server.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef FAKE_PLANNING_SCENE_SERVER_HPP_ 30 | #define FAKE_PLANNING_SCENE_SERVER_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | class FakePlanningSceneServer 46 | { 47 | public: 48 | struct SceneState 49 | { 50 | std::vector collision_objects; 51 | std::vector attached_objects; 52 | }; 53 | 54 | explicit FakePlanningSceneServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 55 | 56 | rclcpp::Node::SharedPtr node() const; 57 | 58 | void setDefaultScene(const SceneState & state); 59 | void pushSceneResponse(const SceneState & state); 60 | void clearSceneResponses(); 61 | void setResponseDelay(std::chrono::milliseconds delay); 62 | 63 | void stopService(); 64 | void startService(); 65 | bool serviceIsReady() const; 66 | 67 | private: 68 | void handleRequest( 69 | const std::shared_ptr request, 70 | std::shared_ptr response); 71 | 72 | SceneState nextState(); 73 | 74 | rclcpp::Node::SharedPtr node_; 75 | rclcpp::Service::SharedPtr service_; 76 | 77 | mutable std::mutex mutex_; 78 | SceneState default_state_; 79 | std::deque queued_states_; 80 | std::chrono::milliseconds response_delay_{0}; 81 | }; 82 | 83 | #endif // FAKE_PLANNING_SCENE_SERVER_HPP_ 84 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6.xrdf: -------------------------------------------------------------------------------- 1 | format: "xrdf" 2 | 3 | format_version: 1.0 4 | 5 | default_joint_positions: 6 | joint1: -0.0 7 | joint2: 0.0 8 | joint3: 1.57 9 | joint4: -0.0 10 | joint5: 0.0 11 | joint6: 0.0 12 | 13 | cspace: 14 | joint_names: 15 | - "joint1" 16 | - "joint2" 17 | - "joint3" 18 | - "joint4" 19 | - "joint5" 20 | - "joint6" 21 | acceleration_limits: [10, 10, 10, 10, 10, 10] 22 | jerk_limits: [10000, 10000, 10000, 10000, 10000, 10000] 23 | 24 | tool_frames: ["link_tcp"] 25 | 26 | collision: 27 | geometry: "auto_generated_collision_sphere_group" 28 | buffer_distance: 29 | link1: 0.04 30 | link2: 0.04 31 | link3: 0.04 32 | link4: 0.04 33 | link5: 0.04 34 | link6: 0.04 35 | 36 | self_collision: 37 | geometry: "auto_generated_collision_sphere_group" 38 | buffer_distance: 39 | link1: 0.05 40 | link2: 0.05 41 | ignore: 42 | world: 43 | - "link_base" 44 | link_base: 45 | - "link1" 46 | - "link2" 47 | link1: 48 | - "link2" 49 | - "link3" 50 | - "link4" 51 | link2: 52 | - "link3" 53 | - "link4" 54 | link3: 55 | - "link4" 56 | - "link6" 57 | link4: 58 | - "link5" 59 | - "link6" 60 | - "link_eef" 61 | - "uflite_gripper_link" 62 | - "link_tcp" 63 | link5: 64 | - "link6" 65 | - "link_eef" 66 | - "uflite_gripper_link" 67 | - "link_tcp" 68 | link6: 69 | - "link_eef" 70 | - "uflite_gripper_link" 71 | - "link_tcp" 72 | link_eef: 73 | - "uflite_gripper_link" 74 | - "link_tcp" 75 | uflite_gripper_link: 76 | - "link_tcp" 77 | 78 | geometry: 79 | auto_generated_collision_sphere_group: 80 | spheres: 81 | link1: 82 | - center: [0.0, 0.0, 0.0] 83 | radius: 0.0475 84 | - center: [0.0, 0.0, -0.05] 85 | radius: 0.0475 86 | - center: [0.0, 0.05, 0.0] 87 | radius: 0.0475 88 | link2: 89 | - center: [0.0, 0.001, 0.105] 90 | radius: 0.0467 91 | - center: [0.2, 0.001, 0.105] 92 | radius: 0.0467 93 | - center: [0.04, 0.001, 0.105] 94 | radius: 0.0467 95 | - center: [0.08, 0.001, 0.105] 96 | radius: 0.0467 97 | - center: [0.12, 0.001, 0.105] 98 | radius: 0.0467 99 | - center: [0.16, 0.001, 0.105] 100 | radius: 0.0467 101 | - center: [0.2, 0.0, 0.075] 102 | radius: 0.0406 103 | link3: 104 | - center: [0.0, 0.0, 0.0] 105 | radius: 0.0475 106 | - center: [0.0, 0.0, -0.05] 107 | radius: 0.0475 108 | - center: [0.05, 0.0, 0.0] 109 | radius: 0.0475 110 | - center: [0.085, 0.0, 0.0] 111 | radius: 0.0475 112 | link4: 113 | - center: [0.0, 0.0, -0.1] 114 | radius: 0.0475 115 | - center: [0.0, 0.0, -0.2] 116 | radius: 0.0475 117 | - center: [0.0, 0.0, -0.15] 118 | radius: 0.0475 119 | - center: [0.0, -0.05, -0.1] 120 | radius: 0.0475 121 | - center: [0.0, -0.05, -0.05] 122 | radius: 0.0475 123 | - center: [0.0, -0.05, 0.0] 124 | radius: 0.0475 125 | link5: 126 | - center: [0.0, 0.0, 0.0] 127 | radius: 0.0451 128 | - center: [0.0, 0.025, 0.0] 129 | radius: 0.0451 130 | link6: 131 | - center: [0.0, 0.0, 0.0] 132 | radius: 0.0387 133 | -------------------------------------------------------------------------------- /manymove_msgs/README.md: -------------------------------------------------------------------------------- 1 | # manymove_msgs 2 | 3 | ## Overview 4 | - ROS 2 interface package that defines the actions, messages, and services used throughout the ManyMove system. 5 | - Supports core workflows including motion planning (`PlanManipulator`/`MoveManipulator`), object management (collision object and pose actions), and robot I/O plus state coordination. 6 | - Refer to the top-level [ManyMove README](../README.md) for installation, build, and full usage guidance. 7 | 8 | ## Interfaces 9 | 10 | ### Actions 11 | - `AddCollisionObject.action` – Adds a primitive or mesh object to the planning scene with pose and optional mesh scaling. 12 | - `AttachDetachObject.action` – Attaches or detaches a named collision object to a robot link while managing touch links. 13 | - `CheckObjectExists.action` – Reports whether an object exists in the scene and whether it is currently attached to the robot. 14 | - `CheckRobotState.action` – Checks controller readiness and returns status codes describing the robot’s current mode and state. 15 | - `GetInput.action` – Reads a discrete tool or controller I/O channel and returns its ON/OFF value. 16 | - `GetObjectPose.action` – Retrieves an object pose, applying optional pre/post transforms relative to a specified link. 17 | - `LoadTrajController.action` – Requests the controller manager to load a trajectory controller and reports progress. 18 | - `MoveManipulator.action` – Executes a planned or supplied trajectory and monitors progress and collision feedback. 19 | - `PlanManipulator.action` – Generates a robot trajectory from a `MoveManipulatorGoal` request. 20 | - `RemoveCollisionObject.action` – Removes a named collision object from the planning scene. 21 | - `ResetRobotState.action` – Triggers a robot state reset sequence and reports success or failure. 22 | - `SetOutput.action` – Writes an ON/OFF value to a tool or controller output channel. 23 | - `UnloadTrajController.action` – Unloads a named trajectory controller and reports completion progress. 24 | 25 | ### Messages 26 | - `MoveManipulatorGoal.msg` – Describes the requested manipulator motion (pose, joint, named target, or cartesian) and associated configuration. 27 | - `MovementConfig.msg` – Captures planning, smoothing, and Cartesian interpolation parameters used to customize motion execution. 28 | 29 | ### Services 30 | - `SetBlackboardValues.srv` – Updates multiple behavior tree blackboard keys with typed string-encoded values in one request. 31 | 32 | ## Dependencies 33 | - `geometry_msgs` – Provides `Pose` primitives for target and object pose representations. 34 | - `trajectory_msgs` – Supplies `JointTrajectory` messages exchanged during planning and execution. 35 | - `control_msgs` and `controller_manager_msgs` – Define controller management interfaces used by the trajectory load/unload actions. 36 | - `std_msgs` and `std_srvs` – Contribute common message and service primitives leveraged across the interface set. 37 | - `shape_msgs` – Enables describing collision object geometry beyond basic primitives. 38 | - `tf2`, `tf2_ros`, `tf2_geometry_msgs` – Support expressing object poses relative to different frames via transforms. 39 | - `rosidl_default_generators` and `rosidl_default_runtime` – Generate and provide runtime support for the ROS 2 interfaces in this package. 40 | 41 | ## Usage 42 | - For workspace setup, dependencies, and launch instructions, follow the top-level [ManyMove README](../README.md). 43 | 44 | ``` 45 | 46 | ## License and Maintainers 47 | This package is licensed under BSD-3-Clause. Maintainer: Marco Pastorio . 48 | See main [ManyMove README](../README.md) for `CONTRIBUTION` details. 49 | -------------------------------------------------------------------------------- /manymove_hmi/include/manymove_hmi/hmi_gui.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_HMI__HMI_GUI_HPP_ 30 | #define MANYMOVE_HMI__HMI_GUI_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | class HmiGui : public QMainWindow 42 | { 43 | Q_OBJECT 44 | 45 | public: 46 | /// The new constructor accepts a list of robot prefixes. 47 | explicit HmiGui( 48 | const std::vector & robotPrefixes, std::vector & robotNames, 49 | QWidget * parent = nullptr); 50 | ~HmiGui(); 51 | 52 | public slots: 53 | /// Update the GUI for a given robot (by prefix). 54 | void updateStatus( 55 | const QString & robotPrefix, bool stop_execution, bool reset, bool collision_detected); 56 | 57 | /// Update a textual message for a given robot. 58 | void updateRobotMessage( 59 | const QString & robotPrefix, const QString & message, const QString & color); 60 | 61 | // TCP server slots 62 | void onNewConnection(); 63 | void onSocketDisconnected(); 64 | 65 | signals: 66 | /// Signals that include the robot prefix. 67 | void startExecutionRequested(const std::string & robotPrefix); 68 | void stopExecutionRequested(const std::string & robotPrefix); 69 | void resetProgramRequested(const std::string & robotPrefix); 70 | 71 | private: 72 | QWidget * centralWidget_; 73 | QTcpServer * tcpServer_; 74 | QTcpSocket * clientSocket_; 75 | 76 | // Structure holding one robot’s UI elements. 77 | struct RobotInterface 78 | { 79 | std::string prefix; 80 | QLabel * prefixLabel; 81 | QPushButton * startButton; 82 | QPushButton * stopButton; 83 | QPushButton * resetButton; 84 | QLabel * messageLabel; 85 | }; 86 | 87 | std::vector robotInterfaces_; 88 | std::vector robotNames_; 89 | 90 | // (Optional) For TCP status broadcasting. 91 | QString lastStatusJson_; 92 | }; 93 | 94 | #endif // MANYMOVE_HMI__HMI_GUI_HPP_ 95 | -------------------------------------------------------------------------------- /manymove_cpp_trees/launch/bt_lite_fake.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the bt lite fake scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch.actions import DeclareLaunchArgument 33 | from launch.substitutions import LaunchConfiguration 34 | from launch_ros.actions import Node 35 | 36 | 37 | def generate_launch_description(): 38 | """Create the launch description entry point.""" 39 | return LaunchDescription( 40 | [ 41 | DeclareLaunchArgument( 42 | 'robot_model', 43 | default_value='lite6', 44 | description='Model of the robot (e.g., lite6, uf850, xarm)', 45 | ), 46 | DeclareLaunchArgument( 47 | 'robot_prefix', 48 | default_value='', 49 | description='Prefix for the name of the robot arms', 50 | ), 51 | DeclareLaunchArgument( 52 | 'tcp_frame', 53 | default_value='link_tcp', 54 | description='Name of the link to attach/detach objects to/from', 55 | ), 56 | DeclareLaunchArgument( 57 | 'is_robot_real', 58 | default_value='false', 59 | description=( 60 | 'Set to true if connected with a real robot exposing the ' 61 | 'necessary services needed by manymove_signals' 62 | ), 63 | ), 64 | Node( 65 | package='manymove_cpp_trees', 66 | executable='bt_client', 67 | name='manymove_cpp_trees_single_robot', 68 | output='screen', 69 | parameters=[ 70 | { 71 | 'robot_model': LaunchConfiguration('robot_model'), 72 | 'robot_prefix': LaunchConfiguration('robot_prefix'), 73 | 'tcp_frame': LaunchConfiguration('tcp_frame'), 74 | 'is_robot_real': LaunchConfiguration('is_robot_real'), 75 | } 76 | ], 77 | ), 78 | ] 79 | ) 80 | -------------------------------------------------------------------------------- /manymove_cpp_trees/launch/bt_lite_real.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the bt lite real scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch.actions import DeclareLaunchArgument 33 | from launch.substitutions import LaunchConfiguration 34 | from launch_ros.actions import Node 35 | 36 | 37 | def generate_launch_description(): 38 | """Create the launch description entry point.""" 39 | return LaunchDescription( 40 | [ 41 | DeclareLaunchArgument( 42 | 'robot_model', 43 | default_value='lite6', 44 | description='Model of the robot (e.g., lite6, uf850, xarm)', 45 | ), 46 | DeclareLaunchArgument( 47 | 'robot_prefix', 48 | default_value='', 49 | description='Prefix for the name of the robot arms', 50 | ), 51 | DeclareLaunchArgument( 52 | 'tcp_frame', 53 | default_value='link_tcp', 54 | description='Name of the link to attach/detach objects to/from', 55 | ), 56 | DeclareLaunchArgument( 57 | 'is_robot_real', 58 | default_value='true', 59 | description=( 60 | 'Set to true if connected with a real robot exposing the ' 61 | 'necessary services needed by manymove_signals' 62 | ), 63 | ), 64 | Node( 65 | package='manymove_cpp_trees', 66 | executable='bt_client', 67 | name='manymove_cpp_trees_single_robot', 68 | output='screen', 69 | parameters=[ 70 | { 71 | 'robot_model': LaunchConfiguration('robot_model'), 72 | 'robot_prefix': LaunchConfiguration('robot_prefix'), 73 | 'tcp_frame': LaunchConfiguration('tcp_frame'), 74 | 'is_robot_real': LaunchConfiguration('is_robot_real'), 75 | } 76 | ], 77 | ), 78 | ] 79 | ) 80 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/cumotion_startup.md: -------------------------------------------------------------------------------- 1 | ## Step 1: Developer Environment Setup 2 | 3 | First, set up the compute and developer environment by following Nvidia’s instructions: 4 | 5 | * [Compute Setup](https://nvidia-isaac-ros.github.io/getting_started/hardware_setup/compute/index.html) 6 | * [Developer Environment Setup](https://nvidia-isaac-ros.github.io/getting_started/dev_env_setup.html) 7 | 8 | It’ very important that you completely follow the above setup steps for the platform you are going to use. Don’t skip the steps for [Jetson Platform](https://nvidia-isaac-ros.github.io/getting_started/hardware_setup/compute/index.html#jetson-platforms) if that’s what you are using, including [VPI](https://nvidia-isaac-ros.github.io/getting_started/hardware_setup/compute/jetson_vpi.html). 9 | 10 | ## Step 2: Download and run the setup script 11 | 12 | Download [manymove_isaac_ros_startup.sh](https://github.com/pastoriomarco/manymove/blob/main/manymove_planner/config/isaac_ros/manymove_isaac_ros_startup.sh), make it executable and run it. It will download all the packages tested with ManyMove and Isaac ROS. 13 | 14 | > **Developers on `dev`**: export `MANYMOVE_BRANCH=dev` before running the script so it checks out the development branch instead of `main`. 15 | 16 | ## Step 3: Check isaac_ros_common-config 17 | 18 | Since I’m working with realsense cameras, the current config includes the realsense package. If you are using them too, skip this step. 19 | 20 | If you don’t need it, modify the following file with your favorite editor: 21 | 22 | ``` 23 | ${ISAAC_ROS_WS}/src/isaac_ros_common/scripts/.isaac_ros_common-config 24 | ``` 25 | 26 | Remove `.realsense` step from the config line from: 27 | 28 | ``` 29 | CONFIG_IMAGE_KEY=ros2_humble.realsense.manymove 30 | ``` 31 | 32 | To: 33 | 34 | ``` 35 | CONFIG_IMAGE_KEY=ros2_humble.manymove 36 | ``` 37 | 38 | ## Step 4: Launch the Docker container 39 | 40 | Launch the docker container using the `run_dev.sh` script: 41 | 42 | ``` 43 | cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ 44 | ./scripts/run_dev.sh 45 | ``` 46 | 47 | ## Step 5: Run the examples: 48 | 49 | ### Franka Panda: 50 | 51 | 1. In **TERMINAL 1** start cumotion_planner_node and wait for it to warm up: 52 | 53 | ``` 54 | ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args \ 55 | -p robot:=${ISAAC_ROS_WS}/src/manymove/manymove_planner/config/isaac_ros/franka.xrdf \ 56 | -p urdf_path:=/opt/ros/humble/share/moveit_resources_panda_description/urdf/panda.urdf 57 | ``` 58 | 59 | 2. In **TERMINAL 2**, first enter the the docker container: 60 | 61 | ``` 62 | cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ 63 | ./scripts/run_dev.sh 64 | ``` 65 | 66 | 3. From inside the docker container in **TERMINAL 2**, source the workspace and run the example: 67 | 68 | ``` 69 | source ${ISAAC_ROS_WS}/install/setup.bash 70 | ros2 launch manymove_bringup panda_cumotion_movegroup_fake_cpp_trees.launch.py 71 | ``` 72 | 73 | ### Ufactory Lite6: 74 | 75 | 1. In **TERMINAL 1** start cumotion_planner_node and wait for it to warm up: 76 | 77 | ``` 78 | ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args \ 79 | -p robot:=${ISAAC_ROS_WS}/src/manymove/manymove_planner/config/isaac_ros/lite6_gr.xrdf \ 80 | -p urdf_path:=${ISAAC_ROS_WS}/src/manymove/manymove_planner/config/isaac_ros/lite6.urdf 81 | ``` 82 | 83 | 2. In **TERMINAL 2**, first enter the the docker container: 84 | 85 | ``` 86 | cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ 87 | ./scripts/run_dev.sh 88 | ``` 89 | 90 | 3. From inside the docker container in **TERMINAL 2**, source the workspace and run the example: 91 | 92 | ``` 93 | source ${ISAAC_ROS_WS}/install/setup.bash 94 | ros2 launch manymove_bringup lite_cumotion_movegroup_fake_cpp_trees.launch.py 95 | ``` 96 | 97 | Let me know if you find some issues running this! 98 | -------------------------------------------------------------------------------- /manymove_planner/include/manymove_planner/compat/cartesian_interpolator_compat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | // Compatibility wrapper for moveit::core::CartesianInterpolator API changes 30 | // between Humble (uses JumpThreshold) and Jazzy (introduces CartesianPrecision). 31 | 32 | #pragma once 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include "manymove_msgs/msg/movement_config.hpp" 42 | #include "manymove_planner/compat/moveit_includes_compat.hpp" 43 | 44 | namespace manymove_planner_compat 45 | { 46 | 47 | // Prefer Jazzy .hpp presence as proxy for CartesianPrecision availability 48 | #if __has_include() 49 | #define MANYMOVE_HAS_MOVEIT_CARTESIAN_PRECISION 1 50 | #else 51 | #define MANYMOVE_HAS_MOVEIT_CARTESIAN_PRECISION 0 52 | #endif 53 | 54 | // Wrapper that selects the correct overload at compile time. 55 | template 56 | inline double computeCartesianPathCompat( 57 | moveit::core::RobotState * start_state, const moveit::core::JointModelGroup * jmg, 58 | std::vector & trajectory_states, 59 | const moveit::core::LinkModel * ee_link, const EigenSTL::vector_Isometry3d & eigen_waypoints, 60 | bool global_reference_frame, const moveit::core::MaxEEFStep & max_eef_step, 61 | const manymove_msgs::msg::MovementConfig & cfg, 62 | const moveit::core::GroupStateValidityCallbackFn & validity_callback, 63 | const kinematics::KinematicsQueryOptions & options, RefStatePtr ref_state) 64 | { 65 | #if MANYMOVE_HAS_MOVEIT_CARTESIAN_PRECISION 66 | moveit::core::CartesianPrecision cp{ 67 | cfg.cartesian_precision_translational, cfg.cartesian_precision_rotational, 68 | cfg.cartesian_precision_max_resolution}; 69 | 70 | return moveit::core::CartesianInterpolator::computeCartesianPath( 71 | start_state, jmg, trajectory_states, ee_link, eigen_waypoints, global_reference_frame, 72 | max_eef_step, cp, validity_callback, options, ref_state); 73 | #else 74 | return moveit::core::CartesianInterpolator::computeCartesianPath( 75 | start_state, jmg, trajectory_states, ee_link, eigen_waypoints, global_reference_frame, 76 | max_eef_step, moveit::core::JumpThreshold(cfg.jump_threshold), validity_callback, options, 77 | ref_state); 78 | #endif 79 | } 80 | 81 | } // namespace manymove_planner_compat 82 | -------------------------------------------------------------------------------- /manymove_cpp_trees/README.md: -------------------------------------------------------------------------------- 1 | # ManyMove C++ Trees 2 | 3 | `manymove_cpp_trees` extends [ManyMove](../README.md) with a BehaviorTree.CPP toolkit that orchestrates planners, object management, and signal handling for ROS 2 Humble applications. It ships reusable BT nodes plus sample clients that demonstrate how to stitch robotic behaviors together. 4 | 5 | **Caution:** the package is experimental and provides no built-in safety features. 6 | 7 | ## Overview 8 | - BehaviorTree.CPP integration for planning, object manipulation, gripper control, I/O checks, and Isaac Sim bridges. 9 | - Runtime blackboard utilities that keep motion goals, object metadata, and HMI feedback configurable without recompiling. 10 | - Example BT clients that exercise end-to-end pick, place, and dual-arm scenarios while spinning an accompanying HMI service node. 11 | 12 | ## Behavior Tree Node Families 13 | - `action_nodes_planner.hpp` – plans and executes manipulator moves via `manymove_planner`, caching trajectories and exposing reset hooks. 14 | - `action_nodes_objects.hpp` – creates, attaches, detaches, and queries collision objects through `manymove_object_manager` actions. 15 | - `action_nodes_signals.hpp` – toggles digital I/O, inspects robot state, and (un)loads controllers using `manymove_msgs` actions. 16 | - `action_nodes_gripper.hpp` – wraps `control_msgs` gripper command and trajectory actions plus a utility to publish `sensor_msgs/JointState`. 17 | - `action_nodes_logic.hpp` – provides flow-control helpers (pause/reset decorators, blackboard guards, wait conditions). 18 | - `action_nodes_isaac.hpp` – exchanges poses with Isaac Sim through `simulation_interfaces` services and detection topics. 19 | 20 | Support code in `tree_helper.hpp`, `blackboard_utils.hpp`, and `main_imports_helper.hpp` builds XML snippets, seeds default parameters, and registers node types with the BehaviorTree.CPP factory. 21 | 22 | ## Runtime Services 23 | - [`HMIServiceNode`](./include/manymove_cpp_trees/hmi_service_node.hpp) exposes the `update_blackboard` service and publishes JSON-formatted blackboard snapshots every 250 ms so external HMIs can monitor or update execution state. 24 | 25 | ## Executables 26 | - `bt_client` – single-arm sample that builds a programmatic tree for pick-and-place, including motion planning, object updates, and signal checks. 27 | - `bt_client_dual` – coordinates two robot prefixes and shared objects to demonstrate cooperative sequencing. 28 | - `bt_client_app_dual` – richer dual-arm application with blackboard-driven parameters, mesh scaling, and HMI messaging. 29 | - `bt_client_isaac` – connects the tree to Isaac Sim entities and publishes joint commands for simulated hardware. 30 | - `collision_test` – regression scenario that stresses collision handling and trajectory resets within the planner pipeline. 31 | - `tutorial_01` / `tutorial_01_complete` – incremental learning examples showing how to assemble and extend the helper utilities. 32 | 33 | Each executable spins the BT factory, registers the custom nodes, and runs the `HMIServiceNode` alongside a `MultiThreadedExecutor`. The created ROS node is named `bt_client_node`. 34 | 35 | ## Usage 36 | - For workspace setup, dependencies, and launch instructions, follow the top-level [ManyMove README](../README.md). 37 | 38 | ## Dependencies 39 | - BehaviorTree.CPP (`behaviortree_cpp_v3`) 40 | - ManyMove core packages: `manymove_planner`, `manymove_object_manager`, `manymove_msgs` 41 | - Motion and transforms: `trajectory_msgs`, `geometry_msgs`, `tf2`, `tf2_ros`, `tf2_geometry_msgs` 42 | - Control and runtime messaging: `rclcpp`, `rclcpp_action`, `control_msgs`, `std_msgs`, `std_srvs`, `topic_based_ros2_control` 43 | - Simulation and perception bridges: `simulation_interfaces`, `vision_msgs` 44 | 45 | ## Notes 46 | - Robot safety mechanisms (stop buttons, workspace supervision) must be handled externally. 47 | - Review the main README for project-wide disclaimers, contribution guidance, and licensing details. 48 | 49 | ## License and Maintainers 50 | This package is licensed under BSD-3-Clause. Maintainer: Marco Pastorio . 51 | See main [ManyMove README](../README.md) for `CONTRIBUTION` details. 52 | -------------------------------------------------------------------------------- /manymove_object_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14.4) 2 | project(manymove_object_manager) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Set the C++ standard to C++17 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | 12 | # Find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(moveit_ros_planning_interface REQUIRED) 16 | find_package(tf2 REQUIRED) 17 | find_package(tf2_ros REQUIRED) 18 | find_package(tf2_geometry_msgs REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(shape_msgs REQUIRED) 21 | find_package(moveit_msgs REQUIRED) 22 | find_package(yaml-cpp REQUIRED) 23 | find_package(geometric_shapes REQUIRED) 24 | find_package(manymove_msgs REQUIRED) 25 | 26 | # Include directories (if you have local includes) 27 | include_directories(include) 28 | 29 | # Core library for ObjectManagerNode logic (without main) 30 | add_library(object_manager_node_lib STATIC src/object_manager_node.cpp) 31 | target_include_directories(object_manager_node_lib PUBLIC include) 32 | ament_target_dependencies(object_manager_node_lib 33 | rclcpp 34 | moveit_ros_planning_interface 35 | tf2 36 | tf2_ros 37 | tf2_geometry_msgs 38 | geometry_msgs 39 | shape_msgs 40 | moveit_msgs 41 | geometric_shapes 42 | manymove_msgs 43 | ) 44 | 45 | # Add executable for object_manager_node with main function 46 | add_executable(object_manager_node src/object_manager_main.cpp) 47 | target_link_libraries(object_manager_node object_manager_node_lib) 48 | 49 | # Add executable for collision_spawner 50 | add_executable(collision_spawner src/collision_spawner.cpp) 51 | ament_target_dependencies(collision_spawner 52 | rclcpp 53 | moveit_ros_planning_interface 54 | tf2 55 | tf2_ros 56 | tf2_geometry_msgs 57 | geometry_msgs 58 | shape_msgs 59 | moveit_msgs 60 | yaml-cpp 61 | manymove_msgs 62 | ) 63 | 64 | # Simply link yaml-cpp (and any other libraries) if needed. 65 | target_link_libraries(collision_spawner yaml-cpp) 66 | 67 | # Install executables 68 | install(TARGETS 69 | object_manager_node 70 | collision_spawner 71 | DESTINATION lib/${PROJECT_NAME} 72 | ) 73 | 74 | # Install launch and config files 75 | install(DIRECTORY launch config 76 | DESTINATION share/${PROJECT_NAME}/ 77 | ) 78 | 79 | install(DIRECTORY meshes/ 80 | DESTINATION share/${PROJECT_NAME}/meshes) 81 | 82 | if(BUILD_TESTING) 83 | find_package(ament_cmake_test QUIET) 84 | if(NOT ament_cmake_test_FOUND) 85 | message(WARNING "ament_cmake_test not found; skipping tests for ${PROJECT_NAME}.") 86 | else() 87 | find_package(Python3 REQUIRED COMPONENTS Interpreter) 88 | execute_process( 89 | COMMAND ${Python3_EXECUTABLE} -c "import ament_cmake_test" 90 | RESULT_VARIABLE AMENT_CMAKE_TEST_IMPORT_RESULT 91 | OUTPUT_QUIET ERROR_QUIET) 92 | if(NOT AMENT_CMAKE_TEST_IMPORT_RESULT EQUAL 0) 93 | message(WARNING 94 | "Python module 'ament_cmake_test' not importable; skipping tests for ${PROJECT_NAME}.") 95 | else() 96 | find_package(ament_lint_auto REQUIRED) 97 | find_package(ament_cmake_uncrustify REQUIRED) 98 | ament_lint_auto_find_test_dependencies() 99 | 100 | find_package(ament_cmake_gtest REQUIRED) 101 | ament_add_gtest(test_object_manager_node 102 | test/test_object_manager_node.cpp 103 | test/fake_planning_scene_server.cpp 104 | ) 105 | target_link_libraries(test_object_manager_node 106 | object_manager_node_lib 107 | ) 108 | ament_target_dependencies(test_object_manager_node 109 | rclcpp 110 | rclcpp_action 111 | moveit_ros_planning_interface 112 | tf2 113 | tf2_ros 114 | tf2_geometry_msgs 115 | geometry_msgs 116 | shape_msgs 117 | moveit_msgs 118 | geometric_shapes 119 | manymove_msgs 120 | ) 121 | endif() 122 | endif() 123 | endif() 124 | 125 | ament_package() 126 | -------------------------------------------------------------------------------- /manymove_hmi/test/test_hmi_gui.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "manymove_hmi/hmi_gui.hpp" 39 | 40 | namespace 41 | { 42 | void pump() 43 | { 44 | QCoreApplication::processEvents(); 45 | } 46 | } // namespace 47 | 48 | TEST(HmiGuiTest, UpdateStatusTogglesButtons) 49 | { 50 | std::vector prefixes{"r1_"}; 51 | std::vector names{"R1"}; 52 | HmiGui gui(prefixes, names); 53 | gui.show(); 54 | pump(); 55 | 56 | auto * start = gui.findChild("startButton"); 57 | auto * stop = gui.findChild("stopButton"); 58 | auto * reset = gui.findChild("resetButton"); 59 | ASSERT_NE(start, nullptr); 60 | ASSERT_NE(stop, nullptr); 61 | ASSERT_NE(reset, nullptr); 62 | 63 | // stop_execution = true → start enabled, stop disabled 64 | gui.updateStatus(QString::fromStdString(prefixes[0]), true, false, false); 65 | pump(); 66 | EXPECT_TRUE(start->isEnabled()); 67 | EXPECT_FALSE(stop->isEnabled()); 68 | EXPECT_TRUE(reset->isEnabled()); 69 | 70 | // stop_execution = false → start disabled, stop enabled 71 | gui.updateStatus(QString::fromStdString(prefixes[0]), false, false, false); 72 | pump(); 73 | EXPECT_FALSE(start->isEnabled()); 74 | EXPECT_TRUE(stop->isEnabled()); 75 | // reset disabled in this state per implementation 76 | EXPECT_FALSE(reset->isEnabled()); 77 | } 78 | 79 | TEST(HmiGuiTest, UpdateRobotMessageSetsTextAndStyle) 80 | { 81 | std::vector prefixes{"r1_"}; 82 | std::vector names{"R1"}; 83 | HmiGui gui(prefixes, names); 84 | gui.show(); 85 | pump(); 86 | 87 | gui.updateRobotMessage(QString::fromStdString(prefixes[0]), "Hello", "red"); 88 | pump(); 89 | 90 | // Find any label that matches the message 91 | bool found = false; 92 | for (auto * lbl : gui.findChildren()) { 93 | if (lbl->text() == "Hello") { 94 | found = true; 95 | // Check that a style got applied 96 | EXPECT_FALSE(lbl->styleSheet().isEmpty()); 97 | break; 98 | } 99 | } 100 | EXPECT_TRUE(found); 101 | } 102 | 103 | int main(int argc, char ** argv) 104 | { 105 | setenv("QT_QPA_PLATFORM", "offscreen", 1); 106 | testing::InitGoogleTest(&argc, argv); 107 | QApplication app(argc, argv); 108 | return RUN_ALL_TESTS(); 109 | } 110 | -------------------------------------------------------------------------------- /manymove_cpp_trees/include/manymove_cpp_trees/hmi_service_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_CPP_TREES__HMI_SERVICE_NODE_HPP_ 30 | #define MANYMOVE_CPP_TREES__HMI_SERVICE_NODE_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "manymove_cpp_trees/blackboard_utils.hpp" 44 | #include "manymove_cpp_trees/tree_helper.hpp" 45 | 46 | namespace manymove_cpp_trees 47 | { 48 | 49 | /** 50 | * @brief A node that provides a single "update_blackboard" service 51 | * (using manymove_msgs::srv::SetBlackboardValues) and publishes 52 | * the status of keys every 250ms. 53 | * 54 | * All data is handled as JSON. The publishBlackboardStatus() method iterates 55 | * over a vector of BlackboardEntry structures to build the JSON message. 56 | */ 57 | class HMIServiceNode : public rclcpp::Node 58 | { 59 | public: 60 | // Constructor: pass the complete vector of keys. 61 | explicit HMIServiceNode( 62 | const std::string & node_name, BT::Blackboard::Ptr blackboard, 63 | std::vector keys = {}); 64 | 65 | private: 66 | BT::Blackboard::Ptr blackboard_; 67 | std::vector keys_; 68 | 69 | // The update_blackboard service. 70 | rclcpp::Service::SharedPtr update_blackboard_srv_; 71 | 72 | // Publisher for blackboard status. 73 | rclcpp::Publisher::SharedPtr publisher_; 74 | 75 | // Timer to publish status every 250ms. 76 | rclcpp::TimerBase::SharedPtr status_timer_; 77 | 78 | // Service callback. 79 | void handleUpdateBlackboard( 80 | const std::shared_ptr request, 81 | std::shared_ptr response); 82 | 83 | std::string serializePoseRPY(const geometry_msgs::msg::Pose & pose); 84 | 85 | // Timer callback. 86 | void publishBlackboardStatus(); 87 | 88 | // Minimal JSON parse for "double_array" (e.g. "[0.01, 0.01, 0.25]"). 89 | std::vector parseJsonDoubleArray(const std::string & json_str); 90 | 91 | // Minimal JSON parse for pose (e.g. 92 | // {"x":0.1,"y":0.2,"z":0.3,"roll":1.57,"pitch":0.0,"yaw":0.0}). 93 | geometry_msgs::msg::Pose parseJsonPose(const std::string & json_str); 94 | }; 95 | 96 | } // namespace manymove_cpp_trees 97 | 98 | #endif // MANYMOVE_CPP_TREES__HMI_SERVICE_NODE_HPP_ 99 | -------------------------------------------------------------------------------- /manymove_cpp_trees/test/test_tf_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include "manymove_cpp_trees/action_nodes_logic.hpp" 42 | 43 | using namespace std::chrono_literals; 44 | 45 | class TFNodeFixture : public ::testing::Test 46 | { 47 | protected: 48 | static void SetUpTestSuite() 49 | { 50 | int argc = 0; 51 | rclcpp::init(argc, nullptr); 52 | } 53 | static void TearDownTestSuite() 54 | { 55 | if (rclcpp::ok()) {rclcpp::shutdown();} 56 | } 57 | }; 58 | 59 | TEST_F(TFNodeFixture, GetLinkPoseWithPreTransform) 60 | { 61 | auto node = std::make_shared("tf_node_test"); 62 | auto bb = BT::Blackboard::create(); 63 | bb->set("node", node); 64 | 65 | // Publish static transform world -> link_tcp (identity rotation, translation 1,2,3) 66 | tf2_ros::StaticTransformBroadcaster broadcaster(node); 67 | geometry_msgs::msg::TransformStamped ts; 68 | ts.header.stamp = node->now(); 69 | ts.header.frame_id = "world"; 70 | ts.child_frame_id = "link_tcp"; 71 | ts.transform.translation.x = 1.0; 72 | ts.transform.translation.y = 2.0; 73 | ts.transform.translation.z = 3.0; 74 | ts.transform.rotation.x = 0; 75 | ts.transform.rotation.y = 0; 76 | ts.transform.rotation.z = 0; 77 | ts.transform.rotation.w = 1; 78 | broadcaster.sendTransform(ts); 79 | 80 | // Allow TF buffer to get the transform 81 | std::this_thread::sleep_for(100ms); 82 | 83 | BT::BehaviorTreeFactory factory; 84 | factory.registerNodeType("GetLinkPoseAction"); 85 | 86 | const std::string xml = 87 | R"( 88 | 89 | 90 | 91 | 92 | 93 | )"; 94 | auto tree = factory.createTreeFromText(xml, bb); 95 | ASSERT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); 96 | 97 | geometry_msgs::msg::Pose out; 98 | ASSERT_TRUE(bb->get("tcp_pose", out)); 99 | EXPECT_NEAR(out.position.x, 1.1, 1e-6); 100 | EXPECT_NEAR(out.position.y, 1.9, 1e-6); 101 | EXPECT_NEAR(out.position.z, 3.2, 1e-6); 102 | EXPECT_NEAR(out.orientation.w, 1.0, 1e-9); 103 | } 104 | -------------------------------------------------------------------------------- /manymove_planner/README.md: -------------------------------------------------------------------------------- 1 | # manymove_planner 2 | 3 | `manymove_planner` packages motion-planning action servers that wrap MoveIt 2 so downstream logic (behavior trees, task planners, HMI) can issue consistent actions for industrial robots. It sits alongside the bringup, object management, and message packages in the ManyMove workspace. 4 | 5 | Refer to the repository-level [README](../README.md) for workspace setup, build instructions, and global safety guidance. 6 | 7 | ## Overview 8 | 9 | - Hosts action servers that plan and execute manipulator motions via MoveIt 2 with collision checking, time parameterisation, and velocity/acceleration scaling. 10 | - Supports joint, pose, Cartesian, and named-target requests and exposes helper actions for loading/unloading FollowJointTrajectory controllers. 11 | - Provides a controlled stop interface that back-drives trajectory controllers to achieve smooth deceleration. 12 | - Ships ready-to-launch examples for fake hardware, real controllers, and dual-arm deployments. 13 | 14 | ## Core Components 15 | 16 | - **ManipulatorActionServer** (`include/manymove_planner/action_server.hpp`, `src/action_server.cpp`): wraps the Move / Plan / Load / Unload actions from `manymove_msgs`, applies collision validation, and dispatches trajectories to the selected controller. 17 | - **PlannerInterface** (`include/manymove_planner/planner_interface.hpp`): abstract API defining `plan`, `applyTimeParameterization`, trajectory validation, and `sendControlledStop`. 18 | - **MoveGroupPlanner** (`src/move_group_planner.cpp`): implements `PlannerInterface` on top of `MoveGroupInterface`, supporting joint, pose, Cartesian, and named goal types with time parameterisation and Cartesian speed limiting. 19 | - **MoveItCppPlanner** (`src/moveit_cpp_planner.cpp`): `PlannerInterface` implementation that reuses a shared `moveit_cpp::MoveItCpp` instance, providing path-length estimation, motion smoothing, and direct access to PlanningSceneMonitor services. 20 | - **Executables** from `CMakeLists.txt`: 21 | - `action_server_node`: parameter-driven node that instantiates either planner backend; supports per-robot prefixes (`node_prefix`) and controller names. 22 | - `moveitcpp_action_server_node`: creates one `MoveItCpp` instance and spins multiple `ManipulatorActionServer`s for multi-robot/dual-arm applications. 23 | 24 | ## Launch & Usage 25 | - For workspace setup, dependencies, and launch instructions, follow the top-level [ManyMove README](../README.md). 26 | 27 | ## Interactions 28 | 29 | - **Messages and actions:** consumes `manymove_msgs` definitions (`PlanManipulator`, `MoveManipulator`, `LoadTrajController`, `MovementConfig`) for goals and constraints. 30 | - **Planning scene:** pairs with `manymove_object_manager` to populate collision objects; launch files start this node alongside the planner so the MoveIt planning scene stays synchronised. 31 | - **Robot bringup:** relies on bringup packages to load URDF, SRDF, controllers, and ros2_control pipelines before motion commands are accepted. 32 | - **MoveIt dependencies:** uses `moveit_core`, `moveit_ros_planning_interface`, and `moveit_cpp::PlanningComponent` for planning, and `control_msgs/FollowJointTrajectory` for execution feedback. 33 | 34 | ## Dependencies 35 | 36 | Key runtime dependencies declared in [`package.xml`](package.xml) include: 37 | 38 | - `rclcpp`, `rclcpp_action`, `action_msgs` for ROS 2 node and action infrastructure. 39 | - `moveit_core`, `moveit_ros_planning_interface`, `geometry_msgs`, `tf2_geometry_msgs`, `moveit_msgs` for planning and scene representation. 40 | - `control_msgs`, `controller_manager_msgs` to command trajectory controllers and manage their lifecycle. 41 | - `manymove_msgs` for custom action/goal definitions shared across the ManyMove stack. 42 | 43 | ## Extending the Planner 44 | 45 | To integrate a new planning backend: 46 | 47 | 1. Implement `PlannerInterface`, providing planning, time-parameterisation, trajectory validation, and stop logic tailored to your backend. 48 | 2. Link the new class into `action_server_node` or a custom executable and expose a `planner_type` parameter so launch files can select it. 49 | 3. Add any backend-specific configuration to the `config/` directory and reference it from your launch description. 50 | 51 | ## License and Maintainers 52 | This package is licensed under BSD-3-Clause. Maintainer: Marco Pastorio . 53 | See main [ManyMove README](../README.md) for `CONTRIBUTION` details. 54 | -------------------------------------------------------------------------------- /manymove_cpp_trees/launch/bt_panda_fake.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the bt panda fake scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch.actions import DeclareLaunchArgument 33 | from launch.substitutions import LaunchConfiguration 34 | from launch_ros.actions import Node 35 | 36 | 37 | def generate_launch_description(): 38 | """Create the launch description entry point.""" 39 | return LaunchDescription( 40 | [ 41 | DeclareLaunchArgument( 42 | 'robot_model', 43 | default_value='panda_arm', 44 | description='Model of the robot (e.g., lite6, uf850, xarm)', 45 | ), 46 | DeclareLaunchArgument( 47 | 'robot_prefix', 48 | default_value='', 49 | description='Prefix for the name of the robot arms', 50 | ), 51 | DeclareLaunchArgument( 52 | 'tcp_frame', 53 | default_value='panda_link8', 54 | description='Name of the link to attach/detach objects to/from', 55 | ), 56 | DeclareLaunchArgument( 57 | 'gripper_action_server', 58 | default_value='/panda_hand_controller/gripper_cmd', 59 | description='Name of the action server to control the gripper', 60 | ), 61 | DeclareLaunchArgument( 62 | 'contact_links', 63 | default_value='["panda_leftfinger", "panda_rightfinger", "panda_hand"]', 64 | description='List of links to exclude from collision checking', 65 | ), 66 | DeclareLaunchArgument( 67 | 'is_robot_real', 68 | default_value='false', 69 | description=( 70 | 'Set to true if connected with a real robot exposing the ' 71 | 'necessary services needed by manymove_signals' 72 | ), 73 | ), 74 | Node( 75 | package='manymove_cpp_trees', 76 | executable='bt_client', 77 | name='manymove_cpp_trees_single_robot', 78 | output='screen', 79 | parameters=[ 80 | { 81 | 'robot_model': LaunchConfiguration('robot_model'), 82 | 'robot_prefix': LaunchConfiguration('robot_prefix'), 83 | 'tcp_frame': LaunchConfiguration('tcp_frame'), 84 | 'gripper_action_server': LaunchConfiguration('gripper_action_server'), 85 | 'contact_links': LaunchConfiguration('contact_links'), 86 | 'is_robot_real': LaunchConfiguration('is_robot_real'), 87 | } 88 | ], 89 | ), 90 | ] 91 | ) 92 | -------------------------------------------------------------------------------- /manymove_hmi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14.4) 2 | project(manymove_hmi VERSION 0.3.2 LANGUAGES CXX) 3 | 4 | # ────────────────────────────────────────────────────────────── 5 | # Global / Qt 6 | # ────────────────────────────────────────────────────────────── 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | 10 | set(CMAKE_AUTOMOC ON) 11 | set(CMAKE_AUTOUIC ON) 12 | set(CMAKE_AUTORCC ON) 13 | 14 | # ────────────────────────────────────────────────────────────── 15 | # ROS 2 & Qt deps 16 | # ────────────────────────────────────────────────────────────── 17 | find_package(ament_cmake REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(std_srvs REQUIRED) 21 | find_package(manymove_msgs REQUIRED) 22 | find_package(Qt5 REQUIRED COMPONENTS Core Widgets Network) 23 | 24 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) 25 | 26 | # ────────────────────────────────────────────────────────────── 27 | # 1. Core HMI library (all Qt widgets) 28 | # ────────────────────────────────────────────────────────────── 29 | set(HMI_LIB_SOURCES 30 | src/hmi_gui.cpp 31 | src/ros2_worker.cpp 32 | src/app_module.cpp 33 | src/default_app_hmi.cpp # <── NEW 34 | ## headers that contain Q_OBJECT 35 | include/manymove_hmi/hmi_gui.hpp 36 | include/manymove_hmi/ros2_worker.hpp 37 | include/manymove_hmi/app_module.hpp 38 | include/manymove_hmi/default_app_hmi.hpp 39 | ) 40 | 41 | add_library(manymove_hmi_lib ${HMI_LIB_SOURCES}) 42 | 43 | target_link_libraries(manymove_hmi_lib Qt5::Core Qt5::Widgets Qt5::Network) 44 | ament_target_dependencies(manymove_hmi_lib 45 | rclcpp std_msgs std_srvs manymove_msgs) 46 | 47 | # ────────────────────────────────────────────────────────────── 48 | # 2. Minimal “empty-GUI” executable (still useful sometimes) 49 | # ────────────────────────────────────────────────────────────── 50 | add_executable(manymove_hmi_executable src/main.cpp) 51 | target_link_libraries(manymove_hmi_executable 52 | manymove_hmi_lib Qt5::Core Qt5::Widgets Qt5::Network) 53 | ament_target_dependencies(manymove_hmi_executable 54 | rclcpp std_msgs std_srvs manymove_msgs) 55 | 56 | # ────────────────────────────────────────────────────────────── 57 | # 3. Default application (uses DefaultAppModule) 58 | # ────────────────────────────────────────────────────────────── 59 | add_executable(manymove_hmi_app_executable src/main_app.cpp) 60 | target_link_libraries(manymove_hmi_app_executable 61 | manymove_hmi_lib Qt5::Core Qt5::Widgets Qt5::Network) 62 | ament_target_dependencies(manymove_hmi_app_executable 63 | rclcpp std_msgs std_srvs manymove_msgs) 64 | 65 | # ────────────────────────────────────────────────────────────── 66 | # 4. Install / Export 67 | # ────────────────────────────────────────────────────────────── 68 | install(DIRECTORY include/ DESTINATION include) 69 | 70 | install(TARGETS manymove_hmi_lib 71 | EXPORT export_manymove_hmi_lib 72 | ARCHIVE DESTINATION lib 73 | LIBRARY DESTINATION lib) 74 | 75 | install(TARGETS 76 | manymove_hmi_executable 77 | manymove_hmi_app_executable 78 | DESTINATION lib/${PROJECT_NAME}) 79 | 80 | ament_export_include_directories(include) 81 | ament_export_targets(export_manymove_hmi_lib HAS_LIBRARY_TARGET) 82 | ament_export_dependencies(rclcpp std_msgs std_srvs manymove_msgs) 83 | 84 | if(BUILD_TESTING) 85 | find_package(ament_lint_auto REQUIRED) 86 | find_package(ament_cmake_uncrustify REQUIRED) 87 | ament_lint_auto_find_test_dependencies() 88 | 89 | find_package(ament_cmake_gtest REQUIRED) 90 | 91 | # AppModule tests (Qt widgets + signals) 92 | ament_add_gtest(test_hmi_app_module 93 | test/test_app_module.cpp) 94 | if(TARGET test_hmi_app_module) 95 | target_link_libraries(test_hmi_app_module 96 | manymove_hmi_lib Qt5::Core Qt5::Widgets Qt5::Network) 97 | ament_target_dependencies(test_hmi_app_module 98 | rclcpp std_msgs std_srvs manymove_msgs) 99 | endif() 100 | 101 | # HmiGui tests (button state updates) 102 | ament_add_gtest(test_hmi_gui 103 | test/test_hmi_gui.cpp) 104 | if(TARGET test_hmi_gui) 105 | target_link_libraries(test_hmi_gui 106 | manymove_hmi_lib Qt5::Core Qt5::Widgets Qt5::Network) 107 | ament_target_dependencies(test_hmi_gui 108 | rclcpp std_msgs std_srvs manymove_msgs) 109 | endif() 110 | endif() 111 | 112 | ament_package() 113 | -------------------------------------------------------------------------------- /manymove_cpp_trees/launch/bt_lite_fake_dual.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the bt lite fake dual scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch.actions import DeclareLaunchArgument 33 | from launch.substitutions import LaunchConfiguration 34 | from launch_ros.actions import Node 35 | 36 | 37 | def generate_launch_description(): 38 | """Create the launch description entry point.""" 39 | return LaunchDescription( 40 | [ 41 | DeclareLaunchArgument( 42 | 'robot_model_1', 43 | default_value='lite6', 44 | description='Model of the robot (e.g., lite6, uf850, xarm)', 45 | ), 46 | DeclareLaunchArgument( 47 | 'robot_prefix_1', 48 | default_value='L_', 49 | description='Prefix for the name of the robot arms', 50 | ), 51 | DeclareLaunchArgument( 52 | 'tcp_frame_1', 53 | default_value='link_tcp', 54 | description='Name of the link to attach/detach objects to/from', 55 | ), 56 | DeclareLaunchArgument( 57 | 'robot_model_2', 58 | default_value='uf850', 59 | description='Model of the robot (e.g., lite6, uf850, xarm)', 60 | ), 61 | DeclareLaunchArgument( 62 | 'robot_prefix_2', 63 | default_value='R_', 64 | description='Prefix for the name of the robot arms', 65 | ), 66 | DeclareLaunchArgument( 67 | 'tcp_frame_2', 68 | default_value='link_tcp', 69 | description='Name of the link to attach/detach objects to/from', 70 | ), 71 | DeclareLaunchArgument( 72 | 'is_robot_real', 73 | default_value='false', 74 | description=( 75 | 'Set to true if connected with a real robot exposing the ' 76 | 'necessary services needed by manymove_signals' 77 | ), 78 | ), 79 | Node( 80 | package='manymove_cpp_trees', 81 | executable='bt_client_dual', 82 | name='manymove_cpp_trees_single_robot', 83 | output='screen', 84 | parameters=[ 85 | { 86 | 'robot_model_1': LaunchConfiguration('robot_model_1'), 87 | 'robot_prefix_1': LaunchConfiguration('robot_prefix_1'), 88 | 'tcp_frame_1': LaunchConfiguration('tcp_frame_1'), 89 | 'robot_model_2': LaunchConfiguration('robot_model_2'), 90 | 'robot_prefix_2': LaunchConfiguration('robot_prefix_2'), 91 | 'tcp_frame_2': LaunchConfiguration('tcp_frame_2'), 92 | 'is_robot_real': LaunchConfiguration('is_robot_real'), 93 | } 94 | ], 95 | ), 96 | ] 97 | ) 98 | -------------------------------------------------------------------------------- /manymove_cpp_trees/launch/bt_lite_fake_dual_app.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2025 Flexin Group SRL 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of the Flexin Group SRL nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | """Launch description for the bt lite fake dual app scenario.""" 30 | 31 | from launch import LaunchDescription 32 | from launch.actions import DeclareLaunchArgument 33 | from launch.substitutions import LaunchConfiguration 34 | from launch_ros.actions import Node 35 | 36 | 37 | def generate_launch_description(): 38 | """Create the launch description entry point.""" 39 | return LaunchDescription( 40 | [ 41 | DeclareLaunchArgument( 42 | 'robot_model_1', 43 | default_value='lite6', 44 | description='Model of the robot (e.g., lite6, uf850, xarm)', 45 | ), 46 | DeclareLaunchArgument( 47 | 'robot_prefix_1', 48 | default_value='L_', 49 | description='Prefix for the name of the robot arms', 50 | ), 51 | DeclareLaunchArgument( 52 | 'tcp_frame_1', 53 | default_value='link_tcp', 54 | description='Name of the link to attach/detach objects to/from', 55 | ), 56 | DeclareLaunchArgument( 57 | 'robot_model_2', 58 | default_value='uf850', 59 | description='Model of the robot (e.g., lite6, uf850, xarm)', 60 | ), 61 | DeclareLaunchArgument( 62 | 'robot_prefix_2', 63 | default_value='R_', 64 | description='Prefix for the name of the robot arms', 65 | ), 66 | DeclareLaunchArgument( 67 | 'tcp_frame_2', 68 | default_value='link_tcp', 69 | description='Name of the link to attach/detach objects to/from', 70 | ), 71 | DeclareLaunchArgument( 72 | 'is_robot_real', 73 | default_value='false', 74 | description=( 75 | 'Set to true if connected with a real robot exposing the ' 76 | 'necessary services needed by manymove_signals' 77 | ), 78 | ), 79 | Node( 80 | package='manymove_cpp_trees', 81 | executable='bt_client_app_dual', 82 | name='manymove_cpp_trees_single_robot', 83 | output='screen', 84 | parameters=[ 85 | { 86 | 'robot_model_1': LaunchConfiguration('robot_model_1'), 87 | 'robot_prefix_1': LaunchConfiguration('robot_prefix_1'), 88 | 'tcp_frame_1': LaunchConfiguration('tcp_frame_1'), 89 | 'robot_model_2': LaunchConfiguration('robot_model_2'), 90 | 'robot_prefix_2': LaunchConfiguration('robot_prefix_2'), 91 | 'tcp_frame_2': LaunchConfiguration('tcp_frame_2'), 92 | 'is_robot_real': LaunchConfiguration('is_robot_real'), 93 | } 94 | ], 95 | ), 96 | ] 97 | ) 98 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [main, dev] 6 | pull_request: 7 | branches: [main, dev] 8 | workflow_dispatch: 9 | 10 | jobs: 11 | build-and-test: 12 | runs-on: ubuntu-latest 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | ros_distro: [humble, jazzy] 17 | 18 | container: 19 | image: ros:${{ matrix.ros_distro }}-ros-base 20 | 21 | env: 22 | ROS_DISTRO: ${{ matrix.ros_distro }} 23 | COLCON_HOME: /tmp/colcon 24 | ROS_WS: /tmp/ros_ws 25 | CCACHE_DIR: /root/.ccache 26 | CCACHE_BASEDIR: /tmp/ros_ws 27 | CCACHE_COMPRESS: "1" 28 | CCACHE_MAXSIZE: 5G 29 | 30 | steps: 31 | - name: Checkout repository 32 | uses: actions/checkout@v4 33 | 34 | # Cache compiler artifacts with ccache 35 | - name: Cache ccache 36 | uses: actions/cache@v4 37 | with: 38 | path: /root/.ccache 39 | key: ccache-${{ matrix.ros_distro }}-${{ runner.os }}-v1 40 | restore-keys: | 41 | ccache-${{ matrix.ros_distro }}- 42 | 43 | - name: Add Isaac ROS apt repository (Jazzy) 44 | if: matrix.ros_distro == 'jazzy' 45 | shell: bash 46 | run: | 47 | set -euo pipefail 48 | apt-get update 49 | apt-get install -y curl gnupg 50 | mkdir -p /usr/share/keyrings 51 | curl -fsSL https://isaac.download.nvidia.com/isaac-ros/repos.key \ 52 | | gpg --dearmor -o /usr/share/keyrings/isaac-ros-archive-keyring.gpg 53 | . /etc/os-release 54 | echo "deb [signed-by=/usr/share/keyrings/isaac-ros-archive-keyring.gpg] https://isaac.download.nvidia.com/isaac-ros/release-4 ${VERSION_CODENAME} main" \ 55 | > /etc/apt/sources.list.d/isaac-ros.list 56 | apt-get update 57 | 58 | - name: Install build tooling 59 | run: | 60 | set -e 61 | apt-get update 62 | apt-get install -y \ 63 | ccache \ 64 | ros-$ROS_DISTRO-topic-based-ros2-control \ 65 | python3-colcon-common-extensions python3-rosdep git \ 66 | ros-$ROS_DISTRO-moveit \ 67 | ros-$ROS_DISTRO-ur \ 68 | ros-$ROS_DISTRO-ur-moveit-config \ 69 | ros-$ROS_DISTRO-robotiq-description \ 70 | ros-$ROS_DISTRO-robotiq-controllers \ 71 | ros-$ROS_DISTRO-joint-state-publisher \ 72 | ros-$ROS_DISTRO-control-toolbox \ 73 | ros-$ROS_DISTRO-controller-manager \ 74 | ros-$ROS_DISTRO-image-view \ 75 | ros-$ROS_DISTRO-tf-transformations \ 76 | ros-$ROS_DISTRO-find-object-2d \ 77 | ros-$ROS_DISTRO-controller-manager-msgs \ 78 | ros-$ROS_DISTRO-joint-state-broadcaster \ 79 | ros-$ROS_DISTRO-joint-trajectory-controller \ 80 | ros-$ROS_DISTRO-diff-drive-controller \ 81 | ros-$ROS_DISTRO-joy \ 82 | ros-$ROS_DISTRO-moveit-servo \ 83 | ros-$ROS_DISTRO-behaviortree-cpp-v3 \ 84 | ros-$ROS_DISTRO-moveit-visual-tools \ 85 | ros-$ROS_DISTRO-vision-msgs \ 86 | ros-$ROS_DISTRO-simulation-interfaces 87 | 88 | - name: Setup rosdep 89 | run: | 90 | rosdep update 91 | 92 | - name: Prepare workspace 93 | run: | 94 | mkdir -p ${ROS_WS}/src 95 | ln -s ${GITHUB_WORKSPACE} ${ROS_WS}/src/manymove 96 | 97 | - name: Install package dependencies 98 | shell: bash 99 | run: | 100 | set -e 101 | source /opt/ros/$ROS_DISTRO/setup.bash 102 | rosdep install --from-paths ${ROS_WS}/src --ignore-src --rosdistro ${ROS_DISTRO} -y 103 | 104 | - name: Build workspace 105 | shell: bash 106 | run: | 107 | set -e 108 | source /opt/ros/$ROS_DISTRO/setup.bash 109 | cd ${ROS_WS} 110 | # Show ccache status before build 111 | ccache -s || true 112 | colcon build \ 113 | --base-paths ${ROS_WS}/src \ 114 | --event-handlers console_cohesion+ \ 115 | --cmake-args \ 116 | -DCMAKE_C_COMPILER_LAUNCHER=ccache \ 117 | -DCMAKE_CXX_COMPILER_LAUNCHER=ccache 118 | # Show ccache status after build 119 | ccache -s || true 120 | 121 | - name: Run tests 122 | shell: bash 123 | run: | 124 | set -e 125 | source /opt/ros/$ROS_DISTRO/setup.bash 126 | cd ${ROS_WS} 127 | colcon test \ 128 | --base-paths src \ 129 | --event-handlers console_cohesion+ 130 | colcon test-result --verbose 131 | -------------------------------------------------------------------------------- /manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef MANYMOVE_CPP_TREES__MAIN_IMPORTS_HELPER_HPP_ 30 | #define MANYMOVE_CPP_TREES__MAIN_IMPORTS_HELPER_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include "manymove_cpp_trees/action_nodes_logic.hpp" 43 | #include "manymove_cpp_trees/action_nodes_objects.hpp" 44 | #include "manymove_cpp_trees/action_nodes_planner.hpp" 45 | #include "manymove_cpp_trees/action_nodes_signals.hpp" 46 | #include "manymove_cpp_trees/bt_converters.hpp" 47 | #include "manymove_cpp_trees/hmi_service_node.hpp" 48 | #include "manymove_cpp_trees/move.hpp" 49 | #include "manymove_cpp_trees/object.hpp" 50 | #include "manymove_cpp_trees/robot.hpp" 51 | #include "manymove_cpp_trees/tree_helper.hpp" 52 | #include "manymove_msgs/action/check_robot_state.hpp" 53 | #include "manymove_msgs/action/get_input.hpp" 54 | #include "manymove_msgs/action/reset_robot_state.hpp" 55 | #include "manymove_msgs/action/set_output.hpp" 56 | #include "manymove_cpp_trees/action_nodes_isaac.hpp" 57 | 58 | using geometry_msgs::msg::Pose; 59 | 60 | using manymove_cpp_trees::defineRobotParams; 61 | using manymove_cpp_trees::defineVariableKey; 62 | using manymove_cpp_trees::defineMovementConfigs; 63 | using manymove_cpp_trees::buildMoveXML; 64 | using manymove_cpp_trees::sequenceWrapperXML; 65 | using manymove_cpp_trees::parallelWrapperXML; 66 | using manymove_cpp_trees::fallbackWrapperXML; 67 | using manymove_cpp_trees::repeatSequenceWrapperXML; 68 | using manymove_cpp_trees::retrySequenceWrapperXML; 69 | using manymove_cpp_trees::BlackboardEntry; 70 | using manymove_cpp_trees::Move; 71 | using manymove_cpp_trees::RobotParams; 72 | using manymove_cpp_trees::createPose; 73 | using manymove_cpp_trees::createPoseRPY; 74 | using manymove_cpp_trees::buildObjectActionXML; 75 | using manymove_cpp_trees::createCheckObjectExists; 76 | using manymove_cpp_trees::createAddObject; 77 | using manymove_cpp_trees::createAttachObject; 78 | using manymove_cpp_trees::createDetachObject; 79 | using manymove_cpp_trees::createRemoveObject; 80 | using manymove_cpp_trees::createGetObjectPose; 81 | using manymove_cpp_trees::ObjectSnippets; 82 | using manymove_cpp_trees::createObjectSnippets; 83 | using manymove_cpp_trees::buildSetOutputXML; 84 | using manymove_cpp_trees::buildWaitForInput; 85 | using manymove_cpp_trees::buildCheckRobotStateXML; 86 | using manymove_cpp_trees::buildResetRobotStateXML; 87 | using manymove_cpp_trees::buildWaitForObject; 88 | using manymove_cpp_trees::buildWaitForKeyBool; 89 | using manymove_cpp_trees::buildCheckKeyBool; 90 | using manymove_cpp_trees::buildSetKeyBool; 91 | using manymove_cpp_trees::buildFoundationPoseSequenceXML; 92 | using manymove_cpp_trees::buildCopyPoseXML; 93 | using manymove_cpp_trees::buildCheckPoseDistanceXML; 94 | using manymove_cpp_trees::mainTreeWrapperXML; 95 | using manymove_cpp_trees::registerAllNodeTypes; 96 | 97 | #endif // MANYMOVE_CPP_TREES__MAIN_IMPORTS_HELPER_HPP_ 98 | -------------------------------------------------------------------------------- /manymove_planner/src/action_server_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "manymove_planner/action_server.hpp" 32 | #include "manymove_planner/move_group_planner.hpp" 33 | #include "manymove_planner/moveit_cpp_planner.hpp" 34 | 35 | int main(int argc, char ** argv) 36 | { 37 | rclcpp::init(argc, argv); 38 | 39 | // Building loader node to get parameters 40 | rclcpp::NodeOptions loader_options; 41 | loader_options.automatically_declare_parameters_from_overrides(true); 42 | auto loader_node = rclcpp::Node::make_shared("param_loader_node", loader_options); 43 | 44 | std::string node_prefix; 45 | loader_node->get_parameter_or("node_prefix", node_prefix, ""); 46 | 47 | // Build the “real” node name from the prefix. 48 | // For multiple robots, e.g. "L_action_server_node" or "R_action_server_node". 49 | std::string node_name = node_prefix + "action_server_node"; 50 | RCLCPP_INFO(loader_node->get_logger(), "Action Server Node name: %s", node_name.c_str()); 51 | 52 | rclcpp::NodeOptions node_options; 53 | node_options.automatically_declare_parameters_from_overrides(true); 54 | auto node = rclcpp::Node::make_shared(node_name, "", node_options); 55 | 56 | // Retrieve parameters 57 | std::string planner_type; 58 | node->get_parameter_or("planner_type", planner_type, "movegroup"); 59 | 60 | std::string planner_prefix; 61 | loader_node->get_parameter_or("planner_prefix", planner_prefix, ""); 62 | std::string planning_group; 63 | node->get_parameter_or("planning_group", planning_group, "lite6"); 64 | std::string base_frame; 65 | node->get_parameter_or("base_frame", base_frame, "link_base"); 66 | std::string tcp_frame; 67 | node->get_parameter_or("tcp_frame", tcp_frame, "link_tcp"); 68 | std::string traj_controller; 69 | node->get_parameter_or("traj_controller", traj_controller, "lite6_traj_controller"); 70 | 71 | planning_group = planner_prefix + planning_group; 72 | base_frame = planner_prefix + base_frame; 73 | tcp_frame = planner_prefix + tcp_frame; 74 | traj_controller = planner_prefix + traj_controller; 75 | 76 | // Instantiate the appropriate planner based on the planner_type parameter 77 | std::shared_ptr planner; 78 | 79 | if (planner_type == "moveitcpp") { 80 | planner = std::make_shared(node, planning_group, base_frame, traj_controller); 81 | RCLCPP_INFO(node->get_logger(), "==================================================="); 82 | RCLCPP_INFO(node->get_logger(), "Using MoveItCppPlanner."); 83 | RCLCPP_INFO(node->get_logger(), "==================================================="); 84 | } else if (planner_type == "movegroup") { 85 | planner = std::make_shared(node, planning_group, base_frame, traj_controller); 86 | RCLCPP_INFO(node->get_logger(), "==================================================="); 87 | RCLCPP_INFO(node->get_logger(), "Using MoveGroupPlanner."); 88 | RCLCPP_INFO(node->get_logger(), "==================================================="); 89 | } else { 90 | RCLCPP_ERROR( 91 | node->get_logger(), 92 | "Unknown planner_type: %s. Valid options are 'moveitcpp' and 'movegroup'.", 93 | planner_type.c_str()); 94 | return 1; 95 | } 96 | 97 | auto server = std::make_shared(node, planner, planner_prefix); 98 | 99 | loader_node.reset(); 100 | rclcpp::executors::MultiThreadedExecutor executor; 101 | executor.add_node(node); 102 | executor.spin(); 103 | 104 | rclcpp::shutdown(); 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /manymove_bringup/srdf/ur_with_robotiq.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /manymove_planner/config/isaac_ros/lite6_gr.xrdf: -------------------------------------------------------------------------------- 1 | format: "xrdf" 2 | 3 | format_version: 1.0 4 | 5 | default_joint_positions: 6 | joint1: 0.0 7 | joint2: 0.0 8 | joint3: 0.0 9 | joint4: -0.0 10 | joint5: 0.0 11 | joint6: 0.0 12 | 13 | cspace: 14 | joint_names: 15 | - "joint1" 16 | - "joint2" 17 | - "joint3" 18 | - "joint4" 19 | - "joint5" 20 | - "joint6" 21 | acceleration_limits: [10, 10, 10, 10, 10, 10] 22 | jerk_limits: [10000, 10000, 10000, 10000, 10000, 10000] 23 | 24 | tool_frames: ["link_tcp"] 25 | 26 | collision: 27 | geometry: "auto_generated_collision_sphere_group" 28 | buffer_distance: 29 | link1: 0.004 30 | link2: 0.004 31 | link3: 0.004 32 | link4: 0.004 33 | link5: 0.004 34 | link6: 0.004 35 | 36 | self_collision: 37 | geometry: "auto_generated_collision_sphere_group" 38 | buffer_distance: 39 | link1: 0.005 40 | link2: 0.005 41 | ignore: 42 | world: 43 | - "link_base" 44 | link_base: 45 | - "link1" 46 | - "link2" 47 | link1: 48 | - "link2" 49 | - "link3" 50 | - "link4" 51 | link2: 52 | - "link3" 53 | - "link4" 54 | link3: 55 | - "link4" 56 | - "link6" 57 | link4: 58 | - "link5" 59 | - "link6" 60 | - "link_eef" 61 | - "uflite_gripper_link" 62 | - "link_tcp" 63 | link5: 64 | - "link6" 65 | - "link_eef" 66 | - "uflite_gripper_link" 67 | - "link_tcp" 68 | link6: 69 | - "link_eef" 70 | - "uflite_gripper_link" 71 | - "link_tcp" 72 | link_eef: 73 | - "uflite_gripper_link" 74 | - "link_tcp" 75 | uflite_gripper_link: 76 | - "link_tcp" 77 | 78 | geometry: 79 | auto_generated_collision_sphere_group: 80 | spheres: 81 | link1: 82 | - center: [0.0, 0.0, 0.0] 83 | radius: 0.042 84 | - center: [0.0, 0.0, -0.05] 85 | radius: 0.042 86 | - center: [0.0, 0.0, -0.025] 87 | radius: 0.042 88 | - center: [0.0, 0.05, 0.0] 89 | radius: 0.042 90 | - center: [0.0, 0.025, 0.0] 91 | radius: 0.042 92 | link2: 93 | - center: [0.0, 0.0, 0.09] 94 | radius: 0.045 95 | - center: [0.025, 0.0, 0.1] 96 | radius: 0.045 97 | - center: [0.175, 0.0, 0.1] 98 | radius: 0.045 99 | - center: [0.05, 0.0, 0.1] 100 | radius: 0.045 101 | - center: [0.075, 0.0, 0.1] 102 | radius: 0.045 103 | - center: [0.1, 0.0, 0.1] 104 | radius: 0.045 105 | - center: [0.125, 0.0, 0.1] 106 | radius: 0.045 107 | - center: [0.15, 0.0, 0.1] 108 | radius: 0.045 109 | - center: [0.2, 0.0, 0.09] 110 | radius: 0.045 111 | - center: [0.2, 0.0, 0.07] 112 | radius: 0.045 113 | link3: 114 | - center: [0.0, 0.0, 0.0] 115 | radius: 0.045 116 | - center: [0.0, 0.0, -0.025] 117 | radius: 0.045 118 | - center: [0.085, 0.0, 0.0] 119 | radius: 0.045 120 | - center: [0.042, 0.005, 0.0] 121 | radius: 0.04 122 | - center: [0.087, -0.018, 0.0] 123 | radius: 0.04 124 | link4: 125 | - center: [0.0, 0.0, -0.17] 126 | radius: 0.045 127 | - center: [0.0, 0.0, -0.09] 128 | radius: 0.045 129 | - center: [0.0, 0.0, -0.143] 130 | radius: 0.045 131 | - center: [0.0, 0.0, -0.117] 132 | radius: 0.045 133 | - center: [0.0, -0.045, -0.095] 134 | radius: 0.045 135 | - center: [0.0, -0.05, 0.0] 136 | radius: 0.04 137 | - center: [0.0, -0.055, -0.048] 138 | radius: 0.0375 139 | link5: 140 | - center: [0.0, 0.0, 0.0] 141 | radius: 0.04 142 | - center: [0.0, 0.0, 0.025] 143 | radius: 0.04 144 | - center: [0.0, 0.025, 0.0] 145 | radius: 0.04 146 | link6: 147 | - center: [0.014, -0.024, -0.011] 148 | radius: 0.015 149 | - center: [-0.011, -0.024, -0.011] 150 | radius: 0.015 151 | - center: [0.019, 0.019, -0.012] 152 | radius: 0.015 153 | - center: [-0.019, 0.017, -0.012] 154 | radius: 0.015 155 | - center: [-0.027, -0.006, -0.012] 156 | radius: 0.015 157 | - center: [0.027, -0.005, -0.012] 158 | radius: 0.015 159 | - center: [0.0, 0.0, -0.008] 160 | radius: 0.016 161 | - center: [0.0, 0.03, -0.008] 162 | radius: 0.016 163 | uflite_gripper_link: 164 | - center: [0.0, -0.004, 0.027] 165 | radius: 0.0304 166 | - center: [-0.0, 0.01, 0.026] 167 | radius: 0.0288 168 | - center: [0.0, 0.024, 0.07] 169 | radius: 0.005 170 | - center: [0.0, -0.024, 0.07] 171 | radius: 0.005 172 | - center: [0.0, -0.024, 0.08] 173 | radius: 0.005 174 | - center: [0.0, 0.024, 0.08] 175 | radius: 0.005 176 | - center: [0.0, 0.03, 0.05] 177 | radius: 0.01 178 | - center: [0.0, -0.03, 0.05] 179 | radius: 0.01 180 | - center: [0.015, 0.0, 0.05] 181 | radius: 0.01 182 | - center: [-0.015, 0.0, 0.05] 183 | radius: 0.01 184 | -------------------------------------------------------------------------------- /manymove_object_manager/test/fake_planning_scene_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2025 Flexin Group SRL 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Flexin Group SRL nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "fake_planning_scene_server.hpp" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | FakePlanningSceneServer::FakePlanningSceneServer(const rclcpp::NodeOptions & options) 40 | : node_(std::make_shared("fake_planning_scene_server", options)) 41 | { 42 | service_ = node_->create_service( 43 | "/get_planning_scene", std::bind( 44 | &FakePlanningSceneServer::handleRequest, this, std::placeholders::_1, 45 | std::placeholders::_2)); 46 | } 47 | 48 | rclcpp::Node::SharedPtr FakePlanningSceneServer::node() const 49 | { 50 | return node_; 51 | } 52 | 53 | void FakePlanningSceneServer::setDefaultScene(const SceneState & state) 54 | { 55 | std::lock_guard lock(mutex_); 56 | default_state_ = state; 57 | } 58 | 59 | void FakePlanningSceneServer::pushSceneResponse(const SceneState & state) 60 | { 61 | std::lock_guard lock(mutex_); 62 | queued_states_.push_back(state); 63 | } 64 | 65 | void FakePlanningSceneServer::clearSceneResponses() 66 | { 67 | std::lock_guard lock(mutex_); 68 | queued_states_.clear(); 69 | } 70 | 71 | void FakePlanningSceneServer::setResponseDelay(std::chrono::milliseconds delay) 72 | { 73 | std::lock_guard lock(mutex_); 74 | response_delay_ = delay; 75 | } 76 | 77 | void FakePlanningSceneServer::stopService() 78 | { 79 | std::lock_guard lock(mutex_); 80 | service_.reset(); 81 | } 82 | 83 | void FakePlanningSceneServer::startService() 84 | { 85 | std::lock_guard lock(mutex_); 86 | if (service_) { 87 | return; 88 | } 89 | service_ = node_->create_service( 90 | "/get_planning_scene", std::bind( 91 | &FakePlanningSceneServer::handleRequest, this, std::placeholders::_1, 92 | std::placeholders::_2)); 93 | } 94 | 95 | bool FakePlanningSceneServer::serviceIsReady() const 96 | { 97 | std::lock_guard lock(mutex_); 98 | return static_cast(service_); 99 | } 100 | 101 | FakePlanningSceneServer::SceneState FakePlanningSceneServer::nextState() 102 | { 103 | std::lock_guard lock(mutex_); 104 | if (!queued_states_.empty()) { 105 | auto state = queued_states_.front(); 106 | queued_states_.pop_front(); 107 | return state; 108 | } 109 | return default_state_; 110 | } 111 | 112 | void FakePlanningSceneServer::handleRequest( 113 | const std::shared_ptr request, 114 | std::shared_ptr response) 115 | { 116 | if (!service_) { 117 | throw std::runtime_error("Planning scene service unavailable"); 118 | } 119 | 120 | const auto state = nextState(); 121 | 122 | std::chrono::milliseconds delay{0}; 123 | { 124 | std::lock_guard lock(mutex_); 125 | delay = response_delay_; 126 | } 127 | if (delay.count() > 0) { 128 | rclcpp::sleep_for(delay); 129 | } 130 | 131 | const auto components = request->components.components; 132 | 133 | if ( 134 | components == 0 || 135 | components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES || 136 | components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY) 137 | { 138 | response->scene.world.collision_objects = state.collision_objects; 139 | } 140 | 141 | if ( 142 | components == 0 || 143 | components & moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS) 144 | { 145 | response->scene.robot_state.attached_collision_objects = state.attached_objects; 146 | } 147 | 148 | response->scene.is_diff = true; 149 | } 150 | --------------------------------------------------------------------------------