基于ROS通信机制的多点导航实验_专栏

一、实验目的

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})

六、实验数据与结果评价

实验数据：
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