ROS2 Robot System, teleporting behaviour in subgroup planning

edit

System: ros2-foxy, moveit2 (latest), ubuntu 20.04 lts

I'm trying to plan for subgroups which do not calculate properly.

My system has two robots, which are both defined as a group within the SRDF, both of them are then included in a compound group, e.g. "all".

When i choose group "all" within moveit, or via code, all behaves as it should. The problem arises as soon as i want to plan for one of the robots by themselves. The plan which is beeing calculated always resets the other robot to its initial values. For example, if i move both robots to arbitrary positions within group "all", all is fine. As soon as i execute a motion for "robot1", the other one gets reset to its initial position. The same behaviour applies to "robot2". Switching from a single robot to the compound group strangely doesnt reset the group which was moved before, so this does execute as i would expect.

I narrowed it down to the Planning Scene Monitor, since i am unable to read the robot status from code. and the /monitored_planning_scene topic is also not updated regularly. What i fail to understand is, where the initial values for the groups come from. I assumed, even if i change the subgroup, the robots wouldn't be able to teleport themselves in the real world, so i could just switch the groups if i needed some specific movement.

How does this one have to be configured for Moveit2?

move_group configuration within launch.py:

move_group_node = Node(package='moveit_ros_move_group',
                           executable='move_group',
                           output='screen',
                           parameters=[robot_description,
                                       robot_description_semantic,
                                       kinematics_yaml,
                                       joint_limits,
                                       ompl_planning_pipeline_config,
                                       trajectory_execution,
                                       moveit_controllers,
                                       planning_scene_monitor_parameters,
                                       moveit_yaml,
                                       {'source_list': ["/joint_states"]}])

move.yaml:

planning_scene_monitor_options:
  name: "planning_scene_monitor"
  robot_description: "robot_description"
  joint_state_topic: "/joint_states"
  attached_collision_object_topic: "/planning_scene_monitor"
  publish_planning_scene_topic: "/publish_planning_scene"
monitored_planning_scene_topic: "/monitored_planning_scene"
  wait_for_initial_state_timeout: 10.0
plan_request_params:
   planning_attempts: 1
   planning_pipeline: ompl
   max_velocity_scaling_factor: 1.0
   max_acceleration_scaling_factor: 1.0