ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

why does my package not enter my main loop when using rosrun?

asked 2018-04-24 09:15:00 -0500

gilprog gravatar image

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!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-04-24 13:19:14 -0500

lucasw gravatar image

updated 2018-04-24 13:39:43 -0500

Put line breaks "\n" (or std::endlhttps://stackoverflow.com/questions/2... ) at the end of your std::cout calls.

You probably also want to dereference msg otherwise you'll only get a point location in hexadecimal:

std::cout << *msg << "\n";

ROS_INFO_STREAM also would work and ends up in a log file among other features, and doesn't need a linebreak.

Additionally, comparing a char* c_str() to a char* isn't what you want, just omit c_str() and do a string to string (automatically created from the char* "yes") comparison: https://stackoverflow.com/questions/2...

edit flag offensive delete link more

Question Tools

Stats

Asked: 2018-04-24 09:15:00 -0500

Seen: 116 times

Last updated: Apr 24 '18