Revision history [back]

Hi!

Finally I managed to solve this issue. If anybody is facing the same problem, here is my approach, which works for me:

ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
  while(collision_object_publisher.getNumSubscribers() < 1)
  {
      ros::WallDuration sleep_t(0.5);
      sleep_t.sleep();
  }

  moveit_msgs::CollisionObject co;
  co.header.frame_id = "box_frame";
  co.id = "box";
  geometry_msgs::Pose pose;
  // pose.orientation.w = 1.0;
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.1;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.1;
  co.primitives.push_back(primitive);
  co.primitive_poses.push_back(pose);
  co.operation = co.ADD;
  collision_object_publisher.publish(co);


  sleep(5);


  moveit_msgs::CollisionObject move_object;
  move_object.id = "box";
  move_object.header.frame_id = "box_frame";
  move_object.primitive_poses.push_back(pose);
  move_object.operation = move_object.MOVE;

  ros::Rate loop_rate(100);

  while(ros::ok()){
    collision_object_publisher.publish(move_object);
    loop_rate.sleep();
    ROS_INFO("Another loop");
  }

Thanks @dornhege for the hints!!