如何将以下结果输出到终端

Question

我正在使用代码 position calculation。那么如何在终端中显示输出结果呢？

#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: %f y: %f z: %f", punto.x,punto.y,punto.z); 以尝试将结果输出到终端，但它似乎无法工作。还有其他方法可以将结果输出到终端吗？

Answer 1

您应该按照 rosconsole wiki 的 Configuration Section 中的说明设置控制台详细级别。有几种设置方法取决于您启动节点的方式。

您可以从配置文件中进行设置 (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环境变量，使用此命令行（在启动节点之前！）：

export ROSCONSOLE_CONFIG_FILE=<config_file_path>/custom_rosconsole.conf

否则你可以在你的启动文件中设置这个属性（如果你有的话）然后忘记它：

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

或直接从您的 C++ 源代码（如果需要）：

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

如何将以下结果输出到终端

How to output the following result to terminal

ros

date-arithmetic

surf

object-detection