Node rate with Turtlebot

edit

Hello everyone,

I have a Turtlebot and I want it to track someone.

There are two nodes : - the first one detects a face with OpenCv and send t the second one the center of the first face - the second one makes the robot move

The problem is the following : the first node get the image irregularly. sometimes I can wait one minute before it performs whathe has to do (get the image, search the face, show it and publish) and sometimes one second.

And I have problemin the second node : whatever the value I put in twist.angular.x or y or z, the robot doesn't turn. He just ove forward.

Can you help me please ?

Commands I launch (each one in a terminal) :

roslaunch turtlebot_bringup 3dsensor.launch
roslaunch turtlebot_bringup minimal.launch
rosrun tracking tracking_node 
rosrun tracking move.py

get_image_node.cpp :

  #include "get_image_node.h"

 RNG rng(12345);

 using namespace std;
 using namespace cv;


  FaceSearch::FaceSearch()
  {

 printf("Version 11\n");
 face_cascade_name = "/home/turtlebot/catkin_ws/data/haarcascade_frontalface_alt.xml";
   //-- 1. Load the cascades
   if( !face_cascade.load( face_cascade_name ) ){ printf("--(!)Error loading\n"); };




  image_transport::ImageTransport it(n_);

  sub_ = it.subscribe("/camera/rgb/image_color",10, &FaceSearch::callback, this);

 pub_ = n_.advertise<geometry_msgs::Point>("center_position", 10);

  }

void FaceSearch::detectAndDisplay( Mat frame )
{
  std::vector<Rect> faces;
  Mat frame_gray;

  cvtColor( frame, frame_gray, CV_BGR2GRAY );
  equalizeHist( frame_gray, frame_gray );

  //-- Detect faces
  face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) );

if(0 != faces.size())
    {    
    Point center( faces[0].x + faces[0].width*0.5, faces[0].y + faces[0].height*0.5 );
    geometry_msgs::Point facePosition;
facePosition.x = center.x;
facePosition.y = center.y;

ellipse( frame, center, Size( faces[0].width*0.5, faces[0].height*0.5), 0, 0, 360, Scalar( 255, 0, 255 ), 4, 8, 0 );
    Mat faceROI = frame_gray( faces[0] );
imshow("blabla", frame );

    pub_.publish(facePosition);
//printf("get_image_node publie\n");
    }
else
{
//printf(" --(!) No face detected"); 
}


}

  void FaceSearch::callback(const sensor_msgs::ImageConstPtr& input)
  {

    cv_bridge::CvImagePtr img;
  img = cv_bridge::toCvCopy(input,sensor_msgs::image_encodings::TYPE_8UC3);


       if( !(img->image).empty() )
       { 
detectAndDisplay( img->image );

     }
       else
       { 
        printf(" --(!) No captured frame -- Break!");   
    }
//}
      int c = waitKey(1);

  }



int main(int argc, char **argv)
{


  //Initiate ROS
  ros::init(argc, argv, "face_search");

  //Create an object of class FaceSearch that will take care of everything
  FaceSearch SAPObject;

  ros::spin();

  return 0;
}

get_image_node.h

#include <ros/ros.h>

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "cv_bridge/cv_bridge.h"
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>

#include "geometry_msgs/Point.h"
 #include "opencv2/objdetect/objdetect.hpp"

 #include <iostream>
 #include <stdio.h>


 using namespace std;
 using namespace cv;

class FaceSearch
{
public:
  FaceSearch();
void detectAndDisplay( Mat frame );
  void callback(const sensor_msgs::ImageConstPtr& input);


private:
  ros::NodeHandle n_; 

  ros::Publisher pub_;
  image_transport::Subscriber sub_;

 CascadeClassifier face_cascade;

String face_cascade_name;
};//End of class FaceSearch

move.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point
from geometry_msgs.msg import Twist


class Move:

    def __init__(self):
        self.angularvalue = 0
    self.twist = Twist()



    self.sub = rospy.Subscriber("center_position", Point, self.update_value)
    print "ssddsdjsjk"
        self.pub = rospy.Publisher('/cmd_vel_mux/input/navi ...