Why doesn't my node publish to the topic cmd_vel?

asked 2015-07-04 12:19:12 -0600

erivera1802
59 ●17 ●19 ●23

updated 2015-07-04 13:32:00 -0600

3045 ●62 ●89 ●110 http://allen-software.com/

Hi, I'm trying to send commands to an Ardrone through cmd_vel topic. But I don't know why it isn't working, 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 <geometry_msgs/Point.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
#include <fstream>
using namespace std;

int posY,posZ;
float ey,ez,ex,dey,eypa,dex,expa,dez,ezpa,iex,iey,iez,uy,ux,uz,posX;
geometry_msgs::Twist twist_msg;
geometry_msgs::Point point_msg;
std_msgs::Empty emp_msg;

    void posCallback(const geometry_msgs::Point& msg)
    {
        ros::NodeHandle neu;
        ros::Rate loop_rate(50);
        posY=msg.x;
        posZ=msg.y;
        posX=msg.z;
        //printf("[%d,%d,%f] \n",posY,posZ,posX);
        ros::Publisher pub_twist= neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

        ey=(319-posY)/320.0;
        ez=(179-posZ)/180.0;
        ex=(40.0-posX)/40.0;
        dey=-(ey-eypa)/1.0;
        dez=-(ez-ezpa)/1.0;
        dex=-(ex-expa)/1.0;
        iey=iey+ey;
        iez=iez+ez;
        iex=iex+ex;
        uy=ey+dey;
        ux=ex/4+dex;
        uz=ez+dez;
        if(abs(uy)>1.0)
        {
            uy=uy/abs(uy);              
        }
        if(abs(ux)>1.0)
        {
            ux=ux/abs(ux);              
        }
        if(abs(uz)>1.0)
        {
            uz=uz/abs(uz);              
        }
        twist_msg.angular.z=uy;
        twist_msg.linear.x=ux;
        //ROS_INFO("Ball Position:[%f,%f,%f,%f]",ex,dex,iex,ux);                            
        pub_twist.publish(twist_msg);
        ros::spinOnce();
    }

int main(int argc, char **argv)
{
    //Initializing ROS
    ros::init(argc, argv, "control");
    ros::NodeHandle nh;
    ros::Rate loop_rate(50);    

    twist_msg.linear.y=0.0;
    twist_msg.linear.x=0.0;
    twist_msg.linear.z=0.0;
    twist_msg.angular.z=0.0;
    twist_msg.angular.y=0.0;
    twist_msg.angular.x=0.0;

    eypa=0.0;
    expa=0.0;
    ezpa=0.0;
    iex=0.0;
    iey=0.0;
    iez=0.0;

    ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
    ros::spin();
}

The idea is that the node subscribe to color_tracking topic, and gets the reference, then with that reference it computes a pid controler, and send it to the drone via cmd_vel. But when i echo cmd_vel, it doesnt gets anything Again I think is a problem of spin() and spinOnce(), but i have no idea. Hope someone can help me, thanks.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2015-07-04 14:33:17 -0600

Stefan Kohlbrecher

24361 ●173 ●299 ●444 https://www.energy-rob...

updated 2015-07-04 14:33:45 -0600

You instantiate a new publisher everytime your callback posCallback is called. There is a time gap between instantiating a publisher till a publish call to it actually publishes, as things are threaded behind the scenes. So what happens everytime your posCallback is called:

Publisher pub_twist gets instantiated
publish() is called on the instantiated pub_twist publisher at the end of your callback. The publisher is not set up correctly yet at that point in time, so nothing actually gets published.
pub_twist goes out of scope and it's destructor is called as it is a local variable of your posCallback.

What you can do is make the Publisher a global variable and use it. Minimal example:

#include <ros/ros.h>
[...]

ros::Publisher pub_twist;

[...]
    void posCallback(const geometry_msgs::Point& msg)
    {
        [...]
        pub_twist.publish(twist_msg);
    }

int main(int argc, char **argv)
{
    [...] 

    pub_twist= nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

    ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
    ros::spin();
}

edit flag offensive delete link

Comments

I tried your solution, but it shows me this error: call to publish() on an invalid publisher I think its because i dont do the advertise stuff in the callback function but in the main, then it doesnt configure correctly the publisher. Any idea?

erivera1802 ( 2015-07-05 11:36:43 -0600 )edit

I writed the code again and your solution worked perfectly! Thank you Stefan

erivera1802 ( 2015-07-05 12:37:24 -0600 )edit

add a comment

answered 2015-07-04 13:17:33 -0600

allenh1

3045 ●62 ●89 ●110 http://allen-software.com/

updated 2015-07-04 13:27:30 -0600

So the issue seems to be in the way you are setting up your publisher. I would recommend publish the messages in the main function. You also seem to have a large amount of unnecessary (at least for this bit of code) includes and variables. It is also not recommended to have using namespace std; in your code. Instead, you should access the members of the namespace directly. For example, cout becomes std::cout. Best of luck!

#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 <geometry_msgs/Point.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
#include <fstream>
using namespace std;

int posY,posZ;
float ey,ez,ex,dey,eypa,dex,expa,dez,ezpa,iex,iey,iez,uy,ux,uz,posX;
geometry_msgs::Twist twist_msg;
geometry_msgs::Point point_msg;
std_msgs::Empty emp_msg;

void posCallback(const geometry_msgs::Point& msg)
{
    posY=msg.x;
    posZ=msg.y;
    posX=msg.z;
    //printf("[%d,%d,%f] \n",posY,posZ,posX);


    ey=(319-posY)/320.0;
    ez=(179-posZ)/180.0;
    ex=(40.0-posX)/40.0;
    dey=-(ey-eypa)/1.0;
    dez=-(ez-ezpa)/1.0;
    dex=-(ex-expa)/1.0;
    iey=iey+ey;
    iez=iez+ez;
    iex=iex+ex;
    uy=ey+dey;
    ux=ex/4+dex;
    uz=ez+dez;
    if(abs(uy)>1.0)
    {
        uy=uy/abs(uy);              
    }
    if(abs(ux)>1.0)
    {
        ux=ux/abs(ux);              
    }
    if(abs(uz)>1.0)
    {
        uz=uz/abs(uz);              
    }
    twist_msg.angular.z=uy;
    twist_msg.linear.x=ux;
}

int main(int argc, char **argv)
{
    //Initializing ROS
    ros::init(argc, argv, "control");
    ros::NodeHandle nh;
    ros::Rate loop_rate(50);    

    twist_msg.linear.y=0.0;
    twist_msg.linear.x=0.0;
    twist_msg.linear.z=0.0;
    twist_msg.angular.z=0.0;
    twist_msg.angular.y=0.0;
    twist_msg.angular.x=0.0;

    eypa=0.0;
    expa=0.0;
    ezpa=0.0;
    iex=0.0;
    iey=0.0;
    iez=0.0;


    ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
    ros::Publisher pub_twist= neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

    while (ros::ok()) {
        pub_twist.publish(twist_msg);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;    
}

edit flag offensive delete link

Comments

Hi! I tried to publish on the main function, but the code never gets to the end of the main, i thinks that is because the callback was too fast, then it keep calling it so it would never look down the main. Im gonna take your advice about the std space, thanks!

erivera1802 ( 2015-07-05 12:36:05 -0600 )edit

add a comment

Why doesn't my node publish to the topic cmd_vel?

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Why doesn't my node publish to the topic cmd_vel? edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Why doesn't my node publish to the topic cmd_vel?