diff --git a/op3_demo/include/op3_demo/face_tracker.h b/op3_demo/include/op3_demo/face_tracker.h index 3289375..79b3d13 100644 --- a/op3_demo/include/op3_demo/face_tracker.h +++ b/op3_demo/include/op3_demo/face_tracker.h @@ -54,6 +54,7 @@ class FaceTracker void setUsingHeadScan(bool use_scan); void setFacePosition(geometry_msgs::Point &face_position); + void goInit(double init_pan, double init_tile); double getPanOfFace() { @@ -79,6 +80,7 @@ class FaceTracker //image publisher/subscriber ros::Publisher module_control_pub_; + ros::Publisher head_joint_offset_pub_; ros::Publisher head_joint_pub_; ros::Publisher head_scan_pub_; diff --git a/op3_demo/include/op3_demo/vision_demo.h b/op3_demo/include/op3_demo/vision_demo.h index 20cd1ad..d97bc41 100644 --- a/op3_demo/include/op3_demo/vision_demo.h +++ b/op3_demo/include/op3_demo/vision_demo.h @@ -45,6 +45,7 @@ class VisionDemo : public OPDemo protected: const int SPIN_RATE; + const int TIME_TO_INIT; void processThread(); void callbackThread(); @@ -74,6 +75,8 @@ class VisionDemo : public OPDemo ros::ServiceClient set_joint_module_client_; geometry_msgs::Point face_position_; + ros::Time prev_time_; + int tracking_status_; }; diff --git a/op3_demo/src/vision/face_tracker.cpp b/op3_demo/src/vision/face_tracker.cpp index f9b3e24..771607f 100644 --- a/op3_demo/src/vision/face_tracker.cpp +++ b/op3_demo/src/vision/face_tracker.cpp @@ -30,7 +30,8 @@ FaceTracker::FaceTracker() count_not_found_(0), on_tracking_(false) { - head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); + head_joint_offset_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); + head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states", 0); head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this); @@ -96,6 +97,19 @@ void FaceTracker::setFacePosition(geometry_msgs::Point &face_position) } } +void FaceTracker::goInit(double init_pan, double init_tile) +{ + sensor_msgs::JointState head_angle_msg; + + head_angle_msg.name.push_back("head_pan"); + head_angle_msg.name.push_back("head_tilt"); + + head_angle_msg.position.push_back(init_pan); + head_angle_msg.position.push_back(init_tile); + + head_joint_pub_.publish(head_angle_msg); +} + int FaceTracker::processTracking() { if (on_tracking_ == false) @@ -172,7 +186,7 @@ void FaceTracker::publishHeadJoint(double pan, double tilt) head_angle_msg.position.push_back(pan); head_angle_msg.position.push_back(tilt); - head_joint_pub_.publish(head_angle_msg); + head_joint_offset_pub_.publish(head_angle_msg); } void FaceTracker::scanFace() diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index 1062e5c..0045a7d 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -22,8 +22,9 @@ namespace robotis_op { VisionDemo::VisionDemo() - : SPIN_RATE(30), - tracking_status_(FaceTracker::Waiting) + : SPIN_RATE(30), + TIME_TO_INIT(10), + tracking_status_(FaceTracker::Waiting) { enable_ = false; @@ -40,6 +41,9 @@ VisionDemo::~VisionDemo() void VisionDemo::setDemoEnable() { + // set prev time for timer + prev_time_ = ros::Time::now(); + // change to motion module setModuleToDemo("action_module"); @@ -78,22 +82,34 @@ void VisionDemo::process() { int tracking_status = face_tracker_.processTracking(); - if(tracking_status_ != tracking_status) + + + switch(tracking_status) { - switch(tracking_status) + case FaceTracker::Found: + if(tracking_status_ != tracking_status) + setRGBLED(0x1F, 0x1F, 0x1F); + prev_time_ = ros::Time::now(); + break; + + case FaceTracker::NotFound: + { + if(tracking_status_ != tracking_status) + setRGBLED(0, 0, 0); + + ros::Time curr_time = ros::Time::now(); + ros::Duration dur = curr_time - prev_time_; + if(dur.sec > TIME_TO_INIT) { - case FaceTracker::Found: - setRGBLED(0x1F, 0x1F, 0x1F); - break; - - case FaceTracker::NotFound: - setRGBLED(0, 0, 0); - break; - - default: - break; + face_tracker_.goInit(0,0); + prev_time_ = curr_time; } + break; } + default: + break; + } + if(tracking_status != FaceTracker::Waiting) tracking_status_ = tracking_status; @@ -176,16 +192,16 @@ void VisionDemo::setModuleToDemo(const std::string &module_name) void VisionDemo::callServiceSettingModule(const std::string &module_name) { - robotis_controller_msgs::SetModule set_module_srv; - set_module_srv.request.module_name = module_name; + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; - if (set_joint_module_client_.call(set_module_srv) == false) - { - ROS_ERROR("Failed to set module"); - return; - } + if (set_joint_module_client_.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } - return ; + return ; } void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)