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

Subscribe and publish velocity [closed]

asked 2014-08-08 18:28:34 -0500

anamcarvalho gravatar image

updated 2014-08-12 10:14:30 -0500

Hi,

I have a code where I want to see the linear velocity of a powered wheelchair and then publish linear velocity to it. An example of the code is:

ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
if(vel.linear.x > 1.8){
    vel.linear.x = 1.8;
    vel_pub_.publish(vel);}

--UPDATE--

To subscribe to the cmd_vel topic I do:

ros::Subscriber vel_sub = n.subscribe("cmd_vel", 0, velCallback);

My question is... What do I insert in the velCallback function? What does it contain? What should the header of the function be?

Thank you!

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by anamcarvalho
close date 2014-08-25 12:10:40.896443

2 Answers

Sort by ยป oldest newest most voted
4

answered 2014-08-10 08:35:58 -0500

Chrissi gravatar image

updated 2014-08-13 09:20:37 -0500

If you want to synchronize the data and use one callback for both of the topics, have a look at the message_filter tutorial.

Edit:

The callback should look something like this (no guarantee that this works though, beware of typos):

void velCallback(geometry_msgs::Twist::ConstPtr& vel) {
 //Since vel is a const pointer you cannot edit the values inside but have to use the copy new_vel.
 geometry_msgs::Twist new_vel = *vel; 
 if(vel->linear.x > 1.8) {
    new_vel.linear.x = 1.8; 
    vel_pub_.publish(new_vel);
  }
}

Please have a look at this tutorial) to see how subscribers and publishers work.

I would also suggest to have a queue size of at least 1: ros::Subscriber vel_sub = n.subscribe("cmd_vel", 1, velCallback);

Edit2:

For the case you mentioned in the comments something like this could work (not tested, beware of typos):

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

ros::Publisher pub;

void velCallback(geometry_msgs::Twist::ConstPtr& vel)
{
   geometry_msgs::Twist new_vel = *vel;
   if (vel->linear.x > 1.8) {
     new_vel.linear.x = 1.8;
   }
   pub.publish(new_vel);
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "my_node");
ros::NodeHandle n;

pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
ros::Subscriber sub = n.subscribe("my_cmd_vel", 10, velCallback);

ros::spin();

return 0;
}

You only have to make sure that the node publishing the original cmd_vel values publishes to my_cmd_vel now which you can achieve via remapping the topic in the launch file. The node I posted above will take that value and pass it on to cmd_vel or change the linear speed if necessary and then pass it on to cmd_vel.

Since the callback is running in a different thread than your main function you cannot rely on always having a value for your linear_actual in the main function. So do whatever you need to do with the actual values in the callback. In this easy example this is good enough.

edit flag offensive delete link more

Comments

Hi @Chrissi, do you this this will work?

void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) {

ros::NodeHandle n;
ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
geometry_msgs::Twist vel;

linear_actual=vel_msg->linear.x;
angular_actual=vel_msg->angular.z;
}
anamcarvalho gravatar image anamcarvalho  ( 2014-08-13 08:01:38 -0500 )edit

What I do in the code I inserted in the comment above is to update linear and angular velocity to the current velocities!

Then, in main function I do:

if(linear_actual > 1.8){
    vel.linear.x = 1.8;
    vel_pub_.publish(vel);}

Is this ok?

anamcarvalho gravatar image anamcarvalho  ( 2014-08-13 08:04:13 -0500 )edit

I wouldn't create the publisher in the callback because then the publisher will be created when ever a new message arrives, this is unnecessary. Even though a kitten dies every time you use global variable, I would make it a global variable. The thing with the cmd_vel is, that still both messages will reach the robot, so the one with linear.x > 1.8 and your new one and the robot will not know what to do and start stuttering. You have to remap the output of whatever publishes the cmd_vel in the first place to a new topic and listen to that. Then you pass on every message you get to cmd_vel and change it where necessary.

Chrissi gravatar image Chrissi  ( 2014-08-13 09:02:33 -0500 )edit

Sorry, my bad! I forgot to delete the publisher in the code!

void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) {

linear_actual=vel_msg->linear.x;
angular_actual=vel_msg->angular.z;
}

The code is just like this.. I just updates the current velocity!

anamcarvalho gravatar image anamcarvalho  ( 2014-08-13 09:46:31 -0500 )edit

The rest of the code, the comparison to see it its greater that 1.8 I need to have in the main, because in the real code I have, it isn't so simple! So I really need to do this comparison inside main!

Is it ok this time?

anamcarvalho gravatar image anamcarvalho  ( 2014-08-13 09:47:32 -0500 )edit

You have to make sure that callback is not writing to the variable while you are trying to read it. See this thread on how to use boost scoped lock

Chrissi gravatar image Chrissi  ( 2014-08-13 10:14:37 -0500 )edit

This updates at a rate of 40Hz... I will be a matter of milliseconds, I am assuming that shouldn't be a problem!

anamcarvalho gravatar image anamcarvalho  ( 2014-08-13 11:20:07 -0500 )edit
1

answered 2014-08-09 00:17:58 -0500

Hamid Didari gravatar image

yes you can subscribe two or more topics in one node there is example of subscribing two laser scanner and merge the data then publish the msg:

#include "sensor_msgs/LaserScan.h"
#include "ros/ros.h"

sensor_msgs::LaserScan downlaser;
ros::Publisher *pub;
bool new_downlaser=false;

void down_laser(const sensor_msgs::LaserScan& msg)
{
    new_downlaser=true;
    downlaser=msg;
}




void up_laser(const sensor_msgs::LaserScan& msg)
{
    sensor_msgs::LaserScan scan;
    scan=downlaser;
    if(new_downlaser)
    {
        ROS_INFO("I heard both");

        for(int i=0;i<msg.ranges.size();i++)
        {
            if(downlaser.ranges[i]<msg.ranges[i])
                scan.ranges.at(i)=downlaser.ranges[i];
            else
                scan.ranges.at(i)=msg.ranges[i];
        }
    }
    pub->publish(scan);
}





int main(int argc, char** argv)
{

    ros::init(argc, argv, "scan_macher");
    ros::NodeHandle nh;



    ros::Subscriber sub = nh.subscribe("upscan", 10, up_laser);
    ros::Subscriber sub1 = nh.subscribe("downscan", 10, down_laser);


    pub=new ros::Publisher(nh.advertise<sensor_msgs::LaserScan>("scan",10));
    ros::spin();
    return 0;


}
edit flag offensive delete link more

Comments

Hi @hamid, my concern now is, what do I insert in the callback function? This verification I do:

if(vel.linear.x > 1.8){
    vel.linear.x = 1.8;
    vel_pub_.publish(vel);}

I have to do it in the main, along with the rest of my code... so what do I put in the callback function?

anamcarvalho gravatar image anamcarvalho  ( 2014-08-10 11:06:25 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2014-08-08 18:28:34 -0500

Seen: 11,610 times

Last updated: Aug 13 '14