ROS通过TCP服务器调用导航，到底位置发布信息（导航＋socket）

很不专业

发布时间 2021.06.30阅读数 4402 评论数 0

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

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

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <sstream>

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<<"socket faild!"<<endl;
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<<"binf fiald!"<<endl;
exit(-1);
}

cout<<"bind ok"<<endl;

//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<<"aceept fiald!"<<endl;
exit(-1);
}

//6.使用第5步返回sockt描述符，进行读写通信。
char *ip = inet_ntoa(client.sin_addr);
cout<<"client ["<<ip<<"] connect !"<<endl;

char buffer[255] = {};
//第一个参数：accept 返回的文件描述符
//第二个参数：存放读取的内容
//第三个参数：内容的大小
int size = read(fd,buffer,sizeof(buffer));

cout<<"accept bet:"<<size<<endl;
cout<<"cantent:"<<buffer<<endl;
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<std_msgs::Bool>("navigation_star",100);

socket_server();

return 0;
}

代码二：

#include <ros/ros.h>
#include "ros/console.h"
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <unistd.h>
#include <iostream>
#include <sys/types.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <std_msgs/Bool.h>


using namespace std;

 

void socket_fun(char* flag)
{
    cout<<"socket1"<<endl;
    int socket_fd = socket(AF_INET,SOCK_STREAM,0);
    if(socket_fd == -1)
    {
        cout<<"socket faild"<<endl;
        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<<"bind faild"<<endl;
        exit(-1);
    }

    cout<<"bind ok"<<endl;

    
    write(socket_fd,flag,sizeof(flag));
    cout<<"send message:"<<flag<<endl;

    char buff[100] = {};
    read(socket_fd,buff,sizeof(buff));
    cout<<buff<<endl;
    

    close(socket_fd);
    
}

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

//订阅move_base操作服务器
actionlib::SimpleActionClient<move_base_msgs::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;
    
            
}

ROS 导航 TCP socket 发布信息

打赏 0

上一篇：ROS下空环境中用激光雷达做跟随

下一篇：学习笔记ROS（moveit 关节空间运动规划--正向运动学）

ROS通过TCP服务器调用导航，到底位置发布信息（导航＋socket）

很不专业

为你推荐

ROS入门21讲笔记——6、客户端Client的编程实现

精选ROS机器人操作系统——tf坐标系广播与监听的编程实现（十二）

浅谈如何通过共用体实现数据转换

ROS机器人操作系统——ROS命令行工具的使用（三）

ROS中发布IMU传感器消息

ROS入门学习之三机器人系统设计

关于作者

很不专业

19

0

1

2

ROS下进行人脸识别并输出人脸坐标位置

学习笔记ROS（moveit 工作空间运动规划--逆向运动学）

ros机器人导航设置原点，目标点

相关推荐

ROS入门(三)——常用功能与总结

关于package.xml和CMakeList.txt的解读

学习笔记ROS（moveit 工作空间运动规划--逆向运动学）

ROS入门（七）：ROS与gazebo plugins

详细介绍如何在ubuntu20.04中安装ROS系统，以及安装过程中出现的常见错误的解决方法，填坑！！！

ROS IDE: VSCode开发环境的搭建

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

ROS通过TCP服务器调用导航，到底位置发布信息（导航＋socket）

很不专业

为你推荐

ROS入门21讲笔记——6、客户端Client的编程实现

精选ROS机器人操作系统——tf坐标系广播与监听的编程实现（十二）

浅谈如何通过共用体实现数据转换

ROS机器人操作系统——ROS命令行工具的使用（三）

ROS中发布IMU传感器消息

ROS入门学习之三 机器人系统设计

评论（0）

关于作者

很不专业

19

0

1

2

ROS下进行人脸识别并输出人脸坐标位置

学习笔记ROS（moveit 工作空间运动规划--逆向运动学）

ros机器人导航设置原点，目标点

相关推荐

ROS入门(三)——常用功能与总结

关于package.xml和CMakeList.txt的解读

学习笔记ROS（moveit 工作空间运动规划--逆向运动学）

ROS入门（七）：ROS与gazebo plugins

详细介绍如何在ubuntu20.04中安装ROS系统，以及安装过程中出现的常见错误的解决方法，填坑！！！

ROS IDE: VSCode开发环境的搭建

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

ROS入门学习之三机器人系统设计

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板