ros通过手机实现小车Navigation坐标导航

安卓端代码就不放了，很简单，向小车ip和端口发送三个数字，中间用逗号隔开。

小车接收并导航代码如下：

#include <iostream>
#include <cstring>
#include <sstream>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

using namespace std;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char** argv) {

ros::init(argc, argv, "demo_simple_goal");
 
  //tell the action client that we want to spin a thread by default
  //声明一个MoveBaseClient对象ac，用来调用和主管监控导航功能的服务。
  MoveBaseClient ac("move_base", true);
 
  //wait for the action server to come up(确定导航服务开启)
  while(!ac.waitForServer(ros::Duration(5.0)))
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }
 
  //用来传递我们导航去的目标信息
  move_base_msgs::MoveBaseGoal goal;
 
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
 
    int sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0) {
        perror("socket()");
        exit(EXIT_FAILURE);
    }

    struct sockaddr_in server_address{};
    memset(&server_address, 0, sizeof(server_address));
    server_address.sin_family = AF_INET;
    server_address.sin_port = htons(8001);
    server_address.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(sock, (struct sockaddr*)&server_address, sizeof(server_address)) < 0) {
        perror("bind()");
        exit(EXIT_FAILURE);
    }

    char buffer[1024];
    while (true) {
        struct sockaddr_in client_address{};
        memset(&client_address, 0, sizeof(client_address));
        socklen_t len = sizeof(client_address);
        int nbytes = recvfrom(sock, buffer, sizeof(buffer)-1, 0, (struct sockaddr*)&client_address, &len);
        buffer[nbytes] = '\0';
       string s = buffer;
       stringstream ss(s);
string token;
int num;
int x=0;
int y=0;
int w=0;

getline(ss, token, ',');
        x = stoi(token);
getline(ss, token, ',');
        y = stoi(token);
getline(ss, token, ',');
        w = stoi(token);
cout<<x<<" "<<y<<" "<<w;
 goal.target_pose.pose.position.x = x;
  goal.target_pose.pose.position.y = y;
   goal.target_pose.pose.orientation.z = -0.00256;
    goal.target_pose.pose.orientation.w = w;
 
 
  ROS_INFO("Sending goal");
  //将导航目标信息传递给导航服务的客户端ac，由ac来监控后面的导航过程。
  ac.sendGoal(goal);
 
  ac.waitForResult();
 
  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Mission complete!");
  else
    ROS_INFO("Mission failed ...");

        cout << "Received message from " << inet_ntoa(client_address.sin_addr) << ":" << ntohs(client_address.sin_port) << endl;
        cout << buffer << endl;
    }

    close(sock);
    return 0;
}

使用时要先启动小车底盘和导航包

然后

rosrun xxxx   xxxxx

就可以了