CollisionObject added with Moveit_cpp gets ignored at motion planning

[NebuDa]

DescriptionDescription

I have built a node to add collision Boxes to the scene, but after adding it, like in the tutorial, the Robot moves through the box instead of around it. When I move and Publish the Collision Box from code manually in RViz, then it is working. Do I have to publish the Boxes inside the code, is my launch File wrong or is there a Bug in moveit_cpp?

In scene Objects is shown only one Box, but in the scene appear both. Deleting the entry let dissapear both Boxes.

Your environment

• ROS Distro: Foxy
• OS Version: Ubuntu 20.04
• ROS2 Binary
• via Debian Packages

Steps to reproduce

Install ROS2_UR_Driver https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/foxy add new Package with node (see code below)

Backtrace or Console output

Code

https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/foxy/ur_bringup/launch/ur_moveit.launch.py Copied and Changed ur_moveit.launch.py Launchfile from Driver to add own node (only changes or relevant parts):

[...]
 planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True,
    }
[...]
#Insert own Nodes
    create_scene = Node(
        package="avoid_obstacle_scene",
        executable="create_scene",
        name="create_scene_node",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            ompl_planning_pipeline_config,
            robot_description_kinematics,
            trajectory_execution,
            moveit_controllers,
            #planning_scene_monitor_parameters, still deactivated
        ],
    )

own Node C++ (relevant Part):

// Initialize MoveItCpp
  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
  auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(nh, moveit_cpp_options, tf_buffer);
  moveit_cpp->getPlanningSceneMonitor()->providePlanningSceneService();
 // A little delay before adding collision Objects
  rclcpp::sleep_for(std::chrono::seconds(2));

// Create collision object, planning shouldn't be too easy
  RCLCPP_INFO(LOGGER, "!!!!! Create Collision Object.");
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = "base";
  collision_object.id = "box";

  //Definition Box1
  shape_msgs::msg::SolidPrimitive box;
  box.type = box.BOX;
  box.dimensions = { 0.5, 0.1, 0.4 };
  geometry_msgs::msg::Pose box_pose;
  box_pose.position.x = 0.0;
  box_pose.position.y = 0.5;
  box_pose.position.z = 0.8;

  //Definition Box2
  shape_msgs::msg::SolidPrimitive box2;
  box2.type = box2.BOX;
  box2.dimensions = { 1.0, 0.1, 0.4 };
  geometry_msgs::msg::Pose box_pose2;
  box_pose2.position.x = 0.0;
  box_pose2.position.y = -0.5;
  box_pose2.position.z = 0.2;

  //Hinzufügen der Boxen zu collision_object
  collision_object.primitives.push_back(box);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;  //Auslassen dieser Zeile ändert anscheinend nichts
  collision_object.primitives.push_back(box2);
  collision_object.primitive_poses.push_back(box_pose2);
  collision_object.operation = collision_object.ADD;  //Auslassen dieser Zeile ändert anscheinend nichts
  RCLCPP_INFO(LOGGER, "!!!!! Collision Objects planted.");

  // Add object to planning scene
  {  // Lock PlanningScene
    planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp->getPlanningSceneMonitor());
    scene->processCollisionObjectMsg(collision_object);   
    RCLCPP_INFO(LOGGER, "!!!!! Objects added to Scene.");
  }  // Unlock PlanningScene

[JensVanhooydonck]

You have created only one collision object with multiple primitive objects. I think you will need to create multiple collision objects, whit each collision object only one primitive and one primitive_pose.

[v4hn]

You have created only one collision object with multiple primitive objects. I think you will need to create multiple collision objects, whit each collision object only one primitive and one primitive_pose.

I did not look at this bug at all, but just respond to this message:

As always there might be bugs somewhere, but it's perfectly fine to add multiple collision shapes to a single object and these should not be ignored anywhere.

[JensVanhooydonck]

You have created only one collision object with multiple primitive objects. I think you will need to create multiple collision objects, whit each collision object only one primitive and one primitive_pose. I did not look at this bug at all, but just respond to this message: As always there might be bugs somewhere, but it's perfectly fine to add multiple collision shapes to a single object and these should not be ignored anywhere.

Indeed, my reply only answers part of the question: In scene Objects is shown only one Box, but in the scene appear both. Deleting the entry let dissapear both Boxes.

[NebuDa]

@v4hn @JensVanhooydonck thanks for your responses. Any suggestions to my problem / bug?

[NebuDa]

I have started working on it again, and changed some parts of the code. But the bug still exists. When I move the Boxes a little bit in RViz and publish them again it works, otherwise not.

int boxcounter = 0;
moveit_msgs::msg::CollisionObject addBox(double x, double y, double z, double posX, double posY, double posZ)
{
  moveit_msgs::msg::CollisionObject tmpColObject;
  tmpColObject.header.frame_id = "base";
  tmpColObject.id = "box" + std::to_string(boxcounter++);
  //Definition Box1
  shape_msgs::msg::SolidPrimitive box;
  box.type = box.BOX;
  box.dimensions = { x, y, z };
  geometry_msgs::msg::Pose box_pose;
  box_pose.position.x = posX;
  box_pose.position.y = posY;
  box_pose.position.z = posZ;
  tmpColObject.primitives.push_back(box);
  tmpColObject.primitive_poses.push_back(box_pose);
  return tmpColObject;
}
[....]
 //Liste mit Kollisionsobjekten
  std::vector<moveit_msgs::msg::CollisionObject> collisionObjects;

  collisionObjects.push_back(addBox(0.5, 0.1, 0.4,0.0,0.8,0.8));
  RCLCPP_INFO(LOGGER, "Box 1 created");
  collisionObjects.push_back(addBox(0.5, 0.1, 0.4,0.0,1.2,0.8));
  RCLCPP_INFO(LOGGER, "Box 2 created");
  collisionObjects.push_back(addBox(0.5, 0.1, 0.4,0.0,1.6,0.8));
  RCLCPP_INFO(LOGGER, "Box 3 created");

  // Add objects to planning scene
  {  // Lock PlanningScene
    planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp->getPlanningSceneMonitor());
    for(int i =0; i<collisionObjects.size(); i++)
    {
      scene->processCollisionObjectMsg(collisionObjects[i]);
      RCLCPP_INFO(LOGGER, "Object No. " + std::to_string(i) + "added to Scene.");  
    }
    RCLCPP_INFO(LOGGER, "All Objects added to Scene.");
  }  // Unlock PlanningScene

Is there a way to activate the publishing from code? Or is there another way to tell planning_scene_monitor::LockedPlanningSceneRW scene to publish it?

[JensVanhooydonck]

When I publish my collision object to the '/colission_object' topic, it works for me, when applying a scene with Diff = True, it does not work. Maybe this is the same issue?

[henningkayser]

I think this has been fixed a while ago. Please report back if this still exists on Humble or Rolling