Ask Your Question
0

how to have subs and publisher in one single cpp

asked 2017-06-02 18:45:15 -0600

brucewang gravatar image

i see some posts about writting the subs and publi in on cpp, but for my code, I will subs some points position from the camera and publish it with 10*those coordinate how do i do that ? (Say my ps4->pose.position.x is 10, i want to have a topic called "positions" which will publish this coordinate to be 100)please help

#include "ros/ros.h"
#include <math.h>
#include <opencv/cv.h>
#include <opencv/cvaux.h>
#include <opencv/highgui.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseWithCovariance.h"
#include <visp/vpDisplayGDI.h>

#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
#include <visp/vpImageIo.h>
#include <visp/vpPixelMeterConversion.h>
#include <visp/vpPose.h>
using namespace std;
using namespace cv;
void imageCallback(const geometry_msgs::PoseStamped::ConstPtr& ps4){

cout<<"pose is"<<ps4->pose.position<<endl;
//ps_cov->publish(ps);
}















int main(int argc, char **argv){
    geometry_msgs::PoseStamped my_vidoe2;
    ros::init(argc, argv, "coordinator");

    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe<geometry_msgs::PoseStamped>("/visp_auto_tracker/object_position", 1000, imageCallback);
    ros::Publisher positions_pub = n.advertise<geometry_msgs::PoseStamped>("positions", 1000);



ros::Rate loop_rate(30);
        int count=0;
    while (ros::ok())
    {
            double x;



    geometry_msgs::PoseStamped msg;



ros::spinOnce();

        loop_rate.sleep();
        ++count;
        }

i know this is a partial code, please help~~ i am new to ros, please explain in details, thanks ~

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-06-03 03:34:03 -0600

Haibo gravatar image

updated 2017-06-03 03:40:11 -0600

A simple example is shown below.

  1. Defined a class call TestRos, which has a subscriber and a publisher.
  2. In the main function, declared a variable test_ros with the type of TestRos.
  3. Using ros::spin() to handle callback function.
  4. In the TestRos class, we defined a callback function called imageCallback, where we receive input message and do some processing and then publish the output message.

I removed unnecessary header files in your original code to make it compilable. You can add them back to integrate with opencv libraries. In your original code example, the message queue size is 1000. However, if you want to use the latest message, you may change it to 1.

Please correct me if I am wrong. Thanks.


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
class TestRos
{
  ros::NodeHandle n;
  ros::Subscriber positions_sub; 
  ros::Publisher positions_pub;

  geometry_msgs::PoseStamped output_msg;

  void imageCallback(const geometry_msgs::PoseStamped::ConstPtr& ps4){
    output_msg.pose.position.x = 10 * ps4->pose.position.x;
    output_msg.pose.position.y = 10 * ps4->pose.position.y;
    output_msg.pose.position.z = 10 * ps4->pose.position.z;
    positions_pub.publish(output_msg);  
  } 

public:
  TestRos(){
    positions_sub = n.subscribe<geometry_msgs::PoseStamped>("/visp_auto_tracker/object_position", 1000, &TestRos::imageCallback, this);
    positions_pub = n.advertise<geometry_msgs::PoseStamped>("positions", 1000);  
  }

  ~TestRos(){
  }

};


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

  ros::init(argc, argv, "coordinator");
  TestRos test_ros;  

  ros::spin();
  ros::shutdown();
  return 0;
}
edit flag offensive delete link more

Comments

Hi,Haibo.I followed your code and compiled successfully,but the publisher did not work.When I try to echo the topic,It give me the responsecannot load msg class[xx],did your msg built? and I confirm that the callback function worked,I just suspect the codepub.publish()did not work.

bobpop gravatar imagebobpop ( 2017-06-13 00:57:31 -0600 )edit

I just wonder that it is right to putpub.publish()in the callback function? I have double checked my cmake file,it seem that no error in it.I rostopic info mytopic and it could display the publisher and subscriber rightly.It seems that the publisher didn't publish data.

bobpop gravatar imagebobpop ( 2017-06-13 01:03:31 -0600 )edit

Hi, callback is just a function. It is fine to put the pub inside a function. I did not use any customized messages in my code. The message type is geometry_msgs/PoseStamped which is in the ros package geometry_msgs. Can you please show me the cmake file you use?

Haibo gravatar imageHaibo ( 2017-06-13 08:29:16 -0600 )edit

I have found the error finally,I just simulated your code.My message is built by me,when I changed the msg I could get the right result.It is obviously that there are some errors occured from my message.Thanks,Haibo.You are right!

bobpop gravatar imagebobpop ( 2017-06-13 09:01:46 -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

Stats

Asked: 2017-06-02 18:45:15 -0600

Seen: 259 times

Last updated: Jun 03 '17