Problem disabling Octomap collisions in MoveIt2. Perception Pipeline

[angeelalg]

Your environment

• ROS Distro: Foxy
• OS Version: Ubuntu 20.04.6 LTS
• Binary
• Cyclone DDS

Description

I am working with a 6 DOF robotic arm in MoveIt2 using ROS 2 Foxy. I am using a Stereolabs ZED 2i camera and a point cloud to create an occupancy map using Octomap following the perception pipeline in MoveIt. I want to temporarily disable collisions with Octomap in certain situations. When the robotic arm moves to an approach position I want to take them into account and then when it makes a final move to hit a switch on a wall I want them disabled.

I have tried using the moveit::planning_interface::PlanningSceneInterface API, but I could not find equivalent methods to getAllowedCollisionMatrix() and setAllowedCollisionMatrix() present in MoveIt (ROS 1).

I have searched the MoveIt2 documentation and forums, but have not found a suitable solution. Could you please provide an example of how to disable Octomap collisions in MoveIt2 for ROS 2 Foxy?

My sensors_3d.yaml file is configured as follows:

sensors: ["zed2_handlerbot"]
zed2_handlerbot:
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /zed2i_filtered_pointcloud
    max_range: 2.0
    point_subsample: 4
    padding_offset: 0.0
    padding_scale: 1.0
    max_update_rate: 2.0
    filtered_cloud_topic: filtered_cloud

My move_group.launch file has the following configuration:

octomap_config = {'octomap_frame': 'zed2i_left_camera_frame', 
                  'octomap_resolution': 0.05,
                  'max_range': 2.0}
run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_kinematics,
            robot_description_planning,
            octomap_config,
            octomap_updater_config,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
        ],
        arguments=["__log_level:=debug"],
    )

To perform the moves, I am following the example of moveit_cpp_example as follows:

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() {executor.spin();}).detach();

static const std::string PLANNING_GROUP = "handlerbot_arm";
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;

// Raw pointers are frequently used to refer to the planning group for improved performance.
const moveit::core::JointModelGroup * joint_model_group =
  move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

move_group.setGoalOrientationTolerance(0.05); 
move_group.setGoalPositionTolerance(0.05);  
move_group.setPlanningTime(10.0);

geometry_msgs::msg::PoseStamped target_pose1, target_pose2;

std::this_thread::sleep_for(std::chrono::seconds(2));

move_group.setPlannerId("PRMstar");

// ############################################################################################################

move_group.setPoseReferenceFrame(pose_40.header.frame_id);
move_group.setMaxVelocityScalingFactor(0.2);
move_group.setPoseTarget(pose_40,"hand_press");
move_group.plan(my_plan);
move_group.move();
std::this_thread::sleep_for(std::chrono::seconds(2));

[sjahr]

Duplication of https://github.com/ros-planning/moveit2/issues/2161

[frankchan12138]

@angeelalg Hello, I wonder if you have solved this problem? What is the certain way to disable collision of octomap and urdf link?