Trouble Achieving Dynamic Obstacle Avoidance with MoveIt2 and Octomap

edit

I'm working with a 6 DOF robotic arm in MoveIt2 using ROS 2 Foxy. I'm successfully visualizing the occupancy map of obstacles generated by Octomap in RViz, but I'm facing difficulties in achieving dynamic obstacle avoidance. The map is only updated when the robot is stationary, and if a dynamic obstacle appears in the arm's trajectory during execution, the arm continues its movement without stopping. I've tried enabling the replanning option, but it doesn't seem to address the issue. Could someone please suggest a solution or provide guidance on how to implement dynamic obstacle avoidance in MoveIt2 using Octomap?

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:

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));

Does anyone have any ideas on how I can troubleshoot and resolve this issue?"