How to output the following result to terminal

Question

I am using code position calculation. So how could I show the output the result in the terminal?

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <QTransform>
#include <geometry_msgs/Point.h>
#include <std_msgs/Int16.h>
#include <find_object_2d/PointObjects.h>
#include <find_object_2d/Point_id.h>

#define dZ0 450
#define alfa 40
#define h 310
#define d 50
#define PI 3.14159265


 void objectsDetectedCallback(const std_msgs::Float32MultiArray& msg)
{
int x,y,z;
ros::NodeHandle nh;
ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);

find_object_2d::PointObjects p_objects;
find_object_2d::Point_id objeto;

p_objects.objeto = std::vector<find_object_2d::Point_id>(msg.data.size()/12);

for(unsigned int i=0; i<msg.data.size(); i+=12)
{
    // get data
    int id = (int)msg.data[i];
    float objectWidth = msg.data[i+1];
    float objectHeight = msg.data[i+2];

    // Find corners Qt
    QTransform qtHomography(msg.data[i+3], msg.data[i+4], msg.data[i+5],
                            msg.data[i+6], msg.data[i+7], msg.data[i+8],
                            msg.data[i+9], msg.data[i+10], msg.data[i+11]);

    QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
    QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
    QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
    QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));

    geometry_msgs::Point punto;

    float widthTop = sqrt(pow(qtTopRight.x() - qtTopLeft.x(),2) + pow(qtTopRight.y() - qtTopLeft.y(),2));
    float widthBottom = sqrt(pow(qtBottomRight.x() - qtBottomLeft.x(),2) + pow(qtBottomRight.y() - qtBottomLeft.y(),2));
    float heightLeft = sqrt(pow(qtBottomLeft.x() - qtTopLeft.x(),2) + pow(qtBottomLeft.y() - qtTopLeft.y(),2));
    float heightRight = sqrt(pow(qtBottomRight.x() - qtTopRight.x(),2) + pow(qtBottomRight.y() - qtTopRight.y(),2));

    float dArea_0 = (objectHeight*objectWidth) - (((widthTop + widthBottom)/2) * ((heightLeft + heightRight)/2));

    float dZ_0 = dZ0 + (dArea_0/10);

    float dY_0 = (((480/2) - (((qtTopLeft.y() + qtTopRight.y())/2) + ((heightLeft + heightRight)/4)))*dZ_0)/585;                

    float beta_0 = atan2(dY_0,dZ_0);        

    objectHeight = objectHeight/cos((alfa*PI)/180);

    float height = ((heightLeft + heightRight)/2)/cos(((alfa*PI)/180)-beta_0);                    

    float dArea = (objectHeight*objectWidth) - (((widthTop + widthBottom)/2) * height);

    float dZ = dZ0 + (dArea/38);

    float dX = (((640/2) - (((qtTopLeft.x() + qtBottomLeft.x())/2) + ((widthTop + widthBottom)/4)))*dZ)/585;

    float dY = (((480/2) - (((qtTopLeft.y() + qtTopRight.y())/2) + ((heightLeft + heightRight)/4)))*dZ)/585;

    float beta = atan2(dY,dZ);

    punto.x = dX;
    punto.y = h-((dZ/cos(beta))*sin(((alfa*PI)/180)-beta));
    punto.z = ((dZ/cos(beta))*cos(((alfa*PI)/180)-beta))-d;
    ROS_INFO("x: %f y: %f z: %f", punto.x,punto.y,punto.z);

    //Validate detection
    int paralelepipedo;

    if (abs(widthTop - widthBottom) < 20 && abs(heightLeft - heightRight) < 15)
    {
                    paralelepipedo = 1;
            }
            else
            {
                    paralelepipedo = 0;
            }

    if (paralelepipedo == 1)
    {       
                    objeto.punto = punto;
                    objeto.id = id;

                    p_objects.objeto[i/12] = objeto;
    }

}

position_pub_.publish(p_objects);

}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "objects_detected");

ros::NodeHandle nh;
ros::Subscriber subs = nh.subscribe("objects", 1, objectsDetectedCallback);

ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);  

ros::spin();


return 0;
}

I added ROS_INFO("x: %f y: %f z: %f", punto.x,punto.y,punto.z); to try to output the result out to the terminal however it can't seem to work. Is there any other way to output the result to the terminal?

Answer 1

You should set the console verbosity level as explained in the Configuration Section of the rosconsole wiki. There are few ways to set it up depending on how you launch your node.

You could set it from a configuration file (custom_rosconsole.conf):

# Set the default ros output to warning and higher
log4j.logger.ros=WARN
# Override my package to output everything (Change the level to fit your needs)
log4j.logger.ros.my_package_name=DEBUG

And then export it to ROSCONSOLE_CONFIG_FILE enviromental variable in every terminal you use, with this command line (before starting the node!):

export ROSCONSOLE_CONFIG_FILE=<config_file_path>/custom_rosconsole.conf

Otherwise you could set this property in your launch file (if you have one) and forget about it:

<launch>
  <env name="ROSCONSOLE_CONFIG_FILE" 
       value="$(find mypackage)/custom_rosconsole.conf"/>
  <node pkg="mypackage" type="mynode" name="mynode" output="screen"/>
</launch>

Or directly from your C++ source code (if you need to):

#include <ros/console.h>
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { // Change the level to fit your needs
   ros::console::notifyLoggerLevelsChanged();
}