导航定位用户向服务器发送位置请求6,ROS通过TCP服务器调用导航，到底位置发布信息（导航＋socket）...

代码一：是等待其他客户端来调用导航的服务器(可以是基于任何平台的ｔｃｐ客户端调用)

代码二：是到达导航点后客户端去发布ＴＣＰ到任何ＴＣＰ的服务器的代码

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

using namespace std;

std_msgs::Bool flag;

ros::Publisher pub;

void socket_server()

{

/*服务端：

1、socket()获得一个sockfd。注意第二个参数必须SOCK_STREAM.

2、准备通信地址(必须服务器的)

3、bind()绑定。(开放了端口，允许客户端连接)

4、监听客户端 listen()函数

5、等待客户端的连接 accept(),返回用于交互的socket描述符

6、使用第5步返回sockt描述符，进行读写通信。

7、关闭sockfd。

*/

//1.socket()获得一个sockfd。注意第二个参数必须SOCK_STREAM.

//第一个参数是地址描述符，常用AF_INET(表示IPV4)

//第三个参数是套接口所用的协议，不想调用就用0

int socket_fd = socket(AF_INET,SOCK_STREAM,0);

if(socket_fd == -1)

{

cout<

exit(-1);

}

//2.准备通讯地址(必须是服务器的)

//sin_family：协议簇一般用AF_INET(表示IPV4)

//sin_port: 端口，一般用htons(填端口：数字随便要和服务器的一样)将十进制转为网络进制

//sin_addr.s_addr:一般用inet_addr(" "填IP地址)或者htonl(INADDR_ANY)直接设置为本地ip

struct sockaddr_in addr;

addr.sin_family = AF_INET;

addr.sin_port = htons(10004);

addr.sin_addr.s_addr = htonl(INADDR_ANY);

//3.bind() 绑定。(开发了端口，允许客服端链接)

//参数一：0的返回值(socket_fd)

//参数二：(struct sockaddr*)&addr 前面结构体，即地址

//参数三: addr结构体的长度

int res = bind(socket_fd, (struct sockaddr*)&addr, sizeof(addr));

if(res == -1)

{

cout<

exit(-1);

}

cout<

//4.监听客户端 listen()函数

//参数二：进程上限，一般小于30

listen(socket_fd,30);

//5.等待客户端的连接 accept(),返回用于交互的socket描述符

struct sockaddr_in client;

socklen_t len = sizeof(client);

int fd = accept(socket_fd,(struct sockaddr*)&client, &len);

if(fd == -1)

{

cout<

exit(-1);

}

//6.使用第5步返回sockt描述符，进行读写通信。

char *ip = inet_ntoa(client.sin_addr);

cout<

char buffer[255] = {};

//第一个参数：accept 返回的文件描述符

//第二个参数：存放读取的内容

//第三个参数：内容的大小

int size = read(fd,buffer,sizeof(buffer));

cout<

cout<

write(fd,"welcome",7);

if(buffer);

{

flag.data = 1;

pub.publish(flag);

}

//7.关闭sockfd。

close(fd);

close(socket_fd);

}

int main(int argc, char** argv)

{

ros::init(argc, argv, "star_navigation");

ros::NodeHandle n;

//准备发布器为服务器接收到消息发布开始命令做准备

pub = n.advertise<:bool>("navigation_star",100);

socket_server();

return 0;

}

代码二：

#include

#include "ros/console.h"

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

using namespace std;

void socket_fun(char* flag)

{

cout<

int socket_fd = socket(AF_INET,SOCK_STREAM,0);

if(socket_fd == -1)

{

cout<

exit(-1);

}

struct sockaddr_in addr;

addr.sin_family = AF_INET;

addr.sin_port = htons(6125);

addr.sin_addr.s_addr = inet_addr("192.168.31.40");

int res = connect(socket_fd,(struct sockaddr*)&addr, sizeof(addr));

if(res == -1)

{

cout<

exit(-1);

}

cout<

write(socket_fd,flag,sizeof(flag));

cout<

char buff[100] = {};

read(socket_fd,buff,sizeof(buff));

cout<

close(socket_fd);

}

void navigation_goal(const std_msgs::Bool::ConstPtr& flag)

{

//订阅move_base操作服务器

actionlib::SimpleActionClient<:movebaseaction> ac("move_base",true);

//设置我们要机器人走的几个点。

geometry_msgs::Point point;

geometry_msgs::Quaternion quaternion;

geometry_msgs::Pose pose_list1;

geometry_msgs::Pose pose_list2;

geometry_msgs::Pose pose_list3;

point.x = 1.46311354637;

point.y = -0.00640559289604;

point.z = 0.000;

quaternion.x = 0.000;

quaternion.y = 0.000;

quaternion.z = -0.700764941719;

quaternion.w = 0.713392245863;

pose_list1.position = point;

pose_list1.orientation = quaternion;

point.x = 1.64680922031;

point.y = 0.411043381691;

point.z = 0.000;

quaternion.x = 0.000;

quaternion.y = 0.000;

quaternion.z = 0.709273408676;

quaternion.w = 0.704933494555;

pose_list2.position = point;

pose_list2.orientation = quaternion;

point.x = 0.50;

point.y = 0.10;

point.z = 0.000;

quaternion.x = 0.000;

quaternion.y = 0.000;

quaternion.z = 0.709273408676;

quaternion.w = 0.704933494555;

pose_list3.position = point;

pose_list3.orientation = quaternion;

ROS_INFO("Waiting for move_base action server...");

//等待60秒以使操作服务器可用

if(!ac.waitForServer(ros::Duration(60)))

{

ROS_INFO("Can't connected to move base server");

exit(-1);

}

ROS_INFO("Connected to move base server");

ROS_INFO("Starting navigation test");

//初始化航点目标

move_base_msgs::MoveBaseGoal goal1;

//使用地图框定义目标姿势

goal1.target_pose.header.frame_id = "map";

//将时间戳设置为“now”

goal1.target_pose.header.stamp = ros::Time::now();

//将目标姿势设置为第i个航点

goal1.target_pose.pose = pose_list1;

//让机器人向目标移动

//将目标姿势发送到MoveBaseAction服务器

ac.sendGoal(goal1);

//等3分钟到达那里

bool finished_within_time1 = ac.waitForResult(ros::Duration(180));

//如果我们没有及时赶到那里，就会中止目标

if(!finished_within_time1)

{

ac.cancelGoal();

ROS_INFO("Timed out achieving goal");

}

else

{

//We made it!

if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("Goal A succeeded!");

char* A = "ONE";

socket_fun(A);

ROS_INFO("send socket succeeded!");

sleep(5);

}

else

{

ROS_INFO("The base failed for some reason");

}

}

//初始化航点目标

move_base_msgs::MoveBaseGoal goal2;

//使用地图框定义目标姿势

goal2.target_pose.header.frame_id = "map";

//将时间戳设置为“now”

goal2.target_pose.header.stamp = ros::Time::now();

//将目标姿势设置为第i个航点

goal2.target_pose.pose = pose_list2;

//让机器人向目标移动

//将目标姿势发送到MoveBaseAction服务器

ac.sendGoal(goal2);

//等3分钟到达那里

bool finished_within_time2 = ac.waitForResult(ros::Duration(180));

//如果我们没有及时赶到那里，就会中止目标

if(!finished_within_time2)

{

ac.cancelGoal();

ROS_INFO("Timed out achieving goal");

}

else

{

//We made it!

if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("Goal B succeeded!");

char* B = "TWO";

//到底Ｂ点后调用socket函数链接视觉

socket_fun(B);

sleep(15);

}

else

{

ROS_INFO("The base failed for some reason");

}

}

//初始化航点目标

move_base_msgs::MoveBaseGoal goal3;

//使用地图框定义目标姿势

goal3.target_pose.header.frame_id = "map";

//将时间戳设置为“now”

goal3.target_pose.header.stamp = ros::Time::now();

//将目标姿势设置为第i个航点

goal3.target_pose.pose = pose_list3;

//让机器人向目标移动

//将目标姿势发送到MoveBaseAction服务器

ac.sendGoal(goal3);

//等3分钟到达那里

bool finished_within_time3 = ac.waitForResult(ros::Duration(180));

//如果我们没有及时赶到那里，就会中止目标

if(!finished_within_time3)

{

ac.cancelGoal();

ROS_INFO("Timed out achieving goal");

}

else

{

//We made it!

if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("end!");

}

else

{

ROS_INFO("The base failed for some reason");

}

}

}

int main(int argc, char** argv)

{

ros::init(argc, argv, "send_goal");

ros::NodeHandle n;

//订阅ｓｔａｒ的信息，得到开始命令就执行导航函数

ros::Subscriber sub = n.subscribe("navigation_star",1000,navigation_goal);

ros::spin();

return 0;

}