I had some problems running benchmarks using the moveit_ros_benchmarks package while following the tutorial: https://moveit.picknik.ai/humble/doc/examples/benchmarking/benchmarking_tutorial.htm
After some time I fixed all my issues. They are not completely fixing the entire package, but at least the benchmark works now fine for me.
I just want to leave the steps I made here so everyone having similar issues can reproduce them. I hope that this is the right place to do it.
Requirements: Working Moveit2 Config
Trying to follow: Benchmarking — MoveIt Documentation: Humble documentation
But outdated tutorial and warehouse_mongo does not exist anymore for ros2.
Saving a scene with queries etc in which the benchmarks shall be done
warehouse_mongo does not exist anymore for ros2.
Instead try to use warehouse_sqlite: Launch your rviz stuff with your robot as before so you can save the robot states. See next example code snippet. Then follow tutorial to save scenes and states and queries. (Maybe you need also: sudo apt install ros-humble-warehouse-ros-sqlite)
warehouse_ros_config = {
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": "localhost"
}
# MoveGroup Node | remapping needed
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[moveit_config.to_dict(), warehouse_ros_config,],
)
# RViz
rviz_config_file = (
get_package_share_directory("kairos_simple_mover_group")
+ "/config/moveit_group_tutorial.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2_moveit2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
warehouse_ros_config,
],
)
Do the benchmark
You need to adjust the benchmark package code, so get the code from github (humble) and put it into your workspace. In this package you will find the launch files and .yamls.
- Take the demo_panda.launch.py as template and modify to use sqlite instead of mongo
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
from launch_param_builder import ParameterBuilder
def generate_launch_description():
moveit_ros_benchmarks_config = (
ParameterBuilder("moveit_ros_benchmarks")
.yaml(
parameter_namespace="benchmark_config",
file_path="demo1.yaml",
)
.to_dict()
)
moveit_configs = MoveItConfigsBuilder("kairosab_ur10_egh").to_dict()
warehouse_ros_config = {
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", # needed to use sqlite instead of mongodb
# "warehouse_host": "localhost" # not needed because host and port are set in benchmark_config
}
ompl_planning_pipeline_config = {
"planning_plugin": "ompl_interface/OMPLPlanner", # currently a workaround for the Error: Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using 'chomp_interface/CHOMPPlanner' for now.
}
# moveit_ros_benchmark demo executable
moveit_ros_benchmarks_node = Node(
name="moveit_run_benchmark",
package="moveit_ros_benchmarks",
# prefix='xterm -e gdb --ex=run --args',
executable="moveit_run_benchmark",
output="screen",
parameters=[
moveit_ros_benchmarks_config,
moveit_configs,
warehouse_ros_config,
ompl_planning_pipeline_config,
],
)
# Warehouse mongodb server
# mongodb_server_node = Node(
# package="warehouse_ros_sqlite",
# executable="mongo_wrapper_ros.py",
# parameters=[
# {"warehouse_port": 33829},
# {"warehouse_host": "localhost"},
# {"warehouse_plugin": "warehouse_ros_sqlit::DatabaseConnection"},
# ],
# output="screen",
# )
return LaunchDescription([moveit_ros_benchmarks_node]) #, mongodb_server_node])
- Then you have your demo1.yaml. You can adjust it according to the tutorial. (mine looks like this).
# This is an example configuration that loads the "Kitchen" scene from the
# local MoveIt warehouse and benchmarks the "panda_arm" group in the Pick1
# query using the Start1 initial state (all pre-stored in the local warehouse)
# Three different planners from OMPL are run a total of 50 times each, with a
# maximum of 10 seconds per run. Output is stored in the /tmp/moveit_benchmarks directory.
warehouse:
host: localhost
port: 33829
scene_name: PickUp # Required
parameters:
name: PickingUpItem
runs: 50
group: arm # Required
timeout: 10.0
output_directory: /tmp/moveit_benchmarks/
queries: .*
start_states: .*
planning_pipelines:
pipelines: [pipeline1]
pipeline1:
name: ompl
planners:
- APSConfigDefault
- SBLkConfigDefault
- Modify BenchmarkOptions.cpp to read in the planners correctly (lines 287-293):
This fixes:
[moveit_run_benchmark-1] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterTypeException'
[moveit_run_benchmark-1] what(): parameter 'benchmark_config.planning_pipelines.pipeline1.name' has invalid type: expected [string_array] got [string]
Looks like this:
std::vector<std::string> planners;
if (!node->get_parameter(pipeline_parameter_name, planners)) // changed
{
RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str());
return;
}
Should look like this:
std::vector<std::string> planners;
const std::string planners_parameter_name = std::string(np).append(".").append(pipeline).append(".planners"); // changed
if (!node->get_parameter(planners_parameter_name, planners)) // changed
{
RCLCPP_ERROR(LOGGER, "Fail to get the parameter in `%s` namespace.", pipeline_parameter_name.c_str());
return;
}
- Modify BenchmarkExecutor.cpp to correctly initialize the planning pipeline
Fixing: Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using 'chomp_interface/CHOMPPlanner' for now. Which leads to a segmentation fault later on.
Modify: void BenchmarkExecutor::initialize(const std::vectorstd::string& planning_pipeline_names)
planning_pipeline::PlanningPipelinePtr pipeline(new planning_pipeline::PlanningPipeline(
planning_scene_->getRobotModel(), node_, "", "planning_plugin", "request_adapters")); // changed child_node to node_ and added "" as the third argument
// from https://moveit.picknik.ai/main/doc/examples/motion_planning_pipeline/motion_planning_pipeline_tutorial.html
// planning_pipeline::PlanningPipelinePtr planning_pipeline(new planning_pipeline::PlanningPipeline(robot_model, node, "", "planning_plugin", "request_adapters"));
This way the planning_pipeline initializes the correct planning interface specified in the launch file.
Now you should be able to run the benchmark. (The boost progress bar is not working)
work