ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-06-28 09:16:42 -0500

130s gravatar image

updated 2023-06-28 09:18:41 -0500

My answer is more toward moveit2 but hope this helps for moveit1.

What "octomap updates" in the following error sentences means sounds ambiguous, or even inaccurate. - No 'octomap_frame' parameter defined for octomap updates - No 3D sensor plugin(s) defined for octomap updates

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

As of moveit 2.7.4 this prints at occupancy_map_monitor_middleware_handle.cpp#L94

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

The Parameter "sensors" is the top level key in sensor yaml file. Make sure the file is formatted properly, and the Parameters are set to the move group node.

File format of sensor_3d.yaml

Looks like the format of the file is different in moveit1 and moveit2:

e.g.

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:
    :

How to set the Parameters from yaml

An example using .launch.py is below (again from moveit_resources!181. xml format should look much more succinct, but launch formatting is not the point here):

:
_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"],
)
edit flag offensive delete link more

Question Tools

2 followers

Stats

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

Seen: 1,238 times

Last updated: Jun 28 '23