一、实验目的

• 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 onegoal_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 twogoal_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 threegoal_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将大于0if(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❌0.033449;y:8.273015;z:0.050003;_x:0;_y:0;_z:0;_w:1.487145;
  two❌-0.207764;y:17.607371;z:0.050003;_x:0;_y:0;_z:0;_w:1.483080;
  three❌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。