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 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 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
↧