Hi, I am trying to implement node, that will check planning scene for a collision. I am successfully loading collision object on the scene and monitoring it with scene monitor. I can see sphere colliding with robot's body, and when I am moving sphere on a scene I can see its position output updated. But collision checking result is telling, that there is no collision, even when it is.

Here's code:

#include "moveit_test.h"


int main(int argc, char* argv[]) {
ros::init(argc, argv, "mvt");
ros::NodeHandle nh;

ros::AsyncSpinner spinner(1);
spinner.start();

robot_model_loader::RobotModelLoader robot_loader("robot_description");
auto kinematic_model = robot_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

moveit::planning_interface::MoveGroupInterface move_group("manipulator");
planning_scene::PlanningScene planning_scene(kinematic_model);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

const std::string ROBOT_DESCRIPTION{"robot_description"};

const moveit::core::JointModelGroup* joint_model_group;

auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
    ROBOT_DESCRIPTION);

moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
collision_object.id = "sphere2";

shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.SPHERE;
primitive.dimensions.resize(1);
primitive.dimensions[0] = 0.5;


geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.8;
box_pose.position.y = 0.0;
box_pose.position.z = 0.5;

collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

planning_scene_interface.addCollisionObjects(collision_objects);


// update the planning scene monitor with the current state
psm->startSceneMonitor("/move_group/monitored_planning_scene");
bool success = psm->requestPlanningSceneState("/get_planning_scene");
ROS_INFO_STREAM("Request planning scene "
                << (success ? "succeeded." : "failed."));


ros::Rate loop_rate(1);

    collision_detection::CollisionResult collision_result;
    collision_detection::CollisionRequest collision_request;

    collision_request.contacts = true;
    collision_request.max_contacts = 1000;
    collision_request.group_name = "manipulator";
    collision_request.distance = true;
    collision_request.cost = false;
    collision_request.verbose = false;

    collision_detection::AllowedCollisionMatrix acm =
        planning_scene.getAllowedCollisionMatrix();

    std::vector<std::string> obj_names;
    acm.getAllEntryNames(obj_names);
    for (int i = 0; i < obj_names.size(); i++)
        std::cout << obj_names[i] << std::endl;

    while (ros::ok()) {
        collision_result.clear();
        auto current_state = psm->getPlanningScene()->getCurrentState();

        // planning_scene.checkSelfCollision(collision_request, collision_result, current_state);

        auto objects = planning_scene_interface.getObjects();
        for(auto names : objects){
            std::cout << names.second.primitive_poses[0].position << std::endl;
        }

        planning_scene.checkCollision(collision_request, collision_result,
                                      current_state);

        ROS_INFO_STREAM(collision_result.distance);
        ROS_INFO_STREAM(planning_scene.isStateColliding());

        ROS_INFO_STREAM(
            "Collision check between robot and collision object "
            << (collision_result.collision ? "failed." : "succeeded."));

        loop_rate.sleep();
}

return 0;
}

Output:

x: 1.27
y: 0
z: 0.5

[ INFO] [1678115582.521497801]: 0.0347054
[ INFO] [1678115582.521577093]: 1
[ INFO] [1678115582.521594489]: Collision check between robot and collision object succeeded.

I want to add, that selfCollision detection is working

I dont have enough karma to upload photo, so here's link to image with rviz