如何将以下结果输出到终端 [英] How to output the following result to terminal

查看:168
本文介绍了如何将以下结果输出到终端的处理方法,对大家解决问题具有一定的参考价值,需要的朋友们下面随着小编来一起学习吧!

问题描述

我正在使用代码位置计算.那么如何在终端中显示输出结果呢?

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;
}

我添加了 ROS_INFO("x: %fy: %fz: %f", punto.x,punto.y,punto.z); 尝试将结果输出到终端但是它似乎无法正常工作.有没有其他方法可以将结果输出到终端?

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?

推荐答案

您应该按照 rosconsole wiki 的>配置部分.有几种设置方法取决于您启动节点的方式.

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.

您可以从配置文件 (custom_rosconsole.conf) 中进行设置:

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

然后使用此命令行(在启动节点之前!)将其导出到每个终端中的ROSCONSOLE_CONFIG_FILE环境变量:

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>

或者直接从您的 C++ 源代码(如果需要):

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();
}

这篇关于如何将以下结果输出到终端的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!

查看全文
登录 关闭
扫码关注1秒登录
发送“验证码”获取 | 15天全站免登陆