ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How do i publish very fast?

asked 2015-06-21 18:54:26 -0500

erivera1802 gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

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?

BennyRe gravatar image BennyRe  ( 2015-06-22 02:29:52 -0500 )edit

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

erivera1802 gravatar image erivera1802  ( 2015-06-22 17:40:30 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2015-06-22 08:07:36 -0500

kramer gravatar image

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.

edit flag offensive delete link more

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?

erivera1802 gravatar image erivera1802  ( 2015-06-22 17:44:10 -0500 )edit
2

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.

kramer gravatar image kramer  ( 2015-06-23 00:50:51 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2015-06-21 18:54:26 -0500

Seen: 1,570 times

Last updated: Jun 22 '15