53 lines
1.5 KiB
C++
53 lines
1.5 KiB
C++
//
|
|
// Created by hshine on 22-12-23.
|
|
//
|
|
|
|
#ifndef RM_SERIAL_RM_SERIAL_HPP
|
|
#define RM_SERIAL_RM_SERIAL_HPP
|
|
|
|
#include "serial/serial.h"
|
|
#include "ros/ros.h"
|
|
#include "rm_serial/gimbal_msg.h"
|
|
#include "geometry_msgs/TwistStamped.h"
|
|
#include <geometry_msgs/TransformStamped.h>
|
|
#include "packet.hpp"
|
|
|
|
#include <tf2/LinearMath/Quaternion.h>
|
|
#include <tf2_ros/transform_broadcaster.h>
|
|
|
|
#include <memory>
|
|
#include <thread>
|
|
namespace rm_serial
|
|
{
|
|
class RMSerial
|
|
{
|
|
public:
|
|
explicit RMSerial();
|
|
~RMSerial() ;
|
|
|
|
void receiveData();
|
|
void sendData();
|
|
void gimbal_cmd_cb(const rm_serial::gimbal_msg::ConstPtr& gimbal_msg);
|
|
void chassis_cmd_cb(const geometry_msgs::Twist::ConstPtr& cmd_vel_msg);
|
|
void reopenPort();
|
|
void set_param();
|
|
private:
|
|
std::string device_name_;
|
|
std::unique_ptr<serial::Serial> serial_;
|
|
std::shared_ptr<ros::NodeHandle> serial_node_;
|
|
std::shared_ptr<ros::Publisher> latency_pub_;//communicate delay
|
|
//std::shared_ptr<ros::Publisher> joint_state_pub_;
|
|
ros::Publisher joint_state_pub,game_progress_pub,outpost_hp_pub,arial_goal_pub;
|
|
ros::Subscriber gimbal_cmd_sub,chassis_cmd_sub;
|
|
double last_cmd_x,last_cmd_y;
|
|
bool initial_set_param_ = false;
|
|
uint8_t previous_receive_color_ = 0;
|
|
|
|
tf2_ros::TransformBroadcaster lidar2baselink;
|
|
std::thread receive_thread_;
|
|
SendPacket packet_;
|
|
};
|
|
}
|
|
|
|
#endif //RM_SERIAL_RM_SERIAL_HPP
|