I've set up a custom node for motion-planning of a robot using MoveIt!. The robot has been set up using the MoveIt! Setup Assistant.

The robot has a 3D sensor mounted on it that is used for identifying obstacles to navigate around. To do this I have defined a 3D sensor plugin in the sensors_3d.yaml file.

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /robot/robot_collision_pointcloud 
    max_range: 50.0
    point_subsample: 1
    padding_offset: 0.1 
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

Later the obstacles should be removed again.

I am trying to clear a pointcloud / Octomap Occupancy Map from the environment using the built-in functionality clearOctomap() from the MoveIt! PlanningSceneMonitor.

However, upon starting the PlanningSceneMonitor interface the following error is thrown in the console output:

[ INFO] [1602157700.053733888] [ros.deicer_motion_planner]: Request planning scene succeeded.
[ INFO] [1602157700.053767604] [ros.moveit_ros_planning.planning_scene_monitor]: Starting planning scene monitor
[ INFO] [1602157700.055528926] [ros.moveit_ros_planning.planning_scene_monitor]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1602157700.058323281] [ros.moveit_ros_planning.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1602157700.058352982] [ros.moveit_ros_planning.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1602157700.059608477] [ros.moveit_ros_planning.planning_scene_monitor]: Listening to '/collision_object'
[ INFO] [1602157700.061673432] [ros.moveit_ros_planning.planning_scene_monitor]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1602157700.062136986] [ros.moveit_ros_occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ WARN] [1602157700.062168852] [ros.moveit_ros_occupancy_map_monitor]: Target frame specified but no TF instance specified. No transforms will be applied to received data.
[ERROR] [1602157700.062688762] [ros.moveit_ros_occupancy_map_monitor]: Failed to find 3D sensor plugin parameters for octomap generation

I am starting the PlanningSceneMonitor with these lines of code:

planning_scene_monitor_ptr_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
// planning_scene_monitor_ptr_ = new planning_scene_monitor::PlanningSceneMonitor("robot_description");

// update the planning scene monitor with the current state
bool success = planning_scene_monitor_ptr_->requestPlanningSceneState("/get_planning_scene");
ROS_INFO_STREAM("Request planning scene " << (success ? "succeeded." : "failed."));

// keep up to date with new changes
planning_scene_monitor_ptr_->startSceneMonitor("/move_group/monitored_planning_scene");
planning_scene_monitor_ptr_->startStateMonitor();
planning_scene_monitor_ptr_->startWorldGeometryMonitor();

These lines of code are used to clear the Octomap:

const planning_scene::PlanningScenePtr planning_scene_ptr = planning_scene_monitor_ptr->getPlanningScene();
// planning_scene_ptr->getWorldNonConst()->removeObject(planning_scene_ptr->OCTOMAP_NS);
// planning_scene_monitor_ptr->triggerSceneUpdateEvent(planning_scene_monitor_ptr->UPDATE_SCENE);
planning_scene_monitor_ptr->clearOctomap();

It seems that the sensor plugin and the associated parameters cannot be found by the PlanningSceneMonitor. Despite this, I can still send data on the sensor plugin topic, which shows up correctly and is correctly being avoided when motion-planning. Additionally, clearing the Octomap from the MoveIt! MotionPlanning panel in RViz succeeds.

I am running ROS Melodic 1.14.9 on Ubuntu 18.04 LTS.

MoveIt! version: 1.0.6-1bionic.20200828.224341

---

Minimal working example below:

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, SRI International
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of SRI International nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/



/* Author:  Albert Seligmann
   Date:    03/09/2020
*/

// IO
#include <iostream>
using namespace std;

// ROS
#include "ros/ros.h"

// MoveIt!
#include <moveit/move_group_interface/move_group_interface.h> // MoveIt! motion planning
#include <moveit/planning_scene_interface/planning_scene_interface.h> // MoveIt! planning for collision objects
#include <moveit/planning_scene_monitor/planning_scene_monitor.h> // Interface for managing the scene

// Messages
/*
  Various message types
*/

// Point cloud
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>

// Eigen - matrix operations
#include <Eigen/Eigen>



class RobotMotionPlanner{
public:
  void clearPointCloudForCollision();

  /*
    Snipped functions
  */

  // Class constructor
  RobotMotionPlanner(const std::string &planning_group) {
    ROS_INFO_STREAM("Starting...");
    // MoveIt! setup
    // Initialise the move group - responsible for planning and moving the robot
    move_group_ptr_ = new moveit::planning_interface::MoveGroupInterface(planning_group);

    // Initialise the planning scene interface - responsible for handling collision objects in the world
    planning_scene_interface_ptr_ = new moveit::planning_interface::PlanningSceneInterface();

    // Raw pointers are frequently used to refer to the planning group for improved performance.
    joint_model_group_ptr_ = RobotMotionPlanner::move_group_ptr_->getCurrentState()->getJointModelGroup("robot");

    // Initialise the movement plan
    motion_plan_ptr_ = new moveit::planning_interface::MoveGroupInterface::Plan();

    // Increase the default planning time allowed
    move_group_ptr_->setPlanningTime(5.0); // Default is 5 seconds
    move_group_ptr_->setNumPlanningAttempts(10);
    move_group_ptr_->setWorkspace(-25, -25, 0, 25, 25, 25);


    // Subscribe to topic containing pose targets
    sub_pose_robot_ = node_handle_.subscribe("/robot/robot_motion_planner", 1000, &RobotMotionPlanner::motionRobotSubscriberCallback, this);
    // Subscribe to topic containing segmented pointcloud data
    sub_objects_ = node_handle_.subscribe("/robot/robot_collision_objects", 1000, &RobotMotionPlanner::collisionObjectSubscriberCallback, this);
    sub_pointcloud_ = node_handle_.subscribe("/cloud_pcd", 1000, &RobotMotionPlanner::publishPointCloudForCollision, this); // TODO: Temporary test functionality
    // Topic for MoveIt! sensor data
    pub_pointcloud_ = node_handle_.advertise<sensor_msgs::PointCloud2>("/robot/robot_collision_pointcloud", 1000);



    /* Manually setting the parameters from the node - still doesn't work

    //<param name="octomap_frame" type="string" value="base_link" />
    // node_handle_.setParam("octomap_frame", "base_link");
    //<param name="octomap_resolution" type="double" value="0.25" />
    // node_handle_.setParam("octomap_resolution", 0.25);
    //<param name="max_range" type="double" value="50.0" />
    // node_handle_.setParam("max_range", 50.0);

    */


    planning_scene_monitor_ptr_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
    // planning_scene_monitor_ptr_ = new planning_scene_monitor::PlanningSceneMonitor("robot_description");

    // update the planning scene monitor with the current state
    bool success = planning_scene_monitor_ptr_->requestPlanningSceneState("/get_planning_scene");
    ROS_INFO_STREAM("Request planning scene " << (success ? "succeeded." : "failed."));

    // keep up to date with new changes
    planning_scene_monitor_ptr_->startSceneMonitor("/move_group/monitored_planning_scene");
    planning_scene_monitor_ptr_->startStateMonitor();
    planning_scene_monitor_ptr_->startWorldGeometryMonitor();

  };

  // Class destructor
  ~RobotMotionPlanner() {
    delete move_group_ptr_;
    delete planning_scene_interface_ptr_;
    delete joint_model_group_ptr_;
    delete motion_plan_ptr_;
  }

  // ROS
  ros::NodeHandle node_handle_;

  // MoveIt! fields
  moveit::planning_interface::MoveGroupInterface* move_group_ptr_;
  moveit::planning_interface::PlanningSceneInterface* planning_scene_interface_ptr_;
  const robot_state::JointModelGroup* joint_model_group_ptr_;
  moveit::planning_interface::MoveGroupInterface::Plan* motion_plan_ptr_;

  // Planning scene monitor
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr_;
  // planning_scene_monitor::PlanningSceneMonitor* planning_scene_monitor_ptr_;

  // Subscribe to pose input messages for various move_groups. 
  ros::Subscriber sub_pose_robot_;
  // Subscribe to path input messages.

  // Subscribe to collision object messages
  ros::Subscriber sub_objects_;

  // Subscribe to collision object messages
  ros::Subscriber sub_pointcloud_;

  // Publish joint trajectories. 
  ros::Publisher pub_pointcloud_; 
};



void RobotMotionPlanner::clearPointCloudForCollision(){

  ROS_INFO_STREAM("Clearing Octomap PointCloud / occupancy map.");

  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr;
  const planning_scene::PlanningScenePtr planning_scene_ptr = planning_scene_monitor_ptr->getPlanningScene();
  // planning_scene_ptr->getWorldNonConst()->removeObject(planning_scene_ptr->OCTOMAP_NS);
  // planning_scene_monitor_ptr->triggerSceneUpdateEvent(planning_scene_monitor_ptr->UPDATE_SCENE);
  planning_scene_monitor_ptr->clearOctomap();

}



/*
  Snipped functions for actual motion-planning etc.
*/



int main(int argc, char** argv) {
  // Setup node
  ros::init(argc, argv, "robot_motion_planner");
  // Start spinner
  ros::AsyncSpinner spinner(4); // 4 threads
  spinner.start();


  ROS_INFO_STREAM("Setting up class");
  RobotMotionPlanner robotClass("robot");
  ROS_INFO_STREAM("Class has been set up");


  // Spin until shutdown
  ros::waitForShutdown();
  return 0;
}

[ros.moveit_ros_occupancy_map_monitor]: Failed to find 3D sensor plugin parameters for octomap generation

  std::vector<std::string> sensor_names;
  if (!node_->get_parameter("sensors", sensor_names))
  {
    RCLCPP_ERROR(LOGGER, "No 3D sensor plugin(s) defined for octomap updates");
  }

sensors:
  - kinect_pointcloud
  - 2nd_sensor
kinect_pointcloud:
    filtered_cloud_topic: filtered_cloud
    max_range: 5.0
    max_update_rate: 1.0
    ns: kinect
    padding_offset: 0.1
    padding_scale: 1.0
    point_cloud_topic: /camera/depth_registered/points
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
2nd_sensor:
    :

:
_PARAM_SHAPEBUFFER_WAITTIME = "robot_description_planning/shape_transform_cache_lookup_wait_time"
moveit_config = (
    MoveItConfigsBuilder("moveit_resources_panda")
    .robot_description(
        file_path="config/panda.urdf.xacro",
        mappings={
            "ros2_control_hardware_type": LaunchConfiguration(
                "ros2_control_hardware_type"
            )
        },
    )
    .robot_description_semantic(file_path="config/panda.srdf")
    .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
    .planning_pipelines(
        pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"])
    .sensors_3d("config/sensors_3d.yaml")
    .to_moveit_configs()
)

_params_movegroup = ParameterBuilder("moveit_resources_panda_moveit_config")

# Start the actual move_group node/action server
move_group_node = Node(
    package="moveit_ros_move_group",
    executable="move_group",
    output="screen",
    parameters=[moveit_config.to_dict()],
    parameters=(
        [moveit_config.to_dict()] + 
        [_octomap_launch_params(_params_movegroup)] +
        [{_PARAM_SHAPEBUFFER_WAITTIME: LaunchConfiguration(
            'shape_transform_cache_lookup_wait_time')}]),
    arguments=["--ros-args", "--log-level", "info"],
)