Revision 06738945
| ID | 067389450ba024bb0b83954efcd4288c4a405feb |
The code compiled! Now to make sure everything does what it's supposed to do.
| vision/src/v2v3_converter.cpp | ||
|---|---|---|
| 1 | 1 |
#include <ros/ros.h> |
| 2 | 2 |
#include <vision/TargetDescriptor.h> |
| 3 |
#include <image_transport/image_transport.h> |
|
| 3 | 4 |
#include <geometry_msgs/Point.h> |
| 4 | 5 |
#include <tf/transform_listener.h> |
| 5 |
#include <opencv/cv.hpp> |
|
| 6 |
//#include <cv_bridge/cv_bridge.h> |
|
| 7 | 6 |
|
| 8 | 7 |
ros::Publisher pub; |
| 9 | 8 |
|
| ... | ... | |
| 11 | 10 |
int sk = 255; |
| 12 | 11 |
int vk = 255; |
| 13 | 12 |
|
| 14 |
bool isRight(cv_bridge::CvImagePtr img, int p){
|
|
| 13 |
bool isRight(sensor_msgs::Image * img, int p){
|
|
| 15 | 14 |
return (img->data[p + 1] == img->data[p]); |
| 16 | 15 |
} |
| 17 | 16 |
|
| 18 |
bool isLeft(cv_bridge::CvImagePtr img, int p){
|
|
| 17 |
bool isLeft(sensor_msgs::Image * img, int p){
|
|
| 19 | 18 |
return (img->data[p - 1] == img->data[p]); |
| 20 | 19 |
} |
| 21 | 20 |
|
| 22 |
bool isUp(cv_bridge::CvImagePtr img, int p){
|
|
| 23 |
return (img->data[p - img->width() * img->channels()] == img->data[p]);
|
|
| 21 |
bool isUp(sensor_msgs::Image * img, int p){
|
|
| 22 |
return (img->data[p - img->width] == img->data[p]); |
|
| 24 | 23 |
} |
| 25 | 24 |
|
| 26 |
bool isDown(cv_bridge::CvImagePtr img, int p){
|
|
| 27 |
return (img->data[p + img->width * img->nChannels] == img->data[p]);
|
|
| 25 |
bool isDown(sensor_msgs::Image * img, int p){
|
|
| 26 |
return (img->data[p + img->width] == img->data[p]); |
|
| 28 | 27 |
} |
| 29 | 28 |
|
| 30 |
bool hasAnAdjacent(cv_bridge::CvImagePtr image, int j){
|
|
| 29 |
bool hasAnAdjacent(sensor_msgs::Image * image, int j){
|
|
| 31 | 30 |
int nl = image->height; // number of lines |
| 32 |
int nc = image->width; // number of columns CHECK TO MAKE SURE THIS IS RIGHT AND NOT WITH CHANNELS |
|
| 33 |
int pos = j / image->nChannels; |
|
| 31 |
int nc = image->width; |
|
| 32 |
int nChannels = image->width / image->step; |
|
| 33 |
int pos = j / nChannels; |
|
| 34 | 34 |
if ((pos % nc) != 0){
|
| 35 |
if isLeft() return true;
|
|
| 35 |
if (isLeft(image, pos)) return true;
|
|
| 36 | 36 |
} |
| 37 | 37 |
if ((pos / nc) != 0){
|
| 38 |
if isUp() return true;
|
|
| 38 |
if (isUp(image, pos)) return true;
|
|
| 39 | 39 |
} |
| 40 | 40 |
if ((pos % nc) != nc - 1){
|
| 41 |
if isRight() return true;
|
|
| 41 |
if (isRight(image, pos)) return true;
|
|
| 42 | 42 |
} |
| 43 | 43 |
if ((pos / nc) != nc - 1){
|
| 44 |
if isDown() return true;
|
|
| 44 |
if (isDown(image, pos)) return true;
|
|
| 45 | 45 |
} |
| 46 | 46 |
else return false; |
| 47 | 47 |
} |
| ... | ... | |
| 49 | 49 |
/* Detects the blobs in an image within above defined constraints. Transforms image into 1 |
| 50 | 50 |
* for pixels in the blob, 0 for not in the blob. |
| 51 | 51 |
*/ |
| 52 |
void detectBlobs(cv_bridge::CvImagePtr image, int * comX, int * comY, int * area){
|
|
| 52 |
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){
|
|
| 53 | 53 |
int nl = image->height; // number of lines |
| 54 |
int nc = image->width * image->nChannels; // number of columns
|
|
| 55 |
int step = image->widthStep; // effective width
|
|
| 54 |
int nc = image->step; // number of columns
|
|
| 55 |
int nChannels = image->step / image->width; // effective width
|
|
| 56 | 56 |
|
| 57 | 57 |
// process image |
| 58 | 58 |
for (int i = 1; i < nl; i++){
|
| 59 |
for (int j = 0; j < nc; j+= image->nChannels){
|
|
| 60 |
if (image->data[j] > 30 || image->data[j] < 20){
|
|
| 61 |
image->data[j] = 1; |
|
| 62 |
image->data[j+1] = 1; |
|
| 63 |
image->data[j+2] = 1; |
|
| 59 |
for (int j = 0; j < nc; j+= nChannels){
|
|
| 60 |
if (image->data[i * image->width + j] < 30 && image->data[j] > 20){
|
|
| 61 |
image->data[i * image->width + j] = 1;
|
|
| 62 |
image->data[i * image->width + j+1] = 1;
|
|
| 63 |
image->data[i * image->width + j+2] = 1;
|
|
| 64 | 64 |
} |
| 65 | 65 |
else {
|
| 66 |
image->data[j] = 0; |
|
| 67 |
image->data[j+1] = 0; |
|
| 68 |
image->data[j+2] = 0; |
|
| 66 |
image->data[i * image->width + j] = 0;
|
|
| 67 |
image->data[i * image->width + j+1] = 0;
|
|
| 68 |
image->data[i * image->width + j+2] = 0;
|
|
| 69 | 69 |
} |
| 70 | 70 |
} |
| 71 | 71 |
} |
| ... | ... | |
| 73 | 73 |
int comXSum = 0; |
| 74 | 74 |
int comYSum = 0; |
| 75 | 75 |
int blobPixelCount = 0; |
| 76 |
int maxHeight = 0; |
|
| 77 |
int minHeight = 0; |
|
| 78 |
for (int i = 0; i < image->width * image->height * image->nChannels; |
|
| 79 |
i+=image->nChannels){
|
|
| 80 |
if ( !(data[i] == 1 && hasAnAdjacent(image, i)) ){
|
|
| 81 |
data[i] = 0; |
|
| 82 |
data[i + 1] = 0; |
|
| 83 |
data[i + 2] = 0; |
|
| 76 |
for (int i = 0; i < image->width * image->height; i+=nChannels){
|
|
| 77 |
if ( !(image->data[i] == 1 && hasAnAdjacent(image, i)) ){
|
|
| 78 |
image->data[i] = 0; |
|
| 79 |
image->data[i + 1] = 0; |
|
| 80 |
image->data[i + 2] = 0; |
|
| 84 | 81 |
} |
| 85 | 82 |
blobPixelCount++; |
| 86 |
comXSum += data[i] % nc + 1; |
|
| 87 |
comYSum += data[i] / nc + 1; |
|
| 83 |
comXSum += image->data[i] % nc + 1;
|
|
| 84 |
comYSum += image->data[i] / nc + 1;
|
|
| 88 | 85 |
} |
| 89 | 86 |
comXSum /= blobPixelCount; |
| 90 | 87 |
comYSum /= blobPixelCount; |
| ... | ... | |
| 94 | 91 |
*comY = comYSum; |
| 95 | 92 |
} |
| 96 | 93 |
|
| 97 |
void target_cb(const sensor_msgs::msg::_Image::Image orig) {
|
|
| 94 |
void convertToHSV(sensor_msgs::Image * orig, sensor_msgs::Image * result){
|
|
| 95 |
int nChannels = orig->width / orig->step; |
|
| 96 |
|
|
| 97 |
// get the max, set it as v |
|
| 98 |
|
|
| 99 |
for (int i = 0; i < orig->height * orig->width; i += nChannels){
|
|
| 100 |
int h; |
|
| 101 |
int s; |
|
| 102 |
int v; |
|
| 103 |
int min; |
|
| 104 |
|
|
| 105 |
// get min, max |
|
| 106 |
if (orig->data[i] >= orig->data[i+1] && orig->data[i] >= orig->data[i+2]){
|
|
| 107 |
v = orig->data[i]; |
|
| 108 |
if (orig->data[i+1] >= orig->data[i+2]){
|
|
| 109 |
min = orig->data[i+2]; |
|
| 110 |
} |
|
| 111 |
else min = orig->data[i+1]; |
|
| 112 |
} |
|
| 113 |
else if (orig->data[i+1] >= orig->data[i] && orig->data[i+1] >= orig->data[i+2]){
|
|
| 114 |
v = orig->data[i+1]; |
|
| 115 |
if (orig->data[i] >= orig->data[i+2]){
|
|
| 116 |
min = orig->data[i+2]; |
|
| 117 |
} |
|
| 118 |
else min = orig->data[i]; |
|
| 119 |
} |
|
| 120 |
else {
|
|
| 121 |
v = orig->data[i+2]; |
|
| 122 |
if (orig->data[i] >= orig->data[i+1]){
|
|
| 123 |
min = orig->data[i+1]; |
|
| 124 |
} |
|
| 125 |
else min = orig->data[i]; |
|
| 126 |
} |
|
| 127 |
|
|
| 128 |
// set s |
|
| 129 |
if (v != 0){
|
|
| 130 |
s = (v - min) / v; |
|
| 131 |
} |
|
| 132 |
else s = 0; |
|
| 133 |
|
|
| 134 |
// set h |
|
| 135 |
if (s == 0) h = 0; |
|
| 136 |
else if (v == orig->data[i]){
|
|
| 137 |
h = 60 * (orig->data[i+1] - orig->data[i+2]) / s; |
|
| 138 |
} |
|
| 139 |
else if (v == orig->data[i+1]){
|
|
| 140 |
h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s; |
|
| 141 |
} |
|
| 142 |
else {
|
|
| 143 |
h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s; |
|
| 144 |
} |
|
| 145 |
|
|
| 146 |
result->data[i] = h; |
|
| 147 |
result->data[i+1] = s; |
|
| 148 |
result->data[i+2] = v; |
|
| 149 |
|
|
| 150 |
} |
|
| 151 |
|
|
| 152 |
} |
|
| 153 |
|
|
| 154 |
void target_cb(const sensor_msgs::Image orig) {
|
|
| 98 | 155 |
tf::TransformListener listener; |
| 99 | 156 |
geometry_msgs::Point point; |
| 100 | 157 |
tf::StampedTransform transform; |
| ... | ... | |
| 107 | 164 |
|
| 108 | 165 |
// convert to OpenCV |
| 109 | 166 |
|
| 110 |
Image * source = &orig; // pointer to the image
|
|
| 111 |
|
|
| 112 |
cv_bridge::CvImagePtr origRGBImage = toCvCopy(const sensor_msgs::Image& source,
|
|
| 113 |
const std::string& encoding = "rgb8"); // convert to OpenCV, CHECK FOR SYNTAX
|
|
| 167 |
sensor_msgs::Image copy = orig;
|
|
| 168 |
sensor_msgs::Image * source = © // pointer to the image |
|
| 169 |
sensor_msgs::Image hsvImage = sensor_msgs::Image();
|
|
| 170 |
sensor_msgs::Image * hsvPtr = &hsvImage;
|
|
| 114 | 171 |
|
| 115 | 172 |
// convert to HSV |
| 116 |
|
|
| 117 |
cv_bridge::CvImagePtr HSVImage; |
|
| 118 |
cvCvtColor (origRGBImage, HSVImage, CV_RGB2HSV); |
|
| 173 |
|
|
| 174 |
convertToHSV(source, hsvPtr); |
|
| 119 | 175 |
|
| 120 | 176 |
// detect blob |
| 121 | 177 |
|
| ... | ... | |
| 123 | 179 |
int comY; |
| 124 | 180 |
int area; |
| 125 | 181 |
|
| 126 |
detectBlobs(HSVImage, &comX, &comY, &area);
|
|
| 182 |
detectBlobs(hsvPtr, &comX, &comY, &area);
|
|
| 127 | 183 |
|
| 128 | 184 |
// give distance as inversely proportional to area, for (x,y,z) |
| 129 | 185 |
|
| ... | ... | |
| 131 | 187 |
|
| 132 | 188 |
// fill point based on target and tf |
| 133 | 189 |
|
| 134 |
geometry_msgs::Point origPoint = new Point_();
|
|
| 190 |
geometry_msgs::Point origPoint;
|
|
| 135 | 191 |
|
| 136 | 192 |
origPoint.x = comX; |
| 137 | 193 |
origPoint.y = comY; |
| 138 | 194 |
origPoint.z = distance; |
| 139 | 195 |
|
| 140 | 196 |
// TODO transform into point |
| 197 |
|
|
| 198 |
// for now just publish that |
|
| 141 | 199 |
|
| 142 |
geometry_msgs::Point point; |
|
| 143 |
|
|
| 144 |
pub.publish(point); |
|
| 200 |
pub.publish(origPoint); |
|
| 145 | 201 |
} |
| 146 | 202 |
|
| 147 | 203 |
int main(int argc, char **argv) {
|
Also available in: Unified diff