After adding planning scene motion planner is unable to retrieve robot state

edit

Hi everyone, 2 days ago I have added a planning scene with a 1 collision object to the moveit package. I am publishing on the

/planning_scene

topic, collision object is visible in the RViz. Also when I run "rosrun moveit_ros_planning moveit_print_planning_scene_info" I can see that there is 1 collision object added. Problem occurs when I want to plan and execute some movement through program or through RViz. Path is planned but seems like motion planner can't retrieve robot state for some reason. for 2 days I am trying to solve this problem but I am keep getting messages and warnings saying like this:

[ INFO] [1601063187.942588786, 12.285000000]: Didn't received robot state (joint angles) with recent timestamp within 1 seconds. Check clock synchronization if your are running ROS across multiple machines!

[ WARN] [1601063187.943674935, 12.285000000]: Failed to fetch current robot state.

Then I tried to retrieve program state programmatically through my node using member function: move_group.getCurrentState(); and it is possible to retrieve robot state with member function but when I want to send robot to random position move_ group is complaining that it didn't received robot state within 1 second.

This is definitely somehow connected with planning scene because if in launch file I comment a node which is publishing on planning scene then I do not have problems with retrieving robot state.

looking forward to you answers

UPDATE 26.09.2020.

Here is update with more information. I've made a class for handling planning scene and for now class has only 1 method for adding objects to the scene "addObject". Method "addObject" is using object name, mesh file path and position where to add a object. Here is the class method which is publishing scene to namespace of the robot

fanuc_1/planning_scene

Here are constructor and 1 method of the class

PlanningSceneHandler::PlanningSceneHandler( std::string referenceFrame,std::string _planningTopic):
                                            _refFrame{referenceFrame}, _planningTopic{_planningTopic}
{   
    _planningSceneDiffPublisher = _nodeHandle.advertise<moveit_msgs::PlanningScene>(_planningTopic, 1);
}
void PlanningSceneHandler::addObject( std::string _objectName, std::string _meshModelPath, geometry_msgs::Pose _collisionObjectPose)
{
    moveit_msgs::CollisionObject co;

    shapes::Mesh* m = shapes::createMeshFromResource(_meshModelPath); 
    shape_msgs::Mesh co_mesh;
    shapes::ShapeMsg co_mesh_msg;  
    shapes::constructMsgFromShape(m, co_mesh_msg);

    co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);  
    co.meshes.resize(1);
    co.mesh_poses.resize(1);
    co.meshes[0] = co_mesh;
    co.header.frame_id = _refFrame;
    co.id = _objectName;     

    // add position and orientation to mesh 
    co.mesh_poses[0].position.x = _collisionObjectPose.position.x;
    co.mesh_poses[0].position.y = _collisionObjectPose.position.y;
    co.mesh_poses[0].position.z = _collisionObjectPose.position.z;
    co.mesh_poses[0].orientation.w = _collisionObjectPose.orientation.w;
    co.mesh_poses[0].orientation.x = _collisionObjectPose.orientation.x;
    co.mesh_poses[0].orientation.y = _collisionObjectPose.orientation.y;
    co.mesh_poses[0].orientation.z = _collisionObjectPose.orientation.z;   

    co.operation = co.ADD;

    //delete everything from planning scene and add collision object on the planning scene
    _planningScene.world.collision_objects.clear();
    _planningScene.world.collision_objects.push_back(co);
    _planningScene.is_diff=true;
    //using planning scene difference
    _planningSceneDiffPublisher.publish(_planningScene);
}

Here is part of the PLANNING_SCENE_HANDLER node code which is shows how do I construct class instance and how I am ... (more)