Navigation package를 이용한 음성인식 자율주행

[정다훈]

# 구현방향

- 우선 음성인식으로 뭔가를 하기전에 이미 만들어져있는 navigation package에서 GUI tool 을 사용하지 않고

특정 메세지를 publish하면 그 메세지를 subscribe해서 초기위치와 목적지를 publish하는걸 구현해보려고 한다. 그 후에 특정 메세지를 음성인식 데이터로 대체할 계획이다.

# Navigation package에서 GUI tool(화살표)를 사용하지 않고 initial pose 정하기

(1) Topic 조사

초기위치와 목표위치를 설정하는 토픽

토픽정보

nav2에서 토픽을 기다리고 있는것으로 보여진다. Type으로 쓰여있는 저 msg에 값을 publish하면 초기위치가 지정될것으로 보임

msg 정보

header 부분은 이 메세지를 어느 토픽에 보낼지를 정한다. 자세히보면 

frame_id는 topic 명을 적어주면되고 stamp는 데이터를 커뮤니케이션할때 어느정도에 속도로 할건지를 정하는것 같다.

우리는 rviz에서 "map"에 띄워야 하므로 frame_id를 "map"으로 설정해줘야한다.

(2) std_msg/msg/string 형태로 메세지를 publish 하는 코드

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

#include <chrono> #include <functional> #include <memory> #include <string> #include <iostream>   #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp"   using namespace std::chrono_literals; using namespace std;     class HelloworldPublisher : public rclcpp::Node { public:   HelloworldPublisher()   : Node("helloworld_publisher")   {     auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));     helloworld_publisher_ = this->create_publisher<std_msgs::msg::String>("msg", qos_profile);     timer_ = this->create_wall_timer(100ms, std::bind(&HelloworldPublisher::publish_helloworld_msg, this));   } private:   void publish_helloworld_msg()   {     auto msg = std_msgs::msg::String();     cout<<"input msg: ";     cin>>msg.data;     RCLCPP_INFO(this->get_logger(), "Published message: '%s'", msg.data.c_str());     helloworld_publisher_->publish(msg);   }   rclcpp::TimerBase::SharedPtr timer_;   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr helloworld_publisher_; }; int main(int argc, char * argv[]) {   rclcpp::init(argc, argv);   auto node = std::make_shared<HelloworldPublisher>();   rclcpp::spin(node);   rclcpp::shutdown();   return 0; } Colored by Color Scripter

cs

"init"을 입력하면 메세지 형태로 init이라는 string문자열이 publish되는 형태이다.

(3) std_msg/msg/string를 subscribe해서 /initialpose토픽을 publish하는 코드

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81

#include <chrono> #include <functional> #include <memory> #include <string> #include <iostream>   #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "std_msgs/msg/header.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"   using namespace std;   using namespace std::chrono_literals; using std::placeholders::_1;   class InitalposePublisher : public rclcpp::Node { public:     InitalposePublisher()     : Node("initalpose_publisher")     {         //initial_pose pub         auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));         initalpose_pub = this->create_publisher             <geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose",qos_profile);         timer_ = this->create_wall_timer(100ms, std::bind(&InitalposePublisher::publish_initalpose_msg, this));           //msg sub         msg_sub=this->create_subscription<std_msgs::msg::String>(             "msg",             qos_profile,             std::bind(&InitalposePublisher::subscribe_topic_message,this,_1));     } private:     void subscribe_topic_message(const std_msgs::msg::String::SharedPtr msg)     {         str=msg->data.c_str();         cout<<str<<endl;         RCLCPP_INFO(this->get_logger(),"Received message: '%s'",msg->data.c_str());     }     void publish_initalpose_msg()     {         pose.header.frame_id=fixed_frame;           //set x,y coord         pose.pose.pose.position.x=0.0;         pose.pose.pose.position.y=0.0;         pose.pose.pose.position.z=0.0;         pose.pose.pose.orientation.w=0.1;                  float x,y,z,w;         x=pose.pose.pose.position.x;         y=pose.pose.pose.position.y;         z=pose.pose.pose.position.z;         w=pose.pose.pose.orientation.w;                  if(str=="init" && !spin && !once_spin){             initalpose_pub->publish(pose);             RCLCPP_INFO(this->get_logger(), "Published message initialpose-> x:%lf, y:%lf, z:%lf, w:%lf",x,y,z,w);             once_spin=true;         }              }     string fixed_frame="map";     string str="null";     bool spin=false,once_spin=false;     rclcpp::TimerBase::SharedPtr timer_;     geometry_msgs::msg::PoseWithCovarianceStamped pose;     rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initalpose_pub;     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr msg_sub; };   int main(int argc,char*argv[]) {     rclcpp::init(argc,argv);     auto node =std::make_shared<InitalposePublisher>();     rclcpp::spin(node);     rclcpp::shutdown();     return 0; } Colored by Color Scripter

cs

string msg를 받아서 그 문자열이 init이면 값을 변수에 넣어서 초기화 하고 1회만 publish하는 코드이다.

(4) 시연영상

https://youtu.be/SEUnL01Dujs

# Navigation package에서 GUI tool(화살표)를 사용하지 않고 GoalPose 정하기

(1) msg 확인

geometry_msgs/msg/PoseStamped 라는 메세지를 사용중이다.

(2) geometry_msgs/msg/PoseStamped로 /goal_pose를 publish하는 코드

# 추가할 헤더파일

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27

//   #ifndef _INITIAL_POSE_ #define _INITIAL_POSE_   #include "geometry_msgs/msg/pose_stamped.hpp" #include <vector>   struct Position{     double x;     double y;     double z; };   struct Orientation{     double x;     double y;     double z;     double w; };   struct Pose{     Position position;     Orientation orientation; };   #endif Colored by Color Scripter

cs

# main 소스

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107

#include <chrono> #include <functional> #include <memory> #include <string> #include <iostream>   #include "initialpos.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"     using namespace std;   using namespace std::chrono_literals; using std::placeholders::_1;   class InitalposePublisher : public rclcpp::Node { public:     InitalposePublisher()     : Node("initalpose_publisher")     {         //initial_pose pub         auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10));         initalpose_pub = this->create_publisher         <geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose",qos_profile);         init_timer = this->create_wall_timer         (100ms, std::bind(&InitalposePublisher::publish_pose_msg, this));           //goal_pose pub         goal_pub = this->create_publisher         <geometry_msgs::msg::PoseStamped>("goal_pose",qos_profile);         goal_timer=this->create_wall_timer         (100ms,std::bind(&InitalposePublisher::publish_pose_msg,this));                  //msg sub         msg_sub=this->create_subscription<std_msgs::msg::String>(             "msg",             qos_profile,             std::bind(&InitalposePublisher::subscribe_topic_message,this,_1));                  //init pose && goal pose         Pose pose1={{1.0,0.0,0.0},{0.0,0.0,0.0,1.0}};         Pose pose2={{1.0,1.0,0.0},{0.0,0.0,0.707,0.707}};         Pose pose3={{0.0,1.0,0.0},{0.0,0.0,0.1,0.0}};         Pose pose_init={{0.0,0.0,0.0},{0.0,0.0,0.0,0.1}};         poses_={pose1,pose2,pose3,pose_init};     } private:     void subscribe_topic_message(const std_msgs::msg::String::SharedPtr msg)     {         str=msg->data.c_str();         RCLCPP_INFO(this->get_logger(),"Received message: '%s'",msg->data.c_str());     }     void publish_pose_msg()     {         if(!spin){              if(str=="init"){                 pose_init.header.frame_id=fixed_frame;                 initalpose_pub->publish(pose_init);                 cout<<pose_init.pose.pose.orientation.w<<endl;                 spin=true;             }         }         else{             if(!init){ //한번만 초기화하기위해서 설정한 조건문                 for(int i=0;i<poses_.size();i++){                     goal_pose[i].header.frame_id=fixed_frame;                     goal_pose[i].pose.position.x=poses_[i].position.x;                     goal_pose[i].pose.position.y=poses_[i].position.y;                     goal_pose[i].pose.position.z=poses_[i].position.z;                     goal_pose[i].pose.orientation.z=poses_[i].orientation.z;                     goal_pose[i].pose.orientation.w=poses_[i].orientation.w;                 }                 init=true;             }             if(str=="goal1") {goal_pub->publish(goal_pose[0]); cout<<goal_pose[0].pose.orientation.w<<endl;}             else if(str=="goal2") {goal_pub->publish(goal_pose[1]); cout<<goal_pose[1].pose.orientation.w<<endl;}             else if(str=="goal3") {goal_pub->publish(goal_pose[2]); cout<<goal_pose[2].pose.orientation.w<<endl;}             else if(str=="goal4") {goal_pub->publish(goal_pose[3]); cout<<goal_pose[3].pose.orientation.w<<endl;}             str="";         }              }     string fixed_frame="map";     string str="";     bool spin=false;     bool init=false;     rclcpp::TimerBase::SharedPtr init_timer;     rclcpp::TimerBase::SharedPtr goal_timer;     geometry_msgs::msg::PoseWithCovarianceStamped pose_init;     geometry_msgs::msg::PoseStamped goal_pose[4];     rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initalpose_pub;     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pub;     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr msg_sub;     vector<Pose> poses_; };   int main(int argc,char*argv[]) {     rclcpp::init(argc,argv);     auto node =std::make_shared<InitalposePublisher>();     rclcpp::spin(node);     rclcpp::shutdown();     return 0; } Colored by Color Scripter

cs

(3) 시연영상

https://youtu.be/t4gux8Zm5hY