一、实验目的

• 1.进一步了解ROS通信机制；
• 2.了解Turtlebot各个节点之间的关系；
• 3.熟悉使用ROS消息类型；
• 4.了解小车闭环控制。
• 5.了解rviz是如何将目标点发送出去的。

  二、实验环境

  Ubuntu16.04+ROS 。

  三、实验原理

  发布者订阅者实现，发布者发出目标点，订阅者接受到后控制Turtlebo进行导航。

  四、实验内容

• 1.获取rviz发送目标点的topic；
• 2.对已经建好的图获取相应目标点的坐标（多个，即小车要去的目标），还没建图先完成建图；
• 3.查阅资料，编写发布一目标点的python或c脚本；
• 4.编写发布多个目标点的python或c脚本。

  五、实验步骤

  1.获取rviz发送目标点的topic；

2.对已经建好的图获取相应目标点的坐标（多个，即小车要去的目标），还没建图先完成建图；

打开gazebo roslaunch nav_sim myrobot_world.launch

rosrun teleop_twist_keyboard teleop_twist_keyboard.py
通过移动小车，设置目标点，记录左侧显示的位置坐标。x y z 和分别绕xyz轴旋转的角度：roll pitch yaw

3.查阅资料，编写发布一目标点的python或c脚本；

#include<iostream>
#include <ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<geometry_msgs/PoseStamped.h>
using namespace std;
int flag=1;
class Goal{
public:
    geometry_msgs::PoseStamped goal;
    Goal(){
     pub=n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",10);
sub=n.subscribe("/cmd_vel",1,&Goal::callback,this);
        goal.header.frame_id = "map";
        //改为自己记录目标点的坐标
        goal.pose.position.x = pose.x; 
        goal.pose.position.y = pose.y;  
        goal.pose.position.z = pose.z;   
        goal.pose.orientation.x = pose._x;
        goal.pose.orientation.y = pose._y;
        goal.pose.orientation.z = pose._z;
        goal.pose.orientation.w = pose._w;   
    }
private:
    ros::NodeHandle n; 
    ros::Publisher pub;
    ros::Subscriber sub;
   void callback(const geometry_msgs::Twist &v);
};
void Goal::callback(const geometry_msgs::Twist &v)
{ 
    if(flag==1&&v.linear.x==0){
            ROS_INFO("Sending goal!");
            pub.publish(goal);
     }
}
int main(int argc,char **argv)
{
    ros::init(argc,argv,"send_goal");
    Goal g;
    ros::spin();
    return 0;
}

4.编写发布多个目标点的python或c脚本。

 #include<iostream>
#include <ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<geometry_msgs/PoseStamped.h>
using namespace std;
int flag=1;
int g1=0,g2=0,g3=0;
class Goal{
public:
    geometry_msgs::PoseStamped goal_1;
    geometry_msgs::PoseStamped goal_2;
    geometry_msgs::PoseStamped goal_3;
    Goal(){
        pub=n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",10);
        sub=n.subscribe("/cmd_vel",1,&Goal::callback,this);
        goal_1.header.frame_id = "map";
        goal_2.header.frame_id = "map";
        goal_3.header.frame_id = "map";
        //以下三个目标的改为自己目标点的信息
//Goal one
        goal_1.pose.position.x = 0.033449; 
        goal_1.pose.position.y = 8.273015;  
        goal_1.pose.position.z = 0.050003;   
        goal_1.pose.orientation.x = 0;
        goal_1.pose.orientation.y = 0;
        goal_1.pose.orientation.z = 0;
        goal_1.pose.orientation.w = 1.487145;   
        //Goal two
        goal_2.pose.position.x = -0.207746;
        goal_2.pose.position.y = 17.607371;
        goal_2.pose.position.z = 0.050003;   
        goal_2.pose.orientation.x = 0;
        goal_2.pose.orientation.y = 0;
        goal_2.pose.orientation.z = 0;
        goal_2.pose.orientation.w = 1.483080;
        //Goal three
        goal_3.pose.position.x = 2.467109;
        goal_3.pose.position.y = 9.938154;
        goal_3.pose.position.z = 0.050002;   
        goal_3.pose.orientation.x = 0;
        goal_3.pose.orientation.y = 0;
        goal_3.pose.orientation.z = 0;
        goal_3.pose.orientation.w = -1.889479;
    }
private:
    ros::NodeHandle n; 
    ros::Publisher pub;
    ros::Subscriber sub;
    void callback(const geometry_msgs::Twist &v);
};

void Goal::callback(const geometry_msgs::Twist &v){
        //发送第一个目标点，如果发送成功，v将大于0
        if(flag==1&&v.linear.x==0){
            ROS_INFO("Sending goal one!");
            pub.publish(goal_1);
            g1=1;
        }

        if(v.linear.x>0&&flag==1)
            flag=2;

        if(flag==2&&v.linear.x==0&&g1){
             ROS_INFO("Sending goal two!");
             pub.publish(goal_2);
             g2=1;
        }

        if(v.linear.x>0&&flag==2&&g2)
            flag=3;

        if(flag==3&&v.linear.x==0&&g2){
             ROS_INFO("Sending goal three!");
             pub.publish(goal_3);
             g3=1;
        }    
    }
int main(int argc,char **argv)
{
    ros::init(argc,argv,"many_goal");
    Goal g;
    ros::spin();
    return 0;
}

在CMakeLists.txt文件中添加

add_executable(send_goal src/send_goal.cpp)
target_link_libraries(send_goal ${catkin_LIBRARIES})
add_executable(many_goal src/many_goal.cpp)
target_link_libraries(many_goal ${catkin_LIBRARIES})

在move_base.launch文件中启动send_goal.cpp或many_goal.cpp
加入两行：

<!--node pkg="nav_sim" type="send_goal" respawn="false" name="send_goal" output="screen"/-->
<node pkg="nav_sim" type="many_goal" respawn="false" name="many_goal" output="screen"/>

编译成功后：运行

roslaunch nav_sim myrobot_world.launch
roslaunch nav_sim move_base.launch

六、实验数据与结果评价

实验数据：

• 1.目标点数：3个
• 2.目标点位置：
  one:x:0.033449;y:8.273015;z:0.050003;_x:0;_y:0;_z:0;_w:1.487145;
  two:x:-0.207764;y:17.607371;z:0.050003;_x:0;_y:0;_z:0;_w:1.483080;
  three:x:2.467109;y:9.938154;z:0.050002;_x:0;_y:0;_z:0;_w:-1.889479;
• 3.坐标系frame_id ：map

  结果评价：

  1.脚本能否发送目标点

  可以，但需要手动点2D Nav Goal
  

2.Turtlebot到达一个目标点后能否继续发送第二个目标点

可以



注：也可以不用Turtlebot，使用nav _sim包的小车或者racecar的minicar。