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