I'm attempting to do collision detection for a robot arm using MoveIt. I'm trying to use the PlanningSceneMonitor to do so. My understanding is that a PlanningSceneMonitor essentially maintains a current planning scene which it updates based on inputs from a few topics: 'joint_states' (for the joint positions of the robot), and 'planning_scene,' 'collision_objects,' and 'world' for world geometry information. The PlanningSceneMonitor will then publish the updated planning scene whenever its internal planning scene is changed. My approach:
1) Create a PlanningSceneMonitor which listens to the current joint positions of my robot. This I have gotten to work correctly
2) Send a few CollisionObjects to the PlanningSceneMonitor that will be added to the planning scene. I want to add the objects at the beginning and never need to change them after that. A plane for the surface the robot is on and a box near the robot. I have not been able to do this successfully (more details in a moment).
3) Write a subscriber that receives the moveit_msgs::planning_scene messages being published by the PlanningSceneMonitor and checks them for collisions. This works when I don't try to add collision objects to the world (and therefore am really just checking for self collisions) but doesn't when I try to add objects.
In the code below, I attempt to add a box by sending it as a moveit_msgs::collision_object over the "collision_objects" topic. I encounter two issues.
First, when I publish the message, a warning is printed "Returning dirty link transforms." I understand what this signifies but I don't see why it should be occurring here. Second, ***the collision object seems to be added to the maintained planning scene temporarily but after about half a second, it dissappears from the scene.*** I know this because if I print the number of objects in the PlanningSceneMonitor's current planning scene, it is 1 shortly after I publish the collision object but then drops to 0. If I re-publish the collision object (changing the id each time) at 10 Hz, only the last 4-5 are maintained in the planning scene at a given time.
**To summarize my problem:** when I send collision objects to the PlanningSceneMonitor in what appears to be a valid manner, they are added momentarily to the monitor's planning scene messages but disappear after a short time.
**My question:** is this really a valid way to add collision objects or do I have some sort of misunderstanding of how I should be doing this? How do I get the CollisionObjects to stay in the world indefinitely?
*Code for for PlanningSceneMonitor, collision object publisher:*
// Initialize ros node
ros::init(argc, argv, "check_collisions");
ros::NodeHandle n;
// set up planning scene monitor
planning_scene_monitor::PlanningSceneMonitorPtr psm_ptr = std::make_shared("robot_description");
psm_ptr->startStateMonitor("joint_states");
psm_ptr->startWorldGeometryMonitor("collision_objects", "world", false);
psm_ptr->startPublishingPlanningScene(psm_ptr->UPDATE_SCENE, "current_planning_scene");
// collision obj publisher for adding objects to scene
ros::Publisher collision_obj_publisher = n.advertise("collision_objects", 1);
// Create box
moveit_msgs::CollisionObject obj1;
obj1.header.frame_id = "/world";
obj1.id = "table";
// Define box
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.6;
primitive.dimensions[1] = 0.6;
primitive.dimensions[2] = 0.6;
// Pose for box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = 0.419;
box_pose.position.z = 0.0;
obj1.primitives.push_back(primitive);
obj1.primitive_poses.push_back(box_pose);
obj1.operation = table.ADD;
// Wait for ps_monitor to connect, publish collision obj
ros::WallDuration sleep_t(0.5);
while (collision_obj_publisher.getNumSubscribers() < 1)
{
sleep_t.sleep();
}
collision_obj_publisher.publish(table);
*Code for collision detector:*
// Global
bool collision_flag = false;
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
collision_detection::AllowedCollisionMatrix acm;
planning_scene::PlanningScene *scene;
*in main...*
// Initialize ros node
ros::init(argc, argv, "check_collisions");
ros::NodeHandle n;
// Initialize planning scene & get allowed collision matrix
robot_model_loader::RobotModelLoader rml("robot_description");
scene = new planning_scene::PlanningScene(rml.getModel());
acm = scene->getAllowedCollisionMatrix();
// Subscriber to listen for updates to planning scene & do collision checking
ros::Subscriber ps_sub = n.subscribe("planning_scene_publisher/current_planning_scene", 1, checkCollisions);
*with callback function...*
void checkCollisions(const moveit_msgs::PlanningSceneConstPtr &msg) {
// Convert planning scene message to real planning scene
scene->setPlanningSceneMsg(*msg);
// Prepare for next collision check
collision_request.distance = true;
collision_result.clear();
// Do collision checking
scene->checkCollision(collision_request, collision_result, scene->getCurrentStateNonConst(), acm);
// Distance between closest bodies
double distance_checkCollision = collision_result.distance;
// Collision y/n
collision_flag = collision_result.collision;
}
**Other hints that may be relevant:**
- when I attempt to add a plane rather than a box, I receive the same dirty transforms warning message but when I call checkCollisions, the node crashes entirely with a very cryptic error message. In the case of the box, the collision check runs but the box is gone from the planning scene
- I have tried sending the objects via a planning scene diff over the "planning _scene" topic as well as using the PlanningSceneInterface 'addCollisionObjects' method (which I'm pretty sure just uses the "planning_scene" topic) and had the same issues in each case
- Running Ubuntu 16.04, ROS Kinetic
- Using rviz to simulate joint movements, collisions - seems to be working properly
Thanks in advance for any assistance!
↧