#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; }
基于UWB和激光雷达的智能跟随与避障系统
猜你喜欢
转载自blog.csdn.net/zhuoyueljl/article/details/78193442
今日推荐
周排行