note-ros-控制小海龟到达指定点

ubnutu：14.04
ros：indigo

1. 工程：catkin_ws
2. 程序包：turtle_crl
3. 涉及ros知识点：发布，订阅。
4. 效果：
   
   
5. 运行步骤：
   1. 新终端1：
      yjh@yjh-pc:~$ roscore
   2. 新终端2
      yjh@yjh-pc:~$ rosrun turtlesim turtlesim_node
   3. 新终端2
      yjh@yjh-pc:~$ source ~/catkin_ws/devel/setup.bash
yjh@yjh-pc:~$ rosrun turtle_crl turtlectrl
6. 代码文件：

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("\nThe 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("\nThe out_ctrl.linear.x:\n\t%f\nThe out_ctrl.angular.z:\n\t%f",out_ctrl.linear.x,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;
}

其他：

------- 利用launch文件同时启动小海龟和控制程序。

1. 代码文件：
   –turtle_l.launch

<launch>

    <node pkg="turtlesim" name="turtle1" type="turtlesim_node"/>
    <node pkg="turtle_crl" name="ctrl1" type="turtlectrl"/>

</launch>

或者

<launch>
<group ns="turtlespcae1">
    <node pkg="turtlesim" name="turtle1" type="turtlesim_node"/>
    <node pkg="turtle_crl" name="ctrl1" type="turtlectrl"/>
</group>
</launch>

两者的不同之处在于，后者新命名了一个新的命名空间“turtlespcae1”

2. 执行步骤：
   1. 新终端1：
      yjh@yjh-pc:~$ roscore
   2. 新终端2
      yjh@yjh-pc:~$ source ~/catkin_ws/devel/setup.bash
yjh@yjh-pc:~$ roslaunch turtle_crl turtle_l
      此时可以发现出现了小海龟，并且执行了程序。
3. 问题：
   没有跳出新的终端现实程序中输出
   的文字消息。