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
#include
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("/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?
↧