Racecar multi-point navigation experiment based on ROS communication mechanism

One 、 The experiment purpose

• 1. Learn more ROS Communication mechanism ;
• 2. understand Turtlebot The relationship between nodes ;
• 3. Familiar use ROS Message type ;
• 4. Understand the closed-loop control of the trolley .
• 5. understand rviz How to send the target point .

Two 、 Experimental environment

Ubuntu16.04+ROS .

3、 ... and 、 Experimental principle

Publisher subscriber implementation , The publisher sends out the target point , Subscribers receive post control Turtlebo For navigation .

Four 、 Experimental content

• 1. obtain rviz Send target point's topic;
• 2. Obtain the coordinates of the corresponding target points for the built map （ Multiple , That is, the target of the car ）, Finish the construction before the construction ;
• 3. Access to information , Write and publish a target point python or c Script ;
• 4. Write and publish multiple target points python or c Script .

5、 ... and 、 The experimental steps

1. obtain rviz Send target point's topic;

2. Obtain the coordinates of the corresponding target points for the built map （ Multiple , That is, the target of the car ）, Finish the construction before the construction ;

open gazebo roslaunch nav_sim myrobot_world.launch

rosrun teleop_twist_keyboard teleop_twist_keyboard.py
By moving the car , Set the target point , Record the position coordinates displayed on the left .x y z And wrap separately xyz Angle of axis rotation ：roll pitch yaw

3. Access to information , Write and publish a target point python or c Script ;

#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";
        // Instead, record the coordinates of the target point by yourself 
        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. Write and publish multiple target points python or c Script .

 #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";
        // The following three goals are changed to the information of your goal points 
//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){
    
        // Send the first target point , If the transmission is successful ,v Will be bigger than the 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;
}

stay CMakeLists.txt Add... To the file

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

stay move_base.launch Start... In file send_goal.cpp or many_goal.cpp
Add two lines ：

<!--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"/>

After successful compilation ： function

roslaunch nav_sim myrobot_world.launch
roslaunch nav_sim move_base.launch

6、 ... and 、 Experimental data and result evaluation

experimental data ：

• 1. Target points ：3 individual
• 2. Target location ：
  one0.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;
  three2.467109;y:9.938154;z:0.050002;_x:0;_y:0;_z:0;_w:-1.889479;
• 3. Coordinate system frame_id ：map

Result evaluation ：

1. Whether the script can send the target point

Sure , But it needs to be manually 2D Nav Goal

2.Turtlebot Can you continue to send the second target point after reaching a target point

Sure

notes ： It can also be done without Turtlebot, Use nav _sim Bag car or racecar Of minicar.

Last week's task ：