ubnutu：14.04 ros：indigo

1. 工程：catkin_ws

2. 程式包：turtle_crl

3. 檔案：

   1.CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(turtle_crl)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)
## Declare a catkin package
catkin_package()
## Build talker and listener
include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(turtlectrl src/turtlectrl.cpp)
target_link_libraries(turtlectrl ${catkin_LIBRARIES})
add_dependencies(turtlectrl turtle_crl_generate_messages_cpp)
 

2. package.xml

<?xml version="1.0"?>
<package format="2">
  <name>turtle_crl</name>
  <version>0.0.0</version>
  <description>The turtle_crl package</description>

  <maintainer email="[email protected]">yjh</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <export>

  </export>
</package>
 

3.turtlectrl.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include<geometry_msgs/Twist.h>
#include<turtlesim/Pose.h>
#include<turtlesim/Color.h>
#include<math.h>

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 x,y,theta,lv,av;
  float err_x,err_y,err_theta,err_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("The pose:\n\tx:%f \n\ty:%f \n\ttheta:%f \n\tlv:%f \n\tav:%f",x,y,theta,lv,av);
	//計算
  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<pi)  out_ctrl.angular.z=-pi;
  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("The hopo theta:%f\n The err_theta:%f\n The out_ctrl.angular.z:%f",hope_pose.theta,err_theta,out_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<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
  ros::spin();

  return 0;
}