ros自定义.srv服务数据,以及服务器的使用

ros自定义.srv服务数据,以及服务器的使用

在创建出自定义服务数据之后,在使用的时候,需要包含生成的头文件:

#include <pkg_EuCluster/EuclusterCommond.h>

pkg_EuCluster为功能包名;
EuclusterCommond为srv文件中自定义的srv文件的名字。


.cpp文件中使用时应注意:

tower_service = nh.advertiseService("EuclusterCommond" , &EuClusterCore::towercallback , this);

EuclusterCommond服务中接收到request后,便调用回调函数towercallback

bool EuClusterCore::towercallback(pkg_EuCluster::EuclusterCommond::Request &req , pkg_EuCluster::EuclusterCommond::Response &res)
{
    ispubtower = req.check;
    ROS_INFO("Begin to publish tower [%s]" , ispubtower == true?"yes":"no");
    res.result = true;
    return true;
}

其中,ispubtower为定义的全局变量

bool ispubtower = false;

当接收到客户端发送的请求req.check为真时,便会将塔顶检测的消息发布出去:

int EuClusterCore::tower_detector(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr tower_pc(new pcl::PointCloud<pcl::PointXYZ>);
    pkg_EuCluster::tower tower_msg;

    for (size_t i = 0; i < in_pc->points.size(); i++)
    {
        //与激光雷达的距离大于origin_distance米的点剔除掉
        //float origin_distance = sqrt(pow(current_pc_ptr->points[i].x, 2) + pow(current_pc_ptr->points[i].y, 2));
        float origin_distance1 = in_pc->points[i].x;

        if( (in_pc->points[i].z >= -0.5) && (in_pc->points[i].z <= 2) && (origin_distance1 <= distance_max) && (origin_distance1 >= distance_min) )
        {
            pcl::PointXYZ tower_point;
            tower_point.x = in_pc->points[i].x;
            tower_point.y = in_pc->points[i].y;
            tower_point.z = in_pc->points[i].z;

            tower_pc->points.push_back(tower_point);
        }
    }

    // pcl::PointXYZ tower_pc_min;	//xyz的最小值
	// pcl::PointXYZ tower_pc_max;	//xyz的最大值
    // pcl::getMinMax3D(*tower_pc, tower_pc_min, tower_pc_max);
    float tower_max_z = -std::numeric_limits<float>::max();
    int tower_max_point_id;
    for (int i = 0; i < tower_pc->points.size(); i++)
    {
        if(tower_pc->points[i].z > tower_max_z)
        {
            tower_max_z = tower_pc->points[i].z;
            tower_max_point_id = i;
        }
    }

    if( tower_pc->points.size() >= 10)
    {
        tower_msg.point_num = tower_pc->points.size();
        tower_msg.point_x = tower_pc->points[tower_max_point_id].x;
        tower_msg.point_y = tower_pc->points[tower_max_point_id].y;
        tower_msg.point_z = tower_pc->points[tower_max_point_id].z;

        ROS_INFO("point number is : %d , tower y : %0.2f , tower z : %0.2f" , tower_msg.point_num , tower_msg.point_y , tower_msg.point_z );

    }

    if( tower_pc->points.size() < 10)
    {
        tower_msg.point_num = 0;

        ROS_INFO("point number is : %d , tower y : %0.2f , tower z : %0.2f" , tower_msg.point_num , tower_msg.point_y , tower_msg.point_z );

    }
	/***********************************************************
	*当接收到客户端发送的请求`req.check`为真时,便会将塔顶检测的消息发布出去
	************************************************************/
    if(ispubtower)
    {
        pub_tower.publish(tower_msg);
    }
    ROS_INFO("publish tower success ? [%s]" , ispubtower == true?"yes":"no");
    //pub_tower.publish(tower_msg);
    return tower_msg.point_num;

}

上一篇:# 356. 直线镜像


下一篇:2021-07-30