Ask Your Question
0

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

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

erivera1802 gravatar image

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

allenh1 gravatar image

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

2 Answers

Sort by ยป oldest newest most voted
1

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

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

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 more

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 gravatar imageerivera1802 ( 2015-07-05 11:36:43 -0500 )edit

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

erivera1802 gravatar imageerivera1802 ( 2015-07-05 12:37:24 -0500 )edit
0

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

allenh1 gravatar image

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

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 more

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 gravatar imageerivera1802 ( 2015-07-05 12:36:05 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-07-04 12:19:12 -0500

Seen: 654 times

Last updated: Jul 04 '15