// handle MessageWhoIsThere message
void MessageHandler::handle(MessageWhoIsThere *msg)
{
// we need to send message about objects in
// received range, so we must create MessageThereYouSee message
MessageThereYouSee *messageThereYouSee = new MessageThereYouSee();
// we also need a list of those objects
std::list<MessageObject> objectsInRange;
Robot *robot = HubModule::modellingSystem->getRobotByPort(msg->port);
std::vector<Object *> objVector = std::vector<Object *>();
objVector = robot->iCanSee();
double degrees = 360 - robot->getOrientation();
double radians = degrees * (PI / 180);
int x = robot->getCoords().first * cos(radians)
+ robot->getCoords().second * sin(radians);
int y = - robot->getCoords().first * sin(radians)
+ robot->getCoords().second * cos(radians);
for (unsigned int i = 0; i < objVector.size(); i++) {
MessageObject messageObject;
Object* obj = objVector.at(i);
// set color
messageObject.type = 1;
messageObject.red = obj->getColor().red();
messageObject.green = obj->getColor().green();
messageObject.blue = obj->getColor().blue();
// set coordinates
int objX = obj->getCoords().first * cos(radians)
+ obj->getCoords().second * sin(radians);
int objY = - obj->getCoords().first * sin(radians)
+ obj->getCoords().second * cos(radians);
messageObject.coordX = objX - x;
messageObject.coordY = y - objY;
// set diameter
messageObject.diameter = objVector.at(i)->getSize();
// set orientation
messageObject.degrees = objVector.at(i)->getOrientation();
objectsInRange.push_front(messageObject);
}
objVector.clear();
messageThereYouSee->objects = objectsInRange;
messageThereYouSee->port = msg->port;
messageThereYouSee->num = msg->num;
// send message to robot
comModule->sendMessage(messageThereYouSee);
}
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;
}
void trackFilteredObject(Object theObject,Mat threshold,Mat HSV, Mat &cameraFeed)
{
vector <Object> objects;
Mat temp;
threshold.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function
findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );
//use moments method to find our filtered object
bool objectFound = false;
if (hierarchy.size() > 0)
{
int numObjects = hierarchy.size();
//if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter
if(numObjects<MAX_NUM_OBJECTS){
for (int index = 0; index >= 0; index = hierarchy[index][0])
{
Moments moment = moments((cv::Mat)contours[index]);
double area = moment.m00;
//if the area is less than 20 px by 20px then it is probably just noise
//if the area is the same as the 3/2 of the image size, probably just a bad filter
//we only want the object with the largest area so we safe a reference area each
//iteration and compare it to the area in the next iteration.
if(area>MIN_OBJECT_AREA)
{
Object object;
object.setXPos(moment.m10/area);
object.setYPos(moment.m01/area);
object.setType(theObject.getType());
object.setColor(theObject.getColor());
objects.push_back(object);
objectFound = true;
}else objectFound = false;
}
//let user know you found an object
if(objectFound ==true)
{
//draw object location on screen
drawObject(objects,cameraFeed,temp,contours,hierarchy);
}
}
else
putText(cameraFeed,"TOO MUCH NOISE! ADJUST FILTER",Point(0,50),1,2,Scalar(0,0,255),2);
}
}
RGB cast(const vector3& here, const vector3& dir) const {
Object* closest = nullptr;
vector3 contact;
RGB color;
for(auto object: objects) {
vector3 p;
bool intersects = object->getIntersection(here, dir, p);
if(!intersects) continue;
if(closest == nullptr || (here - contact).length() > (here - p).length()) {
closest = object;
contact = p;
}
}
if(!closest) return sky(here, dir);
return closest->getColor(here, contact);
}
Color pointColor (Object * obj, R3Vector inter_point, R3Vector V, unsigned int level)
{
Color c = obj->getColor ();
// Ambient light
Material mt = obj->getMaterial ();
double k_a = mt.k_a;
c.r *= ambient_light->getIntensity ().r * k_a;
c.g *= ambient_light->getIntensity ().g * k_a;
c.b *= ambient_light->getIntensity ().b * k_a;
R3Vector N = obj->getNormal (inter_point);
// Diffuse and Specular Light
for (unsigned int j = 0; j < lights.size (); j++)
{
Light * light = lights[j];
// Verifies if its in shadow
R3Vector itToLight = light->getCenter ();
itToLight.x = light->getCenter ().x - inter_point.x;
itToLight.y = light->getCenter ().y - inter_point.y;
itToLight.z = light->getCenter ().z - inter_point.z;
normalize (&itToLight);
Intersection * inter = intersectElement (itToLight, inter_point, true);
if (inter == NULL || inter->object != light)
continue;
delete inter;
// Diffuse light
double k_d = mt.k_d;
Color cd = light->getDiffuseLight (N, inter_point, k_d);
c.r += cd.r;
c.g += cd.g;
c.b += cd.b;
// Specular light
double k_s = mt.k_s;
double n = mt.n;
R3Vector E;
E.x = -V.x;
E.y = -V.y;
E.z = -V.z;
cd = light->getSpecularLight (N, inter_point, E, k_s, n);
c.r += cd.r;
c.g += cd.g;
c.b += cd.b;
}
double k_r = mt.k_r;
if (k_r > 0)
{
// Get reflection intersection
V.x = -V.x;
V.y = -V.y;
V.z = -V.z;
double VN = prod (V, N);
R3Vector R = N;
R.x = 2 * VN * R.x - V.x;
R.y = 2 * VN * R.y - V.y;
R.z = 2 * VN * R.z - V.z;
normalize (&R);
Intersection * inter = intersectElement (R, inter_point, true);
Color refColor;
if (inter != NULL)
{
Object * reflectingObject = inter->object;
R3Vector reflectingPoint = inter->point;
if (reflectingObject->isLight () || level == MAX_REFLECTION)
{
refColor = reflectingObject->getColor ();
}
else
{
refColor = pointColor (reflectingObject, reflectingPoint, R, level + 1);
}
}
c.r += k_r * refColor.r;
c.g += k_r * refColor.g;
c.b += k_r * refColor.b;
}
return c;
}
//----------------------------------------------------------------------------------------------------
Vector3 Scene::getColor (Ray r, double intensity, int depth, double tolerance)
{
Vector3 ret;
Vector3 color;
Object *obj;
Vector3 normal;
Vector3 intersection;
int objIndex;
Vector3 diffuse, phong;
Ray lightR;
double d = getIntersection(&objIndex, r);
// Ray has no intersection
if (d <= 0.0)
return Vector3(0.0, 0.0, 0.0);
// Get intersection point
intersection = r.move(d);
obj = objects[objIndex];
color = obj->getColor();
normal = obj->getNormal(intersection);
// Ambient color
ret = 0.15 * color;
// For each light
int n = lights.size();
for (int i=0; i<n; i++)
{
lightR = Ray( intersection, lights[i].position - intersection );
// Verify if object is not in the shadow
if ( !hasIntersection(lightR, lights[i].position) )
{
Vector3 lightColor = lights[i].getColor();
// Calculate diffuse illumination
diffuse = color * max(normal * lightR.direction, 0.0);
diffuse = Vector3(lightColor.x * diffuse.x, lightColor.y * diffuse.y, lightColor.z * diffuse.z);
// Calculate specular illumination
Vector3 h = (lights[i].position - intersection) + (camera.position - intersection);
h.normalize();
phong += lightColor * pow(max(h * normal, 0.0), 128);
ret += diffuse;
}
}
Vector3 refColor;
double reflect = obj->reflect;
// Test base cases for recursion
if ((depth > 1) && (intensity*reflect > tolerance))
{
Ray refRay(intersection, r.direction - 2*(r.direction * normal) * normal);
refColor = getColor(refRay, reflect*intensity, --depth, tolerance);
}
// Sum all color and check for overflows
ret = intensity * ( (1 - reflect) * (0.9 * ret) + reflect * refColor + phong);
if (ret.x > 1)
ret.x = 1;
if (ret.y > 1)
ret.y = 1;
if (ret.z > 1)
ret.z = 1;
return ret;
}
