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 moveitrosplanning moveitprintplanningsceneinfo" 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

fanuc1/planningscene

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 PLANNINGSCENEHANDLER node code which is shows how do I construct class instance and how I am adding collision object to scene:

...
PlanningSceneHandler _planningSceneHandler( REFERENCE_FRAME, "fanuc_1/planning_scene");

while(    _tfBuffer.canTransform(ROBOT_NAME + "/" + REFERENCE_FRAME, ROBOT_NAME + "/" + CAMERA_LINK, ros::Time(0), ros::Duration(1))
                  &&  _tfBuffer.canTransform( CAMERA_FRAME, OBJECT, ros::Time(0), ros::Duration(1))
                  &&  _tfBuffer.canTransform(ROBOT_NAME + "/" + REFERENCE_FRAME, ROBOT_NAME + "/" + END_EFFECTOR_LINK, ros::Time(0), ros::Duration(1)))
            {    
                //get transform between robot base_link and camera link
                _ts_base_camera = _tfBuffer.lookupTransform(ROBOT_NAME + "/" + REFERENCE_FRAME, ROBOT_NAME + "/" + CAMERA_LINK, ros::Time(0));

                //get transform between camera_link and object
                _ts_camera_object = _tfBuffer.lookupTransform( CAMERA_FRAME, OBJECT, ros::Time(0));

                //get transform between camera_link and object
                _ts_base_tool = _tfBuffer.lookupTransform(ROBOT_NAME + "/" + REFERENCE_FRAME, ROBOT_NAME + "/" + END_EFFECTOR_LINK, ros::Time(0));

                //calculate transform between camera and object
                tf2::Stamped<tf2::Transform> _tf2_ts_camera_object;
                tf2::fromMsg(_ts_camera_object, _tf2_ts_camera_object);

                //calculate transform between base and tool
                tf2::Stamped<tf2::Transform> _tf2_ts_base_tool;
                tf2::fromMsg(_ts_base_tool, _tf2_ts_base_tool);

                tf2::Transform _tf2_ts_base_object = _tf2_ts_base_tool * _tf2_eMc * _tf2_ts_camera_object;

                tf2::toMsg(_tf2_ts_base_object, _wantedPose);

                //calculate pre position, same like end position but 10 mm below
                _pre_wantedPose = _wantedPose;
                _pre_wantedPose.position.z = _pre_wantedPose.position.z+0.1;

                //publish _wantedPose so it can be visualized in RViz as marker
                poseToTransform( _wantedPose, _objectTransform, OBJECT+"_in_scene", REFERENCE_FRAME);

                _br.sendTransform(_objectTransform);

                _planningSceneHandler.addObject("thor_hammer", DAE_MODEL, _wantedPose);

                loop_rate.sleep();
            } 

Here is launch file which launches a node for handling planning scene:

<?xml version="1.0"?>

<launch>

        <!-- start object grasp node -->
        <node name="PLANNING_SCENE_HANDLER" pkg="1_OpenCV_detection" type="test_planning_scene" output="screen">
            <param name = "PLANNING_GROUP"      value = "robot_arm"></param>
            <param name = "END_EFFECTOR_LINK"   value = "tool_gripper_tip"></param>
            <param name = "CAMERA_LINK"         value = "camera_link" ></param>
            <param name = "CAMERA_FRAME"        value = "camera" ></param>
            <param name = "OBJECT"              value = "object" ></param>
            <param name = "REFERENCE_FRAME"     value = "base_link" ></param>
            <param name = "OBJECT_REFERENCE_FRAME"  value = "ref_frame" ></param>
            <param name = "ROBOT_NAME"          value = "fanuc_1" ></param>
            <param name = "STL_MODEL"           value = "file:///home/ros-industrial/thor.stl" ></param>
            <param name = "DAE_MODEL"           value = "file:///home/ros-industrial/thor1.dae" ></param>
            <param name = "yaml_file"           value = "/home/ros-industrial/Desktop/ROS_VISP_ROBOTIQ_WS/src/1_OpenCV_detection/initialization/eMc.yaml" ></param>  <!-- -->
        </node>
</launch>