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 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>
Asked by lbajlo on 2020-09-25 15:14:46 UTC
Comments
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?Asked by fvd on 2020-09-26 01:14:51 UTC
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.
Asked by lbajlo on 2020-09-26 04:46:21 UTC