Initial commit
This commit is contained in:
@@ -0,0 +1,174 @@
|
||||
#ifndef VCU_CORE_H
|
||||
#define VCU_CORE_H
|
||||
|
||||
#define __APP_NAME__ "vcu_node"
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string.h>
|
||||
#include <string>
|
||||
#include <can_msgs/Frame.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include "vcu_driver/RpmCmd.h"
|
||||
#include "nav_msgs/Odometry.h"
|
||||
#include "sensor_msgs/BatteryState.h"
|
||||
#include <autoware_msgs/MotorStatus.h>
|
||||
#include <autoware_msgs/WheelMotor.h>
|
||||
#include <bitset>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <std_srvs/SetBool.h>
|
||||
#include "rc_receiver/rc.h"
|
||||
|
||||
//Covariance matrix for speedometer topic data for robt_pose_ekf feature pack
|
||||
//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包
|
||||
const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 0, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e3 };
|
||||
|
||||
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 1e-9, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e-9 };
|
||||
|
||||
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 0, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e3 };
|
||||
|
||||
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
|
||||
0, 1e-3, 1e-9, 0, 0, 0,
|
||||
0, 0, 1e6, 0, 0, 0,
|
||||
0, 0, 0, 1e6, 0, 0,
|
||||
0, 0, 0, 0, 1e6, 0,
|
||||
0, 0, 0, 0, 0, 1e-9} ;
|
||||
|
||||
//Data structure for speed and position
|
||||
//速度、位置数据结构体
|
||||
typedef struct __Vel_Pos_Data_
|
||||
{
|
||||
double X;
|
||||
double Y;
|
||||
double Z;
|
||||
}Vel_Pos_Data;
|
||||
|
||||
|
||||
class Vcu
|
||||
{
|
||||
public:
|
||||
Vcu();
|
||||
~Vcu();
|
||||
void run();
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle private_nh_;
|
||||
|
||||
// private:
|
||||
// bool left_closed_loop = false;
|
||||
// bool right_closed_loop = false;
|
||||
|
||||
private:
|
||||
std::string input_movebase_ctrl_cmd_topic_;
|
||||
std::string input_extricate_ctrl_cmd_topic_;
|
||||
std::string input_remote_ctrl_cmd_topic_;
|
||||
std::string input_canframe_topic_;
|
||||
std::string output_canframe_topic_;
|
||||
std::string output_rps_topic_;
|
||||
std::string output_vehicleodom_topic_;
|
||||
std::string input_battery_state_topic_;
|
||||
std::string output_motor_status_topic_;
|
||||
std::string input_e_stop_topic_;
|
||||
private:
|
||||
/* 轮距 单位m */
|
||||
double track_base_;
|
||||
/* 轮径 单位m */
|
||||
double track_radius_;
|
||||
double gear_ratio_;
|
||||
double max_rps_;
|
||||
double max_carVelocity_;
|
||||
double cmd_timeout_;
|
||||
double last_vx_{0.0}; // 上一次的线速度
|
||||
double last_angular_{0.0}; // 上一次的角速度
|
||||
double vx_RealSpeed;
|
||||
double vz_RealSpeed;
|
||||
int16_t rps_L_uint,rps_r_uint;
|
||||
double odom_x_scale_;
|
||||
double odom_y_scale_;
|
||||
double odom_z_scale_positive_;
|
||||
double odom_z_scale_negative_;
|
||||
Vel_Pos_Data Robot_Pos; //The position of the robot //机器人的位置
|
||||
Vel_Pos_Data Robot_Vel; //The speed of the robot //机器人的速度
|
||||
ros::Time last_movebase_cmd_time_;
|
||||
ros::Time last_remote_cmd_time_;
|
||||
ros::Time last_can_frame_callback_time_;
|
||||
ros::Time last_clearAlarm_time_;
|
||||
ros::Time _Now, _Last_odom_Time; //Time dependent, used for integration to find displacement (mileage) //时间相关，用于积分求位移(里程)
|
||||
double Sampling_Time; //Sampling time, used for integration to find displacement (mileage) //采样时间，用于积分求位移(里程)
|
||||
std::string robot_frame_id, odom_frame_id;
|
||||
uint8_t power_status;
|
||||
uint8_t extricate_cmd_flag;
|
||||
bool emergencyStopButton_status;
|
||||
bool emergencyStopButton_enabled;
|
||||
uint8_t control_mode;
|
||||
geometry_msgs::TwistStamped twist_;
|
||||
// 全局标志位：清除电机报警
|
||||
uint8_t clearMotorAlarm_flag;
|
||||
|
||||
//服务服务器（服务名：WheelMotor）
|
||||
ros::ServiceServer wheel_motor_srv_;
|
||||
ros::ServiceServer e_stop_is_enabled_srv_;
|
||||
|
||||
// 电机告警触发计时 若不是过温报警则将计时1秒后清除电机报警
|
||||
uint16_t motor_alarm_timecount;
|
||||
// 过温报警标志位
|
||||
uint8_t overTempAlarm_flag;
|
||||
// 告警后的清除等待时间
|
||||
uint16_t clearAlarm_timeSpan;
|
||||
|
||||
private:
|
||||
std::shared_ptr<geometry_msgs::TwistStamped> movebase_ctrl_ctrl_cmd_;
|
||||
std::shared_ptr<geometry_msgs::TwistStamped> extricate_ctrl_ctrl_cmd_;
|
||||
std::shared_ptr<geometry_msgs::TwistStamped> remote_ctrl_ctrl_cmd_;
|
||||
|
||||
private:
|
||||
ros::Subscriber movebase_ctrl_cmd_sub_;
|
||||
ros::Subscriber extricate_ctrl_cmd_sub_;
|
||||
ros::Subscriber remote_ctrl_cmd_sub_;
|
||||
ros::Subscriber can_frame_sub_;
|
||||
ros::Publisher sent_canframe_pub_;
|
||||
ros::Publisher rps_cmd_pub_;
|
||||
ros::Publisher vehicleodom_pub_;
|
||||
ros::Publisher motorstatus_pub_;
|
||||
ros::Subscriber battery_sub_;
|
||||
ros::Subscriber e_stop_sub_;
|
||||
|
||||
private:
|
||||
autoware_msgs::MotorStatus motorStatus;
|
||||
private:
|
||||
void can_frame_callback(const can_msgs::Frame::ConstPtr &msg);
|
||||
void battery_callback(const sensor_msgs::BatteryState::ConstPtr &msg);
|
||||
void e_stop_callback(const std_msgs::Bool::ConstPtr &msg);
|
||||
void movebase_ctrl_ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr &msg);
|
||||
void remote_ctrl_cmd_callback(const rc_receiver::rc::ConstPtr &msg);
|
||||
void extricate_ctrl_ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr &msg);
|
||||
void process(const ros::TimerEvent &e);
|
||||
void can_send(const ros::TimerEvent &e);
|
||||
void Publish_Odom();
|
||||
bool wheelMotorCallback(autoware_msgs::WheelMotor::Request &req,
|
||||
autoware_msgs::WheelMotor::Response &res);
|
||||
bool e_stop_is_enabled_callback(std_srvs::SetBool::Request &req,
|
||||
std_srvs::SetBool::Response &res);
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user