MoveIt! PlanningSceneMonitor fails to find 3D sensor plugin

asked 2020-10-08 07:50:57 -0500

aseligmann gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Any kind of input to what could be happening here, or tips on how to fix this problem would be highly appreciated!

aseligmann gravatar image aseligmann  ( 2020-10-30 07:05:30 -0500 )edit