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