Skip to content

Commit 7b411cb

Browse files
zhao1995zhao1995
committed
refine
1 parent 314a503 commit 7b411cb

File tree

6 files changed

+46
-42
lines changed

6 files changed

+46
-42
lines changed

‎drv_brain/src/drv_brain.cpp‎

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -176,18 +176,19 @@ void searchCallback(const Int8ConstPtr &msg)
176176
{
177177
if (modeType_ == m_search) {
178178
if (msg->data == -1) {
179-
pubInfo("Search around didn't get target, continue searching...");
179+
pubInfo("Search report: Search around didn't find target.");
180180
foundTarget_ = false;
181181
ros::param::set(param_vision_feedback_search, -2);
182182
ros::param::set(param_vision_feedback, 2);
183183
}
184184
else if (msg->data == 0) {
185-
pubInfo("Currently didn't find target, continue searching...");
185+
pubInfo("Search report: In searching...");
186186
ros::param::set(param_vision_feedback_search, -1);
187187
ros::param::set(param_vision_feedback, 1);
188188
foundTarget_ = false;
189189
}
190190
else {
191+
pubInfo("Search report: Found target.");
191192
foundTarget_ = true;
192193
ros::param::set(param_vision_feedback_search, 1);
193194
ros::param::set(param_vision_feedback, 1);
@@ -291,12 +292,12 @@ int main(int argc, char **argv)
291292
bool temp = true;
292293
ros::param::get(param_vision_shared_switch, temp);
293294
if (temp)
294-
ROS_WARN_COND(!centralSwitch_, "Central switch is ON.");
295+
ROS_WARN_COND(!centralSwitch_, "Brain: Central switch is ON.");
295296
centralSwitch_ = temp;
296297
}
297298

298299
if (!centralSwitch_) {
299-
ROS_WARN_THROTTLE(9, "Central switch is OFF.");
300+
ROS_WARN_THROTTLE(9, "Brain: Central switch is OFF.");
300301
resetStatus();
301302
continue;
302303
}
@@ -305,7 +306,7 @@ int main(int argc, char **argv)
305306
if (!servo_initialized_) {
306307
pubServo(90, 90, 1);
307308
servo_initialized_ = true;
308-
ROS_INFO("Servo reset.");
309+
ROS_INFO("Brain: Servo reset.");
309310
}
310311

311312
// Get feedback to determine whether target were found
@@ -317,9 +318,9 @@ int main(int argc, char **argv)
317318

318319
if (isTargetSet_ != targetSetTemp_) {
319320
if (isTargetSet_)
320-
pubInfo("Target set to be '" + targetLabel_ + "'.");
321+
pubInfo("Brain: Target set to be '" + targetLabel_ + "'.");
321322
else {
322-
pubInfo("Target cancelled.");
323+
pubInfo("Brain: Target cancelled.");
323324
resetStatus();
324325
}
325326
targetSetTemp_ = isTargetSet_;
@@ -339,7 +340,7 @@ int main(int argc, char **argv)
339340
if (targetLabel_ == "user selected object") {
340341
// If the target is selected by user but no more been found,
341342
// which means tracking has lost it, reset.
342-
pubInfo("Tracking did not follow.");
343+
pubInfo("Brain: Tracking did not follow.");
343344
resetStatus();
344345
}
345346
else
@@ -358,7 +359,7 @@ int main(int argc, char **argv)
358359
String mode_msg;
359360
mode_msg.data = modeName[modeType_];
360361
drvPubMode_.publish(mode_msg);
361-
ROS_INFO("Current mode: %s.", modeName[modeType_].c_str());
362+
ROS_INFO("Brain: Current mode: %s.", modeName[modeType_].c_str());
362363
modeTypeTemp_ = modeType_;
363364
if (modeType_ == m_wander) {
364365
// Reset head pose

‎drv_grasp/src/drv_grasp.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -122,11 +122,11 @@ void trackResultCallback(const drv_msgs::recognized_targetConstPtr &msg)
122122
int x = msg->tgt_bbox_center.data[0];
123123
int y = msg->tgt_bbox_center.data[1];
124124
if (x <= 0 || x >= 640) {
125-
ROS_WARN_ONCE("Target x value in image is unnormal.");
125+
ROS_WARN_ONCE("Grasp: Target x value is unnormal.");
126126
return;
127127
}
128128
if (y <= 0 || y >= 480) {
129-
ROS_WARN_ONCE("Target y value in image is unnormal.");
129+
ROS_WARN_ONCE("Grasp: Target y value is unnormal.");
130130
return;
131131
}
132132

@@ -275,7 +275,7 @@ void depthCallback(const sensor_msgs::ImageConstPtr& imageDepth)
275275
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
276276
imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
277277
{
278-
ROS_ERROR("Depth image type is wrong.");
278+
ROS_ERROR("Grasp: Depth image type is wrong.");
279279
return;
280280
}
281281

@@ -330,7 +330,7 @@ void depthCallback(const sensor_msgs::ImageConstPtr& imageDepth)
330330
hasGraspPlan_ = true;
331331
}
332332
else
333-
ROS_INFO_THROTTLE(11, "Get grasp point failed!");
333+
ROS_INFO_THROTTLE(11, "Grasp: Get grasp point failed!");
334334
}
335335
#else
336336
void getCloudByInliers(PointCloud::Ptr cloud_in, PointCloud::Ptr &cloud_out,

‎drv_track_kcf/src/drv_track_kcf.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ void searchROICallback(const drv_msgs::recognized_targetConstPtr &msg)
9898
int max_x = msg->tgt_bbox_array.data[2];
9999
int max_y = msg->tgt_bbox_array.data[3];
100100

101-
ROS_INFO("Received ROI %d %d %d %d.", min_x, min_y, max_x, max_y);
101+
ROS_INFO("Track: Received ROI %d %d %d %d.", min_x, min_y, max_x, max_y);
102102

103103
roi_init_ = Rect(min_x, min_y, max_x - min_x, max_y - min_y);
104104
tracker.initialized_ = false;
@@ -125,7 +125,7 @@ bool verifyDetection(Rect roi) {
125125
roi.height = 479 - roi.y;
126126
}
127127
if (roi.area() < MIN_OBJECT_AREA || roi.area() > MAX_OBJECT_AREA) {
128-
ROS_WARN("verifyDetection: Target ROI is unnormal.");
128+
ROS_WARN("Track: Target ROI is unnormal.");
129129
return false;
130130
}
131131
return true;
@@ -162,7 +162,7 @@ void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
162162
return;
163163

164164
if (!verifyDetection(roi_init_)) {
165-
ROS_WARN("verifyDetection: Initial ROI can not be tracked.");
165+
ROS_WARN("Track: Initial ROI can not be tracked.");
166166
isInTracking_ = false;
167167
tracker.initialized_ = false;
168168
return;
@@ -240,7 +240,7 @@ void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
240240
if (!(x_ang >= 0 && x_ang <= 180 && y_ang >= 60 && y_ang <= 140)) {
241241
// Target center out of camera movable region
242242
pubTarget(image_msg->header, mask_id, roi);
243-
ROS_WARN_THROTTLE(31, "Target out of camera movable area.");
243+
ROS_WARN_THROTTLE(31, "Track: Target out of camera movable area.");
244244
// Although out of movable area, still in tracking
245245
// isInTracking_ = false;
246246
// tracker.initialized_ = false;

‎drv_track_kcf/src/fhog.hpp‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -63,12 +63,11 @@
6363

6464
// DataType: STRUCT featureMap
6565
// FEATURE MAP DESCRIPTION
66-
// Rectangular map (sizeX x sizeY),
67-
// every cell stores feature vector (dimension = numFeatures)
68-
// map - matrix of feature vectors
69-
// to set and get feature vectors (i,j)
70-
// used formula map[(j * sizeX + i) * p + k], where
71-
// k - component of feature vector in cell (i, j)
66+
// Rectangular map (sizeX x sizeY),
67+
// every cell stores feature vector (dimension = numFeatures)
68+
// map - matrix of feature vectors to set and get feature vectors (i,j)
69+
// used formula map[(j * sizeX + i) * p + k], where
70+
// k - component of feature vector in cell (i, j)
7271
typedef struct{
7372
int sizeX;
7473
int sizeY;

‎drv_track_kcf/src/kcftracker.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -281,7 +281,9 @@ void KCFTracker::train(cv::Mat x, float train_interp_factor)
281281

282282
}
283283

284-
// Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window).
284+
// Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts
285+
// between input images X and Y, which must both be MxN.
286+
// They must also be periodic (ie., pre-processed with a cosine window).
285287
cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
286288
{
287289
using namespace FFTTools;
@@ -310,7 +312,8 @@ cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
310312
c = real(c);
311313
}
312314
cv::Mat d;
313-
cv::max(( (cv::sum(x1.mul(x1))[0] + cv::sum(x2.mul(x2))[0])- 2. * c) / (size_patch[0]*size_patch[1]*size_patch[2]) , 0, d);
315+
cv::max(((cv::sum(x1.mul(x1))[0] + cv::sum(x2.mul(x2))[0])- 2. * c) /
316+
(size_patch[0]*size_patch[1]*size_patch[2]) , 0, d);
314317

315318
cv::Mat k;
316319
cv::exp((-d / (sigma * sigma)), k);
@@ -406,13 +409,14 @@ cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scal
406409
IplImage z_ipl = z;
407410
CvLSVMFeatureMapCaskade *map;
408411
getFeatureMaps(&z_ipl, cell_size, &map);
409-
normalizeAndTruncate(map,0.2f);
412+
normalizeAndTruncate(map, 0.2f);
410413
PCAFeatureMaps(map);
411414
size_patch[0] = map->sizeY;
412415
size_patch[1] = map->sizeX;
413416
size_patch[2] = map->numFeatures;
414417

415-
FeaturesMap = cv::Mat(cv::Size(map->numFeatures,map->sizeX*map->sizeY), CV_32F, map->map); // Procedure do deal with cv::Mat multichannel bug
418+
// Procedure do deal with cv::Mat multichannel bug
419+
FeaturesMap = cv::Mat(cv::Size(map->numFeatures,map->sizeX*map->sizeY), CV_32F, map->map);
416420
FeaturesMap = FeaturesMap.t();
417421
freeFeatureMapObject(&map);
418422

‎drv_track_kcf/src/utilities.cpp‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ using namespace std;
44
using namespace cv;
55

66
Scalar stroke_color(0, 128, 255);
7-
float thickness = 1.8;
7+
int thickness = 2;
88

99
Utilities::Utilities()
1010
{
@@ -53,7 +53,7 @@ void Utilities::markImage(Mat img_in, Rect roi, Mat &img_out,
5353
vector<vector<Point> > contours;
5454
findContours(closed, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
5555

56-
drawContours(img_out, contours, 0, Scalar(196,0,225), 2);
56+
drawContours(img_out, contours, 0, Scalar(196, 0, 225), 2);
5757
rectangle(img_out, roi, Scalar(0, 255, 0));
5858
drawCross(roi_center.x, roi_center.y, img_out);
5959
}
@@ -67,22 +67,22 @@ void Utilities::markImage(Rect roi, Mat &img_out)
6767

6868
void Utilities::drawCross(int x, int y, Mat &frame)
6969
{
70-
circle(frame, Point(x,y), 12, stroke_color, thickness);
70+
circle(frame, Point(x, y), 10, stroke_color, thickness);
7171
if(y - 25 > 0)
72-
line(frame, Point(x,y), Point(x, y-25), stroke_color, thickness);
73-
else line(frame,Point(x,y),Point(x,0), stroke_color, thickness);
74-
if(y+25<FRAME_HEIGHT)
75-
line(frame,Point(x,y),Point(x,y+25), stroke_color, thickness);
76-
else line(frame,Point(x,y),Point(x,FRAME_HEIGHT), stroke_color, thickness);
77-
if(x-25>0)
78-
line(frame,Point(x,y),Point(x-25,y), stroke_color, thickness);
79-
else line(frame,Point(x,y),Point(0,y), stroke_color, thickness);
80-
if(x+25<FRAME_WIDTH)
81-
line(frame,Point(x,y), Point(x+25, y),stroke_color, thickness);
82-
else line(frame,Point(x,y), Point(FRAME_WIDTH, y), stroke_color, thickness);
72+
line(frame, Point(x, y), Point(x, y - 25), stroke_color, thickness);
73+
else line(frame,Point(x, y),Point(x,0), stroke_color, thickness);
74+
if(y + 25 < FRAME_HEIGHT)
75+
line(frame,Point(x, y),Point(x, y + 25), stroke_color, thickness);
76+
else line(frame,Point(x, y),Point(x,FRAME_HEIGHT), stroke_color, thickness);
77+
if(x - 25 > 0)
78+
line(frame,Point(x, y),Point(x - 25, y), stroke_color, thickness);
79+
else line(frame,Point(x, y),Point(0, y), stroke_color, thickness);
80+
if(x + 25 < FRAME_WIDTH)
81+
line(frame,Point(x, y), Point(x + 25, y),stroke_color, thickness);
82+
else line(frame,Point(x, y), Point(FRAME_WIDTH, y), stroke_color, thickness);
8383

8484
putText(frame, intToString(x) + "," + intToString(y),
85-
Point(x, y+30), 1, 1, stroke_color, thickness);
85+
Point(x, y + 30), 1, 1, stroke_color, thickness);
8686
}
8787

8888
void Utilities::expandGt(Rect &gt, int margin)

0 commit comments

Comments
 (0)