2020/09/23 ros消息转换与发送(改)

Processing_Message/Processing_message.cpp

@@ -9,6 +9,7 @@ Processing_message::Processing_message()
 	mp_socket_receive_data_thread=NULL;
 	mp_ros_public_thread=NULL;
 	mp_socket_send_data_thread=NULL;
+
 }
 Processing_message::~Processing_message()
 {
@@ -16,6 +17,7 @@ Processing_message::~Processing_message()
 }
 void Processing_message::Processing_message_init(std::string server_ip)
 {
+	m_subscriber=m_nodehandele.subscribe("/move_base_simple/goal",1,goalCallback);//订阅话题
 	Processing_message_connect(server_ip);
 }
 void Processing_message::Processing_message_connect(std::string server_ip)
@@ -42,11 +44,15 @@ void Processing_message::Processing_message_run()
 	m_ros_public_condition.reset(false, true, false);
 	mp_ros_public_thread = new std::thread(&Processing_message::ros_public_thread, this);
 
+	m_socket_send_data_condition.reset(false, true, false);
+	mp_socket_send_data_thread = new std::thread(&Processing_message::socket_send_data_thread, this);
+
 }
 //反初始化
 void Processing_message::Processing_message_uninit()
 {
 	m_socket_data_queue.termination_queue();
+	m_send_data_queue.termination_queue();
 	//杀死线程，强制退出
 	if (mp_socket_receive_data_thread)
 	{
@@ -57,6 +63,10 @@ void Processing_message::Processing_message_uninit()
 	{
 		m_ros_public_condition.kill_all();
 	}
+	if (mp_socket_send_data_thread)
+	{
+		m_socket_send_data_condition.kill_all();
+	}
 	//回收线程的资源
 	if (mp_socket_receive_data_thread)
 	{
@@ -102,7 +112,7 @@ void Processing_message::socket_receive_data_thread()
 			{
 					if(m_socket_data_queue.push(t_receive_string))
 					{
-//						std::cout<<"data push success!"<<std::endl;
+						std::cout<<"data push success!"<<std::endl;
 					}
 
 			}
@@ -162,6 +172,23 @@ void Processing_message::ros_public_thread()
 	}
 	return;
 }
+void Processing_message::socket_send_data_thread()
+{
+	while (m_socket_send_data_condition.is_alive())
+	{
+		if ( m_socket_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			std::string t_send_data;
+			if(m_send_data_queue.wait_and_pop(t_send_data))//没有消息就等待 有消息就取出来发送
+			{
+				std::unique_lock<std::mutex> lk(m_socket_mutex);
+				m_socket.send(t_send_data);
+			}
+		}
+	}
+	return;
+}
 sensor_msgs::PointCloud Processing_message::create_cloudmsg(message::PointCloud_msg cloud_pcl)
 {
 	sensor_msgs::PointCloud cloud;
@@ -207,11 +234,33 @@ ros::Publisher* Processing_message::find_topic(std::string topic)
 	}
 	return NULL;
 }
-nnxx::socket& Processing_message::get_socket()
-{
-	return m_socket;
-}
-ros::NodeHandle& Processing_message::get_nodehandele()
+
+ void  Processing_message::goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
 {
-	return m_nodehandele;
+	//填充数据
+
+	double x=msg->pose.position.x;
+	double y=msg->pose.position.y;
+	double z=msg->pose.position.z;
+	double qx=msg->pose.orientation.x;
+	double qy=msg->pose.orientation.y;
+	double qz=msg->pose.orientation.z;
+	double qw=msg->pose.orientation.w;
+
+	std::cout<<"  post goal "<<qx<<","<<qy<<","<<qz<<","<<qw<<std::endl;
+	double theta=acos(qw)*2.0;
+	theta=(qz>0)?theta:-theta;
+
+	message::Goal_msg goal;
+	message::Base_info base_info;
+	base_info.set_sender(message::eEmpty);
+	base_info.set_receiver(message::eAGV);
+	base_info.set_msg_type(message::eGoal_msg);
+	goal.mutable_base_info()->CopyFrom(base_info);
+
+	goal.set_x(x);
+	goal.set_y(y);
+	goal.set_theta(theta);
+
+	Processing_message::get_instance_references().m_send_data_queue.push(goal.SerializeAsString());//存入数据
 }

Processing_Message/Processing_message.h

@@ -46,22 +46,22 @@ public://API functions
 	void Processing_message_uninit();
 	sensor_msgs::PointCloud create_cloudmsg(message::PointCloud_msg);//填充消息
 	geometry_msgs::PoseStamped create_posemsg(message::Pose2d_msg);//填充消息
+	static void goalCallback(const geometry_msgs::PoseStamped::ConstPtr&);
 public://get or set member variable
 	ros::Publisher* find_topic(std::string);
 	Connect_statu get_connect_statu();
-	nnxx::socket& get_socket();
-	ros::NodeHandle& get_nodehandele();
 
 protected:
 	void socket_receive_data_thread();
 	void ros_public_thread();
+	void socket_send_data_thread();
 protected://member variable
 	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS  };
 	std::mutex 							m_socket_mutex;					//m_socket的锁
 	Connect_statu 						m_connect_statu;				//连接状态
 
 	Thread_safe_queue<std::string>		m_socket_data_queue;			//存放数据的queue
-	std::mutex							m_data_mutex;					//数据锁
+	Thread_safe_queue<std::string>		m_send_data_queue;				//存放发送数据的queue
 
 	std::thread*						mp_socket_receive_data_thread; 	//接收消息线程
 	Thread_condition					m_socket_receive_condition;		//接受的条件变量
@@ -72,6 +72,7 @@ protected://member variable
 	std::thread*						mp_socket_send_data_thread;    	//发送的线程指针
 	Thread_condition					m_socket_send_data_condition;	//发送的条件变量
 
+	ros::Subscriber						m_subscriber;					//用于就收rviz上的消息
 	ros::NodeHandle 					m_nodehandele;					//用于创建新的发布器
 	std::map<std::string,ros::Publisher*>		m_ros_publisher_map;	//用于查询是否存在topic
 

src/main.cpp

@@ -1,43 +1,12 @@
 //#include "Processing_message.h"
 #include "../Processing_Message/Processing_message.h"
 
-void  goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
-	//填充数据
-
-	double x=msg->pose.position.x;
-	double y=msg->pose.position.y;
-	double z=msg->pose.position.z;
-	double qx=msg->pose.orientation.x;
-	double qy=msg->pose.orientation.y;
-	double qz=msg->pose.orientation.z;
-	double qw=msg->pose.orientation.w;
-
-	std::cout<<"  post goal "<<qx<<","<<qy<<","<<qz<<","<<qw<<std::endl;
-	double theta=acos(qw)*2.0;
-	theta=(qz>0)?theta:-theta;
-
-	message::Goal_msg goal;
-	message::Base_info base_info;
-	base_info.set_sender(message::eEmpty);
-	base_info.set_receiver(message::eAGV);
-	base_info.set_msg_type(message::eGoal_msg);
-	goal.mutable_base_info()->CopyFrom(base_info);
-
-	goal.set_x(x);
-	goal.set_y(y);
-	goal.set_theta(theta);
-
-	std::cout<<Processing_message::get_instance_references().get_socket().send(goal.SerializeAsString())<<std::endl;//发送数据
-}
 
 int main(int argc,char** argv)
 {
 //"tcp://192.168.2.183:30001"
 	ros::init(argc, argv, "test_node");
 	Processing_message& t_processing_message=Processing_message::get_instance_references();//获取单例
-	ros::Subscriber subscriber=t_processing_message.get_nodehandele().subscribe("/move_base_simple/goal",1,goalCallback);//订阅话题
-
 	t_processing_message.Processing_message_init("tcp://192.168.2.182:30001");//默认为连接
 	ros::spin();
 	t_processing_message.Processing_message_uninit();