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 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 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 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 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 clickablePositions(ClickablePositions::Request &req, ClickablePositions::Response &res)
{
ROS_INFO("ADDING CLICK POSITIONS");
std::vector<geometry_msgs::Point> positions;
for (unsigned int i = 0; i < req.positions.size(); i++)
{
positions.push_back(req.positions.at(i));
}
ClickablePositionsMarker *clickPositions = new ClickablePositionsMarker(imServer,req.frame_id, req.topic_suffix, req.radius,
req.color, positions);
imServer->insert(clickPositions->getMarker(),
boost::bind(&ClickablePositionsMarker::markerFeedback, clickPositions, _1));
imServer->applyChanges();
res.topic = BUT_PositionClicked_TOPIC(req.topic_suffix);
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 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;
}
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 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;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_primitives");
InteractiveMarkerServerPtr server;
server.reset(new InteractiveMarkerServer("test_primitives", "", false));
ColorRGBA c;
c.a = 0.2;
c.r = 1.0;
c.g = 0.0;
c.b = 0.0;
Pose p;
p.position.x = 5;
p.position.y = 2;
p.position.z = 1;
p.orientation.x = M_PI_4;
p.orientation.y = 0;
p.orientation.z = M_PI_4;
Vector3 s;
s.x = 1.0;
s.y = 1.0;
s.z = 1.0;
// Object
Billboard *chairBillboard = new Billboard(server, "/world", "person");
chairBillboard->setType(srs_interaction_primitives::BillboardType::CHAIR);
chairBillboard->setPose(p);
chairBillboard->setScale(s);
chairBillboard->setFrameID("/world");
chairBillboard->insert();
// Bounding box
BoundingBox * chairBoundingBox = new BoundingBox(server, "/world", chairBillboard->getName() + "_bbox");
chairBoundingBox->setAttachedObjectName(chairBillboard->getName());
chairBoundingBox->setPose(p);
chairBoundingBox->setScale(s);
chairBoundingBox->setColor(c);
chairBoundingBox->setDescription("Person bounding box");
chairBoundingBox->insert();
p.position.x = 1;
p.position.y = 2;
p.position.z = 2;
s.x = 1;
s.y = 1;
s.z = 1;
// Object
Billboard *milkBillboard = new Billboard(server, "/world", "milk_billboard");
Quaternion direction;
direction.x = 2.0;
direction.y = 1.0;
direction.z = 3.0;
direction.w = 1.0;
milkBillboard->setType(srs_interaction_primitives::BillboardType::MILK);
milkBillboard->setPose(p);
milkBillboard->setScale(s);
milkBillboard->setDirection(direction);
milkBillboard->setVelocity(3.4);
milkBillboard->setDescription("MLEEEEKO");
milkBillboard->insert();
UnknownObject * unknowObject = new UnknownObject(server, "/world", "unknown_object");
unknowObject->setFrameID("/world");
Pose pp;
pp.position.x = 0;
pp.position.y = 0;
pp.position.z = 0;
unknowObject->setPose(pp);
Vector3 ss;
ss.x = 1;
ss.y = 1;
ss.z = 1;
unknowObject->setScale(ss);
unknowObject->setDescription("Uknown object");
unknowObject->insert();
Plane *plane = new Plane(server, "/world", "plane1");
c.a = 1.0;
p.position.x = 0;
p.position.y = 0;
p.position.z = 0;
s.x = 5;
s.y = 2;
plane->setColor(c);
plane->setFrameID("/world");
plane->setPose(p);
plane->setScale(s);
plane->insert();
c.g = 1.0;
plane->changeColor(c);
PlanePolygon *planePolygon = new PlanePolygon(server, "/world", "plane_polygon");
Polygon pol;
geometry_msgs::Point32 point;
c.r = 1;
c.g = 0;
c.b = 0;
c.a = 1.0;
point.x = 0.99171;
point.y = 0.93265;
point.z = -0.16251;
pol.points.push_back(point);
point.x = 0.47751;
point.y = -0.93946;
point.z = -0.64291;
pol.points.push_back(point);
point.x = -1.28507;
point.y = -0.68923;
point.z = 0.26852;
pol.points.push_back(point);
point.x = -0.77087;
point.y = 1.18289;
point.z = 0.74892;
pol.points.push_back(point);
planePolygon->setPolygon(pol);
Vector3 normal;
normal.x = 0.39652;
normal.y = -0.32885;
normal.z = 0.85710;
//planePolygon->setNormal(normal);
point.x = 0.22078;
point.y = 0.86032;
point.z = -0.40858;
pol.points.push_back(point);
point.x = 0.95152;
point.y = -1.00344;
point.z = 0.31976;
pol.points.push_back(point);
point.x = -0.92901;
point.y = 0.18325;
point.z = 0.50957;
pol.points.push_back(point);
point.x = -0.97683;
point.y = 1.84874;
point.z = -0.42075;
pol.points.push_back(point);
planePolygon->setPolygon(pol);
normal.x = 0.37210;
normal.y = 0.46077;
normal.z = 0.80575;
planePolygon->setNormal(normal);
planePolygon->setColor(c);
planePolygon->insert();
/*
//Object
s.x = 6;
s.y = 3.2;
s.z = 4;
InteractiveMarker object;
Marker sphere;
sphere.type = Marker::SPHERE;
sphere.color.r = c.g;
sphere.color.g = c.r;
sphere.color.b = c.b;
sphere.color.a = c.a;
sphere.scale = s;
object.header.frame_id = "/world";
object.name = "sphere";
object.description = "Sphere";
p.position.x = 20;
object.pose = p;
InteractiveMarkerControl control;
control.name = "sphere_control";
control.interaction_mode = InteractiveMarkerControl::NONE;
control.always_visible = true;
control.markers.push_back(sphere);
object.controls.push_back(control);
server->insert(object);
// Bounding box
c.r = 1;
c.g = 0;
c.b = 0;
BoundingBox * sphereBoundingBox = new BoundingBox(server, "/world", "sphere_bbox");
sphereBoundingBox->setAttachedObjectName("sphere");
sphereBoundingBox->setPose(p);
sphereBoundingBox->setScale(s);
sphereBoundingBox->setColor(c);
sphereBoundingBox->setDescription("Sphere bounding box");
sphereBoundingBox->insert();
Object * obj = new Object(server, "/world", "table_object");
obj->setFrameID("/world");
Pose ppp;
ppp.position.x = 6;
ppp.position.y = 5;
ppp.position.z = 0;
obj->setPose(ppp);
Vector3 sss;
sss.x = 1;
sss.y = 1;
sss.z = 1;
c.a = 1.0;
obj->setScale(sss);
obj->setDescription("Table");
obj->setColor(c);
obj->setResource("package://gazebo_worlds/Media/models/table.dae");
obj->setUseMaterial(false);
obj->insert();
ObjectWithBoundingBox * objbb = new ObjectWithBoundingBox(server, "/world", "table_with_bb");
ppp.position.x = 2;
ppp.position.y = 2;
ppp.position.z = 2;
objbb->setPose(ppp);
c.a = 1.0;
Vector3 gp;
gp.x = 0.7;
gp.y = 1.2;
gp.z = 0;
objbb->setGraspingPosition(GRASP_1, gp);
gp.x = 0;
gp.y = 1.2;
gp.z = 0.9;
objbb->setGraspingPosition(GRASP_2, gp);
gp.x = 0.1;
gp.y = 0.1;
gp.z = 0.1;
objbb->setGraspingPosition(GRASP_3, gp);
Scale sbb;
sbb.x = 0.2;
sbb.y = 0.2;
sbb.z = 0.2;
Point bbm;
bbm = Point();
bbm.x = 1;
bbm.y = 1;
bbm.z = 1;
objbb->setBoundingBoxLWH(bbm);
objbb->setDescription("Table with Bounding Box");
objbb->setColor(c);
objbb->setResource("package://gazebo_worlds/Media/models/table.dae");
objbb->setUseMaterial(true);
arm_navigation_msgs::Shape shape;
Point sp;
sp.x = 0;
sp.y = 0;
sp.z = 0;
shape.vertices.push_back(sp);
sp.x = 1;
shape.vertices.push_back(sp);
sp.y = 2;
shape.vertices.push_back(sp);
shape.triangles.push_back(0);
shape.triangles.push_back(1);
shape.triangles.push_back(2);
objbb->setShape(shape);
objbb->insert();*/
server->applyChanges();
ros::spin();
}
namespace srs_interaction_primitives
{
// Container with primitives
typedef std::map<std::string, Primitive*> tPrimitives;
tPrimitives primitives;
// Interactive Marker server
InteractiveMarkerServerPtr imServer;
bool addPlane(AddPlane::Request &req, AddPlane::Response &res)
{
ROS_INFO("ADDING PLANE");
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;
}
Plane *plane = new Plane(imServer, req.frame_id, req.name);
plane->setPoseType(req.pose_type);
plane->setPose(req.pose);
plane->setScale(req.scale);
plane->setColor(req.color);
plane->insert();
imServer->applyChanges();
primitives.insert(make_pair(req.name, plane));
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 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 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 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 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 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 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 setPreGraspPosition(SetPreGraspPosition::Request &req, SetPreGraspPosition::Response &res)
{
ROS_INFO("SETTING 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])->addPreGraspPosition(req.pos_id, req.pose);
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;
}
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 changePose(ChangePose::Request &req, ChangePose::Response &res)
{
ROS_INFO("CHANGING POSE");
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]->setPose(req.pose);
primitives[req.name]->insert();
imServer->applyChanges();
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 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 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 changeVelocity(ChangeVelocity::Request &req, ChangeVelocity::Response &res)
{
ROS_INFO("CHANGING VELOCITY");
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, velocity cannot be changed!");
return false;
}
((Billboard*)primitives[req.name])->setVelocity(req.velocity);
primitives[req.name]->insert();
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 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 getAllPrimitivesNames(GetAllPrimitivesNames::Request &req, GetAllPrimitivesNames::Response &res)
{
ROS_INFO("GETTING ALL PRIMITIVE'S NAMES");
map<string, Primitive*>::iterator i;
for (i = primitives.begin(); i != primitives.end(); i++)
res.primitives_names.push_back(i->second->getName());
return true;
}
bool getObject(GetObject::Request &req, GetObject::Response &res)
{
ROS_INFO("GETTING OBJECT");
if (primitives.count(req.name) > 0)
{
Object *obj = (Object*)primitives.at(req.name);
if (obj->getPrimitiveType() == srs_interaction_primitives::PrimitiveType::OBJECT)
{
res.name = req.name;
res.frame_id = obj->getFrameID();
res.pose_type = obj->getPoseType();
res.pose = obj->getPose();
res.bounding_box_lwh = obj->getBoundingBoxLWH();
res.description = obj->getDescription();
res.color = obj->getColor();
res.shape = obj->getShape();
res.resource = obj->getResource();
res.use_material = obj->getUseMaterial();
return true;
}
}
ROS_ERROR("Object with that name doesn't exist!");
return false;
}
bool getUnknownObject(GetUnknownObject::Request &req, GetUnknownObject::Response &res)
{
ROS_INFO("GETTING OBJECT");
if (primitives.count(req.name) > 0)
{
UnknownObject *obj = (UnknownObject*)primitives.at(req.name);
if (obj->getPrimitiveType() == srs_interaction_primitives::PrimitiveType::UNKNOWN_OBJECT)
{
res.name = req.name;
res.frame_id = obj->getFrameID();
res.pose_type = obj->getPoseType();
res.pose = obj->getPose();
res.description = obj->getDescription();
res.scale = obj->getScale();
return true;
}
}
ROS_ERROR("Unknown Object with that name doesn't exist!");
return false;
}
bool getBillboard(GetBillboard::Request &req, GetBillboard::Response &res)
{
ROS_INFO("GETTING BILLBOARD");
if (primitives.count(req.name) > 0)
{
Billboard *obj = (Billboard*)primitives.at(req.name);
if (obj->getPrimitiveType() == srs_interaction_primitives::PrimitiveType::BILLBOARD)
{
res.name = req.name;
res.frame_id = obj->getFrameID();
res.pose_type = obj->getPoseType();
res.pose = obj->getPose();
res.description = obj->getDescription();
res.scale = obj->getScale();
res.description = obj->getDescription();
res.type = obj->getType();
res.velocity = obj->getVelocity();
res.direction = obj->getDirection();
return true;
}
}
ROS_ERROR("Billboard with that name doesn't exist!");
return false;
}
bool getBoundingBox(GetBoundingBox::Request &req, GetBoundingBox::Response &res)
{
ROS_INFO("GETTING BOUNDING BOX");
if (primitives.count(req.name) > 0)
{
BoundingBox *obj = (BoundingBox*)primitives.at(req.name);
if (obj->getPrimitiveType() == srs_interaction_primitives::PrimitiveType::BOUNDING_BOX)
{
res.name = req.name;
res.frame_id = obj->getFrameID();
res.pose_type = obj->getPoseType();
res.pose = obj->getPose();
res.description = obj->getDescription();
res.color = obj->getColor();
res.scale = obj->getScale();
res.object_name = obj->getAttachedObjectName();
return true;
}
}
ROS_ERROR("Bounding box with that name doesn't exist!");
return false;
}
bool getPlane(GetPlane::Request &req, GetPlane::Response &res)
{
ROS_INFO("GETTING PLANE");
if (primitives.count(req.name) > 0)
{
Plane *obj = (Plane*)primitives.at(req.name);
if (obj->getPrimitiveType() == srs_interaction_primitives::PrimitiveType::PLANE)
{
res.name = req.name;
res.frame_id = obj->getFrameID();
res.pose_type = obj->getPoseType();
res.pose = obj->getPose();
res.description = obj->getDescription();
res.color = obj->getColor();
res.scale = obj->getScale();
return true;
}
}
ROS_ERROR("Plane with that name doesn't exist!");
return false;
}
bool clickablePositions(ClickablePositions::Request &req, ClickablePositions::Response &res)
{
ROS_INFO("ADDING CLICK POSITIONS");
std::vector<geometry_msgs::Point> positions;
for (unsigned int i = 0; i < req.positions.size(); i++)
{
positions.push_back(req.positions.at(i));
}
ClickablePositionsMarker *clickPositions = new ClickablePositionsMarker(imServer,req.frame_id, req.topic_suffix, req.radius,
req.color, positions);
imServer->insert(clickPositions->getMarker(),
boost::bind(&ClickablePositionsMarker::markerFeedback, clickPositions, _1));
imServer->applyChanges();
res.topic = BUT_PositionClicked_TOPIC(req.topic_suffix);
ROS_INFO("DONE");
return true;
}
bool robotPosePrediction(RobotPosePrediction::Request &req, RobotPosePrediction::Response &res)
{
ROS_INFO("ADDING ROBOT'S PREDICTED POSITIONS");
/*visualization_msgs::InteractiveMarker predictedPositions;
predictedPositions.header.frame_id = "/map";
predictedPositions.name = "robot_pose_prediction";
visualization_msgs::InteractiveMarkerControl control;
control.always_visible = true;
control.independent_marker_orientation = true;
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE;*/
visualization_msgs::Marker marker;
marker.header.frame_id = "/map";
marker.ns = "interaction_primitives";
marker.id = 0;
marker.header.stamp = ros::Time();
marker.lifetime = (req.ttl <= 0.0) ? ros::Duration(5) : ros::Duration(req.ttl);
marker.color.g = 1;
marker.color.a = 0.5;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.scale.x = 0.05;
for (unsigned int i = 0; i < req.positions.size(); i++)
{
marker.points.push_back(req.positions.at(i).position);
}
// control.markers.push_back(marker);
vis_pub.publish(marker);
marker.id = 1;
marker.type = visualization_msgs::Marker::SPHERE_LIST;
marker.color.b = 1;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
// control.markers.push_back(marker);
vis_pub.publish(marker);
// predictedPositions.controls.push_back(control);
// imServer->insert(predictedPositions);
// imServer->applyChanges();
ROS_INFO("DONE");
return true;
}
}
