基于UWB和激光雷达的智能跟随与避障系统

#include <ros/ros.h>
#include "std_msgs/String.h"
#include <math.h>
#include <iostream>
#include <geometry_msgs/Pose.h>
#include "message_filters/subscriber.h"
#include "geometry_msgs/Twist.h"

#include <std_msgs/Header.h>
#include <leg_tracker/Person.h>
#include <leg_tracker/PersonArray.h>
#include <std_msgs/Header.h>
#include <sstream>

using namespace  std;
ros::Publisher cmdpub_;

bool start_follower = false;

float global_x = 0;
float global_y = 0;
int   master_id = 0;
float euclidean_Distance = 100;

void poseCB(const leg_tracker::PersonArrayConstPtr& ptr)
{
    std_msgs::Header header;
    header = ptr->header;

    stringstream ss;
    //int seq;
    //ss << "---" << endl << "header:" << endl << ptr->header << "people:" << endl << " - " << endl;
    std::vector<int>::size_type size1 = ptr->people.size();    //people is a array

    if(start_follower == false)
    {
      ROS_INFO("start_follower is close ");
      euclidean_Distance = 100;
        for (unsigned i=0; i<size1; i++)
        {
            ss << ptr->people[i];
            //master_id = ptr->people[i].id;
            global_x = ptr->people[i].pose.position.x;
            global_y = ptr->people[i].pose.position.y;

            float dst;
            dst = sqrt(global_x * global_x + global_y * global_y);
            if(euclidean_Distance > dst)     //get minimum distance
            {
                euclidean_Distance = dst;
                master_id = ptr->people[i].id;
            }
        }
        if(euclidean_Distance < 0.6)   //start condition
        {
          start_follower = true;
           ROS_INFO("start_follower is opening ");
        }
     }

    else
    {
        int cnt = 0;
        for (unsigned k=0; k<size1; k++)
        {
            if(ptr->people[k].id == master_id)
            {
                global_x = ptr->people[k].pose.position.x;
                global_y = ptr->people[k].pose.position.y;
                ROS_INFO("master_id:%d",master_id);
                float distance;
                distance = sqrt(global_x * global_x + global_y * global_y);
                if(distance > 0.7 && distance < 2.6)
                {
                    geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
                    cmd->linear.x = distance/1.6*0.8;                            //maxmum v is 0.8m/s
                    //cmd->linear.x = 0.6;
                    cmd->angular.z = -atan2(global_y,global_x);
                    cmdpub_.publish(cmd);
                    ROS_INFO("linear:%f,angular:%f",cmd->linear.x,cmd->angular.z);

                }
                else
                {
                    geometry_msgs::TwistPtr cmd_stop(new geometry_msgs::Twist());
                    cmd_stop->linear.x = 0;
                    cmd_stop->angular.z = 0;
                    cmdpub_.publish(cmd_stop);
                    ROS_INFO("linear:%f,angular:%f",cmd_stop->linear.x,cmd_stop->angular.z);
                }
            }
            else
            {
                cnt++;
            }
        }
        if(cnt == size1)
        {
          start_follower = false;
          ROS_INFO("start_follower is closing");
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "master_pose");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("people_tracked", 1000, poseCB);
    cmdpub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel",1,true);

    while(ros::ok())
    {
       ros::spinOnce();
    }
    return 0;
}