ROS底盘简单人体跟随代码

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <stdio.h>
#include"geometry_msgs/Point.h" //!!!!!

using namespace std;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int x_aim=0;///!!!!
int y_aim=0;///!!!
int z_aim=0;///!!!
/

ros::Publisher pub;
geometry_msgs::Twist vel;

void chatterCallback(const geometry_msgs::Point::ConstPtr& msg)
{
   x_aim=(int)msg->x;
   y_aim=(int)msg->y;
   z_aim=(int)msg->z;

    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.angular.z = 0;
 
   float P_vel = 1;
   float P_ang = 1;
    if(z_aim > 1000)
    {
        if(z_aim >1500)
        {
            vel.linear.x = 0.5 * P_vel;
            vel.angular.z = 0;
            if(x_aim > 100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }
            if(x_aim < -100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }
        }
        else
        {
            vel.linear.x = 0.3 * P_vel;
            vel.angular.z = 0;
            if(x_aim > 100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }
            if(x_aim < -100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }
            
        }
        if(z_aim<1000)
        {
            vel.linear.x = 0;
            vel.angular.z = 0;
            if(x_aim > 100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }
            if(x_aim < -100)
            {
                vel.angular.z = x_aim/100 * P_ang;
            }        
        }
    }
    else if(z_aim<600)
    {
        vel.linear.x = -0.3*P_vel;
        vel.angular.z = 0;        
    }

    //ROS_INFO("--------------------------------");
    //ROS_INFO("vel.linear.x:[%lf]",vel.linear.x);
    //ROS_INFO("vel.linear.y:[%lf]",vel.linear.y);
    //ROS_INFO("vel.angular.z:[%lf]",vel.angular.z);
    cout<<"-----------------------------------"<<endl;;
    cout<<"vel.linear.x:"<<vel.linear.x<<endl;
    cout<<"vel.linear.y:"<<vel.linear.y<<endl;
    cout<<"vel.angular.z:"<<vel.angular.z<<endl;

    pub.publish(vel);

 

   
}

//

int main(int argc, char** argv){

  ros::init(argc, argv, "simple_navigation_goals");
  ros::NodeHandle n;///!!!!!
  cout<<"follow start"<<endl;

  ros::Subscriber sub = n.subscribe("aim_pose",1000,chatterCallback);///!!!!!
  pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1000);
  usleep(50*1000);
  ros::spin();/  

  return 0;
}