Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo into develop

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_;
 

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_;
 };
 

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<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
   head_scan_pub_ = nh_.advertise<std_msgs::String>("/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()

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)