Processing_message.h 3.0 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283
  1. //
  2. // Created by wk on 2020/9/17.
  3. //
  4. #ifndef SRC_PROCESSING_MESSAGE_H
  5. #define SRC_PROCESSING_MESSAGE_H
  6. #include <nnxx/socket>
  7. #include <nnxx/message>
  8. #include <thread>
  9. #include <ros/ros.h>
  10. #include <sensor_msgs/PointCloud.h>
  11. #include <pcl/point_types.h>
  12. #include <pcl/point_cloud.h>
  13. #include <math.h>
  14. #include <geometry_msgs/PoseStamped.h>
  15. #include <map>
  16. #include "../message/message_base.pb.h"
  17. #include "../message/ros_message.pb.h"
  18. #include "../tool/thread_condition.h"
  19. #include "../tool/thread_safe_queue.h"
  20. #include "../tool/singleton.h"
  21. class Processing_message:public Singleton<Processing_message>
  22. {
  23. // 子类必须把父类设定为友元函数,这样父类才能使用子类的私有构造函数。
  24. friend class Singleton<Processing_message>;
  25. public:
  26. enum Connect_statu
  27. {
  28. CONNECT_ERROR =0, //连接错误
  29. CONNECT_NORMAL =1, //连接正常
  30. };
  31. private:
  32. // 父类的构造函数必须保护,子类的构造函数必须私有。
  33. Processing_message();
  34. public:
  35. //必须关闭拷贝构造和赋值构造,只能通过 get_instance 函数来进行操作唯一的实例。
  36. Processing_message(const Processing_message& other)= delete;
  37. Processing_message& operator =(const Processing_message& other)= delete;
  38. ~Processing_message();
  39. public://API functions
  40. void Processing_message_init(std::string);//默认为连接
  41. void Processing_message_connect(std::string);
  42. void Processing_message_band(std::string);
  43. void Processing_message_run();
  44. void Processing_message_uninit();
  45. sensor_msgs::PointCloud create_cloudmsg(message::PointCloud_msg);//填充消息
  46. geometry_msgs::PoseStamped create_posemsg(message::Pose2d_msg);//填充消息
  47. static void goalCallback(const geometry_msgs::PoseStamped::ConstPtr&);
  48. public://get or set member variable
  49. ros::Publisher* find_topic(std::string);
  50. Connect_statu get_connect_statu();
  51. protected:
  52. void socket_receive_data_thread();
  53. void ros_public_thread();
  54. void socket_send_data_thread();
  55. protected://member variable
  56. nnxx::socket m_socket { nnxx::SP, nnxx::BUS };
  57. std::mutex m_socket_mutex; //m_socket的锁
  58. Connect_statu m_connect_statu; //连接状态
  59. Thread_safe_queue<std::string> m_socket_data_queue; //存放数据的queue
  60. Thread_safe_queue<std::string> m_send_data_queue; //存放发送数据的queue
  61. std::thread* mp_socket_receive_data_thread; //接收消息线程
  62. Thread_condition m_socket_receive_condition; //接受的条件变量
  63. std::thread* mp_ros_public_thread; //发布的线程指针
  64. Thread_condition m_ros_public_condition; //发布的条件变量
  65. std::thread* mp_socket_send_data_thread; //发送的线程指针
  66. Thread_condition m_socket_send_data_condition; //发送的条件变量
  67. ros::Subscriber m_subscriber; //用于就收rviz上的消息
  68. ros::NodeHandle m_nodehandele; //用于创建新的发布器
  69. std::map<std::string,ros::Publisher*> m_ros_publisher_map; //用于查询是否存在topic
  70. private:
  71. };
  72. #endif //SRC_PROCESSING_MESSAGE_H