Ask Your Question
0

ROS Publishing On a Topic, But Not With the Right Values

asked 2019-11-04 16:12:00 -0600

Fisherman21 gravatar image

updated 2019-11-04 21:50:16 -0600

Hello! I'm new to ROS, so apologies if this is just due to me doing something dumb, but I can't figure out what's going on with my publisher. I wrote a simple node in C++ that reads geometry_msgs::PoseStamped messages from one topic, duplicates them, and just publishes them directly to another topic. At least, that's what it's supposed to do: my node successfully advertises on the new topic and publishes PoseStamped messages, but the messages it publishes are unrelated to the messages it reads in. The values in the PostStamped messages are different for each message, but after the first message (which I think matches the first message read by the node) they appear to stay within 0.1% of the values in the first message, regardless of how the messages read by the node vary over time. Here is my code:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

geometry_msgs::PoseStamped current;
bool ready_to_transmit = false;
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
  current = *msg;
  ready_to_transmit = true;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "vicon_node");
  ros::NodeHandle nh;

  ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
    ("vrpn_client_node/quadpeter/pose", 1, pose_cb);
  ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>
    ("mavros/vision_pose/pose", 1);

  bool printed = false;

  while (ros::ok()) {
    ros::spinOnce();

    if (!ready_to_transmit) continue;

    if (!printed) {
      ROS_INFO("Ready to transmit position data");
      printed = true;
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = current.pose.position.x;
    pose.pose.position.y = current.pose.position.y;
    pose.pose.position.z = current.pose.position.z;
    pose.pose.orientation.x = current.pose.orientation.x;
    pose.pose.orientation.y = current.pose.orientation.y;
    pose.pose.orientation.z = current.pose.orientation.z;
    pose.pose.orientation.w = current.pose.orientation.w;

    pose_pub.publish(pose);
  }

  return 0;
}

I am using rostopic echo to check the messages read in and published out. EDIT: I am away from the Up board for a couple days and will post the terminal window contents when I get a chance, but in the meantime if anyone's seen this problem before or sees any issues with the code I'd really appreciate any tips. Thank you!

I am using Ubuntu 18.04 on an Up board (sort of like a suped-up Raspberry Pi) with kernel 4.15.0 for Up.

Probably not critical to my question, but for context, the ultimate goal for this node is to read data from a Vicon system, do simple transformations on it, and send it to PX4 via MAVROS for external position estimation so that I can control a quadrotor drone with motion capture. I am reading position data from the vrpn_client_ros package using its sample.launch file.

edit retag flag offensive close merge delete

Comments

1

Rather than uploading screenshots, it would be better to copy the contents of your terminal window (since it's just text) and paste it into a code block.

jschornak gravatar imagejschornak ( 2019-11-04 20:49:26 -0600 )edit

As a high-level comment/observation: the code the OP shows implements a periodic sampling system on top of an event-based datastream (and needs book keeping variables (ie: ready_to_transmit to be able to recognise when there is no "new data") . The code posted by @jschornak makes proper use of the event-based nature of all of this, by only doing work when there is work to be done (ie: when a message has come in).

Both have their (dis)advantages, neither is necessarily better or worse than the other approach (but they do have to implemented correctly, or things won't work).

gvdhoorn gravatar imagegvdhoorn ( 2019-11-05 01:34:48 -0600 )edit

@gvdhoorn The ready_to_transmit variable is actually because in the final application, I will need to read and store an initial pose and then compare all subsequent poses to the initial pose as part of the transformation. So I need some sort of flag to trigger a switch in the callback function so that I go from storing the initial pose in one variable (e.g. initial_pose) to storing all subsequent poses in another variable (e.g. current_pose).

Fisherman21 gravatar imageFisherman21 ( 2019-11-05 22:27:21 -0600 )edit

No flag needed I believe: rospy.wait_for_message(..).

gvdhoorn gravatar imagegvdhoorn ( 2019-11-06 01:14:06 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-11-11 15:21:59 -0600

Fisherman21 gravatar image

Okay, I think I figured it out. I think it was the result of some combination of two factors, one ROS-related and one embarrassingly not. The non-ROS problem: I had a Vicon motion-capture system publishing to the vrpn_client_node/quadpeter/pose topic, and somehow the Vicon software kept getting paused. Even when I made sure the live-stream was on, though, it still didn't work properly -- until I added a ros::Rate object and a ros::Rate::sleep() function call to the while loop.

My best guess is that I was calling publish() faster than ROS could actually publish the messages, and so the publish requests kept getting overwritten or something like that. When I added code so that it published at a steady rate of 100 times per second, the code worked as expected.

Thank you all for your help. It was useful to know that my code was written correctly (except for publishing faster than ROS could handle), and I appreciate the tips for eliminating global variables.

edit flag offensive delete link more
0

answered 2019-11-04 21:28:11 -0600

jschornak gravatar image

Using global variables isn't good practice and is probably the source of the problems you're having now.

I'd suggest going about this in a different way. A better option is to make a class with the publisher and subscriber as class members. The main advantage is that you can publish the "transformed" message directly from the subscriber callback. This will also be much easier to expand and troubleshoot as you add functionality to your node.

Here's a minimal example, and here's a tutorial showing how to do this:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

class PoseTransformer
{
public:
    PoseTransformer(ros::NodeHandle &nh)
    {
        pose_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("vrpn_client_node/quadpeter/pose", 1, &PoseTransformer::pose_cb, this);
        pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("mavros/vision_pose/pose", 1);
    }

private:
    void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        geometry_msgs::PoseStamped pose_transformed = *msg;
        // do transforms, etc. here
        pose_pub_.publish(pose_transformed);
    }

    ros::Subscriber pose_sub_;
    ros::Publisher pose_pub_;
};


int main(int argc, char** argv) {
  ros::init(argc, argv, "vicon_node");
  ros::NodeHandle nh;
  PoseTransformer pose_transformer(nh);
  ros::spin();
  return 0;
}

Things may need to change a bit depending on the specific needs of your application, e.g. if you need to publish at a different rate than your subscriber.

edit flag offensive delete link more

Comments

Thank you, I am away from my Up board at the moment but should have access tomorrow, and I'll try your suggestion then to see if eliminating global variables solves the problem.

Fisherman21 gravatar imageFisherman21 ( 2019-11-05 22:28:16 -0600 )edit

You may have to reevaluate your testing procedure too. The code as posted should be working fine, global variables or not. In particular, there's no possibility of a message you publish being any different from a message received from the subscriber. I would suggest running a low-frequency manual message publisher on the command line (rostopic pub) or with rqt to verify this.

jdlangs gravatar imagejdlangs ( 2019-11-06 16:26:20 -0600 )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

3 followers

Stats

Asked: 2019-11-04 16:12:00 -0600

Seen: 76 times

Last updated: Nov 11