houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/vcu_driver/CMakeLists.txt
+139
View File
@@ -0,0 +1,139 @@
cmake_minimum_required(VERSION 3.0.2)
project(vcu_driver)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
can_msgs
geometry_msgs
roscpp
rospy
message_generation
nav_msgs
tf
sensor_msgs
autoware_msgs
std_srvs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
RpmCmd.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES vcu_driver
CATKIN_DEPENDS can_msgs geometry_msgs roscpp rospy nav_msgs tf std_msgs sensor_msgs autoware_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(vcu_driver_node src/vcu_driver_core.cpp src/vcu_driver_node.cpp)
target_link_libraries(vcu_driver_node ${catkin_LIBRARIES})
add_dependencies(vcu_driver_node
can_msgs_generate_messages_cpp
vcu_driver_generate_messages_cpp
)
add_executable(useful_info_visualizer_on_rviz_main src/useful_info_visualizer_on_rviz_main.cpp)
target_link_libraries(useful_info_visualizer_on_rviz_main ${catkin_LIBRARIES})
add_dependencies(useful_info_visualizer_on_rviz_main vcu_driver_generate_messages_cpp)
sensors/vcu_driver/include/vcu_driver_core.h
+174
View File
@@ -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
sensors/vcu_driver/launch/vcu_driver.launch
+66
View File
@@ -0,0 +1,66 @@
<launch>
<!-- 自动驾驶的指令 -->
<arg name="input_movebase_ctrl_cmd_topic_" default="/cmd_vel"/>
<!-- 脱困状态下的控制指令 -->
<arg name="input_extricate_ctrl_cmd_topic_" default="/extricate_cmd"/>
<!-- 远程控制的指令 -->
<arg name="input_remote_ctrl_cmd_topic_" default="/remote_ctrl"/>
<arg name="input_canframe_topic_" default="/received_messages_vcuDriver"/>
<arg name="output_canframe_topic_" default="/sent_messages_vcuDriver"/>
<!-- 里程计 -->
<arg name="output_vehicleodom_topic_" default="/vehicleodom"/>
<!-- 电池数据 -->
<arg name="input_battery_state_topic_" default="/sensor/battery_state"/>
<!-- 急停状态 -->
<arg name="input_e_stop_topic_" default="/e_stop_states"/>
<!-- 输出的轮速指令 -->
<arg name="output_rps_topic_" default="vcu_driver/rps_cmd"/>
<!-- 车轮电机状态 -->
<arg name="output_motor_status_topic_" default="/motor_status"/>
<!-- 轮距 -->
<arg name="track_base_" default="0.435"/>
<!-- 轮子直径 -->
<arg name="track_radius_" default="0.205"/>
<!-- 减速比 -->
<arg name="gear_ratio_" default="1.0"/>
<!-- 最大转速限制 -->
<arg name="max_rps_" default="300.0"/>
<!-- 最大车直行速度和角速度限制 -->
<arg name="max_carVelocity_" default="2.0"/>
<!-- 指令时间间隔限制 -->
<arg name="cmd_timeout_" default="0.3"/>
<!-- 里程计误差修正参数 -->
<arg name="odom_x_scale_" default="1.0"/>
<arg name="odom_y_scale_" default="1.0"/>
<arg name="odom_z_scale_positive_" default="1.0"/>
<arg name="odom_z_scale_negative_" default="1.0"/>
<node pkg="vcu_driver" type="vcu_driver_node" name="vcu_driver_node" output="screen">
<param name="input_movebase_ctrl_cmd_topic_" value="$(arg input_movebase_ctrl_cmd_topic_)" type="string" />
<param name="input_extricate_ctrl_cmd_topic_" value="$(arg input_extricate_ctrl_cmd_topic_)" type="string" />
<param name="input_remote_ctrl_cmd_topic_" value="$(arg input_remote_ctrl_cmd_topic_)" type="string" />
<param name="input_canframe_topic_" value="$(arg input_canframe_topic_)" type="string" />
<param name="output_canframe_topic_" value="$(arg output_canframe_topic_)" type="string" />
<param name="output_rps_topic_" value="$(arg output_rps_topic_)" type="string" />
<param name="output_vehicleodom_topic_" value="$(arg output_vehicleodom_topic_)" type="string" />
<param name="output_motor_status_topic_" value="$(arg output_motor_status_topic_)" type="string" />
<param name="track_base_" value="$(arg track_base_)" type="double" />
<param name="track_radius_" value="$(arg track_radius_)" type="double" />
<param name="gear_ratio_" value="$(arg gear_ratio_)" type="double" />
<param name="max_rps_" value="$(arg max_rps_)" type="double" />
<param name="cmd_timeout_" value="$(arg cmd_timeout_)" type="double" />
<param name="odom_x_scale_" value="$(arg odom_x_scale_)" type="double" />
<param name="odom_y_scale_" value="$(arg odom_y_scale_)" type="double" />
<param name="odom_z_scale_positive_" value="$(arg odom_z_scale_positive_)" type="double" />
<param name="odom_z_scale_negative_" value="$(arg odom_z_scale_negative_)" type="double" />
</node>
</launch>
sensors/vcu_driver/msg/RpmCmd.msg
+2
View File
@@ -0,0 +1,2 @@
float32 left_rps
float32 right_rps
sensors/vcu_driver/package.xml
+82
View File
@@ -0,0 +1,82 @@
<?xml version="1.0"?>
<package format="2">
<name>vcu_driver</name>
<version>0.0.0</version>
<description>The vcu_driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="zhangshu@todo.todo">zhangshu</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/vcu_driver</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>can_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_export_depend>can_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_depend>std_msgs</build_depend> <!-- 若已存在则保留 -->
<build_depend>sensor_msgs</build_depend> <!-- 假设需要 -->
<exec_depend>can_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<build_depend>nav_msgs</build_depend>
<exec_depend>nav_msgs</exec_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_depend>autoware_msgs</build_depend>
<exec_depend>autoware_msgs</exec_depend>
<depend>std_srvs</depend>
<depend>tf</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/vcu_driver/src/useful_info_visualizer_on_rviz_main.cpp
+116
View File
@@ -0,0 +1,116 @@
#include <iomanip>
#include <sstream>
#include <string>
#include <jsk_rviz_plugins/OverlayText.h>
#include <ros/ros.h>
#include <sensor_msgs/BatteryState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/ColorRGBA.h>
#include "rc_receiver/rc.h"
#include "autoware_msgs/MotorStatus.h"
template <typename T>
std::string FormatFloat(T value, int precision) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(precision) << value;
return oss.str();
}
class UsefulInfoVisualizer {
public:
UsefulInfoVisualizer() : nh_(""), private_nh_("~") {
timer_ =
nh_.createTimer(ros::Duration(1.0), &UsefulInfoVisualizer::main_process, this, false, true);
useful_text_pub_ =
nh_.advertise<jsk_rviz_plugins::OverlayText>("/chassis/useful_info_text", 10);
const std::string estop_topic = private_nh_.param<std::string>("estop_topic", "/e_stop_replay");
const std::string battery_state_topic =
private_nh_.param<std::string>("battery_state_topic", "/sensor/battery_state");
const std::string remote_ctrl_topic =
private_nh_.param<std::string>("remote_ctrl_topic", "/remote_ctrl");
const std::string motor_status_topic =
private_nh_.param<std::string>("motor_status_topic", "/motor_status");
estop_sub_ = nh_.subscribe(estop_topic, 5, &UsefulInfoVisualizer::estop_callback, this);
battery_sub_ =
nh_.subscribe(battery_state_topic, 5, &UsefulInfoVisualizer::battery_state_callback, this);
remote_ctrl_sub_ =
nh_.subscribe(remote_ctrl_topic, 5, &UsefulInfoVisualizer::remote_ctrl_callback, this);
motor_status_sub_ =
nh_.subscribe(motor_status_topic, 5, &UsefulInfoVisualizer::motor_status_callback, this);
}
void estop_callback(const std_msgs::Bool& estop_msg) {
estop_text_ = (estop_msg.data ? "True" : "False");
}
void battery_state_callback(const sensor_msgs::BatteryState& battery_msg) {
battery_text_ = FormatFloat(battery_msg.percentage, 1) + "%";
}
void remote_ctrl_callback(const rc_receiver::rc& remote_ctrl_msg) {
remote_ctrl_text_ = (remote_ctrl_msg.control_mode == 1 ? "Remote control" : "Auto drive");
}
void motor_status_callback(const autoware_msgs::MotorStatus& motor_status_msg) {
motor_status_text_ = (motor_status_msg.alarm_code == 0 ? "False" : "True");
}
void main_process(const ros::TimerEvent& event) {
jsk_rviz_plugins::OverlayText text_msg;
text_msg.width = 320;
text_msg.height = 90;
text_msg.left = 10;
text_msg.top = 240;
text_msg.text_size = 9;
text_msg.line_width = 1;
text_msg.font = "DejaVu Sans Mono";
std::string text = "Chassis info display:\n";
text += "- current time: " + std::to_string(ros::Time::now().toSec()) + "\n";
text += "- E-Stop: " + estop_text_ + "\n";
text += "- Battery level: " + battery_text_ + "\n";
text += "- Control mode: " + remote_ctrl_text_ + "\n";
text += "- Motor alarm: " + motor_status_text_ + "\n";
text_msg.text = text;
std_msgs::ColorRGBA bg_color;
bg_color.r = 0.0;
bg_color.g = 0.0;
bg_color.b = 0.0;
bg_color.a = 0.4;
text_msg.bg_color = bg_color;
std_msgs::ColorRGBA fg_color;
fg_color.r = 1.0;
fg_color.g = 1.0;
fg_color.b = 1.0;
fg_color.a = 1.0;
text_msg.fg_color = fg_color;
useful_text_pub_.publish(text_msg);
}
private:
ros::Publisher useful_text_pub_;
ros::Subscriber estop_sub_;
ros::Subscriber battery_sub_;
ros::Subscriber remote_ctrl_sub_;
ros::Subscriber motor_status_sub_;
ros::Timer timer_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
std::string useful_text_;
std::string estop_text_ = "None";
std::string battery_text_ = "None";
std::string remote_ctrl_text_ = "None";
std::string motor_status_text_ = "None";
};
int main(int argc, char** argv) {
ros::init(argc, argv, "useful_info_visualizer");
UsefulInfoVisualizer useful_info_visualizer;
ros::spin();
}
sensors/vcu_driver/src/vcu_driver_core.cpp
+857
View File
@@ -0,0 +1,857 @@
#include "vcu_driver_core.h"
Vcu::Vcu() : nh_(""), private_nh_("~")
{
private_nh_.param<std::string>("input_movebase_ctrl_cmd_topic_", input_movebase_ctrl_cmd_topic_,
"");
private_nh_.param<std::string>("input_extricate_ctrl_cmd_topic_", input_extricate_ctrl_cmd_topic_,
"");
private_nh_.param<std::string>("input_remote_ctrl_cmd_topic_", input_remote_ctrl_cmd_topic_, "");
private_nh_.param<std::string>("input_canframe_topic_", input_canframe_topic_, "");
private_nh_.param<std::string>("input_e_stop_topic_", input_e_stop_topic_, "");
private_nh_.param<std::string>("input_battery_state_topic_", input_battery_state_topic_, "");
private_nh_.param<std::string>("output_canframe_topic_", output_canframe_topic_, "");
private_nh_.param<std::string>("output_rps_topic_", output_rps_topic_, "");
private_nh_.param<std::string>("output_vehicleodom_topic_", output_vehicleodom_topic_, "");
private_nh_.param<std::string>("output_motor_status_topic_", output_motor_status_topic_, "");
private_nh_.param<double>("track_base_", track_base_, 0.435);
private_nh_.param<double>("track_radius_", track_radius_, 0.205);
private_nh_.param<double>("gear_ratio_", gear_ratio_, 1.0);
private_nh_.param<double>("max_rps_", max_rps_, 300.0);
private_nh_.param<double>("max_carVelocity_", max_carVelocity_, 2.0);
private_nh_.param<double>("cmd_timeout_", cmd_timeout_, 0.3); // 新增：命令超时时间(秒)
// Odometer correction parameters
// 里程计误差修正参数
private_nh_.param<double>("odom_x_scale_", odom_x_scale_, 1.0);
private_nh_.param<double>("odom_y_scale_", odom_y_scale_, 1.0);
private_nh_.param<double>("odom_z_scale_positive_", odom_z_scale_positive_, 1.0);
private_nh_.param<double>("odom_z_scale_negative_", odom_z_scale_negative_, 1.0);
private_nh_.param<std::string>("odom_frame_id", odom_frame_id,
"odom_combined"); // The odometer topic corresponds to the parent
// TF coordinate //里程计话题对应父TF坐标
private_nh_.param<std::string>("robot_frame_id", robot_frame_id,
"base_footprint"); // The odometer topic corresponds to sub-TF
// coordinates //里程计话题对应子TF坐标
// 注册10字母服务（服务名：WheelMotor）
wheel_motor_srv_ = nh_.advertiseService("WheelMotor", // 严格10字母服务名称
&Vcu::wheelMotorCallback, this);
// 注册服务，服务名为 "e_stop_is_enabled_service"，回调函数为 e_stop_is_enabled_callback
e_stop_is_enabled_srv_ =
nh_.advertiseService("e_stop_is_enabled_service", &Vcu::e_stop_is_enabled_callback, this);
// 初始化变量
last_movebase_cmd_time_ = ros::Time::now();
extricate_cmd_flag = 0;
last_remote_cmd_time_ = ros::Time::now();
last_can_frame_callback_time_ = ros::Time::now();
_Last_odom_Time = ros::Time::now();
last_clearAlarm_time_ = ros::Time::now();
clearAlarm_timeSpan = 100;
last_vx_ = 0.0;
last_angular_ = 0.0;
vx_RealSpeed = 0.0;
vz_RealSpeed = 0.0;
rps_L_uint = 0;
rps_r_uint = 0;
power_status = 0;
control_mode = 0;
clearMotorAlarm_flag = 0;
motor_alarm_timecount = 0;
overTempAlarm_flag = 0;
emergencyStopButton_status = false;
emergencyStopButton_enabled = true;
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
}
Vcu::~Vcu() {}
void Vcu::run()
{
movebase_ctrl_cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(
input_movebase_ctrl_cmd_topic_, 1, &Vcu::movebase_ctrl_ctrl_cmd_callback, this);
extricate_ctrl_cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(
input_extricate_ctrl_cmd_topic_, 1, &Vcu::extricate_ctrl_ctrl_cmd_callback, this);
remote_ctrl_cmd_sub_ = nh_.subscribe("/remote_ctrl", 10, &Vcu::remote_ctrl_cmd_callback, this);
can_frame_sub_ =
nh_.subscribe<can_msgs::Frame>(input_canframe_topic_, 1, &Vcu::can_frame_callback, this);
battery_sub_ = nh_.subscribe<sensor_msgs::BatteryState>(input_battery_state_topic_, 1,
&Vcu::battery_callback, this);
e_stop_sub_ = nh_.subscribe("/e_stop_states", 10, &Vcu::e_stop_callback, this);
battery_sub_ = nh_.subscribe("/sensor/battery_state", 10, &Vcu::battery_callback, this);
sent_canframe_pub_ = nh_.advertise<can_msgs::Frame>(output_canframe_topic_, 1);
rps_cmd_pub_ = nh_.advertise<vcu_driver::RpmCmd>(output_rps_topic_, 1);
vehicleodom_pub_ = nh_.advertise<nav_msgs::Odometry>(output_vehicleodom_topic_, 1);
motorstatus_pub_ = nh_.advertise<autoware_msgs::MotorStatus>(output_motor_status_topic_, 10);
ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &Vcu::process, this, false, true);
ros::Timer can_send_timer = nh_.createTimer(ros::Duration(0.02), &Vcu::can_send, this, false, true);
ros::spin();
}
void Vcu::can_frame_callback(const can_msgs::Frame::ConstPtr &msg)
{
if (msg->id == 0x581)
{
if (msg->data[1] == 0x6c && msg->data[2] == 0x60 &&
msg->data[3] == 0x03) // 左右电机实时速度返回
{
short motorL_realSpeed = (unsigned short)msg->data[4] + (unsigned short)(msg->data[5] << 8);
short motorR_realSpeed = (unsigned short)msg->data[6] + (unsigned short)(msg->data[7] << 8);
/* 将电机实时转速转换成线速度 */
float vL = motorL_realSpeed / 600.0 * track_radius_ * M_PI;
float vR = -motorR_realSpeed / 600.0 * track_radius_ * M_PI;
// /* 转换成x轴 z轴速度(0.001m/s) 用于里程计计算 */
vx_RealSpeed = ((vL + vR) * 0.5);
vz_RealSpeed = ((-vL + vR) / track_base_);
// ROS_INFO_STREAM("real_vel_Odometer: "
// << "\nlinear : " << vx_RealSpeed
// << "\nangular : " << vz_RealSpeed);
}
else if (msg->data[1] == 0x3F && msg->data[2] == 0x60 && msg->data[3] == 0x00) // 报警信息
{
motorStatus.alarm_code = (unsigned int)msg->data[4] + (unsigned int)(msg->data[5] << 8) +
(unsigned int)(msg->data[6] << 16) +
(unsigned int)(msg->data[7] << 24);
if (motorStatus.alarm_code == 0)
{
motorStatus.alarm_status = "no alarm";
}
else
{
std::string status;
// 检查每个可能的错误位并追加英文描述
if (motorStatus.alarm_code & 0x00010000)
status += (status.empty() ? std::string("") : std::string(", ")) + "overvoltage";
if (motorStatus.alarm_code & 0x00020000)
status += (status.empty() ? std::string("") : std::string(", ")) + "undervoltage";
if (motorStatus.alarm_code & 0x01000000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "EEPROM read/write error";
// 左电机错误 (Left motor errors)
if (motorStatus.alarm_code & 0x00000004)
status += (status.empty() ? std::string("") : std::string(", ")) + "Lmotor overcurrent";
if (motorStatus.alarm_code & 0x00000008)
status += (status.empty() ? std::string("") : std::string(", ")) + "Lmotor overload";
if (motorStatus.alarm_code & 0x00000010)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Lmotor current deviation";
if (motorStatus.alarm_code & 0x00000020)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Lmotor encoder deviation";
if (motorStatus.alarm_code & 0x00000040)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Lmotor speed deviation";
if (motorStatus.alarm_code & 0x00000080)
status += (status.empty() ? std::string("") : std::string(", ")) +
"Lmotor reference voltage error";
if (motorStatus.alarm_code & 0x00000200)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Lmotor hall sensor error";
if (motorStatus.alarm_code & 0x00000400)
{
status += (status.empty() ? std::string("") : std::string(", ")) + "Lmotor overheating";
overTempAlarm_flag = 1;
}
if (motorStatus.alarm_code & 0x00000800)
status += (status.empty() ? std::string("") : std::string(", ")) + "Lmotor encoder error";
if (motorStatus.alarm_code & 0x00002000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Lmotor speed command error";
// 右电机错误 (Right motor errors)
if (motorStatus.alarm_code & 0x00040000)
status += (status.empty() ? std::string("") : std::string(", ")) + "Rmotor overcurrent";
if (motorStatus.alarm_code & 0x00080000)
status += (status.empty() ? std::string("") : std::string(", ")) + "Rmotor overload";
if (motorStatus.alarm_code & 0x00100000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Rmotor current deviation";
if (motorStatus.alarm_code & 0x00200000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Rmotor encoder deviation";
if (motorStatus.alarm_code & 0x00400000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Rmotor speed deviation";
if (motorStatus.alarm_code & 0x00800000)
status += (status.empty() ? std::string("") : std::string(", ")) +
"Rmotor reference voltage error";
if (motorStatus.alarm_code & 0x02000000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Rmotor hall sensor error";
if (motorStatus.alarm_code & 0x04000000)
{
status += (status.empty() ? std::string("") : std::string(", ")) + "Rmotor overheating";
overTempAlarm_flag = 1;
}
if (motorStatus.alarm_code & 0x08000000)
status += (status.empty() ? std::string("") : std::string(", ")) + "Rmotor encoder error";
if (motorStatus.alarm_code & 0x20000000)
status +=
(status.empty() ? std::string("") : std::string(", ")) + "Rmotor speed command error";
motorStatus.alarm_status = status;
}
}
else if (msg->data[1] == 0x41 && msg->data[2] == 0x60 && msg->data[3] == 0x00) // 状态字
{
motorStatus.status_code = (unsigned int)msg->data[4] + (unsigned int)(msg->data[5] << 8) +
(unsigned int)(msg->data[6] << 16) +
(unsigned int)(msg->data[7] << 24);
}
else if (msg->data[1] == 0x77 && msg->data[2] == 0x60 && msg->data[3] == 0x03) // 输出电流
{
motorStatus.leftMotor_current = ((short)msg->data[4] + (short)(msg->data[5] << 8)) / 10.0f;
motorStatus.rightMotor_current = ((short)msg->data[6] + (short)(msg->data[7] << 8)) / 10.0f;
}
else if (msg->data[1] == 0x32 && msg->data[2] == 0x20 && msg->data[3] == 0x01) // 左电机温度
{
motorStatus.leftMotor_temperature =
(int)((unsigned int)msg->data[4] + (unsigned int)(msg->data[5] << 8) +
(unsigned int)(msg->data[6] << 16) + (unsigned int)(msg->data[7] << 24)) /
10.0f;
}
else if (msg->data[1] == 0x32 && msg->data[2] == 0x20 && msg->data[3] == 0x02) // 右电机温度
{
motorStatus.rightMotor_temperature =
((int)msg->data[4] + (int)(msg->data[5] << 8) +
(int)(msg->data[6] << 16) + (int)(msg->data[7] << 24)) /
10.0f;
}
else if (msg->data[1] == 0x32 && msg->data[2] == 0x20 && msg->data[3] == 0x03) // 驱动器温度
{
motorStatus.driver_temperature =
((int)msg->data[4] + (int)(msg->data[5] << 8) +
(int)(msg->data[6] << 16) + (int)(msg->data[7] << 24)) /
10.0f;
}
// 记录最新指令时间
last_can_frame_callback_time_ = ros::Time::now();
}
}
void Vcu::battery_callback(const sensor_msgs::BatteryState::ConstPtr &msg)
{
power_status = msg->power_supply_status;
// ROS_INFO("Battery power supply status: %d", power_status);
}
void Vcu::e_stop_callback(const std_msgs::Bool::ConstPtr &msg)
{
emergencyStopButton_status = msg->data;
if (emergencyStopButton_status == false)
{
emergencyStopButton_enabled = true;
}
if (emergencyStopButton_status == true && emergencyStopButton_enabled == false)
{
emergencyStopButton_status = false;
}
// ROS_INFO("e_stop status: %d", emergencyStopButton_status);
}
void Vcu::movebase_ctrl_ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
geometry_msgs::TwistStamped twist_;
twist_.header.stamp = ros::Time().now();
twist_.header.frame_id = "base_link";
twist_.twist = *msg;
movebase_ctrl_ctrl_cmd_ = std::make_shared<geometry_msgs::TwistStamped>(twist_);
// 记录最新指令时间
last_movebase_cmd_time_ = ros::Time::now();
// ROS_INFO_STREAM("Received movebase command: "
// << "\nlinear : " << msg->linear.x
// << "\nangular : " << msg->angular.z);
}
void Vcu::extricate_ctrl_ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
geometry_msgs::TwistStamped twist_;
twist_.header.stamp = ros::Time().now();
twist_.header.frame_id = "base_link";
twist_.twist = *msg;
extricate_ctrl_ctrl_cmd_ = std::make_shared<geometry_msgs::TwistStamped>(twist_);
// 记录最新指令标记置1
extricate_cmd_flag = 1;
// ROS_INFO_STREAM("Received movebase command: "
// << "\nlinear : " << msg->linear.x
// << "\nangular : " << msg->angular.z);
}
void Vcu::remote_ctrl_cmd_callback(const rc_receiver::rc::ConstPtr &msg)
{
// 直接使用接收到的rc消息
twist_.header.frame_id = "base_link";
twist_.header.stamp = ros::Time::now();
// 从rc消息中提取线速度和角速度
twist_.twist.linear.x = msg->linear_x;
twist_.twist.angular.z = msg->angular_z;
control_mode = msg->control_mode;
remote_ctrl_ctrl_cmd_ = std::make_shared<geometry_msgs::TwistStamped>(twist_);
last_remote_cmd_time_ = ros::Time::now();
}
void Vcu::process(const ros::TimerEvent &e)
{
double vx = 0.0;
double angular = 0.0;
float rps_l, rps_r;
if (emergencyStopButton_status == true && control_mode != 1) // 急停被按下且控制模式不为遥控模式
{
rps_l = 0.0;
rps_r = 0.0;
// 更新上一次的速度值
last_vx_ = 0.0;
last_angular_ = 0.0;
// ROS_WARN_STREAM("e_StopButton is pressed");
}
else
{
/* 若控制模式为航模遥控模式 */
if (control_mode && (ros::Time::now() - last_remote_cmd_time_).toSec() < cmd_timeout_ &&
remote_ctrl_ctrl_cmd_)
{
vx = remote_ctrl_ctrl_cmd_->twist.linear.x;
angular = remote_ctrl_ctrl_cmd_->twist.angular.z;
// ROS_INFO_STREAM("Using remote control command: " << "\nlinear : " << vx
// << "\nangular : " << angular);
}
else
{
/* 若控制模式为自动驾驶 */
if (control_mode == 0 &&
(ros::Time::now() - last_movebase_cmd_time_).toSec() < cmd_timeout_ &&
movebase_ctrl_ctrl_cmd_)
{
vx = movebase_ctrl_ctrl_cmd_->twist.linear.x;
angular = movebase_ctrl_ctrl_cmd_->twist.angular.z;
// ROS_INFO_STREAM("Using movebase command: " << "\nlinear : " << vx
// << "\nangular : " << angular);
}
else if (extricate_cmd_flag && extricate_ctrl_ctrl_cmd_ &&
(ros::Time::now() - extricate_ctrl_ctrl_cmd_->header.stamp).toSec() <
cmd_timeout_)
{
vx = extricate_ctrl_ctrl_cmd_->twist.linear.x;
angular = extricate_ctrl_ctrl_cmd_->twist.angular.z;
extricate_cmd_flag = 0;
ROS_INFO_STREAM("Using extricate commond: "
<< " linear : " << vx << " angular : " << angular);
// ROS_INFO_STREAM("Using movebase command: " << "\nlinear : " << vx
// << "\nangular : " << angular);
}
else
{
vx = 0.0;
angular = 0.0;
}
}
static uint8_t ros_info_count = 0; // 1秒打印一次
if (ros_info_count++ > 15)
{
ROS_INFO_STREAM("mode : " << (control_mode == 0 ? "auto" : "remote")
<< " linear : " << vx << " angular : " << angular);
ros_info_count = 0;
}
// 限制整车速度大小
if (vx > max_carVelocity_)
vx = max_carVelocity_;
if (vx < -max_carVelocity_)
vx = -max_carVelocity_;
if (angular > max_carVelocity_)
angular = max_carVelocity_;
if (angular < -max_carVelocity_)
angular = -max_carVelocity_;
// 速度变化限幅控制
const double MAX_CHANGE = 0.14; // 最大变化量
if (vx > last_vx_ + MAX_CHANGE)
vx = last_vx_ + MAX_CHANGE;
else if (vx < last_vx_ - MAX_CHANGE)
vx = last_vx_ - MAX_CHANGE;
if (angular > last_angular_ + MAX_CHANGE)
angular = last_angular_ + MAX_CHANGE;
else if (angular < last_angular_ - MAX_CHANGE)
angular = last_angular_ - MAX_CHANGE;
// 更新上一次的速度值
last_vx_ = vx;
last_angular_ = angular;
/* 计算左轮和右轮的线速度 */
double vl, vr;
vr = vx + angular * track_base_ / 2.0;
vl = vx - angular * track_base_ / 2.0;
/* 计算左右轮转速 rps 右侧电机是反向安装的 */
rps_l = gear_ratio_ * vl / (M_PI * track_radius_) * 60.0;
rps_r = -gear_ratio_ * vr / (M_PI * track_radius_) * 60.0;
// 限制rps大小
if (rps_l > max_rps_)
rps_l = max_rps_;
if (rps_l < -max_rps_)
rps_l = -max_rps_;
if (rps_r > max_rps_)
rps_r = max_rps_;
if (rps_r < -max_rps_)
rps_r = -max_rps_;
}
// vcu_driver::RpmCmd rpm_cmd;
// rpm_cmd.left_rps = rps_l;
// rpm_cmd.right_rps = rps_r;
// rps_cmd_pub_.publish(rpm_cmd);
rps_L_uint = static_cast<int16_t>(rps_l + 0.5); // 转换为16位整数
rps_r_uint = static_cast<int16_t>(rps_r + 0.5); // 转换为16位整数
}
void Vcu::can_send(const ros::TimerEvent &e)
{
static uint8_t time_slice = 0;
if (time_slice == 0)//利用时间片计算每100ms下发一次速度控制指令
{
/* 下发速度控制CAN指令 */
can_msgs::Frame sent_can_frame_speedControl;
sent_can_frame_speedControl.header.stamp = ros::Time().now();
sent_can_frame_speedControl.id = 0x601;
sent_can_frame_speedControl.is_extended = false;
sent_can_frame_speedControl.dlc = 0x08;
sent_can_frame_speedControl.data[0] = 0x23; // 命令码
sent_can_frame_speedControl.data[1] = 0xff; // 索引低位
sent_can_frame_speedControl.data[2] = 0x60; // 索引高位
sent_can_frame_speedControl.data[3] = 0x03;
sent_can_frame_speedControl.data[4] = rps_L_uint & 0xFF; // 低字节
sent_can_frame_speedControl.data[5] = (rps_L_uint >> 8) & 0xFF; // 高字节
sent_can_frame_speedControl.data[6] = rps_r_uint & 0xFF; // 低字节
sent_can_frame_speedControl.data[7] = (rps_r_uint >> 8) & 0xFF; // 高字节
sent_canframe_pub_.publish(sent_can_frame_speedControl);
time_slice = 1;
}
else if (time_slice == 1)
{
/* 下发速度查询CAN指令 */
can_msgs::Frame sent_can_frame_speedQuery;
sent_can_frame_speedQuery.header.stamp = ros::Time().now();
sent_can_frame_speedQuery.id = 0x601;
sent_can_frame_speedQuery.is_extended = false;
sent_can_frame_speedQuery.dlc = 0x08;
sent_can_frame_speedQuery.data[0] = 0x40;
sent_can_frame_speedQuery.data[1] = 0x6C;
sent_can_frame_speedQuery.data[2] = 0x60;
sent_can_frame_speedQuery.data[3] = 0x03;
sent_can_frame_speedQuery.data[4] = 0x00;
sent_can_frame_speedQuery.data[5] = 0x00;
sent_can_frame_speedQuery.data[6] = 0x00;
sent_can_frame_speedQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_speedQuery);
time_slice = 2;
}
else if (time_slice == 2)
{
Publish_Odom();
if (power_status == 1) // 正在充电
{
static int count = 0; // 减少发送频次
if (count == 0)
{
/* 下发进入驻车模式CAN指令 */
// 开启指令： 帧 ID：601 数据：2B 26 20 04 01 00 00 00
// 关闭指令： 帧 ID：601 数据：2B 26 20 04 00 00 00 00
can_msgs::Frame sent_can_frame_parkingMode;
sent_can_frame_parkingMode.header.stamp = ros::Time().now();
sent_can_frame_parkingMode.id = 0x601;
sent_can_frame_parkingMode.is_extended = false;
sent_can_frame_parkingMode.dlc = 0x08;
sent_can_frame_parkingMode.data[0] = 0x2B;
sent_can_frame_parkingMode.data[1] = 0x26;
sent_can_frame_parkingMode.data[2] = 0x20;
sent_can_frame_parkingMode.data[3] = 0x04;
sent_can_frame_parkingMode.data[4] = 0x01;
sent_can_frame_parkingMode.data[5] = 0x00;
sent_can_frame_parkingMode.data[6] = 0x00;
sent_can_frame_parkingMode.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_parkingMode);
// ROS_INFO("parkingMode............................ ");
count = 1;
}
else if (count == 1)
{
count = 2;
}
else if (count == 2)
{
count = 0;
}
}
time_slice = 3;
}
else if (time_slice == 3)
{
static uint32_t count_time = 0;
switch (count_time % 7)
{
case 0:
{
/* 下发报警查询CAN指令 */
can_msgs::Frame sent_can_frame_alarmQuery;
sent_can_frame_alarmQuery.header.stamp = ros::Time().now();
sent_can_frame_alarmQuery.id = 0x601;
sent_can_frame_alarmQuery.is_extended = false;
sent_can_frame_alarmQuery.dlc = 0x08;
sent_can_frame_alarmQuery.data[0] = 0x40;
sent_can_frame_alarmQuery.data[1] = 0x3F;
sent_can_frame_alarmQuery.data[2] = 0x60;
sent_can_frame_alarmQuery.data[3] = 0x00;
sent_can_frame_alarmQuery.data[4] = 0x00;
sent_can_frame_alarmQuery.data[5] = 0x00;
sent_can_frame_alarmQuery.data[6] = 0x00;
sent_can_frame_alarmQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_alarmQuery);
break;
}
case 1:
{
/* 下发状态字查询CAN指令 */
can_msgs::Frame sent_can_frame_alarmQuery;
sent_can_frame_alarmQuery.header.stamp = ros::Time().now();
sent_can_frame_alarmQuery.id = 0x601;
sent_can_frame_alarmQuery.is_extended = false;
sent_can_frame_alarmQuery.dlc = 0x08;
sent_can_frame_alarmQuery.data[0] = 0x40;
sent_can_frame_alarmQuery.data[1] = 0x41;
sent_can_frame_alarmQuery.data[2] = 0x60;
sent_can_frame_alarmQuery.data[3] = 0x00;
sent_can_frame_alarmQuery.data[4] = 0x00;
sent_can_frame_alarmQuery.data[5] = 0x00;
sent_can_frame_alarmQuery.data[6] = 0x00;
sent_can_frame_alarmQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_alarmQuery);
break;
}
case 2:
{
/* 下发输出电流查询CAN指令 */
can_msgs::Frame sent_can_frame_currentQuery;
sent_can_frame_currentQuery.header.stamp = ros::Time().now();
sent_can_frame_currentQuery.id = 0x601;
sent_can_frame_currentQuery.is_extended = false;
sent_can_frame_currentQuery.dlc = 0x08;
sent_can_frame_currentQuery.data[0] = 0x40;
sent_can_frame_currentQuery.data[1] = 0x77;
sent_can_frame_currentQuery.data[2] = 0x60;
sent_can_frame_currentQuery.data[3] = 0x03;
sent_can_frame_currentQuery.data[4] = 0x00;
sent_can_frame_currentQuery.data[5] = 0x00;
sent_can_frame_currentQuery.data[6] = 0x00;
sent_can_frame_currentQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_currentQuery);
break;
}
case 3:
{
/* 下发温度查询CAN指令 */
can_msgs::Frame sent_can_frame_temperatureMotorLeftQuery;
sent_can_frame_temperatureMotorLeftQuery.header.stamp = ros::Time().now();
sent_can_frame_temperatureMotorLeftQuery.id = 0x601;
sent_can_frame_temperatureMotorLeftQuery.is_extended = false;
sent_can_frame_temperatureMotorLeftQuery.dlc = 0x08;
sent_can_frame_temperatureMotorLeftQuery.data[0] = 0x40;
sent_can_frame_temperatureMotorLeftQuery.data[1] = 0x32;
sent_can_frame_temperatureMotorLeftQuery.data[2] = 0x20;
sent_can_frame_temperatureMotorLeftQuery.data[3] = 0x01;
sent_can_frame_temperatureMotorLeftQuery.data[4] = 0x00;
sent_can_frame_temperatureMotorLeftQuery.data[5] = 0x00;
sent_can_frame_temperatureMotorLeftQuery.data[6] = 0x00;
sent_can_frame_temperatureMotorLeftQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_temperatureMotorLeftQuery);
break;
}
case 4:
{
can_msgs::Frame sent_can_frame_temperatureMotorRightQuery;
sent_can_frame_temperatureMotorRightQuery.header.stamp = ros::Time().now();
sent_can_frame_temperatureMotorRightQuery.id = 0x601;
sent_can_frame_temperatureMotorRightQuery.is_extended = false;
sent_can_frame_temperatureMotorRightQuery.dlc = 0x08;
sent_can_frame_temperatureMotorRightQuery.data[0] = 0x40;
sent_can_frame_temperatureMotorRightQuery.data[1] = 0x32;
sent_can_frame_temperatureMotorRightQuery.data[2] = 0x20;
sent_can_frame_temperatureMotorRightQuery.data[3] = 0x02;
sent_can_frame_temperatureMotorRightQuery.data[4] = 0x00;
sent_can_frame_temperatureMotorRightQuery.data[5] = 0x00;
sent_can_frame_temperatureMotorRightQuery.data[6] = 0x00;
sent_can_frame_temperatureMotorRightQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_temperatureMotorRightQuery);
break;
}
case 5:
{
can_msgs::Frame sent_can_frame_temperatureDriveQuery;
sent_can_frame_temperatureDriveQuery.header.stamp = ros::Time().now();
sent_can_frame_temperatureDriveQuery.id = 0x601;
sent_can_frame_temperatureDriveQuery.is_extended = false;
sent_can_frame_temperatureDriveQuery.dlc = 0x08;
sent_can_frame_temperatureDriveQuery.data[0] = 0x40;
sent_can_frame_temperatureDriveQuery.data[1] = 0x32;
sent_can_frame_temperatureDriveQuery.data[2] = 0x20;
sent_can_frame_temperatureDriveQuery.data[3] = 0x03;
sent_can_frame_temperatureDriveQuery.data[4] = 0x00;
sent_can_frame_temperatureDriveQuery.data[5] = 0x00;
sent_can_frame_temperatureDriveQuery.data[6] = 0x00;
sent_can_frame_temperatureDriveQuery.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_temperatureDriveQuery);
break;
}
case 6:
{
motorStatus.stamp = ros::Time::now();
motorstatus_pub_.publish(motorStatus); //
break;
}
default:
{
break;
}
}
count_time++;
if (count_time >= 69999)
{
count_time = 0;
}
time_slice = 4;
}
else if (time_slice == 4)
{
if ((motorStatus.alarm_code != 0 && overTempAlarm_flag == 0) || (overTempAlarm_flag == 1 && motorStatus.leftMotor_temperature < 60.0 &&
motorStatus.rightMotor_temperature < 60.0 && motorStatus.driver_temperature < 60.0))
{
motor_alarm_timecount++;
if (motor_alarm_timecount > clearAlarm_timeSpan) // 超过规定时间自动清除告警
{
if ((ros::Time::now() - last_clearAlarm_time_).toSec() * 10 < (clearAlarm_timeSpan + 6000)) // 避免频繁清除（10分钟两次告警判定为频繁）
{
if (clearAlarm_timeSpan < 12000) // 最大清除等待时间为20分钟（20*60*10ms）
clearAlarm_timeSpan *= 2;
}
else
{ // 恢复默认清除等待时间10s
clearAlarm_timeSpan = 100;
}
clearMotorAlarm_flag = 1;
ROS_INFO("Auto clear the alarm");
motor_alarm_timecount = 0;
overTempAlarm_flag = 0;
last_clearAlarm_time_ = ros::Time::now();
}
}
else
{
motor_alarm_timecount = 0;
}
// 处理电机报警清楚的请求
static uint8_t alarmClear_step = 0;
if (clearMotorAlarm_flag)
{
can_msgs::Frame sent_can_frame_clearAlarm; // 清除报警
sent_can_frame_clearAlarm.header.stamp = ros::Time().now();
sent_can_frame_clearAlarm.id = 0x601;
sent_can_frame_clearAlarm.is_extended = false;
sent_can_frame_clearAlarm.dlc = 0x08;
sent_can_frame_clearAlarm.data[0] = 0x2B;
sent_can_frame_clearAlarm.data[1] = 0x40;
sent_can_frame_clearAlarm.data[2] = 0x60;
sent_can_frame_clearAlarm.data[3] = 0x00;
sent_can_frame_clearAlarm.data[4] = 0x80;
sent_can_frame_clearAlarm.data[5] = 0x00;
sent_can_frame_clearAlarm.data[6] = 0x00;
sent_can_frame_clearAlarm.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_clearAlarm);
alarmClear_step = 1;
clearMotorAlarm_flag = 0;
}
else if ((alarmClear_step == 1) && ((motorStatus.status_code & 0x00800080) == 0)) // 当状态码bit7 bit23置0，则可以恢复使能
{
can_msgs::Frame sent_can_frame_recoveryenabling1; // 恢复速度环控制使能1
sent_can_frame_recoveryenabling1.header.stamp = ros::Time().now();
sent_can_frame_recoveryenabling1.id = 0x601;
sent_can_frame_recoveryenabling1.is_extended = false;
sent_can_frame_recoveryenabling1.dlc = 0x08;
sent_can_frame_recoveryenabling1.data[0] = 0x2B;
sent_can_frame_recoveryenabling1.data[1] = 0x40;
sent_can_frame_recoveryenabling1.data[2] = 0x60;
sent_can_frame_recoveryenabling1.data[3] = 0x00;
sent_can_frame_recoveryenabling1.data[4] = 0x06;
sent_can_frame_recoveryenabling1.data[5] = 0x00;
sent_can_frame_recoveryenabling1.data[6] = 0x00;
sent_can_frame_recoveryenabling1.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_recoveryenabling1);
alarmClear_step = 2;
}
else if (alarmClear_step == 2)
{
can_msgs::Frame sent_can_frame_recoveryenabling2; // 恢复速度环控制使能2
sent_can_frame_recoveryenabling2.header.stamp = ros::Time().now();
sent_can_frame_recoveryenabling2.id = 0x601;
sent_can_frame_recoveryenabling2.is_extended = false;
sent_can_frame_recoveryenabling2.dlc = 0x08;
sent_can_frame_recoveryenabling2.data[0] = 0x2B;
sent_can_frame_recoveryenabling2.data[1] = 0x40;
sent_can_frame_recoveryenabling2.data[2] = 0x60;
sent_can_frame_recoveryenabling2.data[3] = 0x00;
sent_can_frame_recoveryenabling2.data[4] = 0x07;
sent_can_frame_recoveryenabling2.data[5] = 0x00;
sent_can_frame_recoveryenabling2.data[6] = 0x00;
sent_can_frame_recoveryenabling2.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_recoveryenabling2);
alarmClear_step = 3;
}
else if (alarmClear_step == 3)
{
can_msgs::Frame sent_can_frame_recoveryenabling3; // 恢复速度环控制使能3
sent_can_frame_recoveryenabling3.header.stamp = ros::Time().now();
sent_can_frame_recoveryenabling3.id = 0x601;
sent_can_frame_recoveryenabling3.is_extended = false;
sent_can_frame_recoveryenabling3.dlc = 0x08;
sent_can_frame_recoveryenabling3.data[0] = 0x2B;
sent_can_frame_recoveryenabling3.data[1] = 0x40;
sent_can_frame_recoveryenabling3.data[2] = 0x60;
sent_can_frame_recoveryenabling3.data[3] = 0x00;
sent_can_frame_recoveryenabling3.data[4] = 0x0F;
sent_can_frame_recoveryenabling3.data[5] = 0x00;
sent_can_frame_recoveryenabling3.data[6] = 0x00;
sent_can_frame_recoveryenabling3.data[7] = 0x00;
sent_canframe_pub_.publish(sent_can_frame_recoveryenabling3);
ROS_INFO("Clear alarm ,code %x", motorStatus.status_code);
alarmClear_step = 0;
}
time_slice = 0;
}
}
/**************************************
Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular
velocity about triaxial, TF parent-child coordinates, and covariance matrix 功能:
发布里程计话题，包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵
***************************************/
void Vcu::Publish_Odom()
{
if ((ros::Time::now() - last_can_frame_callback_time_).toSec() > cmd_timeout_)
{
vx_RealSpeed = 0.0;
vz_RealSpeed = 0.0;
}
Robot_Vel.X = vx_RealSpeed;
Robot_Vel.Y = 0;
Robot_Vel.Z = vz_RealSpeed;
_Now = ros::Time::now();
Sampling_Time =
(_Now - _Last_odom_Time)
.toSec(); // Retrieves time interval, which is used to integrate velocity to obtain
// displacement (mileage) 获取时间间隔，用于积分速度获得位移(里程)
// Odometer correction parameters
// 里程计误差修正
Robot_Vel.X = Robot_Vel.X * odom_x_scale_;
Robot_Vel.Y = Robot_Vel.Y * odom_y_scale_;
if (Robot_Vel.Z >= 0)
Robot_Vel.Z = Robot_Vel.Z * odom_z_scale_positive_;
else
Robot_Vel.Z = Robot_Vel.Z * odom_z_scale_negative_;
Robot_Pos.X += (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) *
Sampling_Time; // Calculate the displacement in the X direction, unit: m
// //计算X方向的位移，单位：m
Robot_Pos.Y += (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) *
Sampling_Time; // Calculate the displacement in the Y direction, unit: m
// //计算Y方向的位移，单位：m
Robot_Pos.Z += Robot_Vel.Z * Sampling_Time; // The angular displacement about the Z axis, in rad
// //绕Z轴的角位移，单位：rad
// Convert the Z-axis rotation Angle into a quaternion for expression
// 把Z轴转角转换为四元数进行表达
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z);
nav_msgs::Odometry odom; // Instance the odometer topic data //实例化里程计话题数据
odom.header.stamp = ros::Time::now();
odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
odom.pose.pose.position.x = Robot_Pos.X; // Position //位置
odom.pose.pose.position.y = Robot_Pos.Y;
odom.pose.pose.position.z = Robot_Pos.Z;
odom.pose.pose.orientation = odom_quat; // Posture, Quaternion converted by Z-axis rotation
// //姿态，通过Z轴转角转换的四元数
odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
odom.twist.twist.linear.x = Robot_Vel.X; // Speed in the X direction //X方向速度
odom.twist.twist.linear.y = Robot_Vel.Y; // Speed in the Y direction //Y方向速度
odom.twist.twist.angular.z = Robot_Vel.Z; // Angular velocity around the Z axis //绕Z轴角速度
// There are two types of this matrix, which are used when the robot is at rest and when it is
// moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature
// pack
// 这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
if (Robot_Vel.X == 0 && Robot_Vel.Y == 0 && Robot_Vel.Z == 0)
// If the velocity is zero, it means that the error of the encoder will be relatively small, and
// the data of the encoder will be considered more reliable
// 如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
else
// If the velocity of the trolley is non-zero, considering the sliding error that may be brought
// by the encoder in motion, the data of IMU is considered to be more reliable
// 如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));
vehicleodom_pub_.publish(odom); // Pub odometer topic //发布里程计话题
_Last_odom_Time = _Now;
}
// 当请求指令为"clear alarm"时，触发清除任务
bool Vcu::wheelMotorCallback(autoware_msgs::WheelMotor::Request &req,
autoware_msgs::WheelMotor::Response &res)
{
// 判断请求指令是否为"clear alarm"
if (req.command == "clear alarm")
{
clearMotorAlarm_flag = 1; // 触发清除任务
res.success = true;
res.info = "Clear alarm triggered";
// ROS_INFO_STREAM(res.info);
}
else
{
res.success = false;
res.info = "Invalid command (only 'clear alarm' is supported)";
// ROS_WARN_STREAM(res.info);
}
return true;
}
// 当请求bool值为false时，使急停功能失效
bool Vcu::e_stop_is_enabled_callback(std_srvs::SetBool::Request &req,
std_srvs::SetBool::Response &res)
{
// req.data 是传入的bool值
emergencyStopButton_enabled = req.data;
ROS_INFO(req.data ? "e_stop is enabled" : "e_stop is disabled");
// 处理逻辑（简单返回成功）
res.success = true;
res.message = "successed to " + std::string(req.data ? "enable" : "disable");
return true;
}
sensors/vcu_driver/src/vcu_driver_node.cpp
+8
View File
@@ -0,0 +1,8 @@
#include "vcu_driver_core.h"
int main(int argc, char **argv){
ros::init(argc, argv, "vcu_node");
Vcu vcu;
vcu.run();
return 0;
}