How to update the topic in callback function?

asked 2016-09-17 13:38:54 -0600

sean819 gravatar image

I want to write a program to subscribe two topics.And use these two topics in one callback function.

Here is my code

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "moveit_msgs/DisplayTrajectory.h" 
#include "trajectory_msgs/JointTrajectory.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include <iostream>
#include <math.h>

using namespace std;

class Controller{
    public:
        Controller();
    private:
        ros::NodeHandle n;
        ros::Subscriber curr_pose_sub;
        ros::Subscriber traj_sub;
        ros::Publisher local_pub;
        moveit_msgs::DisplayTrajectory traj_msg;
        geometry_msgs::PoseStamped curr_pose_msg,next_posi;

        void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& pose){
        curr_pose_msg.pose = pose->pose;
        }

        void traj_callback(const moveit_msgs::DisplayTrajectory::ConstPtr& traj){
            if(traj->trajectory[0].multi_dof_joint_trajectory.points.size()>0){  
                for(int i=0;i < traj->trajectory[0].multi_dof_joint_trajectory.points.size(); ){
                    float curr_pose = sqrt(pow(curr_pose_msg.pose.position.x,2) + pow(curr_pose_msg.pose.position.y,2) + pow(curr_pose_msg.pose.position.z,2) )    ;
                    float traj_pose = sqrt(pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.x,2) + pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.y,2) + pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.z,2) )    ;

                    if(abs(curr_pose-traj_pose)<0.1 ){
                        i++; 
                    }
                    else {  
                        next_posi.pose.position.x =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.x;
                        next_posi.pose.position.y =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.y;
                        next_posi.pose.position.z =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.z;
                        next_posi.header =  traj-> trajectory[0].multi_dof_joint_trajectory.header;
                        print_curr_pose();
                        print_next_pose();
                        local_pub.publish(next_posi);
                    }
                }
            }

        }

        void print_curr_pose(){
        ROS_INFO("Seq: [%d]", curr_pose_msg.header.seq);
        ROS_INFO("Current Position-> x: [%f], y: [%f], z: [%f]", curr_pose_msg.pose.position.x,curr_pose_msg.pose.position.y, curr_pose_msg.pose.position.z);
        }

        void print_next_pose(){
        ROS_INFO("Seq: [%d]", next_posi.header.seq);
        ROS_INFO("Next Position-> x: [%f], y: [%f], z: [%f]", next_posi.pose.position.x,next_posi.pose.position.y, next_posi.pose.position.z);
        }

};

Controller::Controller(){
    curr_pose_sub = n.subscribe("/rtabmap/pose_cov", 1, &Controller::pose_callback, this);
    traj_sub = n.subscribe("/move_group/display_planned_path", 1, &Controller::traj_callback, this);
    local_pub = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1, true);

}

int main(int argc, char **argv){
  ros::init(argc, argv, "controller");
  Controller controller;
  ros::spin();

  return 0;
}

But curr_pose_msg is always the same in void traj_callback.I think maybe is in for loop so curr_pose_msg will not update. Is there anyone can help me with how to update curr_pose_msg in for loop?

edit retag flag offensive close merge delete

Comments

Where is the curr_pose_msg passed to traj_callback?

A_KN gravatar imageA_KN ( 2016-10-12 16:46:11 -0600 )edit

Hi,I try some methods. But I still don't know how to pass curr_pose_msg to traj_callback,I pass successfully with add curr_pose_sub=n.subscribe("/rtabmap/pose_cov", 1,boost::bind(&Controller::pose_callback, this,_1)); in for loop of callback.But the curr_pose_msg would not update in traj_callback.

sean819 gravatar imagesean819 ( 2016-10-17 11:10:19 -0600 )edit
  1. void traj_callback(const moveit_msgs::DisplayTrajectory::ConstPtr& traj, const geometry_msgs::PoseStamped::ConstPtr& pose) { curr_pose_msg.pose = pose->pose;...}
  2. curr_pose_sub=n.subscribe("/rtabmap/pose_cov", 1,boost::bind(&Controller::traj_callback, this,_1)); 3.Delete pose_callback
A_KN gravatar imageA_KN ( 2016-10-17 12:55:40 -0600 )edit

I would sync the topics using message fiters and then use sync.registerCallback(boost::bind(&traj_callback, _1, _2));

A_KN gravatar imageA_KN ( 2016-10-17 12:58:50 -0600 )edit

Ah,i got it with the first method.I will try it later. I had seen message filter implement before.But I was a little confuse of how it work and how to write message filter in this case.So I am learning how to use message filter now. Thank you a lot.

sean819 gravatar imagesean819 ( 2016-10-18 11:58:11 -0600 )edit