After adding planning scene motion planner is unable to retrieve robot state
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 ...
Does it happen with any collision object? How are you publishing it? Can you post a few lines that reproduce your error using
roslaunch your_robot_moveit_config demo.launch
to make sure it is not a problem with your physical robot?Hi @fvd thanks for answer. I didn't try with any other collision object, for now I am adding collision object described with mesh file. please check update on the question.