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