越疆M1-B1 ROS下使用TCP加导航加红外校准功能

使用越疆科技的M1-B1机器人进行ROS下完成 TCP加导航加红外校准功能

#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>
#include "iostream"

#include "std_msgs/Int16.h"

using namespace std;

//导航结束后成功和失败的flag
int navigation_flag = 0;
//TCP接收到的消息
string TCP_Receive;
//红外校准发布器
ros::Publisher recharge_pub;
//红外校准数据变量
int recharge_status = 0;


//函数声明
//导航部分
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion);
//TCP服务器函数
void socket_server(char* message);
//回充校准函数
void jiaozhun_recharge(ros::NodeHandle &n,ros::Publisher recharge_pub);
//回调函数获取红外校准数据
void sub_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg);

//导航部分
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion)
{
	//设置我们要机器人走的几个点。
	geometry_msgs::Pose pose_list;

	pose_list.position = point;
	pose_list.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_list;

        //让机器人向目标移动
        //将目标姿势发送到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");
	    navigation_flag = 0;
        }
	else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Goal succeeded!");
		navigation_flag =1;
            }
            else
            {
                ROS_INFO("The base failed for some reason");
		navigation_flag = 0;
            }
        }
}

//TCP服务器函数
void socket_server(char* message)
{
/*服务端：
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;
	//接受到信息并赋值给全局变量
	TCP_Receive = buffer;
	//反馈给客户端信息
	write(fd,message,7);	

	//7.关闭sockfd。
	close(fd);
	close(socket_fd);	
}

//回充校准函数
void jiaozhun_recharge(ros::NodeHandle &n,ros::Publisher recharge_pub)
{
	std_msgs::Int16 msg1;
	msg1.data = 1;
	std_msgs::Int16 msg0;
	msg0.data = 0;	
	//启动校准发布3次
	for(int i =0;i<3;i++)
	{
		recharge_pub.publish(msg1);  //启动红外校准
	}
	//订阅校准状态,完成后就停止回充
	while(recharge_status != 2 && recharge_status != 3 && recharge_status != 4 && ros::ok())
	{
		cout<<"开始订阅对准状态:"<<recharge_status<<endl;
		//订阅红外校准状态，成功后返回值为int 3
	    	ros::Subscriber sub1 = n.subscribe("/recharge_status", 1, sub_recharge_status_callback);
		ros::Rate loop_rate(0.5);
		int i = 1;
		while(i >= 0 && ros::ok())
		{
			ros::spinOnce();
			i = i-1;
			//cout<<"call :"<<i<<endl;
			loop_rate.sleep();
		}		
	}
	cout<<"校准结束！"<<endl;
}

//回调函数获取红外校准数据
void sub_recharge_status_callback(const std_msgs::Int16::ConstPtr& status_msg)
{
	cout<<"回调函数处理数据:"<<status_msg->data<<endl;
	recharge_status = status_msg->data;
}


int main(int argc, char** argv)
{
	ros::init(argc, argv, "send_goal");
	ros::NodeHandle n;
	
	//红外校准发布器
	recharge_pub = n.advertise<std_msgs::Int16>("/recharge_handle",10);
	//订阅move_base操作服务器
	actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

	//服务器部分
	char* A = "这里是服务器！";
	socket_server(A);
	cout<<"TCP_Receive:"<<TCP_Receive<<endl;
	cout<<"len:"<<TCP_Receive.size()<<endl;

	//设置我们要机器人走的几个点。
	geometry_msgs::Point point;
	geometry_msgs::Quaternion quaternion;
	
	//导航部分
	point.x = 6.06296396255;
	point.y = 4.37734413147;
	point.z = 0;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.408275405284;
	quaternion.w = 0.912858802576;
	
	navigation(ac, point, quaternion);

	cout<<"--------------"<<endl;
	//启动回充
	jiaozhun_recharge(n, recharge_pub);

	point.x = 4.98605394363;
	point.y = 7.20435905457;
	point.z = 0;
	quaternion.x = 0.000;
	quaternion.y = 0.000;
	quaternion.z = 0.997947341797;
	quaternion.w = 0.0640398547754;

	navigation(ac, point, quaternion);	

	return 0;
	
			
}