Robotics StackExchange | Archived questions

How do i publish very fast?

Hi! I am working with the ar drone and ROS. Im trying to make it track a ball. The problem that Im having is that the rate at which I publish message to cmd_vel topic is too slow. However, the rate at which i show the command in the terminal is normal (fast), so i know that it is not problem of the algorithm itself, but the publishing of messages. Here is my code:

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
using namespace cv;
//Values for Trackbar, Hue, Saturation Value
int iLowH=150;
int iHighH=179;
int iLowS=87;
int iHighS=255;
int iLowV=71;
int iHighV=152;

int posX,posY;
float ex,ey;

geometry_msgs::Twist twist_msg;
std_msgs::Empty emp_msg;

class BallTracking
{

public:

    void imageCallback(const sensor_msgs::ImageConstPtr& msg)
    {
        ros::Rate loop_rate(50);
        int count=0;
        Point pt;
        ros::NodeHandle nh;
        ros::NodeHandle neu;
        ros::Publisher pub;
        ros::Publisher pub_empty_land;
        ros::Publisher pub_twist;
        ros::Publisher chatter_pub=neu.advertise<std_msgs::String>("chatter", 1000);
        geometry_msgs::Twist twist_msg_hover;
        pub_empty_land = neu.advertise<std_msgs::Empty>("/ardrone/land", 1); /* Message queue length is just 1 */
        pub = neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

        //Communicating between ROS an OpenCV
        cv_bridge::CvImagePtr cv_ptr;

        cv::namedWindow("Control", CV_WINDOW_AUTOSIZE); 

        cv::Mat img_thr;

          try
          {
            cv_ptr=cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
            cvCreateTrackbar("LowH", "Control", &iLowH, 179); 
            cvCreateTrackbar("HighH", "Control", &iHighH, 179); 
            cvCreateTrackbar("LowS", "Control", &iLowS, 255); 
            cvCreateTrackbar("HighS", "Control", &iHighS, 255); 
            cvCreateTrackbar("LowV", "Control", &iLowV, 255); 
            cvCreateTrackbar("HighV", "Control", &iHighV, 255);
            //Thresholding
            cv::cvtColor(cv_ptr->image,img_thr,CV_BGR2HSV); 
            cv::inRange(img_thr, cv::Scalar(iLowH,iLowS,iLowV), cv::Scalar(iHighH,iHighS,iHighV), img_thr); 
            //Voilá
            Moments oMoments = moments(img_thr);

            double dM01 = oMoments.m01;
            double dM10 = oMoments.m10;
            double dArea = oMoments.m00;

            if (dArea > 10000)
            {
                //calculate the position of the ball
                 posX = dM10 / dArea;
                 posY = dM01 / dArea;
                pt.x=posX;
                pt.y=posY;  
                circle(cv_ptr->image, pt, 50, Scalar(0,0,255), 1, 8, 0);
                ex=(319-posX)/319.0;
                ey=(179-posY)/179.0;
                ROS_INFO("Ball Position:[%f,%f]",ex,ey);
                twist_msg.linear.y=ex;
                //ROS_INFO("%d", msg.data);                         
                pub.publish(twist_msg);
                ros::spinOnce();



            }
            else
            {
                posX=319;
                posY=179;
                ROS_INFO("Landing!!!!");
                pt.x=posX;
                pt.y=posY;
                ex=(319-posX)/319.0;
                ey=(179-posY)/179.0;  
                circle(cv_ptr->image, pt, 50, Scalar(0,0,255), 1, 8, 0);
                std_msgs::String msg;
                std::stringstream ss;
                ss<< "Holi world" << count;
                msg.data==ss.str();
                //chatter_pub.publish(msg);
                //pub_empty_land.publish(emp_msg);
            }


            cv::imshow("view",img_thr);
            cv::imshow("Control",cv_ptr->image);
            cv::waitKey(30);
          }
          catch (cv_bridge::Exception& e)
          {
            ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
          }

    ros::spinOnce();
    }
};

int main(int argc, char **argv)
{
    //Initializing ROS
    ros::init(argc, argv, "tracker");
    ros::NodeHandle nh;
    ros::Rate loop_rate(50);    
    cv::namedWindow("view");
    cv::startWindowThread();
    BallTracking bt;
    twist_msg.linear.x=0.0;
    twist_msg.linear.y=0.0;
    twist_msg.angular.z=0.0;
    twist_msg.angular.x=0.0;
    twist_msg.angular.y=0.0;



    //Subscribing to ardrone camera node
    image_transport::ImageTransport it(nh);
    image_transport::Subscriber sub = it.subscribe("/ardrone/front/image_raw", 1, &BallTracking::imageCallback, &bt);
    ROS_INFO("Ball Position:%f",ex);
    ros::spin();
    cv::destroyWindow("view");
}

I think it could be the spin() stuff, I havent understood it well yet. Thanks

Asked by erivera1802 on 2015-06-21 18:54:26 UTC

Comments

I don't understand your question completely. On the console your topic is fast enough? That means $ rostopic hz /cmd_vel outputs a rate that is high enough?

Asked by BennyRe on 2015-06-22 02:29:52 UTC

No, what I mean is that i used ROS_INFO to display the commands, and it is fast, but when I publish to cmd_vel, it doesnt publish continously

Asked by erivera1802 on 2015-06-22 17:40:30 UTC

Answers

First, every time you receive a message (that is, when the imageCallback method is executed), you are re-creating your publishers, etc. Clearly, that's not what you want to do: all that should happen in the callback (as you have it set up) is image message processing.

Start there...straightening that out may also take care of your ros.spinOnce in the callback and ros.spin in main.

Asked by kramer on 2015-06-22 08:07:36 UTC

Comments

I think I tried creating the publishers in the main function, but it show me the error "pub was not declared in this scope". Do you know if maybe i can make the publishers work as inputs in the callback?

Asked by erivera1802 on 2015-06-22 17:44:10 UTC

What you probably want to do is make the publishers member variables (of class BallTracking, likely private) and create/set them up in your constructor.

Asked by kramer on 2015-06-23 00:50:51 UTC