ROS底盘简单人体跟随代码

最新推荐文章于 2024-07-08 16:47:37 发布

原创最新推荐文章于 2024-07-08 16:47:37 发布 · 1.5k 阅读

5 ·

CC 4.0 BY-SA版权

ros基础学习专栏收录该内容

55 篇文章

订阅专栏

本文介绍了一个基于ROS的机器人导航控制系统，通过订阅目标位置并调整线速度和角速度来实现对机器人的精确控制。系统使用了MoveBaseAction和Twist消息类型，通过PID控制器调整速度，确保机器人能够稳定地接近目标。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

#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;
}