ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Moveit: adding objects to the scene using laser scanner

asked 2018-08-20 08:52:51 -0500

enrico gravatar image

updated 2018-08-20 08:53:13 -0500

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

edit retag flag offensive close merge delete

Comments

Just a comment: why are you continuously recreating all the PlanningScene, MoveGroup and other *Interface objects? That is very wasteful and also prone to incur delays.

gvdhoorn gravatar image gvdhoorn  ( 2018-08-20 09:00:26 -0500 )edit

you are right, I'm going to implement rising/falling edges to solve this. But I'm afraid this will not be enough

enrico gravatar image enrico  ( 2018-08-20 09:21:33 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-10-11 16:10:53 -0500

enrico gravatar image

updated 2018-10-11 16:11:20 -0500

Given that the code above has been optimized, the problem was on the functions themselves.

"addCollisionObject" and "removeCollisionObjects(object_id)" need that kind of delay in order to work with Rviz.

To solve the delay issue, I used "applyCollisionObject" instead:

ROS_INFO("Add an object into the world");
collision_object.operation = collision_object.ADD;    
collision_objects.push_back(collision_object);
planning_scene_interface.applyCollisionObjects(collision_objects);  


ROS_INFO("Remove the object from the world"); 
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box1";
remove_object.header.frame_id = "world";
remove_object.operation = remove_object.REMOVE;
collision_objects.push_back(remove_object);
planning_scene_interface.applyCollisionObjects(collision_objects);
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-08-20 08:52:51 -0500

Seen: 277 times

Last updated: Oct 11 '18