why does my package not enter my main loop when using rosrun?
I have written a few packages which should communicate with each other. However one of them seems to be malfunctioning entirely, this is the code at this moment.
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <pcl/common/centroid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
int right_person;
ros::Publisher man;
void AmirCallback(const std_msgs::String::ConstPtr& msg)
{
std::cout << msg;
if(msg->data.c_str() == "yes"){
right_person = 1;
std::cout << "message received \n";
}
else{
right_person = 0;
}
}
void personcallback (const sensor_msgs::PointCloud2ConstPtr& input)
{
std::cout << "responding";
if(right_person ==1){
pcl::PCLPointCloud2 inputcloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*input, inputcloud);
pcl::fromPCLPointCloud2(inputcloud, *cloud);
Eigen::Vector4f c;
pcl::compute3DCentroid<pcl::PointXYZ> (*cloud, c);
if(c[0] < 2 && c[1]>-1 &&c[1]<1){
sensor_msgs::PointCloud2 output;
pcl::PCLPointCloud2* outputcloud = new pcl::PCLPointCloud2;
pcl::toPCLPointCloud2(*cloud, *outputcloud);
pcl_conversions::fromPCL(*outputcloud, output);
man.publish(output);
std::cout << "model centroid position = "<< c[0] << c[1] << c[2] << "\n";
std::cout << "model sent \n";
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "personchecker");
ros::NodeHandle n;
ros::Subscriber subamir = n.subscribe("/amirs_code", 1, AmirCallback);
ros::Subscriber sub = n.subscribe("/possible_humans", 1, personcallback);
man = n.advertise<sensor_msgs::PointCloud2> ("/right_person", 1);
std::cout << "here";
ros::spin ();
}
However when I run this code none of the calls to std::cout are printed. When I publish to either one of the subscribed topics, with rostopic pub or another file nothing happens. I have checked the cmakelists already and it is fine, there are no errors when using catkin_make and only this file has this problem. My other packages are fine. Has anyone run into this problem before and if so what is the solution?
Thanks in advance!