Moveit: adding objects to the scene using laser scanner

edit

HI all,

I have an issue regarding the insertion of new objects to the scene. I am using a laser scanner for detecting obstacles, and when the variable obs_detection becomes true, I make a box appear in the environment. The problem is that if I don't place a delay before the appearance of the obstacle , the obstacle doesn't not appear at all in the scene. Same thing for the obstacle disappearance. Here is my code:

#include "obstacle_detector_ur5/obs_det.h"

 obs_det::obs_det() {   

   obs_det::laser_sub_ = nh.subscribe("scan_raw",1,&obs_det::laserCB,this); 
   obs_detection = false;
   rate = new ros::Rate(50);

   visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world","/moveit_visual_markers"));
   //visual_tools = new moveit_visual_tools::MoveItVisualTools ("world");
 }

 obs_det::~obs_det()   
 {
 }


 void obs_det::spinner() {
    ros::spinOnce();
    this->refresh();
    rate->sleep();
 }

 void obs_det::refresh() {

    static const std::string PLANNING_GROUP = "manipulator";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("world");


    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();

    // The id of the object is used to identify it.
    collision_object.id = "box1";

    // Define a box to add to the world.
    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = 0.13;          
    primitive.dimensions[1] = 0.14;          
    primitive.dimensions[2] = 0.42;          

    // Define a pose for the box (specified relative to frame_id)
    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.29;    //old val  0.4
    box_pose.position.y = 0.39;   //old val -0.25
    box_pose.position.z = 0.21;    //old val  0.43

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object); 

    if (obs_detection) {
      ros::Duration(0.4).sleep(); //to make sure the node can publish
      ROS_INFO_NAMED("tutorial", "Add an object into the world");
      planning_scene_interface.addCollisionObjects(collision_objects);
    }

    else {
      ros::Duration(0.4).sleep();
      ROS_INFO_NAMED("tutorial", "Remove the object from the world"); 
      std::vector<std::string> object_ids;
      object_ids.push_back(collision_object.id);
      planning_scene_interface.removeCollisionObjects(object_ids);
    }

 void obs_det::laserCB(const sensor_msgs::LaserScan::ConstPtr& laser) {
 for (int i = 0; i < laser->ranges.size(); i++)
 {
   if (laser->ranges[i] < 0.20 && laser->ranges[i] > 0.05)
 {
     obs_detection = true;
    return;
 }
   else
     obs_detection = false;
 }
 }

Does anybody have an idea why this happens? Isn't there a way to add the object instantaneously? Thanks for support

ROS Kinetic, Ubuntu 16.04