Node rate with Turtlebot
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 ...