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;
}
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();
}
