Subscriber and publisher in a file
I am using ubuntu 16.04 kinetic
I am trying to get pose information from Phantom omni device and send the pose information to my robotic arm in gazebo.
For now, my subscribe node works well but I would like to add publisher part in the same file.
Do I need to use different nodeHandle with the subscriber for the publisher?
Can I put the following line things into the main function and use the callback function for the publisher?
ros::Publisher endPose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100, popub_callback);
Here is my subscribe node to get pose information from Phantom omni
#include "ros/ros.h"
#include <sstream>
#include "geometry_msgs/PoseStamped.h"
#include <vector>
std::vector<geometry_msgs::PoseStamped::ConstPtr> pose;
double x_current = 0;
double y_current = 0;
double z_current = 0;
double x_quat = 0;
double y_quat = 0;
double z_quat = 0;
double w_quat = 0;
void po_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
ROS_INFO_STREAM("Received pose: " << msg);
x_current = msg->pose.position.x;
y_current = msg->pose.position.y;
z_current = msg->pose.position.z;
x_quat = msg->pose.orientation.x;
y_quat = msg->pose.orientation.y;
z_quat = msg->pose.orientation.z;
w_quat = msg->pose.orientation.w;
ROS_INFO_STREAM(x_current);
ROS_INFO_STREAM(y_current);
ROS_INFO_STREAM(z_current);
ROS_INFO_STREAM(x_quat);
ROS_INFO_STREAM(y_quat);
ROS_INFO_STREAM(z_quat);
ROS_INFO_STREAM(w_quat);
// pose.push_back(msg);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "pose_subscriber");
ros::NodeHandle nh;
ros::Subscriber endPose_sub = nh.subscribe("/omniEthernet/pose/", 100, po_callback);
ros::spin();
return(0);
}