bool addBillboard(AddBillboard::Request &req, AddBillboard::Response &res)
{
ROS_INFO("ADDING BILLBOARD");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
Billboard *billboard = new Billboard(imServer, req.frame_id, req.name);
billboard->setType(req.type);
billboard->setPoseType(req.pose_type);
billboard->setPose(req.pose);
billboard->setScale(req.scale);
billboard->setDescription(req.description);
billboard->setDirection(req.direction);
billboard->setVelocity(req.velocity);
billboard->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, billboard));
ROS_INFO("..... DONE");
return true;
}
bool addMarker(AddMarker::Request &req, AddMarker::Response &res)
{
ROS_INFO("ADDING MARKER");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
InteractiveMarker object;
Marker marker;
marker.type = req.type;
marker.color = req.color;
marker.scale = req.scale;
object.header.frame_id = req.frame_id;
object.header.stamp = ros::Time::now();
object.name = req.name;
object.description = req.description;
object.pose = req.pose;
InteractiveMarkerControl control;
control.name = object.name + "_control";
control.interaction_mode = InteractiveMarkerControl::NONE;
control.always_visible = true;
control.markers.push_back(marker);
object.controls.push_back(control);
imServer->insert(object);
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool setAllowObjectInteraction(SetAllowObjectInteraction::Request &req, SetAllowObjectInteraction::Response &res)
{
ROS_INFO("SETTING OBJECT'S INTERACTION");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
if ( (primitives[req.name]->getPrimitiveType() != PrimitiveType::OBJECT) && (primitives[req.name]->getPrimitiveType() != PrimitiveType::UNKNOWN_OBJECT) )
{
ROS_ERROR("Primitive is not of type Object");
return false;
}
if (primitives[req.name]->getPrimitiveType() != PrimitiveType::OBJECT) {
((Object*)primitives[req.name])->setAllowObjectInteraction(req.allow);
} else
if (primitives[req.name]->getPrimitiveType() != PrimitiveType::UNKNOWN_OBJECT) {
((UnknownObject*)primitives[req.name])->setAllowObjectInteraction(req.allow);
}
ROS_INFO("..... DONE");
return true;
}
bool addUnknownObject(AddUnknownObject::Request &req, AddUnknownObject::Response &res)
{
ROS_INFO("ADDING UNKNOWN OBJECT");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
UnknownObject * unknownObject = new UnknownObject(imServer, req.frame_id, req.name);
unknownObject->setPoseType(req.pose_type);
unknownObject->setPose(req.pose);
unknownObject->setScale(req.scale);
unknownObject->setDescription(req.description);
unknownObject->useMaterial((req.disable_material != 0) ? false : true);
unknownObject->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, unknownObject));
ROS_INFO("..... DONE");
return true;
}
bool addObject(AddObject::Request &req, AddObject::Response &res)
{
ROS_INFO("ADDING OBJECT");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
Object * object = new Object(imServer, req.frame_id, req.name);
if (req.resource == "")
{
object->setShape(req.shape);
}
else
{
object->setResource(req.resource);
object->setUseMaterial(req.use_material);
}
// object->setPose(req.pose);
object->setPoseType(req.pose_type);
object->setPoseLWH(req.pose, req.bounding_box_lwh);
object->setColor(req.color);
object->setDescription(req.description);
object->setAllowPregrasp(req.allow_pregrasp);
object->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, object));
ROS_INFO("..... DONE");
return true;
}
bool addBoundingBox(AddBoundingBox::Request &req, AddBoundingBox::Response &res)
{
ROS_INFO("ADDING BOUNDING BOX");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
BoundingBox * boundingBox = new BoundingBox(imServer, req.frame_id, req.name);
boundingBox->setAttachedObjectName(req.object_name);
boundingBox->setPoseType(req.pose_type);
boundingBox->setPose(req.pose);
boundingBox->setScale(req.scale);
boundingBox->setColor(req.color);
boundingBox->setDescription(req.description);
boundingBox->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, boundingBox));
ROS_INFO("..... DONE");
return true;
}
bool getUpdateTopic(GetUpdateTopic::Request &req, GetUpdateTopic::Response &res)
{
ROS_INFO("GETTING UPDATE TOPIC");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
res.update_topic = primitives[req.name]->getUpdateTopic(req.type);
ROS_INFO("..... DONE");
return true;
}
bool changeScale(ChangeScale::Request &req, ChangeScale::Response &res)
{
ROS_INFO("CHANGING SCALE");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
primitives[req.name]->setScale(req.scale);
primitives[req.name]->insert();
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool changeDescription(ChangeDescription::Request &req, ChangeDescription::Response &res)
{
ROS_INFO("CHANGING DESCRIPTION");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
primitives[req.name]->setDescription(req.description);
primitives[req.name]->insert();
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool changeColor(ChangeColor::Request &req, ChangeColor::Response &res)
{
ROS_INFO("CHANGING COLOR");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
res.old_color = primitives[req.name]->getColor();
primitives[req.name]->setColor(req.color);
primitives[req.name]->insert();
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool removePrimitive(RemovePrimitive::Request &req, RemovePrimitive::Response &res)
{
ROS_INFO("REMOVING PRIMITIVE");
InteractiveMarker tmp;
if (!imServer->get(req.name, tmp))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
tPrimitives::iterator it = primitives.find(req.name);
if (it != primitives.end())
{
delete it->second;
primitives.erase(it);
}
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool addPlanePolygon(AddPlanePolygon::Request &req, AddPlanePolygon::Response &res)
{
ROS_INFO("ADDING PLANE POLYGON");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
PlanePolygon *plane = new PlanePolygon(imServer, req.frame_id, req.name);
plane->setPolygon(req.polygon);
plane->setNormal(req.normal);
plane->setColor(req.color);
plane->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, plane));
ROS_INFO("..... DONE");
return true;
}
bool changeDirection(ChangeDirection::Request &req, ChangeDirection::Response &res)
{
ROS_INFO("CHANGING DIRECTION");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
if (primitives[req.name]->getPrimitiveType() != PrimitiveType::BILLBOARD)
{
ROS_WARN("This is object is not a billboard, direction cannot be changed!");
return false;
}
((Billboard*)primitives[req.name])->setDirection(req.direction);
primitives[req.name]->insert();
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool removePreGraspPosition(RemovePreGraspPosition::Request &req, RemovePreGraspPosition::Response &res)
{
ROS_INFO("REMOVING PRE-GRASP POSITION");
InteractiveMarker tmp;
if ((!imServer->get(req.name, tmp)) || (primitives.count(req.name) != 1))
{
ROS_ERROR("Primitive with that name doesn't exist!");
return false;
}
if (primitives[req.name]->getPrimitiveType() != PrimitiveType::OBJECT)
{
ROS_ERROR("Primitive is not of type Object");
return false;
}
((Object*)primitives[req.name])->removePreGraspPosition(req.pos_id);
primitives[req.name]->insert();
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}