ros小海龟定点移动程序

此文件包为控制ros中小海龟的定点移动，其中有两个程序，大同小异。一个需要输入目标点的xy坐标，一个是在程序中固定了xy的坐标。具体情感博客：note-ros-控制小海龟到达指定点 （https://blog.csdn.net/qq_33168256/article/details/82950222）

#include “ros/ros.h“
#include “std_msgs/String.h“
#include
#include
#include
#include

using namespace std;

#define ox 7
#define oy 7
#define pi 3.1415926

int irate=0;
ros::Publisher pub;
void Callback（const turtlesim::Pose& pose）
{
  float xythetalvav;
  float err_xerr_yerr_thetaerr_dis;
  geometry_msgs::Twist out_ctrl;
  turtlesim::Pose hope_pose;
	//初始化
  out_ctrl.linear.x=1.0;
  out_ctrl.linear.y=0.0;
  out_ctrl.linear.z=0.0;
  out_ctrl.angular.x=0.0;
  out_ctrl.angular.y=0.0;
  out_ctrl.angular.z=0.0;
  x	=  pose.x;
  y	=  pose.y;
  theta	=  pose.theta;
  if（theta<0）theta=2*pi+theta;
  lv	=  pose.linear_velocity;
  av	=  pose.angular_velocity;
  ROS_INFO（“\nThe pose:\n\tx:%f \n\ty:%f \n\ttheta:%f \n\tlv:%f \n\tav:%f“xythetalvav）;
	//计算
  err_x=ox-x;
  err_y=oy-y;
  err_dis=err_x*err_x+err_y*err_y;

  if	 （err_x==0&&err_y==0）  hope_pose.theta=0;
  else if（err_x>0&&err_y==0）  hope_pose.theta=0;
  else if（err_x>0&&err_y>0）  hope_pose.theta=atan（err_y/err_x）;
  else if（err_x==0&&err_y>0）  hope_pose.theta=pi/2;
  else if（err_x<0&&err_y>0）  hope_pose.theta=atan（err_y/err_x）+pi;
  else if（err_x<0&&err_y==0）  hope_pose.theta=pi;
  else if（err_x<0&&err_y<0）  hope_pose.theta=atan（err_y/err_x）+pi;
  else if（err_x==0&&err_y<0）  hope_pose.theta=pi/2+pi;
  else if（err_x>0&&err_y<0）  hope_pose.theta=atan（err_y/err_x）+2*pi;

  err_theta=theta-hope_pose.theta;
  if（err_theta>pi）{
    err_theta=-（2*pi-err_theta）;
  }
  else if（err_theta<-pi）{
    err_theta=2*pi+err_theta;
  }
	//附值
  if（err_theta<-0.05 && err_theta>-pi）  out_ctrl.angular.z=pi;
  else if（err_theta>0.05 && err_theta  else out_ctrl.angular.z=0;
  
  if（abs（err_dis）>0.01 && out_ctrl.angular.z==0）out_ctrl.linear.x=5;
  else out_ctrl.linear.x=0.0;
  
  ROS_INFO（“\nThe out_ctrl.linear.x:\n\t%f\nThe out_ctrl.angular.z:\n\t%f“out_ctrl.linear.xout_ctrl.angular.z）;
  pub.publish（out_ctrl）;

  irate=0;
}

int main（int argc char **argv）
{
  ros::init（argc argv “turtlectrl“）;
  ros::NodeHandle n1;
  ros::Subscriber sub = n1.subscribe（“turtle1/pose“ 1000 Callback）;

  pub = n1.advertise（“turtle1/cmd_vel“ 1000）;
  ros::spin（）;

  return 0;
}