機器人視覺專案：視覺檢測識別+機器人跟隨（9）
1。討論行人檢測標定過程時實施方案，這邊的技術細節已經基本確定，寫成一個類或者cpp檔案，有tracker.cpp呼叫函式，獲得標定訊號 using kinect do hand recognition and then give the true or false signal
2。行人檢測著一塊，用svm做檢測現在的問題是執行出現段錯誤記憶體轉出，溝通後現在需要除錯看怎麼解決
@tld_turtlebot_follower.cpp //用openTLD做跟隨的程式碼
#include "ros/ros.h" #include "geometry_msgs/Twist.h" #include <algorithm> #include <math.h> #include <cstring> #include <tld_msgs/BoundingBox.h> #include <std_msgs/String.h> #include <kobuki_msgs/BumperEvent.h> /*視訊顯示框的中心點*/ #define VIDEO_CENTER_X 320 #define VIDEO_CENTER_Y 240 /*中心點橫縱座標允許的誤差偏移*/ #define ERROR_OFFSET_X 2 #define ERROR_OFFSET_Y 2 /*BoundingBox的面積允許的誤差偏移*/ #define AREA_ERROR_OFFSET_RATIO 0.2 /*turtleBot運動線速度角速度預設值*/ #define CONTROL_SPEED 0.1 #define CONTROL_TURN 0.1 /*線速度控制和與初始面積差的比例*/ #define CONTROL_SPEED_RATIO 0.0003 /*角速度控制和偏移距離的比例*/ #define CONTROL_TURN_RATIO 0.1 /*線速度和角速度最大值*/ //#define CONTROL_SPEED_MAX 0.4 #define CONTROL_SPEED_MAX 0.3 #define CONTROL_TURN_MAX 0.5 /*訊息計數的最大值*/ #define COUNT_MAX 1000000
void transform_callback(const tld_msgs::BoundingBoxConstPtr & tracked_object); void bumperEventCB(const kobuki_msgs::BumperEventConstPtr & msg);
struct bounding_box_info{ int centerX; int centerY; int area; }BB_info;//boundingBox的資訊 double control_speed = 0;//turtlebot線速度控制 double control_turn = 0;//turtlebot角速度控制 int initialBBArea = 0;//第一幀boundingBox的面積 double error_offset_area = 0; int count = 0;//訊息釋出計數 /*為bumper反饋的狀態設定標誌位*/ bool bumper_left_pressed_ = false; bool bumper_center_pressed_ = false; bool bumper_right_pressed_ = false; bool change_direction_ = false; bool enable_obs_avoid_ = false;
ros::Publisher pub;//宣告一個全域性的pub物件
int main(int argc, char **argv) { ros::init(argc, argv, "tld_turtlebot_follower"); ros::NodeHandle m; pub = m.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop", 1000); ros::Subscriber bumper_event_sub = m.subscribe("/mobile_base/events/bumper", 1000, bumperEventCB); ros::Subscriber tld_tracked_object_sub = m.subscribe("tld_tracked_object", 1000, transform_callback); ros::spin(); return 0; }
/************************************************************************ *ROS_openTLD反饋的跟蹤目標大小和位置資訊與turtlebot的速度控制的轉換； *如果發生碰撞，那麼首先處理碰撞事件，碰撞資訊與turtlebot控制資訊的轉換 ***********************************************************************/ void transform_callback(const tld_msgs::BoundingBoxConstPtr & tracked_object) { ROS_INFO("transform_callback start! "); ROS_INFO("bool enable_obs_avoid_ in function transform_callback(): %d", enable_obs_avoid_); ros::Rate loop_rate(10);//loop_rate(10)可以控制while(ros:ok()的迴圈頻率 if (!enable_obs_avoid_) { ROS_INFO("tracking control program start!"); int x = tracked_object -> x; int y = tracked_object -> y; int width = tracked_object -> width; int height = tracked_object -> height; ROS_INFO("x,y,width,height:%d %d %d %d",x,y,width,height); ROS_INFO("turtlebot tracking control message count: %d",count); if(count == 0) { initialBBArea = width * height;//獲取第一幀中boundingBox面積並作為保距跟蹤參考值 error_offset_area = AREA_ERROR_OFFSET_RATIO * initialBBArea; ROS_INFO("initial bounding bos area: %d", initialBBArea); count++; } else { BB_info.centerX = x+width/2; BB_info.centerY = y-height/2;//由於turtleBot不能在三維平面移動，centerY在這裡實際上沒有起作用 BB_info.area = width * height; if(BB_info.area == 1)//如果跟蹤不到目標（BB_info.area == 1）,角速度和線速度置0。 { control_turn = 0; control_speed = 0; count++; } else { ROS_INFO("BB_info.area,initialBBArea:%d %d",BB_info.area,initialBBArea); /*控制turtleBot角速度模組*/ if ((BB_info.centerX > (VIDEO_CENTER_X - ERROR_OFFSET_X) ) && (BB_info.centerX < (VIDEO_CENTER_X + ERROR_OFFSET_X))) control_turn = 0; else if ((BB_info.centerX < (VIDEO_CENTER_X - ERROR_OFFSET_X)) || (BB_info.centerX == (VIDEO_CENTER_X - ERROR_OFFSET_X))) control_turn = std::min((CONTROL_TURN * CONTROL_TURN_RATIO * (VIDEO_CENTER_X - ERROR_OFFSET_X - BB_info.centerX)),CONTROL_TURN_MAX); else if ((BB_info.centerX > (VIDEO_CENTER_X + ERROR_OFFSET_X)) || (BB_info.centerX == (VIDEO_CENTER_X + ERROR_OFFSET_X))) control_turn = (-1)*(std::min((CONTROL_TURN * CONTROL_TURN_RATIO * (BB_info.centerX - (VIDEO_CENTER_X + ERROR_OFFSET_X))),CONTROL_TURN_MAX)); else control_turn = 0; /*控制turtleBot線速度模組*/ if ((BB_info.area > (initialBBArea - error_offset_area) ) && (BB_info.area < (initialBBArea + error_offset_area))) control_speed = 0; else if ((BB_info.area > (initialBBArea + error_offset_area)) || (BB_info.area == (initialBBArea + error_offset_area))) control_speed = (-1)*std::min(CONTROL_SPEED*CONTROL_SPEED_RATIO*(BB_info.area- (error_offset_area + initialBBArea)),CONTROL_SPEED_MAX); else if ((BB_info.area < (initialBBArea - error_offset_area)) || (BB_info.area == (initialBBArea - error_offset_area))) control_speed = std::min(CONTROL_SPEED*CONTROL_SPEED_RATIO*(initialBBArea - error_offset_area - BB_info.area),CONTROL_SPEED_MAX); else control_speed = 0; ROS_INFO("control_speed, control_turn:%lf %lf",control_speed,control_turn) ; count++; } ROS_INFO("transformat successs!") ; /*釋出turtleBot控制訊息*/ geometry_msgs::Twist twist; twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0; twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn; pub.publish(twist); //ros::spinOnce(); ROS_INFO("Publish turtlebot control message sucess!"); } } /*如果enable_obs_avoid被置位，那麼啟動避障模組obs_avoid*/ else { ROS_INFO("Obstacle avoiding program start!"); geometry_msgs::Twist cmd2; int cmd_count = 0; if(bumper_left_pressed_) { ROS_INFO("Bumper_left_ is pressing!"); while(ros::ok()&&change_direction_) { cmd_count ++; ROS_INFO("I am changing!"); cmd2.angular.z = -0.4; cmd2.linear.x = -0.2; pub.publish(cmd2); loop_rate.sleep(); if (cmd_count > 15) { change_direction_ = false; cmd_count = 0; } //break ; } } else if(bumper_center_pressed_) { while(ros::ok()&&change_direction_) { cmd_count ++; ROS_INFO("I am changing!"); cmd2.angular.z = -0.5; cmd2.linear.x = -0.2 ; pub.publish(cmd2); loop_rate.sleep(); if (cmd_count > 20) { change_direction_ = false; cmd_count = 0; } //break; } } else if(bumper_right_pressed_) { while(ros::ok()&&change_direction_) { cmd_count ++; ROS_INFO("I am changing!"); cmd2.angular.z = 0.4; cmd2.linear.x = -0.2; pub.publish(cmd2); loop_rate.sleep(); if (cmd_count > 15) { change_direction_ = false; cmd_count = 0; } //break; } } enable_obs_avoid_ = false;//避障程式結束之後，把enable_obs_avoid之後重新置為false. } //if(count == COUNT_MAX) count = 1;//count大於一定值，可能會報溢位，每釋出100000條訊息，把count重置1 }
/************************************************************************************ 碰撞事件的回撥函式，當碰撞發生時，檢測左側碰撞，中心碰撞， 還是右側碰撞， 分別置不同的flag位：bumper_left_pressed_，bumper_center_pressed_，bumper_right_pressed_ *************************************************************************************/ void bumperEventCB(const kobuki_msgs::BumperEventConstPtr & msg) { ROS_INFO("bumperEventCB() start!"); ROS_INFO("bool enable_obstacle_avoid_ in function bumperEventCB(): %d", enable_obs_avoid_); if (msg->state == kobuki_msgs::BumperEvent::PRESSED) { enable_obs_avoid_ = true;//如果檢測到碰撞，把避障使能位置為true switch (msg->bumper) { case kobuki_msgs::BumperEvent::LEFT: if (!bumper_left_pressed_) { bumper_left_pressed_ = true; ROS_INFO("bumper_left_pressed_,bumperEventCB") ; change_direction_ = true; } break; case kobuki_msgs::BumperEvent::CENTER: if (!bumper_center_pressed_) { bumper_center_pressed_ = true; ROS_INFO("bumper_center_pressed_,bumperEventCB") ; change_direction_ = true; } break; case kobuki_msgs::BumperEvent::RIGHT: if (!bumper_right_pressed_) { bumper_right_pressed_ = true; change_direction_ = true; ROS_INFO("bumper_right_pressed_") ; } break; } } else { switch (msg->bumper) { case kobuki_msgs::BumperEvent::LEFT: bumper_left_pressed_ = false; break; case kobuki_msgs::BumperEvent::CENTER: bumper_center_pressed_ = false; break; case kobuki_msgs::BumperEvent::RIGHT: bumper_right_pressed_ = false; break; } } ROS_INFO("bumperEventCB end!"); }