Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(bms_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  can_msgs
+)
+
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES bms_driver
+#  CATKIN_DEPENDS roscpp sensor_msgs
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(bms_driver_node src/bms_driver_node.cpp src/bms_driver_core.cpp)
+target_link_libraries(bms_driver_node ${catkin_LIBRARIES})
+add_dependencies(bms_driver_node 
+  can_msgs_generate_messages_cpp
+)
+
@@ -0,0 +1,38 @@
+#include "ros/ros.h"
+#include "can_msgs/Frame.h"
+#include "sensor_msgs/BatteryState.h"
+
+#define __APP_NAME__ "bms_driver"
+
+#ifndef BMS_DRIVER_CORE_H
+#define BMS_DRIVER_CORE_H
+
+class BmsDriver{
+    public:
+        BmsDriver();
+        ~BmsDriver();
+        void run();
+
+    private:
+        ros::NodeHandle nh_;
+        ros::NodeHandle private_nh_;
+        ros::Timer timer_;
+        
+    private:
+        std::string input_bms_canframe_topic_;
+        std::string output_bms_canframe_topic_;
+        std::string output_battery_state_topic_;
+    
+    private:
+        sensor_msgs::BatteryState battery_state_;
+    
+    private:
+        ros::Subscriber can_bms_frame_sub_;
+        ros::Publisher battery_status_pub_;
+        ros::Publisher sent_bms_canframe_pub_;
+
+    private:
+        void can_frame_callback(const can_msgs::Frame::ConstPtr& msg);
+        void process(const ros::TimerEvent &e);
+    };
+#endif
@@ -0,0 +1,11 @@
+<launch>
+    <arg name="input_bms_canframe_topic_" default="/receive_canMessages"/>
+    <arg name="output_bms_canframe_topic_" default="/sent_messages_bms"/>
+    <arg name="output_battery_state_topic_" default="/sensor/battery_state"/>
+  
+    <node name="bms_driver_node" pkg="bms_driver" type="bms_driver_node" output="screen">
+        <param name="input_bms_canframe_topic_" value="$(arg input_bms_canframe_topic_)" type="string"/>
+        <param name="output_bms_canframe_topic_" value="$(arg output_bms_canframe_topic_)" type="string"/>
+        <param name="output_battery_state_topic_" value="$(arg output_battery_state_topic_)" type="string"/>
+    </node>
+  </launch>
@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>bms_driver</name>
+  <version>0.0.0</version>
+  <description>The bms_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/bms_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>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>can_msgs</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>can_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>can_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
@@ -0,0 +1,86 @@
+#include "bms_driver_core.h"
+
+BmsDriver::BmsDriver():nh_(""), private_nh_("~"){
+    private_nh_.param<std::string>("input_bms_canframe_topic_", input_bms_canframe_topic_, "");
+    private_nh_.param<std::string>("output_bms_canframe_topic_", output_bms_canframe_topic_, "");
+    private_nh_.param<std::string>("output_battery_state_topic_", output_battery_state_topic_, "");
+    can_bms_frame_sub_ = nh_.subscribe<can_msgs::Frame>(input_bms_canframe_topic_, 1, &BmsDriver::can_frame_callback, this);
+    sent_bms_canframe_pub_ = nh_.advertise<can_msgs::Frame>(output_bms_canframe_topic_, 1);
+    battery_status_pub_ = nh_.advertise<sensor_msgs::BatteryState>(output_battery_state_topic_, 1);
+}
+
+BmsDriver::~BmsDriver(){}
+
+void BmsDriver::run()
+{
+    timer_ = nh_.createTimer(ros::Duration(1.0), &BmsDriver::process, this);
+}
+
+void BmsDriver::can_frame_callback(const can_msgs::Frame::ConstPtr& msg){
+    switch (msg->id)
+    {
+    case 0x04028001:
+        /* 电压 单位v*/
+        battery_state_.voltage = ((msg->data[0]<<8) + msg->data[1])*0.1;
+        /* 电流 单位A */
+        battery_state_.current = 0.1*((msg->data[2]<<8) + msg->data[3] - 30000);
+        /* soc 0.0-1.0 */
+        battery_state_.percentage = ((msg->data[4] <<8) + msg->data[5])/10.0;
+        battery_status_pub_.publish(battery_state_);
+        break;
+
+    case 0x04078001:
+        /* 充放电状态 */
+        switch (msg->data[0])
+        {
+        case 0x00:
+            battery_state_.power_supply_status = battery_state_.POWER_SUPPLY_STATUS_UNKNOWN;
+            break;
+
+        case 0x01:
+            battery_state_.power_supply_status = battery_state_.POWER_SUPPLY_STATUS_CHARGING;
+            // ROS_INFO("POWER_SUPPLY_STATUS_CHARGING");
+            break;
+
+        case 0x02:
+            battery_state_.power_supply_status = battery_state_.POWER_SUPPLY_STATUS_DISCHARGING;
+            break;
+
+        default:
+            break;
+        }
+        battery_status_pub_.publish(battery_state_);
+        break;
+    
+    case 0x040380001:
+        /* 温度 */
+        battery_state_.temperature = msg->data[5]-40.0;
+        battery_status_pub_.publish(battery_state_);
+        break;
+    
+    default:
+        break;
+    }
+}
+
+
+void BmsDriver::process(const ros::TimerEvent& event)
+{
+    /* 下发电池状态查询CAN指令 */
+    can_msgs::Frame sent_can_frame_batteryState;
+    sent_can_frame_batteryState.header.stamp = ros::Time().now();
+    sent_can_frame_batteryState.id = 0x400FF80;
+    sent_can_frame_batteryState.is_extended = true;
+    sent_can_frame_batteryState.dlc = 0x08;
+    sent_can_frame_batteryState.data[0] = 0x00; 
+    sent_can_frame_batteryState.data[1] = 0x00; 
+    sent_can_frame_batteryState.data[2] = 0x00; 
+    sent_can_frame_batteryState.data[3] = 0x00;
+    sent_can_frame_batteryState.data[4] = 0x00;
+    sent_can_frame_batteryState.data[5] = 0x00;
+    sent_can_frame_batteryState.data[6] = 0x00;
+    sent_can_frame_batteryState.data[7] = 0x00;
+
+    sent_bms_canframe_pub_.publish(sent_can_frame_batteryState);
+    // ROS_INFO("sent_bms_canframe_pub");
+}
@@ -0,0 +1,9 @@
+#include "bms_driver_core.h"
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "bms_driver_node");
+    BmsDriver bms_driver;
+    bms_driver.run();
+    ros::spin();
+    return 0;
+}