clamp_safety/main.cpp

//
// Created by zx on 22-8-30.
//
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>

#include "detect/clamp_safety_detect.h"
#include "plc/snap7_communication_base.h"

#include <fstream>
#include "lidar/wanji_716N_device.h"
#include <byteswap.h>
#include "tool/pathcreator.h"
#include <glog/logging.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include "clamp_parameter.pb.h"
#include "tool/proto_tool.h"
#include<stdio.h>
#include<stdlib.h>
#include<unistd.h>
#include<string.h>
#include<sys/types.h>
#include<arpa/inet.h>
#include<net/if.h>
#include<string.h>
#include<signal.h>
#include<sys/wait.h>
#include<sys/ioctl.h>
#include<regex.h>

using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

int set_ip(const char* eth, const char* ip)
{
    printf("---Debug--- %s %s.\n", eth, ip);
	int sock_set_ip;     
    struct sockaddr_in sin_set_ip;     
    struct ifreq ifr_set_ip;     
 
    memset( &ifr_set_ip,0,sizeof(ifr_set_ip));
   
	sock_set_ip = socket( AF_INET, SOCK_STREAM, 0 );
	//printf("sock_set_ip=====%d\n",sock_set_ip);
    if(sock_set_ip<0)  
    {     
        perror("socket create failse...SetLocalIp!");     
        return -1;     
    }     
 
    memset( &sin_set_ip, 0, sizeof(sin_set_ip));     
    strncpy(ifr_set_ip.ifr_name, eth, sizeof(ifr_set_ip.ifr_name)-1);        
 
    sin_set_ip.sin_family = AF_INET;     
    sin_set_ip.sin_addr.s_addr = inet_addr(ip);     
    memcpy( &ifr_set_ip.ifr_addr, &sin_set_ip, sizeof(sin_set_ip));     
    printf("ip===%s\n",ip);
    if( ioctl( sock_set_ip, SIOCSIFADDR, &ifr_set_ip) < 0 )     
    {     
        perror( "Not setup interface");     
        return -1;     
    }     
 
    //设置激活标志      
    ifr_set_ip.ifr_flags |= IFF_UP |IFF_RUNNING;      
 
    //get the status of the device      
    if( ioctl( sock_set_ip, SIOCSIFFLAGS, &ifr_set_ip ) < 0 )     
    {     
         perror("SIOCSIFFLAGS");     
         return -1;     
    }     
    
    close( sock_set_ip );     
	return 0;
}

#if __VIEW__PCL
pcl::visualization::PCLVisualizer g_viewer;
#endif

GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
    time_t tt;
    time( &tt );
    tt = tt + 8*3600;  // transform the time zone
    tm* t= gmtime( &tt );
    char buf[255]={0};
    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
            t->tm_year + 1900,
            t->tm_mon + 1,
            t->tm_mday,
            t->tm_hour,
            t->tm_min,
            t->tm_sec);

    FILE* tp_file=fopen(buf,"w");
    fprintf(tp_file,data,strlen(data));
    fclose(tp_file);

}

void init_glog()
{
    time_t tt = time(0);//时间cuo
    struct tm* t = localtime(&tt);

    char strYear[255]={0};
    char strMonth[255]={0};
    char strDay[255]={0};

    sprintf(strYear,"%04d", t->tm_year+1900);
    sprintf(strMonth,"%02d", t->tm_mon+1);
    sprintf(strDay,"%02d", t->tm_mday);

    char buf[255]={0};
    getcwd(buf,255);
    char strdir[255]={0};
    sprintf(strdir,"%s/log/%s/%s/%s", buf,strYear,strMonth,strDay);
    PathCreator creator;
    creator.Mkdir(strdir);

    char logPath[255] = { 0 };
    sprintf(logPath, "%s/", strdir);
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("clamp_safety");
    google::SetStderrLogging(google::INFO);
    google::SetLogDestination(0, logPath);
    google::SetLogFilenameExtension("zxlog");
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&shut_down_logging);
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}


typedef struct
{
  Wanji_716N_lidar_device lidar;
  clamp_safety_detect  detector;
  Safety_status safety_statu;
  std::mutex mutex;
}Clamp;


float SWAP_float(float x)
{
    int b;
    memcpy(&b,&x,sizeof(float));
    int t= ((((b) & 0xff000000) >> 24) | (((b) & 0x00ff0000) >>  8) |(((b) & 0x0000ff00) <<  8) | (((b) & 0x000000ff) << 24));
    float value=0;
    memcpy(&value,&t,sizeof(float));
    return value;
}


bool run(Wanji_716N_lidar_device* device,clamp_safety_detect* pdetector,Safety_status* safety)
{
//   auto t1 = std::chrono::steady_clock::now();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  device->get_last_cloud(cloud, std::chrono::system_clock::now());

  if (cloud->size() < 100)
  {
    return false;
  }
  pdetector->detect(cloud,*safety);


//   auto t2 = std::chrono::steady_clock::now();
//   auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);

//   double time = double(duration.count()) * std::chrono::microseconds::period::num /
//           std::chrono::microseconds::period::den;

  return true;

}
void thread_func(Clamp* clamp)
{
    while(1)
    {
      clamp->mutex.lock();
        Safety_status safety_t;
      if(run(&(clamp->lidar),&(clamp->detector),&safety_t))
          clamp->safety_statu=safety_t;
      clamp->mutex.unlock();

      usleep(100*1000);
    }
}
bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3]={0};
    for(int i=0;i<3;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0];
    point.y=data[1];
    point.z=data[2];
    return true;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            printf("%f, %f, %f.\n", point.x, point.y, point.z);
            cloud->push_back(point);
        }
    }
    return cloud;
}

// int test() {
//     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = ReadTxtCloud("points.txt");
//     Line line;
//     clamp_safety_detect detector;
//     Safety_status safety;
//     int v1;
//     g_viewer.createViewPort(0.0, 0.0, 1, 1, v1);
//     g_viewer.addText("192.168.0.2", 10,10, 20, 0,1,0, "ip v1", v1);
//     detector.set_viewer(&g_viewer,v1);
//     g_viewer.addCoordinateSystem(0.2,"v1_axis",v1);
//     detector.fit_line(cloud, line);
//     detector.check_wheel_line(line,safety);
//     g_viewer.spinOnce();
//     printf("\n%f.\n", line.cx);
//     return 0;
// }

int main(int argc, char* argv[])
{
    // return test();
    clamp_safety::clamp_parameter parameter;
    std::string cfg_file = "../setting/parameter.prototxt";
    if(proto_tool::read_proto_param(cfg_file,parameter)==false)
    {
        LOG(ERROR)<<" read prototxt failed";
        return -1;
    }
    
    set_ip(parameter.local_parameter().eth_name().c_str(), parameter.local_parameter().local_ip().c_str());

    int CLAMP_SIZE=4;
   //usleep(10000*1000);
   //初始化日志
    init_glog();
    //调试模式下，先打开调试窗口
#if __VIEW__PCL
    int v1,v2,v3,v4;
    //窗口参数分别对应 x_min, y_min, x_max, y_max, viewport
    g_viewer.createViewPort(0.0, 0.5, 0.5, 1, v1);
    g_viewer.createViewPort(0.5, 0.5, 1.0, 1, v2);
    g_viewer.createViewPort(0.0, 0, 0.5, 0.5, v3);
    g_viewer.createViewPort(0.5, 0, 1.0, 0.5, v4);
    g_viewer.addText("192.168.0.2", 10,10, 20, 0,1,0, "ip v1", v1);
    g_viewer.addText("192.168.0.x2", 10,10, 20, 0,1,0, "ip v2", v2);
    g_viewer.addText("192.168.0.x3", 10,10, 20, 0,1,0, "ip v3", v3);
    g_viewer.addText("192.168.0.x4", 10,10, 20, 0,1,0, "ip v4", v4);

    g_viewer.addCoordinateSystem(0.2,"v1_axis",v1);
    g_viewer.addCoordinateSystem(0.2,"v2_axis",v2);
    g_viewer.addCoordinateSystem(0.2,"v3_axis",v3);
    g_viewer.addCoordinateSystem(0.2,"v4_axis",v4);

#endif
    //雷达
    Clamp clamps[CLAMP_SIZE];

    if(parameter.lidars_size()>CLAMP_SIZE)
    {
        printf("lidar num > viewer num\n");
        return -2;
    }

    for(int i=0;i<parameter.lidars_size();++i)
    {
        clamp_safety::LidarParameter lidar=parameter.lidars(i);
        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;
        Point2D_tool::Point2D_box t_point2D_box;
        Point2D_tool::Point2D_transform t_point2D_transform;
        t_polar_coordinates_box.angle_min = lidar.angle_min();
        t_polar_coordinates_box.angle_max = lidar.angle_max();
        t_polar_coordinates_box.distance_min = lidar.range_min();
        t_polar_coordinates_box.distance_max = lidar.range_max();

        t_point2D_box.x_min = lidar.scan_box_limit().minx();
        t_point2D_box.x_max = lidar.scan_box_limit().maxx();
        t_point2D_box.y_min = lidar.scan_box_limit().miny();
        t_point2D_box.y_max = lidar.scan_box_limit().maxy();

        Error_manager code=clamps[i].lidar.init(lidar.net_config().ip_address(),lidar.net_config().port(),
                t_polar_coordinates_box,t_point2D_box,t_point2D_transform);
        if(code!=SUCCESS)
        {
            LOG(ERROR)<<code.get_error_description();
            return -3;
        }
        clamps[i].safety_statu.set_timeout(0.3);
    }

    //调试状态下，绑定识别器输出窗口
#if __VIEW__PCL
    if (parameter.lidars_size()>0)
        clamps[0].detector.set_viewer(&g_viewer,v1);
    if (parameter.lidars_size()>1)
        clamps[1].detector.set_viewer(&g_viewer,v2);
    if (parameter.lidars_size()>2)
        clamps[2].detector.set_viewer(&g_viewer,v3);
    if (parameter.lidars_size()>3)
        clamps[3].detector.set_viewer(&g_viewer,v4);

    while(1)
    {
        usleep(100*1000);
        auto t1=std::chrono::steady_clock::now();
        //调试状态下，顺序执行，因为viewer不支持多线程
        if (parameter.lidars_size()>0)
            run(&(clamps[0].lidar),&(clamps[0].detector),&(clamps[0].safety_statu));
        if (parameter.lidars_size()>1)
            run(&(clamps[1].lidar),&(clamps[1].detector),&(clamps[1].safety_statu));
        if (parameter.lidars_size()>2)
            run(&(clamps[2].lidar),&(clamps[2].detector),&(clamps[2].safety_statu));
        if (parameter.lidars_size()>3)
            run(&(clamps[3].lidar),&(clamps[3].detector),&(clamps[3].safety_statu));
            auto t2=std::chrono::steady_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);

            double time= double(duration.count()) * std::chrono::microseconds::period::num /
                    std::chrono::microseconds::period::den;

        //printf("total time :%.3f s\n",time);
        g_viewer.spinOnce();
    }
#else
    //plc相关
    Snap7_communication_base snap7_client;
    std::string plc_ip = parameter.plc_parameter().ip_address();
    Error_manager code = snap7_client.communication_init(plc_ip);
    while (code != SUCCESS)
    {
        Snap7_communication_base::Snap7_communication_statu status = snap7_client.get_status();
        switch (status)
        {
        case Snap7_communication_base::SNAP7_COMMUNICATION_READY:
        case Snap7_communication_base::SNAP7_COMMUNICATION_RECEIVE:
            code = SUCCESS;
            break;
        
        default:
            break;
        }
        LOG(ERROR)<<code;
        sleep(1);
        // return -1;
    }

    //正式代码，多线程执行,连接plc。
    std::thread threads[CLAMP_SIZE];
    for(int i=0;i<CLAMP_SIZE;++i)
    {
      threads[i]=std::thread(thread_func,&clamps[i]);
    }

    #pragma pack(push,1)
    struct PLCData
    {
        unsigned short      pingpong;

        unsigned short      wheel_exist1;
        float               offset1;
        float               gap1;
        unsigned short      clamp_completed1;

        unsigned short      wheel_exist2;
        float               offset2;
        float               gap2;
        unsigned short      clamp_completed2;

        unsigned short      wheel_exist3;
        float               offset3;
        float               gap3;
        unsigned short      clamp_completed3;

        unsigned short      wheel_exist4;
        float               offset4;
        float               gap4;
        unsigned short      clamp_completed4;

        void info() {
            printf("pingdong = %d.\n", pingpong);
            printf("wheel_exist1 = %d, offset1 = %f, gap1 = %f, clamp_completed1 = %d.\n", wheel_exist1, offset1, gap1, clamp_completed1);
            printf("wheel_exist2 = %d, offset2 = %f, gap2 = %f, clamp_completed2 = %d.\n", wheel_exist1, offset1, gap1, clamp_completed1);
            printf("wheel_exist3 = %d, offset3 = %f, gap3 = %f, clamp_completed3 = %d.\n", wheel_exist1, offset1, gap1, clamp_completed1);
            printf("wheel_exist4 = %d, offset4 = %f, gap4 = %f, clamp_completed4 = %d.\n", wheel_exist1, offset1, gap1, clamp_completed1);
        }
    };
#pragma pack(pop)
    Snap7_buf buf;
    buf.m_communication_mode = Snap7_buf::Communication_mode::LOOP_COMMUNICATION;
    int heart=0;
    struct PLCData  plc_data;
    while(true)
    {
        usleep(100*1000);
        memset(&plc_data,0,sizeof(plc_data));
        heart=(heart+1)%255;
        plc_data.pingpong=bswap_16(heart);
        clamps[0].mutex.lock();
        if(clamps[0].safety_statu.is_timeout()==false)
        {
            plc_data.wheel_exist1=bswap_16((unsigned short)clamps[0].safety_statu.wheel_exist);
            plc_data.offset1=SWAP_float(clamps[0].safety_statu.cx*1000);
            plc_data.gap1=SWAP_float(clamps[0].safety_statu.gap*1000);
            plc_data.clamp_completed1=bswap_16((unsigned short)clamps[0].safety_statu.clamp_completed);
            //printf("safety 1  off :%.3f   gap:%.3f\n",clamps[0].safety_statu.cx,clamps[0].safety_statu.gap);
        }
      clamps[0].mutex.unlock();
      clamps[1].mutex.lock();
        if(clamps[1].safety_statu.is_timeout()==false)
        {
            plc_data.wheel_exist2=bswap_16((unsigned short)clamps[1].safety_statu.wheel_exist);
            plc_data.offset2=SWAP_float(clamps[1].safety_statu.cx*1000);
            plc_data.gap2=SWAP_float(clamps[1].safety_statu.gap*1000);
            plc_data.clamp_completed2=bswap_16((unsigned short)clamps[1].safety_statu.clamp_completed);
            //printf("safety 2  off :%.3f   gap:%.3f\n",clamps[1].safety_statu.cx,clamps[1].safety_statu.gap);
        }
      clamps[1].mutex.unlock();
      clamps[2].mutex.lock();
        if(clamps[2].safety_statu.is_timeout()==false)
        {
            plc_data.wheel_exist3=bswap_16((unsigned short)clamps[2].safety_statu.wheel_exist);
            plc_data.offset3=SWAP_float(clamps[2].safety_statu.cx*1000);
            plc_data.gap3=SWAP_float(clamps[2].safety_statu.gap*1000);
            plc_data.clamp_completed3=bswap_16((unsigned short)clamps[2].safety_statu.clamp_completed);
            //printf("safety 3  off :%.3f   gap:%.3f\n",clamps[2].safety_statu.cx,clamps[2].safety_statu.gap);
        }
      clamps[2].mutex.unlock();
      clamps[3].mutex.lock();
        if(clamps[3].safety_statu.is_timeout()==false)
        {
            plc_data.wheel_exist4=bswap_16((unsigned short)clamps[3].safety_statu.wheel_exist);
            plc_data.offset4=SWAP_float(clamps[3].safety_statu.cx*1000);
            plc_data.gap4=SWAP_float(clamps[3].safety_statu.gap*1000);
            plc_data.clamp_completed4=bswap_16((unsigned short)clamps[3].safety_statu.clamp_completed);
            //printf("safety 4  off :%.3f   gap:%.3f\n",clamps[3].safety_statu.cx,clamps[3].safety_statu.gap);
        }
      clamps[3].mutex.unlock();

    printf("cx1 = %.4f, cx2 = %.4f, cx3 = %.4f, cx4 = %.4f.\n", clamps[0].safety_statu.cx, clamps[1].safety_statu.cx, clamps[2].safety_statu.cx, clamps[3].safety_statu.cx);
        // plc_data.info();
        buf.m_id=9070;
        buf.m_size=sizeof(plc_data);
        buf.m_start_index=0;
        buf.mp_buf_reverse=&plc_data;

        code=snap7_client.write_data_buf(buf);
         if(code!=SUCCESS)
        {
            snap7_client.communication_uninit();
            usleep(500*1000);
            snap7_client.communication_init(plc_ip);
            usleep(500*1000);
            std::cout<<code<<std::endl;
        }

    }

#endif
    printf("end");
    return 0;
}