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
.catkin_workspace
+1
View File
@@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration
.clang-format
+40
View File
@@ -0,0 +1,40 @@
---
BasedOnStyle: Google
---
Language: Cpp
Cpp11BracedListStyle: true
Standard: Cpp11
CommentPragmas: '^ NOLINT'
ColumnLimit: 100
# Mimic cpplint style
IncludeCategories:
# Note that the "main" header is priority 0
# The priority is assigned to first match in the ordered list
# Miscelaneous system libraries
- Regex: '<(cxxabi.h|immintrin.h|malloc.h|wait.h|x86intrin.h|cuda.*)>'
Priority: 3
# third-party libraries
- Regex: '<(tinyxml2.h)>'
Priority: 3
# C standard libraries
- Regex: '<(arpa/|netinet/|net/if|sys/)?[^\./]*\.h>'
Priority: 1
# C++ standard libraries
- Regex: '<[^/\./]*>'
Priority: 2
# Experimental or other system libraries
- Regex: '<'
Priority: 3
# Test libs
- Regex: '"(gtest|gmock)/'
Priority: 4
# Protobuf Files
- Regex: '\.pb\.h'
Priority: 6
# Apollo libs
- Regex: '^"(cyber|modules)'
Priority: 7
# The rest
- Regex: '.*'
Priority: 5
---
.gitignore
+20
View File
@@ -0,0 +1,20 @@
.vscode/
build/
devel/
data/
.idea/
Logs
*.active
*.bag
*.tar.gz
*.log
*.py[cod]
*.egg
*.egg-info
/**/build
/**/dist
venv/
.history/
*.bag
使用说明视频/
*.pcd
build_auto_drive.sh
+35
View File
@@ -0,0 +1,35 @@
# vehicle chassis
catkin_make -DCATKIN_WHITELIST_PACKAGES=bms_driver -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=can_driver -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=autoware_msgs -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=vcu_driver -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=e_stop_relay -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=rc_receiver -j10
# sensors
catkin_make -DCATKIN_WHITELIST_PACKAGES=lslidar_msgs -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=lslidar_cx_driver -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=fdilink_ahrs -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=beitian-bg620 -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=pointcloud_to_laserscan -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=mid_360_filter -j10
# need to install Livox-SDK first, if not use livox lidar, comment out this line
catkin_make -DCATKIN_WHITELIST_PACKAGES=livox_ros_driver2 -j10
# localization
catkin_make -DCATKIN_WHITELIST_PACKAGES=ndt_omp -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=fast_gicp -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=hdl_global_localization -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=hdl_localization -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=vel_pose_connect -j10
# navigation
catkin_make -DCATKIN_WHITELIST_PACKAGES=map_server -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=base_local_planner -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=clear_costmap_recovery -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=navfn -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=rotate_recovery -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=move_base -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=global_planner -j10
catkin_make -DCATKIN_WHITELIST_PACKAGES=teb_local_planner -j10
build_depends.sh
Executable
+31
View File
@@ -0,0 +1,31 @@
echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib ' >> ~/.bashrc
echo 'export DISABLE_ROS1_EOL_WARNINGS=1 ' >> ~/.bashrc
echo 'export ROSCONSOLE_FORMAT='\''[${severity}] [${walltime}] [${logger}:${line}] ${message}'\' >> ~/.bashrc
source ~/.bashrc
sudo apt-get install ros-noetic-tf2-sensor-msgs
sudo apt-get install ros-noetic-mbf-costmap-core
sudo apt-get install ros-noetic-mbf-msgs
sudo apt-get install ros-noetic-uuid-msgs
sudo apt-get install ros-noetic-move-base-msgs
sudo apt-get install ros-noetic-nmea-msgs
sudo apt-get install ros-noetic-can-msgs
sudo apt-get install ros-noetic-serial
sudo apt-get install ros-noetic-libg2o
sudo apt-get install ros-noetic-geodesy
sudo apt-get install ros-noetic-costmap-converter
sudo apt-get install ros-noetic-velodyne
sudo apt-get install ros-noetic-mrt-cmake-modules
sudo apt-get install ros-noetic-jsk-rviz-plugins
sudo apt-get install ros-noetic-apriltag
sudo apt-get install libsuitesparse-dev
sudo apt-get install libpugixml-dev
sudo apt-get install libgeographic-dev
sudo apt-get install libudev-dev
sudo apt-get install libmuparser-dev
sudo apt-get install libsdl-image1.2-dev
sudo apt-get install libserial-dev
sudo apt-get install libv4l-dev
sudo apt-get install libpcap-dev
depends/gtsam-4.0.2.zip
BIN
View File
Binary file not shown.
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad.zip
BIN
View File
Binary file not shown.
sensors/beitian-bg620/CMakeLists.txt
+220
View File
@@ -0,0 +1,220 @@
cmake_minimum_required(VERSION 3.0.2)
project(beitian-bg620)
## 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
geometry_msgs
nmea_msgs
nav_msgs
roscpp
rospy
sensor_msgs
std_msgs
tf2
tf2_geometry_msgs
tf2_ros
)
find_package(PkgConfig)
pkg_check_modules(SERIAL libserial)
## 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
# Message1.msg
# Message2.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
# geometry_msgs# nav_msgs# sensor_msgs# std_msgs# tf2_geometry_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 beitian-bg620
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros nmea_msgs serial
# 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}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/MKT4052A_DRIVER.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/MKT4052A_DRIVER_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_MKT4052A_DRIVER.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
add_executable(bg620_serial_reader_node src/bg620_serial_reader_node.cpp src/bg620_serial_reader_core.cpp)
target_link_libraries(bg620_serial_reader_node ${catkin_LIBRARIES} ${SERIAL_LDFLAGS})
add_executable(bg620_sentence_parse_node src/bg620_sentence_parse_node.cpp src/bg620_sentence_parse_core.cpp)
target_link_libraries(bg620_sentence_parse_node ${catkin_LIBRARIES})
sensors/beitian-bg620/README.txt
+1
View File
@@ -0,0 +1 @@
该包为北天BG-620 RTK组合导航驱动包。
sensors/beitian-bg620/include/bg620_sentence_parse_core.h
+106
View File
@@ -0,0 +1,106 @@
#ifndef BG620_SENTENCE_PARSE_H
#define BG620_SENTENCE_PARSE_H
#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include "geometry_msgs/TransformStamped.h"
#include "nmea_msgs/Sentence.h"
#include "sensor_msgs/Imu.h"
#include "nav_msgs/Path.h"
#include "tf/tf.h"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "gpsTools.hpp"
// Added by dawei
#include "sensor_msgs/NavSatFix.h"
#include "nav_msgs/Odometry.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "Eigen/Dense"
#define __APP_NAME__ "bg620_sentence_parse"
class BG620SentenceParse{
public:
BG620SentenceParse();
~BG620SentenceParse();
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
tf2_ros::TransformListener listener;
tf2_ros::Buffer buffer;
private:
std::string outputNavSatFixTopic_;
std::string outputNavSatFixFrameId;
std::string outputOdometryTopic_;
std::string outputOdometryFrameId;
std::string outputLidarOdometryTopic_;
std::string outputLidarOdometryFrameId;
std::string outputTransFromStampedTopic_;
std::string outputTransFromStampedFrameId;
std::string outputImuTopic_;
std::string outputImuFrameId;
std::string outputGpsPathTopic_;
std::string robotOdomFrameId;
bool publishDynamicTf;
std::string inputNmeaSentenceTopic_;
double orinLon;
double orinLat;
double orinAlt;
double e_bias;
double n_bias;
private:
ros::Publisher navSatFixPublisher;
ros::Publisher odometryPublisher;
ros::Publisher lidarOdometryPublisher;
ros::Publisher transformStampedPublisher;
ros::Publisher imuPublisher;
ros::Publisher gpsPathPublisher;
ros::Subscriber nmeaSentenceSubscriber;
tf2_ros::TransformBroadcaster transformBroadcaster;
private:
GpsTools gpsTools;
nav_msgs::Path gpsPath;
private:
std::vector<std::string> splitStr(std::string str, const std::string &pattern);
void nmeaSentenceCallback(const nmea_msgs::Sentence::ConstPtr& msg);
void processBG620Data();
// Addd by Dawei
struct {
sensor_msgs::NavSatFix nav_fix;
double speed = 0.0;
double track_angle = 0.0;
double heading_deg = 0.0;
geometry_msgs::Quaternion orientation;
bool has_gga = false;
bool has_rmc = false;
bool has_heading = false;
ros::Time stamp;
} bg620_data_;
geometry_msgs::Quaternion current_orientation_;
double current_speed_;
Eigen::Quaternionf q;
// 新增辅助函数
double dmToDeg(double dm, char direction);
ros::Time parseUTCTime(const std::string& utc_time, const std::string& date = "");
double deg2rad(double deg) { return deg * M_PI / 180.0; }
};
#endif
sensors/beitian-bg620/include/bg620_serial_reader_core.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef RTK_SERIAL_READER_JIAKAO_H
#define RTK_SERIAL_READER_JIAKAO_H
#include "ros/ros.h"
#include "nmea_msgs/Sentence.h"
#define __APP_NAME__ "rtk_serial_reader_bg620"
class RtkSerialReaderBG620{
public:
RtkSerialReaderBG620();
~RtkSerialReaderBG620();
void run();
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
ros::Publisher sentence_publisher;
private:
std::string outPutSentenceTopic;
std::string gpsComChannel;
int gpsComBaudRate;
};
#endif
sensors/beitian-bg620/include/gpsTools.hpp
+143
View File
@@ -0,0 +1,143 @@
//
// Created by echo on 2019/11/26.
//
#ifndef PCD_COMPARE_GPSTOOLS_H
#define PCD_COMPARE_GPSTOOLS_H
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <boost/foreach.hpp>
#include <geometry_msgs/QuaternionStamped.h>
#include <Eigen/Core>
#define DEG_TO_RAD 0.01745329252
#define EARTH_MAJOR 6378137.0 ///< WGS84 MAJOR AXIS
#define EARTH_MINOR 6356752.31424518 ///< WGS84 MINOR AXIS
class GpsTools {
public:
GpsTools() { lla_origin_.setIdentity(); };
// Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla);
// Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef);
// Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef);
// Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu);
//static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs);
// void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs);
/**
* ros msg to eigen
* @param gps_msgs
* @return
*/
static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs) {
Eigen::Vector3d
lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
return lla;
}
/**
* //2. LLA经度(longitude),纬度(latitude)和高度(altitude)经纬高坐标系 转(Earth-Centered, Earth-Fixed)
* Z轴指向指向北，但不完全精确地与地球转动轴重合。转动轴有微小“摆动”，称之为“极运动(polar motion)”
* X轴在球面上与格林威治线和赤道的交点
* @param lla
* @return
*/
Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla) {
Eigen::Vector3d ecef;
double lat = deg2rad(lla.x());
double lon = deg2rad(lla.y());
double alt = lla.z();
double earth_r = pow(EARTH_MAJOR, 2)
/ sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);
return ecef;
}
Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef) {
double e =
sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2));
double e_ =
sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2));
double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2));
double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR);
double lon = atan2(ecef.y(), ecef.x());
double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)),
p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3));
double earth_r = pow(EARTH_MAJOR, 2)
/ sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
double alt = p / cos(lat) - earth_r;
Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt);
return lla;
}
Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef) {
double lat = deg2rad(lla_origin_.x());
double lon = deg2rad(lla_origin_.y());
Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
Eigen::Matrix3d r;
r << -sin(lon), cos(lon), 0,
-cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);
Eigen::Vector3d enu;
enu = ecef + t;
enu = r * enu;
return enu;
}
Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu) {
double lat = deg2rad(lla_origin_.x());
double lon = deg2rad(lla_origin_.y());
Eigen::Vector3d t = LLA2ECEF(lla_origin_);
Eigen::Matrix3d r;
r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat),
cos(lon), -sin(lon) * sin(lat), sin(lon) * cos(lat),
0, cos(lat), sin(lat);
Eigen::Vector3d ecef;
ecef = r * enu + t;
return ecef;
}
void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs) {
//检查状态4
if (gps_msgs.status.status == 4 || gps_msgs.status.status == 5 || gps_msgs.status.status == 1
|| gps_msgs.status.status == 2) {
//第一个的时候设置为起点
if (lla_origin_ == Eigen::Vector3d::Identity()) {
Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
lla_origin_ = lla;
std::cout << "GPS origin: " << lla_origin_ << "\n status: " << gps_msgs.status.status << std::endl;
} else {
Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
Eigen::Vector3d ecef = LLA2ECEF(lla);
Eigen::Vector3d enu = ECEF2ENU(ecef);
gps_pos_ = enu;
std::cout << "GPS lla_origin_: " << lla_origin_ << "\n curr" << gps_pos_ << std::endl;
}
}
}
//变量部分
//1.lla的起点
Eigen::Vector3d lla_origin_;
//2.enu下的坐标
Eigen::Vector3d gps_pos_;
private:
static inline double deg2rad(const double &deg) {
return deg * DEG_TO_RAD;
};
static inline double rad2deg(const double &rad) {
return rad / DEG_TO_RAD;
}
};
#endif //PCD_COMPARE_GPSTOOLS_H
sensors/beitian-bg620/launch/beitian_bg620_driver.launch
+4
View File
@@ -0,0 +1,4 @@
<launch>
<include file="$(find beitian-bg620)/launch/include/beitian_bg620_serial_reader.launch" />
<include file="$(find beitian-bg620)/launch/include/beitian_bg620_sentence_parse.launch" />
</launch>
sensors/beitian-bg620/launch/include/beitian_bg620_sentence_parse.launch
+69
View File
@@ -0,0 +1,69 @@
<launch>
<arg name="outputNavSatFixTopic_" default="/bg620/gpsFix"/>
<arg name="outputNavSatFixFrameId" default="gps"/>
<arg name="outputOdometryTopic_" default="/bg620/gpsOdom"/>
<arg name="outputOdometryFrameId" default="gps"/>
<arg name="outputLidarOdometryTopic_" default="/bg620/lidarOdom"/>
<arg name="outputLidarOdometryFrameId" default="c32_L"/>
<arg name="outputTransFromStampedTopic_" default="/bg620/gpsTransFromStamped"/>
<arg name="outputTransFromStampedFrameId" default="gps"/>
<arg name="outputImuTopic_" default="/bg620/gpsImu"/>
<arg name="outputImuFrameId" default="gps"/>
<arg name="outputGpsPathTopic_" default="/bg620/gpsPath"/>
<arg name="robotOdomFrameId" default="base_link"/>
<arg name="inputNmeaSentenceTopic_" default="/bg620/nmeaSentence"/>
<arg name="publishDynamicTf" default="false"/>
<!-- origin of dd_yuanqu -->
<!-- <arg name="orinLon" default="116.178987"/>
<arg name="orinLat" default="39.903197"/>
<arg name="orinAlt" default="75.0848"/> -->
<!-- origin of haidian_park -->
<arg name="orinLon" default="116.2899854448"/>
<arg name="orinLat" default="39.98534"/>
<arg name="orinAlt" default="48.389"/>
<!-- origin of caihonghu_park -->
<!-- <arg name="orinLon" default="117.2874076873333"/>
<arg name="orinLat" default="36.67463832666667"/>
<arg name="orinAlt" default="115.1813"/> -->
<arg name="e_bias" default="0.0"/>
<arg name="n_bias" default="0.0"/>
<node name="bg620_sentence_parse_node" pkg="beitian-bg620" type="bg620_sentence_parse_node" output="screen">
<param name="outputNavSatFixTopic_" value="$(arg outputNavSatFixTopic_)" type="string"/>
<param name="outputNavSatFixFrameId" value="$(arg outputNavSatFixFrameId)" type="string"/>
<param name="outputOdometryTopic_" value="$(arg outputOdometryTopic_)" type="string"/>
<param name="outputOdometryFrameId" value="$(arg outputOdometryFrameId)" type="string"/>
<param name="outputLidarOdometryTopic_" value="$(arg outputLidarOdometryTopic_)" type="string"/>
<param name="outputLidarOdometryFrameId" value="$(arg outputLidarOdometryFrameId)" type="string"/>
<param name="outputTransFromStampedTopic_" value="$(arg outputTransFromStampedTopic_)" type="string"/>
<param name="outputTransFromStampedFrameId" value="$(arg outputTransFromStampedFrameId)" type="string"/>
<param name="outputImuTopic_" value="$(arg outputImuTopic_)" type="string"/>
<param name="outputImuFrameId" value="$(arg outputImuFrameId)" type="string"/>
<param name="outputGpsPathTopic_" value="$(arg outputGpsPathTopic_)" type="string"/>
<param name="robotOdomFrameId" value="$(arg robotOdomFrameId)" type="string"/>
<param name="inputNmeaSentenceTopic_" value="$(arg inputNmeaSentenceTopic_)" type="string"/>
<param name="publishDynamicTf" value="$(arg publishDynamicTf)" type="bool"/>
<param name="orinLon" value="$(arg orinLon)" type="double"/>
<param name="orinLat" value="$(arg orinLat)" type="double"/>
<param name="orinAlt" value="$(arg orinAlt)" type="double"/>
<param name="e_bias" value="$(arg e_bias)" type="double"/>
<param name="n_bias" value="$(arg n_bias)" type="double"/>
</node>
</launch>
sensors/beitian-bg620/launch/include/beitian_bg620_serial_reader.launch
+11
View File
@@ -0,0 +1,11 @@
<launch>
<arg name="outPutSentenceTopic" default="/bg620/nmeaSentence"/>
<arg name="gpsComChannel" default="/dev/ttyACM0"/>
<arg name="gpsComBaudRate" default="115200"/>
<node name="bg620_serial_reader_node" pkg="beitian-bg620" type="bg620_serial_reader_node" output="screen">
<param name="outPutSentenceTopic" value="$(arg outPutSentenceTopic)" type="string"/>
<param name="gpsComChannel" value="$(arg gpsComChannel)" type="string"/>
<param name="gpsComBaudRate" value="$(arg gpsComBaudRate)" type="int"/>
</node>
</launch>
sensors/beitian-bg620/package.xml
+97
View File
@@ -0,0 +1,97 @@
<?xml version="1.0"?>
<package format="2">
<name>beitian-bg620</name>
<version>0.0.0</version>
<description>The beitian_bg620 package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="buaazs17@163.com">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/newton-m2</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>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>nmea_msgs</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf2</build_export_depend>
<build_export_depend>tf2_geometry_msgs</build_export_depend>
<build_export_depend>tf2_ros</build_export_depend>
<build_export_depend>nmea_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf2</exec_depend>
<exec_depend>tf2_geometry_msgs</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>nmea_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/beitian-bg620/src/bg620_sentence_parse_core.cpp
+381
View File
@@ -0,0 +1,381 @@
#include "bg620_sentence_parse_core.h"
BG620SentenceParse::BG620SentenceParse():nh_(""), private_nh_("~"), listener(buffer), current_speed_(0.0){
// 参数初始化
private_nh_.param<std::string>("outputNavSatFixTopic_", outputNavSatFixTopic_, "");
private_nh_.param<std::string>("outputNavSatFixFrameId", outputNavSatFixFrameId, "");
// [Debug] Print parameters
ROS_INFO("[BG620] Parameters loaded:");
ROS_INFO("outputNavSatFixTopic: %s", outputNavSatFixTopic_.c_str());
ROS_INFO("outputOdometryTopic: %s", outputOdometryTopic_.c_str());
ROS_INFO("inputNmeaSentenceTopic_: %s", inputNmeaSentenceTopic_.c_str());
private_nh_.param<std::string>("outputOdometryTopic_", outputOdometryTopic_, "");
private_nh_.param<std::string>("outputOdometryFrameId", outputOdometryFrameId, "");
private_nh_.param<std::string>("outputLidarOdometryTopic_", outputLidarOdometryTopic_, "");
private_nh_.param<std::string>("outputLidarOdometryFrameId", outputLidarOdometryFrameId, "");
private_nh_.param<std::string>("outputTransFromStampedTopic_", outputTransFromStampedTopic_, "");
private_nh_.param<std::string>("outputTransFromStampedFrameId", outputTransFromStampedFrameId, "");
private_nh_.param<std::string>("outputImuTopic_", outputImuTopic_, "");
private_nh_.param<std::string>("outputImuFrameId", outputImuFrameId, "");
private_nh_.param<std::string>("outputGpsPathTopic_", outputGpsPathTopic_, "");
private_nh_.param<std::string>("robotOdomFrameId", robotOdomFrameId, "");
private_nh_.param<std::string>("inputNmeaSentenceTopic_", inputNmeaSentenceTopic_, "");
private_nh_.param<bool>("publishDynamicTf", publishDynamicTf, false);
private_nh_.param<double>("orinLon", orinLon, 0.0);
private_nh_.param<double>("orinLat", orinLat, 0.0);
private_nh_.param<double>("orinAlt", orinAlt, 0.0);
private_nh_.param<double>("e_bias", e_bias, 0.0);
private_nh_.param<double>("n_bias", n_bias, 0.0);
// 初始化发布器
imuPublisher = nh_.advertise<sensor_msgs::Imu>(outputImuTopic_, 1);
navSatFixPublisher = nh_.advertise<sensor_msgs::NavSatFix>(outputNavSatFixTopic_, 1);
odometryPublisher = nh_.advertise<nav_msgs::Odometry>(outputOdometryTopic_, 1);
lidarOdometryPublisher = nh_.advertise<nav_msgs::Odometry>(outputLidarOdometryTopic_, 1);
transformStampedPublisher = nh_.advertise<geometry_msgs::TransformStamped>(outputTransFromStampedTopic_, 1);
gpsPathPublisher = nh_.advertise<nav_msgs::Path>(outputGpsPathTopic_, 1);
ROS_INFO("[BG620] Publishers initialized");
// 初始化订阅器
nmeaSentenceSubscriber = nh_.subscribe<nmea_msgs::Sentence>(inputNmeaSentenceTopic_, 1, &BG620SentenceParse::nmeaSentenceCallback, this);
// 初始化GPS工具
gpsTools.lla_origin_<<orinLat, orinLon, orinAlt;
}
BG620SentenceParse::~BG620SentenceParse(){
}
// 度分转换实现
double BG620SentenceParse::dmToDeg(double dm, char direction) {
int degree = static_cast<int>(dm / 100);
double minutes = dm - degree * 100;
double decimal_deg = degree + minutes / 60.0;
return (direction == 'S' || direction == 'W') ? -decimal_deg : decimal_deg;
}
// UTC时间解析实现
ros::Time BG620SentenceParse::parseUTCTime(const std::string& utc_time, const std::string& date) {
try {
// 如果没有提供日期，使用当前日期
std::string use_date = date.empty() ? "010100" : date; // 默认2000-01-01
int day = std::stoi(use_date.substr(0,2));
int month = std::stoi(use_date.substr(2,2));
int year = 2000 + std::stoi(use_date.substr(4,2));
int hours = std::stoi(utc_time.substr(0,2));
int minutes = std::stoi(utc_time.substr(2,2));
int seconds = std::stoi(utc_time.substr(4,2));
int nsec = 0;
if (utc_time.length() > 6 && utc_time[6] == '.') {
double frac_sec = std::stod("0." + utc_time.substr(7));
nsec = static_cast<int>(frac_sec * 1e9);
}
tm time_struct = {};
time_struct.tm_year = year - 1900;
time_struct.tm_mon = month - 1;
time_struct.tm_mday = day;
time_struct.tm_hour = hours;
time_struct.tm_min = minutes;
time_struct.tm_sec = seconds;
time_t time_utc = timegm(&time_struct);
return ros::Time(time_utc, nsec);
} catch (...) {
ROS_WARN("Failed to parse UTC time: %s", utc_time.c_str());
return ros::Time::now();
}
}
// 字符串分割实现
std::vector<std::string> BG620SentenceParse::splitStr(std::string str, const std::string &pattern){
std::string::size_type pos;
std::vector<std::string> result;
str += pattern;
int size = str.size();
for (int i = 0; i < size; i++)
{
pos = str.find(pattern, i);
if (pos < size)
{
std::string s = str.substr(i, pos - i);
result.push_back(s);
i = pos + pattern.size() - 1;
}
}
return result;
}
// 核心回调函数
void BG620SentenceParse::nmeaSentenceCallback(const nmea_msgs::Sentence::ConstPtr& msg){
try{
// ROS_DEBUG("[BG620] Received NMEA: %s", msg->sentence.c_str());
std::string raw_msg = msg->sentence;
// 统一分隔符处理
std::vector<std::string> fields = splitStr(raw_msg.substr(0, raw_msg.find("*")), ",");
if (fields.empty()) {
ROS_WARN("[BG620] Empty NMEA message");
return;
}
// GNGGA消息处理（第一条）
if (fields[0] == "$GNGGA") {
// ROS_DEBUG("[BG620] Processing GNGGA message");
bg620_data_.has_gga = true;
bg620_data_.stamp = msg->header.stamp;
// 解析经纬度（度分格式）
bg620_data_.nav_fix.latitude = dmToDeg(std::stod(fields[2]), fields[3][0]);
bg620_data_.nav_fix.longitude = dmToDeg(std::stod(fields[4]), fields[5][0]);
bg620_data_.nav_fix.altitude = std::stod(fields[9]);
// 设置状态
bg620_data_.nav_fix.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
int gps_status = std::stoi(fields[6]); // 先将字段转换为整数
bg620_data_.nav_fix.status.status = gps_status;
// ROS_INFO("[BG620] GPS - Lat: %.6f, Lon: %.6f, Alt: %.2f",
// bg620_data_.nav_fix.latitude,
// bg620_data_.nav_fix.longitude,
// bg620_data_.nav_fix.altitude);
}
// // GNRMC消息处理（第二条）
// else if (fields[0] == "$GNRMC") {
//// ROS_DEBUG("[BG620] Processing GNRMC message");
// bg620_data_.has_rmc = true;
// bg620_data_.speed = std::stod(fields[7]) * 0.514444; // 节→m/s
// bg620_data_.track_angle = std::stod(fields[8]); // 真北航向角
//
// ROS_INFO("[BG620] Speed - %.2f m/s, Track: %.2f°",
// bg620_data_.speed,
// bg620_data_.track_angle);
// }
// HEADINGA消息处理（第三条）
else if (fields[0] == "#HEADINGA") {
// ROS_DEBUG("[BG620] Processing HEADINGA message");
bg620_data_.has_heading = true;
// 解析姿态角（单位：度）
double yaw = std::stod(fields[11]);
double pitch = std::stod(fields[12]);
double roll = std::stod(fields[13]);
// acturally fields[12] is the heading angle of rtk
bg620_data_.heading_deg = pitch;
double yaw_rad = yaw * M_PI / 180.0;
double pitch_rad = pitch * M_PI / 180.0;
double roll_rad = roll * M_PI / 180.0;
// 转换为四元数
geometry_msgs::Quaternion q;
q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
bg620_data_.orientation = q;
// 三条消息都收到后处理
// if (bg620_data_.has_gga && bg620_data_.has_rmc && bg620_data_.has_heading) {
if (bg620_data_.has_gga && bg620_data_.has_heading) {
// ROS_INFO("[BG620] All required messages received, processing data");
processBG620Data();
// 重置标志位
bg620_data_.has_gga = false;
// bg620_data_.has_rmc = false;
bg620_data_.has_heading = false;
}
// ROS_INFO("[BG620] Orien - Yaw: %.2f, Pitch: %.2f, Roll: %.2f",
// std::stod(fields[11]),
// std::stod(fields[12]),
// std::stod(fields[13]));
}
} catch (std::exception& e) {
ROS_ERROR("NMEA parsing error [%s]: %s", __APP_NAME__, e.what());
}
}
// 新增的统一处理函数
void BG620SentenceParse::processBG620Data() {
// 设置消息头
const ros::Time current_frame_time = ros::Time::now();
bg620_data_.nav_fix.header.stamp = current_frame_time;
bg620_data_.nav_fix.header.frame_id = outputNavSatFixFrameId;
// 发布NavSatFix
// ROS_INFO("[BG620] Publishing NavSatFix");
navSatFixPublisher.publish(bg620_data_.nav_fix);
// 发布里程计
nav_msgs::Odometry gpsOdometry;
gpsOdometry.header.frame_id = "map";
gpsOdometry.header.stamp = current_frame_time;
gpsOdometry.child_frame_id = outputOdometryFrameId;
// 坐标系转换
Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(bg620_data_.nav_fix);
Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
// ROS_INFO("[BG620] ENU - E: %.2f, N: %.2f, U: %.2f",
// enu[0], enu[1], enu[2]);
gpsOdometry.pose.pose.position.x = enu[0]+e_bias;
gpsOdometry.pose.pose.position.y = enu[1]+n_bias;
gpsOdometry.pose.pose.position.z = enu[2];
// ROS_INFO("E=%.4f, N=%.4f, U=%.4f", enu[0],enu[1],enu[2]);
gpsOdometry.pose.pose.orientation.x = bg620_data_.orientation.x;
gpsOdometry.pose.pose.orientation.y = bg620_data_.orientation.y;
gpsOdometry.pose.pose.orientation.z = bg620_data_.orientation.z;
gpsOdometry.pose.pose.orientation.w = bg620_data_.orientation.w;
// just a trick to transpose solution status and yaw_angle
gpsOdometry.pose.covariance[1] = bg620_data_.nav_fix.status.status;
gpsOdometry.pose.covariance[2] = bg620_data_.heading_deg;
// 设置协方差
switch (bg620_data_.nav_fix.status.status) {
case 1: // 单点定位
gpsOdometry.pose.covariance[0] = 1.0; // x (m^2)
gpsOdometry.pose.covariance[7] = 1.0; // y (m^2)
gpsOdometry.pose.covariance[14] = 10.0; // z (m^2)
break;
case 2: // 差分定位
gpsOdometry.pose.covariance[0] = 0.5; // x (m^2)
gpsOdometry.pose.covariance[7] = 0.5; // y (m^2)
gpsOdometry.pose.covariance[14] = 5.0; // z (m^2)
break;
case 4: // RTK固定解
gpsOdometry.pose.covariance[0] = 0.01; // x (m^2)
gpsOdometry.pose.covariance[7] = 0.01; // y (m^2)
gpsOdometry.pose.covariance[14] = 0.1; // z (m^2)
break;
case 5: // RTK浮点解
gpsOdometry.pose.covariance[0] = 0.1; // x (m^2)
gpsOdometry.pose.covariance[7] = 0.1; // y (m^2)
gpsOdometry.pose.covariance[14] = 1.0; // z (m^2)
break;
case 6: // 惯导模式
gpsOdometry.pose.covariance[0] = 0.2; // x (m^2)
gpsOdometry.pose.covariance[7] = 0.2; // y (m^2)
gpsOdometry.pose.covariance[14] = 2.0; // z (m^2)
break;
default: // 其他状态 0,3,7,8 等（含无效状态）
gpsOdometry.pose.covariance[0] = 1000.0;
gpsOdometry.pose.covariance[7] = 1000.0;
gpsOdometry.pose.covariance[14] = 1000.0;
}
// 暂时不发布里程计的速度信息
// ROS_INFO("[BG620] Publishing gpsOdometry");
odometryPublisher.publish(gpsOdometry);
/* 发布geometry_msgs::TransFromed,用于标定组合导航与激光雷达的外参 */
geometry_msgs::TransformStamped ts;
ts.header.stamp = current_frame_time;
ts.header.frame_id = "map";
ts.child_frame_id = outputTransFromStampedFrameId;
ts.transform.translation.x = enu[0]+e_bias;
ts.transform.translation.y = enu[1]+n_bias;
ts.transform.translation.z = enu[2];
ts.transform.rotation.x = bg620_data_.orientation.x;
ts.transform.rotation.y = bg620_data_.orientation.y;
ts.transform.rotation.z = bg620_data_.orientation.z;
ts.transform.rotation.w = bg620_data_.orientation.w;
// ROS_INFO("[BG620] Publishing ts");
transformStampedPublisher.publish(ts);
// 发布Path 测试gps性能时用，部署时注释
gpsPath.header.frame_id = "map";
gpsPath.header.stamp = current_frame_time;
geometry_msgs::PoseStamped pose;
if(gpsPath.poses.size()>=600){
gpsPath.poses.clear();
}
pose.header = gpsPath.header;
pose.pose.position.x = enu[0]+e_bias;
pose.pose.position.y = enu[1]+n_bias;
pose.pose.position.z = enu[2];
pose.pose.orientation.x = bg620_data_.orientation.x;
pose.pose.orientation.y = bg620_data_.orientation.y;
pose.pose.orientation.z = bg620_data_.orientation.z;
pose.pose.orientation.w = bg620_data_.orientation.w;
gpsPath.poses.push_back(pose);
// ROS_INFO("[BG620] Publishing gpsPath");
gpsPathPublisher.publish(gpsPath);
Eigen::Affine3f transform;
Eigen::Matrix4f mat_pose;
geometry_msgs::TransformStamped map_wrt_frame;
geometry_msgs::TransformStamped frame_wrt_odom;
Eigen::Matrix4f frame2odom;
geometry_msgs::TransformStamped map_wrt_odom;
tf2::Transform odom_wrt_map;
geometry_msgs::TransformStamped odom_trans;
/* 生成4阶矩阵 */
transform = Eigen::Affine3f::Identity();
transform.translation() << enu[0]+e_bias, enu[1]+n_bias, enu[2];
Eigen::Quaternionf q(bg620_data_.orientation.w,
bg620_data_.orientation.x,
bg620_data_.orientation.y,
bg620_data_.orientation.z);
transform.rotate(q);
mat_pose = transform.matrix();
map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(mat_pose.inverse().cast<double>()));
map_wrt_frame.header.stamp = current_frame_time;
map_wrt_frame.header.frame_id = outputOdometryFrameId;
map_wrt_frame.child_frame_id = "map";
/* 发布激光里程计处的ODOM*/
nav_msgs::Odometry lidar_odom;
lidar_odom.header.frame_id = "map";
lidar_odom.header.stamp = current_frame_time;
lidar_odom.child_frame_id = outputLidarOdometryFrameId;
frame_wrt_odom = buffer.lookupTransform(outputLidarOdometryFrameId, outputOdometryFrameId, ros::Time(0));
frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();
tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);
tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
odom_wrt_map = odom_wrt_map.inverse();
odom_trans.transform = tf2::toMsg(odom_wrt_map);
lidar_odom.pose.pose.position.x = odom_trans.transform.translation.x;
lidar_odom.pose.pose.position.y = odom_trans.transform.translation.y;
lidar_odom.pose.pose.position.z = odom_trans.transform.translation.z;
lidar_odom.pose.pose.orientation.x = odom_trans.transform.rotation.x;
lidar_odom.pose.pose.orientation.y = odom_trans.transform.rotation.y;
lidar_odom.pose.pose.orientation.z = odom_trans.transform.rotation.z;
lidar_odom.pose.pose.orientation.w = odom_trans.transform.rotation.w;
// ROS_INFO("[BG620] PUblishing lidar_odom");
lidarOdometryPublisher.publish(lidar_odom);
// 发布动态TF
if(publishDynamicTf && bg620_data_.nav_fix.status.status == sensor_msgs::NavSatStatus::STATUS_GBAS_FIX){
frame_wrt_odom = buffer.lookupTransform(robotOdomFrameId, outputOdometryFrameId, ros::Time(0));
frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();
tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);
tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
odom_wrt_map = odom_wrt_map.inverse();
odom_trans.transform = tf2::toMsg(odom_wrt_map);
odom_trans.header.stamp = current_frame_time;
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = robotOdomFrameId;
transformBroadcaster.sendTransform(odom_trans);
}
}
sensors/beitian-bg620/src/bg620_sentence_parse_node.cpp
+9
View File
@@ -0,0 +1,9 @@
#include "bg620_sentence_parse_core.h"
int main(int argc, char **argv){
setlocale(LC_CTYPE, "zh_CN.utf8");
ros::init(argc, argv, "bg620_nmea_sentence_parse");
BG620SentenceParse bg620SentenceParse;
ros::spin();
return 0;
}
sensors/beitian-bg620/src/bg620_serial_reader_core.cpp
+72
View File
@@ -0,0 +1,72 @@
#include "bg620_serial_reader_core.h"
#include <cstdlib>
#include <cstring>
#include <unistd.h>
#include <libserial/SerialPort.h>
RtkSerialReaderBG620::RtkSerialReaderBG620():nh_(""), private_nh_("~"){
private_nh_.param<std::string>("outPutSentenceTopic", outPutSentenceTopic, "");
private_nh_.param<std::string>("gpsComChannel", gpsComChannel, "");
private_nh_.param<int>("gpsComBaudRate", gpsComBaudRate, 115200);
}
RtkSerialReaderBG620::~RtkSerialReaderBG620(){
}
void RtkSerialReaderBG620::run(){
sentence_publisher = nh_.advertise<nmea_msgs::Sentence>(outPutSentenceTopic, 1);
LibSerial::SerialPort serial_port;
try {
serial_port.Open(gpsComChannel);
switch (gpsComBaudRate) {
case 9600:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_9600);
break;
case 19200:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_19200);
break;
case 38400:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_38400);
break;
case 57600:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_57600);
break;
case 115200:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_115200);
break;
default:
serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_115200);
}
serial_port.SetCharacterSize(LibSerial::CharacterSize::CHAR_SIZE_8);
serial_port.SetFlowControl(LibSerial::FlowControl::FLOW_CONTROL_NONE);
serial_port.SetParity(LibSerial::Parity::PARITY_NONE);
serial_port.SetStopBits(LibSerial::StopBits::STOP_BITS_1);
} catch (const LibSerial::OpenFailed &) {
ROS_ERROR("The serial port did not open correctly.");
return;
}
const size_t ms_timeout = 150;
const char line_terminator = '\n';
while(ros::ok()){
std::string recvMsg;
try {
serial_port.ReadLine(recvMsg, line_terminator, ms_timeout);
} catch (const LibSerial::ReadTimeout &) {
std::cerr << "\nThe ReadLine() call has timed out." << std::endl;
}
recvMsg.erase(std::remove_if(recvMsg.begin(), recvMsg.end(), ::isspace), recvMsg.end());
if(recvMsg.empty()){
continue;
}
nmea_msgs::Sentence sentence;
sentence.header.frame_id = "rtk";
sentence.header.stamp = ros::Time().now();
sentence.sentence = recvMsg;
sentence_publisher.publish(sentence);
ros::spinOnce();
}
serial_port.Close();
return;
}
sensors/beitian-bg620/src/bg620_serial_reader_node.cpp
+9
View File
@@ -0,0 +1,9 @@
#include "bg620_serial_reader_core.h"
int main(int argc, char **argv){
setlocale(LC_CTYPE, "zh_CN.utf8");
ros::init(argc, argv, "rtk_bg620");
RtkSerialReaderBG620 rtkSerialReaderBG620;
rtkSerialReaderBG620.run();
return 0;
}
sensors/bms_driver_jikongBMS/CMakeLists.txt
+27
View File
@@ -0,0 +1,27 @@
cmake_minimum_required(VERSION 3.0.2)
project(bms_driver_jikongbms)
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_jikongbms_node src/bms_driver_node.cpp src/bms_driver_core.cpp)
target_link_libraries(bms_driver_jikongbms_node ${catkin_LIBRARIES})
add_dependencies(bms_driver_jikongbms_node
can_msgs_generate_messages_cpp
)
sensors/bms_driver_jikongBMS/include/bms_driver_core.h
+36
View File
@@ -0,0 +1,36 @@
#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include "sensor_msgs/BatteryState.h"
#define __APP_NAME__ "bms_driver_jikongbms"
#ifndef BMS_DRIVER_CORE_H
#define BMS_DRIVER_CORE_H
class BmsDriver{
public:
BmsDriver();
~BmsDriver();
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);
};
#endif
sensors/bms_driver_jikongBMS/launch/bms_driver.launch
+11
View File
@@ -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_jikongbms_node" pkg="bms_driver_jikongbms" type="bms_driver_jikongbms_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>
sensors/bms_driver_jikongBMS/package.xml
+69
View File
@@ -0,0 +1,69 @@
<?xml version="1.0"?>
<package format="2">
<name>bms_driver_jikongbms</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>
sensors/bms_driver_jikongBMS/src/bms_driver_core.cpp
+48
View File
@@ -0,0 +1,48 @@
#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::can_frame_callback(const can_msgs::Frame::ConstPtr& msg){
switch (msg->id)
{
case 0x02F4:
/* 电压 单位v*/
battery_state_.voltage = ((msg->data[1]<<8) + msg->data[0])*0.1;
/* 电流 单位A */
battery_state_.current = 0.1*((msg->data[3]<<8) + msg->data[2] - 4000);
if(battery_state_.current > 0.5){
battery_state_.power_supply_status = 1;
}else{
battery_state_.power_supply_status = 0;
}
/* soc 0.0-1.0 */
battery_state_.percentage = msg->data[4];
battery_status_pub_.publish(battery_state_);
break;
case 0x05F4:
/* 温度 */
battery_state_.temperature = msg->data[0]-40.0;
battery_status_pub_.publish(battery_state_);
break;
case 0x18F328F4:
/* 告警信息 */
battery_state_.power_supply_health = (msg->data[0]<<16) + (msg->data[0]<<8 )+ msg->data[0];
battery_status_pub_.publish(battery_state_);
break;
default:
break;
}
}
sensors/bms_driver_jikongBMS/src/bms_driver_node.cpp
+8
View File
@@ -0,0 +1,8 @@
#include "bms_driver_core.h"
int main(int argc, char **argv){
ros::init(argc, argv, "bms_driver_node");
BmsDriver bms_driver;
ros::spin();
return 0;
}
sensors/bms_driver_mingnuo/CMakeLists.txt
+205
View File
@@ -0,0 +1,205 @@
cmake_minimum_required(VERSION 3.0.2)
project(bms_driver_mingnuo)
## 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
rospy
serial
)
## 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
# Message1.msg
# Message2.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 # Or other packages containing 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 bms_driver_mingnuo
# CATKIN_DEPENDS rospy serial
# 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}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/bms_driver_mingnuo.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/bms_driver_mingnuo_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_bms_driver_mingnuo.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
sensors/bms_driver_mingnuo/launch/bms_driver.launch
+7
View File
@@ -0,0 +1,7 @@
<launch>
<node name="bms_driver_mingnuo" pkg="bms_driver_mingnuo" type="bms_driver_node.py" output="screen">
<!-- 仅保留可能需要动态调整的参数 -->
<param name="port" value="/dev/ttyS2" />
<param name="baudrate" value="9600" />
</node>
</launch>
sensors/bms_driver_mingnuo/package.xml
+65
View File
@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>bms_driver_mingnuo</name>
<version>0.0.0</version>
<description>The bms_driver_mingnuo package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dreamdeck@todo.todo">dreamdeck</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_mingnuo</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>rospy</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/bms_driver_mingnuo/readme.md
+52
View File
@@ -0,0 +1,52 @@
/sensor/battery_state话题格式解析
---
header:
seq: 527
stamp:
secs: 0
nsecs: 0
frame_id: ''
voltage: 53.79999923706055 #电池电压V
temperature: 0.0 #电池温度℃
current: 0.0 #电池电流A
charge: 0.0
capacity: 0.0
design_capacity: 0.0
percentage: 100.0 #电池电量百分比
power_supply_status: 0 #充电状态 1：在充电 0：未在充电
power_supply_health: 0 #报警信息1 0：表示正常 非0：有报警
power_supply_technology: 0 #报警信息2 0：表示正常 非0：有报警
present: False
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---
报警信息1 power_supply_health 8位数据解析
二进制位 描述 地址
D0 0表示正常 0
D1 1表示故障 1
D2 1表示充电过流 2
D3 1表示放电过流 3
D4 1表示短路保护 4
D5 1表示充电高温保护 5
D6 1表示放电高温保护 6
D7 1表示充电低温保护 7
报警信息2 power_supply_technology 8位数据解析
二进制位 描述 地址
D0 1表示放电低温保护 0
D1 1表示充电 MOS 损坏 1
D2 1表示放电 MOS 损坏 2
D3 1表示内部通讯异常 3
D4 1表示过充电压保护 4
D5 1表示过放电压保护 5
D6 6
D7 7
sensors/bms_driver_mingnuo/scripts/bms_driver_node.py
+173
View File
@@ -0,0 +1,173 @@
#!/usr/bin/env python3
import rospy
import serial
import binascii
import time
from std_msgs.msg import String
from sensor_msgs.msg import BatteryState
class BMSDriverNode:
def __init__(self):
rospy.init_node('bms_driver_mingnuo', anonymous=False)
self.port = rospy.get_param('~port', '/dev/ttyUSB0')
self.baudrate = rospy.get_param('~baudrate', 9600)
self.timeout = 0.5
self.battery_cmd = b'\x01\x03\x00\x00\x00\x1E\xC5\xC2' # 电量查询指令
self.alarm_cmd = b'\x01\x01\x00\x00\x00\x34\x3D\xDD' # 报警查询指令
self.polling_rate = 1.0
try:
self.ser = serial.Serial(
port=self.port,
baudrate=self.baudrate,
timeout=self.timeout,
parity='N',
stopbits=1,
bytesize=8
)
rospy.loginfo(f"Successfully connected to BMS: {self.port} @ {self.baudrate}bps")
except serial.SerialException as e:
rospy.logerr(f"Failed to connect to BMS: {e}")
raise
# 初始化全局消息对象
self.global_msg = BatteryState()
self.battery_pub = rospy.Publisher('/sensor/battery_state', BatteryState, queue_size=10)
self.rate = rospy.Rate(self.polling_rate)
def update_battery_data(self, hex_data):
"""更新全局消息中的电池数据"""
try:
# if len(hex_data) < 65:
# rospy.logwarn(f"Battery response frame too short: {hex_data.hex()}")
# return False
# if hex_data[0] != 0x01 or hex_data[1] != 0x03 or hex_data[2] != 0x3C:
# rospy.logwarn(f"Battery response frame format error: {hex_data.hex()}")
# return False
# 更新电池数据
self.global_msg.header.stamp = rospy.Time.now()
self.global_msg.voltage = ((hex_data[3] << 8) | hex_data[4]) / 100.0
self.global_msg.percentage = ((hex_data[7] << 8) | hex_data[8])
unsigned_value = (hex_data[13] << 8) | hex_data[14]
# 转换为16位有符号整数
if unsigned_value & 0x8000:
signed_value = unsigned_value - 0x10000
else:
signed_value = unsigned_value # 正数直接使用
self.global_msg.current = signed_value / 100.0
self.global_msg.power_supply_status = 1 if self.global_msg.current > 0.5 else 0
self.global_msg.present = True
self.global_msg.temperature = (hex_data[61] << 8) | hex_data[62]
return True
except Exception as e:
rospy.loginfo(f"Error updating battery data: {e}")
return False
def update_alarm_data(self, hex_data):
"""更新全局消息中的报警数据"""
try:
# if len(hex_data) < 12:
# rospy.logwarn(f"Alarm response frame too short: {hex_data.hex()}")
# return False
# if hex_data[0] != 0x01 or hex_data[1] != 0x01 or hex_data[2] != 0x07:
# rospy.logwarn(f"Alarm response frame format error: {hex_data.hex()}")
# return False
# 更新报警数据
self.global_msg.header.stamp = rospy.Time.now()
alarm_code1 = hex_data[3]
self.global_msg.power_supply_health = alarm_code1
if ((hex_data[7] == 0) and (hex_data[8] == 0) and (hex_data[9] == 0)):
alarm_code_over_discharge_voltage = 0
else:
alarm_code_over_discharge_voltage = 1
if ((hex_data[6] == 0) and (hex_data[5] == 0) and (hex_data[4] < 16)):
alarm_code_over_charge_voltage = 0
else:
alarm_code_over_charge_voltage = 1
self.global_msg.power_supply_technology = (hex_data[4] & 0xF) | (alarm_code_over_charge_voltage << 4) | (alarm_code_over_discharge_voltage << 5)
return True
except Exception as e:
rospy.loginfo(f"Error updating alarm data: {e}")
return False
def receive_and_update(self):
"""接收数据并更新全局消息"""
data = self.ser.read_all()
if not data:
return False, "No data"
# 判断是否为电量响应
if data[0] == 0x01 and data[1] == 0x03 and data[2] == 0x3C:
success = self.update_battery_data(data)
return success, "Battery data updated"
# 判断是否为报警响应
elif data[0] == 0x01 and data[1] == 0x01 and data[2] == 0x07:
success = self.update_alarm_data(data)
return success, "Alarm data updated"
# 不符合任何类型
else:
rospy.loginfo(f"Unknown response frame: {data.hex()}")
return False, "Unknown frame"
def run(self):
while not rospy.is_shutdown():
try:
# 1. 发送电量指令并更新
self.ser.flushInput()
self.ser.write(self.battery_cmd)
rospy.logdebug(f"Sent battery command: {binascii.hexlify(self.battery_cmd).decode().upper()}")
time.sleep(0.19)
battery_success, battery_msg = self.receive_and_update()
if battery_success:
self.battery_pub.publish(self.global_msg)
# rospy.loginfo(f"Battery state updated: Voltage={self.global_msg.voltage:.2f}V, SOC={self.global_msg.percentage:.1f}%, Current={self.global_msg.current:.2f}A")
else:
rospy.loginfo(f"Battery update failed: {battery_msg}")
time.sleep(0.2)
# 2. 发送报警指令并更新
self.ser.flushInput()
self.ser.write(self.alarm_cmd)
rospy.logdebug(f"Sent alarm command: {binascii.hexlify(self.alarm_cmd).decode().upper()}")
time.sleep(0.08)
alarm_success, alarm_msg = self.receive_and_update()
if alarm_success:
self.battery_pub.publish(self.global_msg)
# rospy.loginfo(f"Alarm state updated: Health={self.global_msg.power_supply_health}")
else:
rospy.loginfo(f"Alarm update failed: {alarm_msg}")
except Exception as e:
rospy.logerr(f"Communication error: {e}")
self.rate.sleep()
if __name__ == '__main__':
try:
node = BMSDriverNode()
node.run()
except rospy.ROSInterruptException:
pass
finally:
if 'node' in locals() and hasattr(node, 'ser') and node.ser.is_open:
node.ser.close()
rospy.loginfo("BMS serial port closed")
sensors/bms_driver_xinguang/CMakeLists.txt
+28
View File
@@ -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
)
sensors/bms_driver_xinguang/include/bms_driver_core.h
+38
View File
@@ -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
sensors/bms_driver_xinguang/launch/bms_driver.launch
+11
View File
@@ -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>
sensors/bms_driver_xinguang/package.xml
+69
View File
@@ -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>
sensors/bms_driver_xinguang/src/bms_driver_core.cpp
+86
View File
@@ -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");
}
sensors/bms_driver_xinguang/src/bms_driver_node.cpp
+9
View File
@@ -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;
}
sensors/bozi_ws/.catkin_workspace
+1
View File
@@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration
sensors/bozi_ws/mqtt_py/mqtt_brige.py
+126
View File
@@ -0,0 +1,126 @@
#!/usr/bin/env python3
import rospy
import paho.mqtt.client as mqtt
from std_msgs.msg import String
import pkg_resources
import json
# 获取 paho-mqtt 版本
try:
PAHO_VERSION = pkg_resources.get_distribution("paho-mqtt").version
PAHO_MAJOR_VERSION = int(PAHO_VERSION.split('.')[0])
except Exception:
PAHO_VERSION = "unknown"
PAHO_MAJOR_VERSION = 1
# MQTT 配置
MQTT_BROKER = "211.154.252.186" # MQTT 代理地址
MQTT_PORT = 1883 # MQTT 代理端口
MQTT_CLIENT_ID = "ros_mqtt_bridge"
MQTT_TOPIC_CONTROL = "head_servo/control/1" # 接收控制指令的 MQTT 主题
# MQTT_TOPIC_CONTROL = "robot/yuntai_control/+"
MQTT_TOPIC_STATUS = "head_servo/status" # 发布状态的 MQTT 主题
# ROS 配置
ROS_TOPIC_CONTROL = "/head_servo/control_cmd" # 发布控制指令的 ROS 话题
ROS_TOPIC_STATUS = "/head_servo/status" # 接收状态的 ROS 话题
class MQTTROSBridge:
def __init__(self):
# 初始化 ROS 节点
rospy.init_node("mqtt_ros_bridge", anonymous=True)
rospy.loginfo(f"MQTT-ROS 桥接器启动中 (paho-mqtt 版本: {PAHO_VERSION})")
# 根据版本选择初始化方式
if PAHO_MAJOR_VERSION >= 2:
# 版本 2.0+ 使用新 API
self.mqtt_client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2, clean_session=True)
else:
# 版本 1.x 使用旧 API
self.mqtt_client = mqtt.Client(client_id=MQTT_CLIENT_ID)
self.mqtt_client.on_connect = self.on_connect
self.mqtt_client.on_message = self.on_message
# 连接 MQTT 代理
try:
self.mqtt_client.connect(MQTT_BROKER, MQTT_PORT, 60)
self.mqtt_client.loop_start() # 启动 MQTT 网络循环（非阻塞）
rospy.loginfo(f"已连接到 MQTT 代理: {MQTT_BROKER}:{MQTT_PORT}")
except Exception as e:
rospy.logerr(f"MQTT 连接失败: {e}")
exit(1)
# 创建 ROS 发布者和订阅者
self.control_pub = rospy.Publisher(ROS_TOPIC_CONTROL, String, queue_size=10)
rospy.Subscriber(ROS_TOPIC_STATUS, String, self.status_callback)
rospy.loginfo("MQTT-ROS 桥接器已就绪")
def on_connect(self, client, userdata, flags, rc, properties):
"""MQTT 连接成功回调"""
rospy.loginfo(f"MQTT 连接成功，返回代码: {rc}")
# 订阅 MQTT 控制主题
client.subscribe(MQTT_TOPIC_CONTROL)
rospy.loginfo(f"已订阅 MQTT 主题: {MQTT_TOPIC_CONTROL}")
# def on_message(self, client, userdata, msg):
# """MQTT 消息到达回调"""
# try:
# # 解析 MQTT 消息
# payload = msg.payload.decode()
# rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
# # 将消息发布到 ROS 话题
# ros_msg = String()
# ros_msg.data = payload
# self.control_pub.publish(ros_msg)
# except Exception as e:
# rospy.logerr(f"处理 MQTT 消息时出错: {e}")
# 修改 MQTT-ROS 桥接脚本中的消息处理部分
def on_message(self, client, userdata, msg):
try:
payload = msg.payload.decode()
rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
# 解析 JSON 命令
try:
cmd_data = json.loads(payload)
ros_msg = String()
ros_msg.data = json.dumps(cmd_data) # 保持 JSON 格式
self.control_pub.publish(ros_msg)
except json.JSONDecodeError:
# 如果不是 JSON 格式，作为普通字符串处理
ros_msg = String()
ros_msg.data = payload
self.control_pub.publish(ros_msg)
except Exception as e:
rospy.logerr(f"处理 MQTT 消息时出错: {e}")
def status_callback(self, msg):
"""ROS 状态消息回调"""
try:
# 将 ROS 状态消息发布到 MQTT 主题
self.mqtt_client.publish(MQTT_TOPIC_STATUS, msg.data)
rospy.loginfo(f"已发布到 MQTT 主题 [{MQTT_TOPIC_STATUS}]: {msg.data}")
except Exception as e:
rospy.logerr(f"发布到 MQTT 时出错: {e}")
def run(self):
"""运行桥接器"""
rospy.spin() # 进入 ROS 循环
# 关闭 MQTT 连接
self.mqtt_client.loop_stop()
self.mqtt_client.disconnect()
rospy.loginfo("桥接器已关闭")
if __name__ == "__main__":
try:
bridge = MQTTROSBridge()
bridge.run()
except rospy.ROSInterruptException:
pass
sensors/bozi_ws/readme/readme.txt
+11
View File
@@ -0,0 +1,11 @@
roslaunch rosbridge_server rosbridge_websocket.launch
python mqtt_bridge.py
roslaunch head_servo_controller head_servo.launch
记得改串口号
launch中
head_servo_core.cpp中
sensors/bozi_ws/src/CMakeLists.txt
+1
View File
@@ -0,0 +1 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
sensors/bozi_ws/src/head_servo_controller.zip
BIN
View File
Binary file not shown.
sensors/bozi_ws/src/head_servo_controller/CMakeLists.txt
+39
View File
@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(head_servo_controller)
find_package(catkin REQUIRED COMPONENTS
autoware_msgs
roscpp
sensor_msgs
serial
message_generation
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES head_servo_controller
# CATKIN_DEPENDS autoware_msgs roscpp sensor_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/msg
)
add_executable(head_servo_controller_node
src/head_servo_node.cpp
src/head_servo_core.cpp
)
target_link_libraries(head_servo_controller_node
${catkin_LIBRARIES}
)
add_dependencies(head_servo_controller_node
autoware_msgs_generate_messages_cpp
)
sensors/bozi_ws/src/head_servo_controller/include/head_servo_core.h
+98
View File
@@ -0,0 +1,98 @@
#ifndef HEAD_SERVO_CORE_H
#define HEAD_SERVO_CORE_H
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <thread>
// #include <head_servo_controller/HeadControlCmd.h>
// #include <head_servo_controller/HeadAngleMsg.h>
typedef struct
{
int32_t PU;
} DispModbusData;
class Head_servo
{
private:
ros::NodeHandle nh_;
serial::Serial serial_;
ros::Subscriber control_sub_; // 订阅控制指令
ros::Publisher status_pub_; // 发布状态
// 串口相关配置
std::string head_servo_com_;
std::string __APP_NAME__;
int baudrate_;
// // 串口线程控制
// std::thread serial_thread_;
// bool serial_thread_running = false;
// 控制标志位（对应原STM32代码中的flag）
uint8_t flag_1 = 0; // 查询当前角度
uint8_t flag_2 = 0; // 按速度转到指定角度
uint8_t flag_3 = 0; // 进入巡航模式
uint8_t flag_4 = 0; // 退出巡航模式
uint8_t flag_5 = 0; // 一键回正（90°）
uint8_t flag_6 = 0; // 重启（ROS中可忽略或调用节点重启逻辑）
uint8_t flag_7 = 0; // 设置当前位置为零点
// 目标参数
int16_t target_speed = 0xFFFF; // 0xFFFF表示速度不变
int16_t target_angle = 0xFFFF; // 0xFFFF表示角度不变
// // 通讯数据缓冲区
// uint8_t recvData_Uart3[100];
// uint8_t recvData_Uart2[100];
// // 命令帧定义（根据原代码推测）
// uint8_t ModusEn[8] = {0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A};
// uint8_t param_save[8] = {0x01, 0x06, 0x00, 0x01, 0x00, 0x01, 0xF9, 0xCA};
// uint8_t Motor_speed_target[11] = {0x01, 0x10, 0x00, 0x10, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00};
// uint8_t Motor_speed_target_2[11] = {0x01, 0x10, 0x00, 0x10, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00};
// uint8_t PosAngle_P135[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x87, 0x00, 0x00};
// uint8_t PosAngle_P45[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x2D, 0x00, 0x00};
// uint8_t PosAngle_P90[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x5A, 0x00, 0x00};
// uint8_t PosAngle_N90[13] = {0x01, 0x10, 0x00, 0x12, 0x00, 0x02, 0x04, 0xFF, 0xFF, 0xFF, 0xA6, 0x00, 0x00};
// ROS话题相关
ros::Subscriber control_cmd_sub_;
ros::Publisher current_angle_pub_;
// CRC校验函数（原代码中使用）
uint16_t usMBCRC16(uint8_t *pucFrame, uint16_t usLen);
// // 串口接收回调函数
// void serialCallback(const ros::TimerEvent& e);
// 控制指令回调函数
// void control_cmd_callback(const head_servo_controller::HeadControlCmd::ConstPtr& msg);
// 核心处理函数（移植原STM32 while(1)中的逻辑）
void process(const ros::TimerEvent &e);
// 控制指令回调
void controlCallback(const std_msgs::String::ConstPtr& msg);
// 辅助功能函数
void position_stop();
void Turn_angle(int16_t angle);
int16_t read_current_angle();
void setCurrentPositionZero();
void fun_response(uint8_t cmd, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4);
void calculate_modbus_data(uint16_t modbus_data_11, uint16_t modbus_data_10, DispModbusData *disp_modbus_data);
public:
Head_servo(ros::NodeHandle nh);
~Head_servo();
// 串口初始化
void serial_initial();
// 运行节点
void run();
};
#endif // HEAD_SERVO_CORE_H
sensors/bozi_ws/src/head_servo_controller/launch/head_servo.launch
+13
View File
@@ -0,0 +1,13 @@
<launch>
<arg name="head_servo_com_" default="/dev/ttyACM0"/>
<arg name="baudrate_" default="19200"/>
<arg name="input_ctr_mode_topic_" default="/remote_ctrl"/>
<arg name="input_battery_state_topic_" default="/sensor/battery_state"/>
<node name="head_servo_controller_node" pkg="head_servo_controller" type="head_servo_controller_node" output="screen">
<param name="head_servo_com_" value="$(arg head_servo_com_)" type="string"/>
<param name="baudrate_" value="$(arg baudrate_)" type="int"/>
<param name="input_ctr_mode_topic_" value="$(arg input_ctr_mode_topic_)" type="string"/>
<param name="input_battery_state_topic_" value="$(arg input_battery_state_topic_)" type="string"/>
</node>
</launch>
sensors/bozi_ws/src/head_servo_controller/package.xml
+33
View File
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<package format="2">
<name>head_servo_controller</name>
<version>0.0.0</version>
<description>The head_servo_controller package</description>
<maintainer email="zhangshu@todo.todo">zhangshu</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>autoware_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>autoware_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>autoware_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/bozi_ws/src/head_servo_controller/src/head_mqtt_core.cpp
+149
View File
@@ -0,0 +1,149 @@
#include "head_servo_core.h"
#include <mqtt/async_client.h> // Paho MQTT C++库
#include <thread> // 多线程支持
#include <mutex> // 线程安全锁
// MQTT配置
const std::string MQTT_ADDRESS("tcp://0.0.0.0:1883"); // 本地MQTT服务器地址
const std::string MQTT_CLIENT_ID("HeadServo_MQTT_Server");
const std::string MQTT_TOPIC("head_servo/control"); // 订阅的控制指令主题
// 线程安全锁（保护flag变量）
std::mutex flag_mutex;
// MQTT回调类（处理连接和消息接收）
class MQTTCallback : public virtual mqtt::callback {
private:
Head_servo* head_servo_ptr; // 指向Head_servo实例的指针
public:
MQTTCallback(Head_servo* ptr) : head_servo_ptr(ptr) {}
// 连接丢失回调
void connection_lost(const std::string& cause) override {
ROS_WARN("MQTT连接丢失: %s", cause.c_str());
}
// 消息到达回调（核心：解析MQTT消息并修改flag）
void message_arrived(mqtt::const_message_ptr msg) override {
ROS_INFO("收到MQTT消息: [%s] %s", msg->get_topic().c_str(), msg->to_string().c_str());
// 解析JSON格式的控制指令（示例格式：{"flag_1":1, "flag_2":1, "target_angle":90, ...}）
// 实际应用中可根据需求简化格式（如直接发送flag名称和值）
std::string payload = msg->to_string();
// 线程安全地修改flag（根据消息内容设置对应flag）
std::lock_guard<std::mutex> lock(flag_mutex);
// 示例：解析简单指令（实际需根据通信协议完善）
if (payload.find("query_angle") != std::string::npos) {
head_servo_ptr->flag_1 = 1; // 查询当前角度
} else if (payload.find("set_target") != std::string::npos) {
head_servo_ptr->flag_2 = 1; // 设置目标角度/速度
// 提取目标角度（示例：假设消息中包含"angle:90"）
size_t angle_pos = payload.find("angle:");
if (angle_pos != std::string::npos) {
head_servo_ptr->target_angle = std::stoi(payload.substr(angle_pos + 6));
}
} else if (payload.find("enter_cruise") != std::string::npos) {
head_servo_ptr->flag_3 = 1; // 进入巡航模式
} else if (payload.find("exit_cruise") != std::string::npos) {
head_servo_ptr->flag_4 = 1; // 退出巡航模式
} else if (payload.find("home_position") != std::string::npos) {
head_servo_ptr->flag_5 = 1; // 返回90°
} else if (payload.find("set_zero") != std::string::npos) {
head_servo_ptr->flag_7 = 1; // 设置当前位置为零点
}
}
// 消息发送完成回调
void delivery_complete(mqtt::delivery_token_ptr token) override {
ROS_DEBUG("MQTT消息发送完成");
}
};
// MQTT连接监听器
class ActionListener : public virtual mqtt::iaction_listener {
private:
void on_failure(const mqtt::token& tok) override {
ROS_WARN("MQTT操作失败");
}
void on_success(const mqtt::token& tok) override {
ROS_INFO("MQTT操作成功");
}
};
// 扩展Head_servo类，添加MQTT相关成员
class HeadServoWithMQTT : public Head_servo {
public:
mqtt::async_client mqtt_client; // MQTT客户端
MQTTCallback mqtt_callback; // MQTT回调实例
ActionListener mqtt_listener; // MQTT连接监听器
// 构造函数
HeadServoWithMQTT(ros::NodeHandle nh)
: Head_servo(nh),
mqtt_client(MQTT_ADDRESS, MQTT_CLIENT_ID),
mqtt_callback(this) {}
// 初始化MQTT服务器
void mqtt_init() {
// 设置MQTT回调
mqtt_client.set_callback(mqtt_callback);
// 配置连接选项
mqtt::connect_options conn_opts;
conn_opts.set_keep_alive_interval(20); // 心跳间隔20秒
conn_opts.set_clean_session(true); // 清理会话
// 连接MQTT服务器
try {
ROS_INFO("连接MQTT服务器: %s", MQTT_ADDRESS.c_str());
mqtt::token_ptr conntok = mqtt_client.connect(conn_opts);
conntok->wait(); // 等待连接完成
ROS_INFO("MQTT服务器连接成功");
// 订阅控制指令主题
mqtt_client.subscribe(MQTT_TOPIC, 1, nullptr, mqtt_listener);
ROS_INFO("已订阅MQTT主题: %s", MQTT_TOPIC.c_str());
} catch (const mqtt::exception& e) {
ROS_ERROR("MQTT初始化失败: %s", e.what());
exit(1);
}
}
// 重写run()函数，同时启动ROS和MQTT
void run() override {
serial_initial(); // 初始化串口
mqtt_init(); // 初始化MQTT
// 创建线程运行MQTT循环（非阻塞）
std::thread mqtt_thread([this]() {
while (ros::ok()) {
// 处理MQTT消息（非阻塞模式）
mqtt_client.loop(100); // 超时100ms，避免阻塞
ros::Duration(0.01).sleep(); // 短暂休眠
}
});
// 启动ROS定时器和主循环
ros::Timer timer = nh_.createTimer(ros::Duration(0.01), &Head_servo::process, this);
ros::spin();
// 退出时清理
mqtt_client.disconnect()->wait(); // 断开MQTT连接
mqtt_thread.join(); // 等待MQTT线程结束
}
};
// 主函数
int main(int argc, char**argv) {
ros::init(argc, argv, "head_servo_with_mqtt");
ros::NodeHandle nh("~");
// 创建带MQTT功能的节点实例并运行
HeadServoWithMQTT node(nh);
node.run();
return 0;
}
sensors/bozi_ws/src/head_servo_controller/src/head_servo_core.cpp
+733
View File
@@ -0,0 +1,733 @@
#include "head_servo_core.h"
#include <jsoncpp/json/json.h>
#include <nlohmann/json.hpp>
using json = nlohmann::json;
/* 以下逆时针需要用到32768的负数那种，可以通过covertPU这个函数计算得到 */
/* 位置模式： 转动到指定位置模板 */
static uint8_t Angle_Target[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
/* 位置模式： 顺时针到180° */
static uint8_t PosAngle_P180[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x02, 0xF3, 0x48};
/* 位置模式：45° */
static uint8_t PosAngle_P45[] = {0X01, 0X10, 0X00, 0X16, 0X00, 0X02, 0X04, 0X80, 0X00, 0X00, 0X00, 0X5B, 0X49};
/* 位置模式：135° */
static uint8_t PosAngle_P135[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x80, 0x00, 0x00, 0x01, 0x9A, 0x89};
/* 位置模式： 1° */
static uint8_t PosAngle_N1[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x01, 0x00, 0x00, 0x23, 0x49};
/* 位置模式：顺时针转到90° */
static uint8_t PosAngle_P90[] = {0x01, 0x10, 0x00, 0x16, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x01, 0xB3, 0x49};
/* modbus使能 */
static uint8_t ModusEn[] = {0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A};
/* 发送目标速度 */
static uint8_t Motor_speed_target[] = {0x01, 0x06, 0x00, 0x02, 0x00, 0xC8, 0x29, 0x9C};
/* 发送目标速度 */
static uint8_t Motor_speed_target_2[] = {0x01, 0x06, 0x00, 0x02, 0x00, 0x28, 0x28, 0x14};
/* 发送参数保存标志 */
static uint8_t param_save[] = {0x01, 0x06, 0x00, 0x14, 0x00, 0x01, 0x08, 0x0E};
/* 电子齿轮发0 */
static uint8_t Motor_mole_zero[] = {0x01, 0x06, 0x00, 0x0A, 0x00, 0x00, 0xA9, 0xC8};
/* 电子齿轮发 60006 */
static uint8_t Motor_mole_set_zero_1[] = {0x01, 0x06, 0x00, 0x0A, 0xEA, 0x66, 0x66, 0x82};
/* 电子齿轮发 60016 */
static uint8_t Motor_mole_set_zero_2[] = {0x01, 0x06, 0x00, 0x0A, 0xEA, 0x70, 0xE7, 0x4C};
/* 增量位置发0 */
static uint8_t Motor_incre_zero[] = {0x01, 0x10, 0x00, 0x0C, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0xF3, 0xFA};
/* CRC16 数组 */
static const uint8_t aucCRCHi[] = {0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80,
0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
static const uint8_t aucCRCLo[] = {0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D,
0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB,
0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2,
0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37,
0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8,
0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24,
0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63,
0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F,
0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C,
0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46,
0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
Head_servo::Head_servo(ros::NodeHandle nh) : nh_(nh)
{
// Get ROS params
nh_.param<std::string>("head_servo_com", head_servo_com_, "/dev/ttyS0");
nh_.param<std::string>("app_name", __APP_NAME__, "head_servo_controller");
nh_.param<int>("baudrate_", baudrate_, 19200);
// 初始化控制标志
flag_1 = 0;
flag_2 = 0;
flag_3 = 0;
flag_4 = 0;
flag_5 = 0;
flag_7 = 0;
target_speed = 0xFFFF;
target_angle = 0xFFFF;
// 初始化订阅者和发布者
control_sub_ = nh_.subscribe("/head_servo/control_cmd", 10, &Head_servo::controlCallback, this);
status_pub_ = nh_.advertise<std_msgs::String>("/head_servo/status", 10);
}
// // 控制指令解析回调
// void Head_servo::controlCallback(const std_msgs::String::ConstPtr& msg)
// {
// std::string cmd = msg->data;
// ROS_INFO("[%s] 收到控制指令: %s", __APP_NAME__.c_str(), cmd.c_str());
// // 解析命令并设置对应标志
// if (cmd == "query_angle") {
// flag_1 = 1; // 查询当前角度
// } else if (cmd == "enter_cruise") {
// flag_3 = 1; // 进入巡航模式
// } else if (cmd == "exit_cruise") {
// flag_4 = 1; // 退出巡航模式
// } else if (cmd == "home_position") {
// flag_5 = 1; // 返回90°
// } else if (cmd == "set_zero") {
// flag_7 = 1; // 设置零点
// // 解析带参数的命令
// } else if (cmd.find("set_target") != std::string::npos) {
// flag_2 = 1; // 设置目标角度/速度
// // 提取目标角度和速度（格式示例："set_target,angle:90,speed:100"）
// size_t angle_pos = cmd.find("angle:");
// size_t speed_pos = cmd.find("speed:");
// if (angle_pos != std::string::npos) {
// std::string angle_str = cmd.substr(angle_pos + 6);
// angle_str = angle_str.substr(0, angle_str.find(','));
// target_angle = std::stoi(angle_str);
// }
// if (speed_pos != std::string::npos) {
// std::string speed_str = cmd.substr(speed_pos + 6);
// target_speed = std::stoi(speed_str);
// }
// }
// // 发布状态消息
// std_msgs::String status_msg;
// status_msg.data = "Command received: " + cmd;
// status_pub_.publish(status_msg);
// }
// mosquitto_pub -t "head_servo/control" -m '{"command":"set_target","angle":0,"speed":40}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"query_angle"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"reset_zero"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"enter_cruise"}'
// mosquitto_pub -t "head_servo/control" -m '{"command":"stop"}'
/*
主题： ‘robot/yuntai_control/{device_id}’
Qos: 2
消息内容：
绝对位置角度控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "angle",
"data":{
"angle": 15, # 0-180度。0对应最左边，180对应最右边
"speed": 30, # 060度。最⼤移动速度。不传值则为默认30度。
}
}
相对位置角度控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "direction",
"data":{
"direction": "left", # "left"/"right", 向左/向右移动指定⻆度
"angle": 15, # 5-30度。移动的度数。不传值则为默认15度
}
}
循环旋转控制：
{
"uuid": "f47ac10b-58cc-4372-a567-0e02b2c3d465",
"task_type:": "yuntai_control",
"command": "loop",
"data": {
"speed": 30, # 0-60度。最⼤移动速度。不传值则为默认30度。
}
}
回中控制：
{
"uuid": "f47ac10b58cc4372a5670e02b2c3d465",
"tasktype:": "yuntaicontrol",
"command": "center",
"data": {
"speed": 30, # 060度。最⼤移动速度。不传值则为默认30度。
}
}
*/
// 修改 C++ 代码中的控制回调函数
void Head_servo::controlCallback(const std_msgs::String::ConstPtr& msg)
{
std::string cmd = msg->data;
ROS_INFO("[%s] 收到控制指令: %s", __APP_NAME__.c_str(), cmd.c_str());
try {
// 尝试解析 JSON 命令
json root = json::parse(cmd);
// 处理 JSON 格式命令
if (root.contains("command")) {
std::string command = root["command"].get<std::string>();
if (command == "query_angle") {
flag_1 = 1;
ROS_INFO("1");
} else if (command == "enter_cruise") {
flag_3 = 1;
ROS_INFO("3");
} else if (command == "set_target") {
flag_2 = 1;
ROS_INFO("2");
if (root.contains("angle"))
target_angle = root["angle"].get<int>();
if (root.contains("speed"))
target_speed = root["speed"].get<int>();
} else if (command == "reset_zero") {
flag_7 = 1;
ROS_INFO("7");
} else if (command == "stop") {
flag_4 = 1;
ROS_INFO("4");
}
// 其他命令...
}
} catch (const json::parse_error& e) {
// 处理JSON解析错误
ROS_WARN("[%s] 解析JSON命令时出错: %s", __APP_NAME__.c_str(), e.what());
} catch (const json::type_error& e) {
// 处理类型错误
ROS_WARN("[%s] JSON类型不匹配: %s", __APP_NAME__.c_str(), e.what());
} catch (const std::exception& e) {
// 处理其他异常
ROS_WARN("[%s] 处理命令时出错: %s", __APP_NAME__.c_str(), e.what());
}
// try {
// // 尝试解析 JSON 命令
// Json::Value root;
// Json::Reader reader;
// if (reader.parse(cmd, root)) {
// // 处理 JSON 格式命令
// if (root.isMember("command")) {
// std::string command = root["command"].asString();
// if (command == "query_angle") {
// flag_1 = 1;
// } else if (command == "enter_cruise") {
// flag_3 = 1;
// } else if (command == "set_target") {
// flag_2 = 1;
// if (root.isMember("angle"))
// target_angle = root["angle"].asInt();
// if (root.isMember("speed"))
// target_speed = root["speed"].asInt();
// }
// // 其他命令...
// }
// } else {
// // 处理普通字符串命令（保持原有逻辑）
// // ...
// }
// } catch (const std::exception& e) {
// ROS_WARN("[%s] 解析命令时出错: %s", __APP_NAME__.c_str(), e.what());
// }
// 发布状态消息
std_msgs::String status_msg;
status_msg.data = "Command received: " + cmd;
status_pub_.publish(status_msg);
}
Head_servo::~Head_servo()
{
if (serial_.isOpen())
{
serial_.close();
}
}
// CRC16 check implementation
uint16_t Head_servo::usMBCRC16(uint8_t *pucFrame, uint16_t usLen)
{
uint8_t ucCRCHi = 0xFF;
uint8_t ucCRCLo = 0xFF;
int iIndex;
while (usLen--)
{
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
}
return (uint16_t)(ucCRCLo << 8 | ucCRCHi);
}
// Serial init
void Head_servo::serial_initial()
{
serial::Timeout timeOut = serial::Timeout::simpleTimeout(1000);
try
{
serial_.setPort(head_servo_com_);
serial_.setBaudrate(baudrate_);
serial_.setTimeout(timeOut);
serial_.open();
}
catch (serial::IOException &e)
{
ROS_ERROR("[%s] Serial open failed: %s", __APP_NAME__.c_str(), e.what());
return;
}
if (serial_.isOpen())
{
ROS_INFO("[%s] Serial init success, starting init sequence.", __APP_NAME__.c_str());
// Send 485 enable
serial_.write(ModusEn, sizeof(ModusEn));
ros::Duration(0.01).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.01).sleep();
// Init speed
serial_.write(Motor_speed_target_2, sizeof(Motor_speed_target_2));
ros::Duration(0.005).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.005).sleep();
// Stop multiple times
for (uint8_t i = 0; i < 100; i++)
{
position_stop();
ros::Duration(0.02).sleep();
}
// // Return to 0
Turn_angle(0);
ros::Duration(1.0).sleep();
ROS_INFO("[%s] Init done, waiting for commands", __APP_NAME__.c_str());
}
}
// // Control cmd callback
// void Head_servo::control_cmd_callback(const head_servo_controller::HeadControlCmd::ConstPtr& msg)
// {
// // Set flags per msg fields
// if (msg->query_angle) flag_1 = 1;
// if (msg->set_target) { flag_2 = 1; target_speed = msg->target_speed; target_angle = msg->target_angle; }
// if (msg->enter_cruise) flag_3 = 1;
// if (msg->exit_cruise) flag_4 = 1;
// if (msg->home_position) flag_5 = 1;
// if (msg->set_zero) flag_7 = 1;
// ROS_INFO("[%s] Recv cmd: query=%d, set=%d, enter_cruise=%d, exit_cruise=%d, home=%d, set_zero=%d",
// __APP_NAME__.c_str(), msg->query_angle, msg->set_target, msg->enter_cruise,
// msg->exit_cruise, msg->home_position, msg->set_zero);
// }
// Position stop func
void Head_servo::position_stop()
{
serial_.write(Motor_mole_zero, sizeof(Motor_mole_zero));
ros::Duration(0.05).sleep();
serial_.write(Motor_incre_zero, sizeof(Motor_incre_zero));
ros::Duration(0.05).sleep();
}
// Turn to specified angle
void Head_servo::Turn_angle(int16_t target_angle)
{
ROS_INFO("[0", __APP_NAME__.c_str());
// target_angle = target_angle > 135 ? 135 : target_angle;
// target_angle = target_angle < 45 ? 45 : target_angle;
// 目标角度不能是0度，最多到1度
// (对于脖子来说0度和1度无所谓，保证正确，0度就变成1度，，，其他而言，零度发1个脉冲，而不是1度)
target_angle = target_angle == 0 ? 1 : target_angle;
// 读取当前角度
// int16_t current_angle = read_current_angle();
// while (current_angle == 0x7FFF)
// {
// current_angle = read_current_angle();
// }
// printf("current angle is %d\r\n", current_angle);
// 转换成要发送给电机的数据
int32_t po_angle = 0xFFFF;
if (target_angle<0)
{
po_angle = -(-target_angle * (32768 * 8) / 360);
}
else if(target_angle>0){
po_angle = target_angle * (32768 * 8) / 360;
}
// 构件要发送的帧数据
Angle_Target[8] = po_angle & 0xFF;
Angle_Target[7] = (po_angle >> 8) & 0xFF;
Angle_Target[10] = (po_angle >> 16) & 0xFF;
Angle_Target[9] = (po_angle >> 24) & 0xFF;
uint16_t crc_temp = usMBCRC16(Angle_Target, 11);
Angle_Target[11] = (crc_temp >> 8) & 0xFF;
Angle_Target[12] = crc_temp & 0xFF;
/* 转到指定角度 */
serial_.write(Angle_Target, sizeof(Angle_Target));
ros::Duration(0.001).sleep();
serial_.write(Angle_Target, sizeof(Angle_Target));
ROS_INFO("[1", __APP_NAME__.c_str());
ros::Duration(7).sleep();
/* 每隔1ms查询一次当前位置，到达目标位置为止 */
/* 发送命令，读取当前位置以作验证 */
// int16_t angle = 0x7FFF;
// int16_t de_angle = 0x7FFF;
// // flag_count_1s = 0;
// while (1)
// {
// // if (flag_count_1s >= 20)
// // {
// // flag_count_1s = 0;
// // break;
// // }
// angle = read_current_angle();
// de_angle = (target_angle - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
}
/*
将两个16位数据转换成一个32位数据
11是高16位，10是低16位
左移优先级很低，使用时记得加括号
*/
void Head_servo::calculate_modbus_data(uint16_t modbus_data_11, uint16_t modbus_data_10, DispModbusData *disp_modbus_data)
{
// printf("%d\t\t%d\r\n", modbus_data_11, modbus_data_10);
if (modbus_data_11 > 32767)
{ // If high byte exceeds 32767, interpret as negative value
// Calculate PU for negative value
disp_modbus_data->PU = ((modbus_data_11)-32768) * 65536 + modbus_data_10;
// Convert to negative value (two's complement)
disp_modbus_data->PU = -((0x7FFFFFFF - disp_modbus_data->PU) + 1);
}
else
{
// Calculate PU for positive value
disp_modbus_data->PU = modbus_data_11 * 65536 + modbus_data_10;
}
}
// Read current angle
int16_t Head_servo::read_current_angle()
{
// Send read angle cmd
uint8_t read_cmd[] = {0x01, 0x03, 0x00, 0x16, 0x00, 0x02, 0x25, 0xCF};
DispModbusData recvData = {0};
serial_.write(read_cmd, sizeof(read_cmd));
// Wait for response
// ros::Time start_time = ros::Time::now();
// while ((ros::Time::now() - start_time).toSec() < 0.1) // 100ms timeout
// {
// ros::Duration(0.003).sleep();
if (serial_.available() >= 7) // Min 7 bytes response
{
uint8_t buffer[100];
// size_t bytes_read = serial_.read(buffer, serial_.available());
size_t bytes_read = serial_.read(buffer, std::min(serial_.available(), (size_t)100));
// 打印原始数据（以十六进制格式）
ROS_INFO("[%s] 收到串口数据 (%zu bytes):", __APP_NAME__.c_str(), bytes_read);
for (size_t i = 0; i < bytes_read; i++) {
ROS_INFO(" [%02zu]: 0x%02X", i, buffer[i]);
}
// Simple parse (assumed correct format)
if (bytes_read >= 7 && buffer[0] == 0x01 && buffer[1] == 0x03)
{
// Extract angle (high byte first)
int16_t angle_1 = (buffer[3] << 8) | buffer[4];
int16_t angle_2 = (buffer[5] << 8) | buffer[6];
calculate_modbus_data(angle_2, angle_1, &recvData);
return (recvData.PU * 360 / (32768 * 8));
}
}
return 0x7FFF;
ros::Duration(0.001).sleep();
// }
// return 0x7FFF; // Invalid on timeout
}
// Set current pos as zero
void Head_servo::setCurrentPositionZero()
{
serial_.write(Motor_mole_set_zero_1, sizeof(Motor_mole_set_zero_1));
ros::Duration(0.050).sleep();
serial_.write(Motor_mole_set_zero_2, sizeof(Motor_mole_set_zero_2));
ros::Duration(0.050).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.050).sleep();
}
// Response func (replace original fun_response)
void Head_servo::fun_response(uint8_t cmd, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4)
{
// Build response frame
uint8_t response[] = {0x01, cmd, 0x04, data1, data2, data3, data4, 0x00, 0x00};
// Calc CRC
uint16_t crc = usMBCRC16(response, 7);
response[7] = (crc >> 8) & 0xFF;
response[8] = crc & 0xFF;
// Send response
serial_.write(response, 9);
}
// Core process func
void Head_servo::process(const ros::TimerEvent &e)
{
if (!serial_.isOpen()) return;
// Handle flag_6 (restart, log only)
// 这里不是单片机，不需要重启
// if (flag_6 == 1)
// {
// ROS_WARN("[%s] Recv restart cmd", __APP_NAME__.c_str());
// flag_6 = 0;
// return;
// }
// Handle flag_1: query angle
// ROS_INFO("process");
if (flag_1 == 1)
{
ROS_INFO("1");
int16_t angle = read_current_angle();
while (angle == 0x7FFF) // Wait for valid
{
angle = read_current_angle();
ROS_INFO("2");
ros::Duration(0.01).sleep();
}
ROS_INFO("[%s] Current angle: %d°", __APP_NAME__.c_str(), angle);
flag_1 = 0;
}
// Handle flag_2: turn to angle at speed
if (flag_2 == 1)
{
ROS_INFO("[%s] Recv target: speed=%d, angle=%d", __APP_NAME__.c_str(), target_speed, target_angle);
// target_angle 是从外部获取得到
if ((target_angle <-45 || target_angle > 45) && target_angle != 0xFFFF)
{
ROS_WARN("[%s] Angle out of range (-45 to 45)", __APP_NAME__.c_str());
flag_2 = 0;
return;
}
// Handle speed setting
if (target_speed != 0xFFFF)
{
uint8_t Motor_speed_target_copy[8];
memcpy(Motor_speed_target_copy, Motor_speed_target, sizeof(Motor_speed_target));
// Update speed
Motor_speed_target_copy[4] = (target_speed >> 8) & 0xFF;
Motor_speed_target_copy[5] = target_speed & 0xFF;
// Recalc CRC
uint16_t crc = usMBCRC16(Motor_speed_target_copy, 6);
Motor_speed_target_copy[6] = (crc >> 8) & 0xFF;
Motor_speed_target_copy[7] = crc & 0xFF;
// Send cmd
serial_.write(Motor_speed_target_copy, sizeof(Motor_speed_target_copy));
ros::Duration(0.005).sleep();
serial_.write(param_save, sizeof(param_save));
ros::Duration(0.005).sleep();
ROS_INFO("[%s] Speed set: %d", __APP_NAME__.c_str(), target_speed);
}
// Handle angle setting
if (target_angle != 0xFFFF)
{
position_stop(); // Stop first
ros::Duration(0.01).sleep();
Turn_angle(target_angle); // Turn to target
position_stop(); // Stop after arrival
ROS_INFO("[%s] Turned to: %d°", __APP_NAME__.c_str(), target_angle);
}
flag_2 = 0;
}
// Handle flag_3: enter cruise (45°↔135°)
if (flag_3 == 1)
{
ROS_INFO("[%s] Enter cruise mode", __APP_NAME__.c_str());
if(flag_4==1){
flag_3 = 0;
return ;
}
Turn_angle(30);
if(flag_4==1){
flag_3 = 0;
return ;
}
Turn_angle(-30);
if(flag_4==1) {
flag_3 = 0;
return ;
}
// // Turn to 135°
// position_stop();
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P135, sizeof(PosAngle_P135));
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P135, sizeof(PosAngle_P135));
// // Wait to reach 135° (≤3° error)
// /* 每隔1ms查询一次当前位置（读取当前位置中有个20ms延时），到达目标位置为止 */
// /* 发送命令，读取当前位置以作验证 */
// int16_t angle = 0x7FFF;
// int16_t de_angle = 0x7FFF; // 当前位置与目标位置的偏差
// int16_t last_angle = 0x7FFF;
// while (1)
// {
// // 终端中收到了停止指令，进入下一轮主循环
// if (flag_4 == 1)
// {
// flag_3 = 0;
// return;
// }
// angle = read_current_angle();
// de_angle = (135 - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
// /* 逆时针转到45° */
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P45, sizeof(PosAngle_P135));
// ros::Duration(0.001).sleep();
// serial_.write(PosAngle_P45, sizeof(PosAngle_P135));
// /* 每隔1ms查询一次当前位置，到达目标位置为止 */
// /* 发送命令，读取当前位置以作验证 */
// angle = 0x7FFF;
// de_angle = 0x7FFF;
// last_angle = 0x7FFF;
// while (1)
// {
// if (flag_4 == 1)
// {
// flag_3 = 0;
// return;
// }
// angle = read_current_angle();
// de_angle = (45 - angle);
// de_angle = de_angle > 0 ? de_angle : -de_angle;
// if (de_angle >= 0 && de_angle <= 3)
// {
// break;
// }
// ros::Duration(0.001).sleep();
// }
}
// Handle flag_4: exit cruise
if (flag_4 == 1)
{
position_stop();
ROS_INFO("[%s] Exited cruise mode", __APP_NAME__.c_str());
flag_4 = 0;
}
// Handle flag_5: return to 90°
if (flag_5 == 1)
{
ROS_INFO("[%s] Return to 90°", __APP_NAME__.c_str());
Turn_angle(0);
flag_5 = 0;
}
// Handle flag_7: set current as zero
if (flag_7 == 1)
{
ROS_INFO("[%s] Set current pos as zero", __APP_NAME__.c_str());
setCurrentPositionZero();
flag_7 = 0;
}
}
// Run node
void Head_servo::run()
{
serial_initial(); // Init serial and hardware
// Init topics
// control_cmd_sub_ = nh_.subscribe("head_control_cmd", 1, &Head_servo::control_cmd_callback, this);
// current_angle_pub_ = nh_.advertise<head_servo_controller::HeadAngleMsg>("current_angle", 1);
// Start timer (run process every 10ms)
ros::Timer timer = nh_.createTimer(ros::Duration(0.01), &Head_servo::process, this);
// ROS spin
ros::spin();
}
// Main
int main(int argc, char **argv)
{
ros::init(argc, argv, "head_servo_controller");
ros::NodeHandle nh("~");
Head_servo head_servo(nh);
head_servo.run();
return 0;
}
sensors/bozi_ws/src/head_servo_controller/src/head_servo_node.cpp
+10
View File
@@ -0,0 +1,10 @@
// #include "head_servo_core.h"
// int main(int argc, char **argv){
// ros::init(argc, argv, "head_servo_node");
// ros::NodeHandle nh;
// Head_servo head_servo_contrl(nh);
// ros::spin();
// // head_servo_contrl.run();
// return 0;
// }
sensors/can_driver/CMakeLists.txt
+39
View File
@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(can_driver)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
can_msgs
message_generation
)
generate_messages(
DEPENDENCIES
std_msgs
)
set(LOCAL_LIBS ${CMAKE_CURRENT_SOURCE_DIR}/libcontrolcan.so)
catkin_package(
CATKIN_DEPENDS roscpp std_msgs message_runtime
)
include_directories(
${catkin_INCLUDE_DIRS}
include
)
add_executable(can_ros_node
src/can_ros_node.cpp
src/can_driver.cpp
)
target_link_libraries(can_ros_node
${catkin_LIBRARIES}
${LOCAL_LIBS}
)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
sensors/can_driver/include/can_ros_package/can_ros_node.h
+59
View File
@@ -0,0 +1,59 @@
#ifndef CAN_ROS_PACKAGE_CAN_ROS_NODE_H
#define CAN_ROS_PACKAGE_CAN_ROS_NODE_H
#include <ros/ros.h>
#include <vector>
#include <thread>
#include <mutex>
#include <atomic>
#include <condition_variable>
#include "controlcan.h"
#include "can_msgs/Frame.h"
namespace can_ros_package {
class CanDriver {
public:
CanDriver();
~CanDriver();
bool init();
bool initCanChannel(int channel);
bool sendToCan1(VCI_CAN_OBJ& msg);
bool sendToCan2(VCI_CAN_OBJ& msg);
std::vector<VCI_CAN_OBJ> getReceivedMessages();
void stop();
bool isCan1Connected();
bool isCan2Connected();
private:
void receiveLoop();
void reconnect(int channel);
bool tryReconnectChannel(int channel);
std::atomic<bool> is_running_;
std::atomic<bool> can1_connected_;
std::atomic<bool> can2_connected_;
DWORD device_type_;
DWORD device_ind_;
VCI_BOARD_INFO board_info_;
std::thread receive_thread_;
std::mutex receive_mutex_;
std::mutex send_mutex_;
std::mutex reconnect_mutex_;
std::vector<VCI_CAN_OBJ> can_msgs_;
// 接收缓冲区状态管理
std::condition_variable receive_cv_;
bool new_data_available_;
const int reconnect_interval_ms_ = 600;
const int max_reconnect_attempts_ = 5; // 最大重连尝试次数
};
} // namespace can_ros_package
#endif // CAN_ROS_PACKAGE_CAN_ROS_NODE_H
sensors/can_driver/include/can_ros_package/controlcan.h
+104
View File
@@ -0,0 +1,104 @@
#ifndef CONTROLCAN_H
#define CONTROLCAN_H
////文件版本：v2.02 20190609
//接口卡类型定义
#define VCI_USBCAN1 3
#define VCI_USBCAN2 4
#define VCI_USBCAN2A 4
#define VCI_USBCAN_E_U 20
#define VCI_USBCAN_2E_U 21
//函数调用返回状态值
#define STATUS_OK 1
#define STATUS_ERR 0
#define USHORT unsigned short int
#define BYTE unsigned char
#define CHAR char
#define UCHAR unsigned char
#define UINT unsigned int
#define DWORD unsigned int
#define PVOID void*
#define ULONG unsigned int
#define INT int
#define UINT32 UINT
#define LPVOID void*
#define BOOL BYTE
#define TRUE 1
#define FALSE 0
//1.ZLGCAN系列接口卡信息的数据类型。
typedef struct _VCI_BOARD_INFO{
USHORT hw_Version;
USHORT fw_Version;
USHORT dr_Version;
USHORT in_Version;
USHORT irq_Num;
BYTE can_Num;
CHAR str_Serial_Num[20];
CHAR str_hw_Type[40];
USHORT Reserved[4];
} VCI_BOARD_INFO,*PVCI_BOARD_INFO;
//2.定义CAN信息帧的数据类型。
typedef struct _VCI_CAN_OBJ{
UINT ID;
UINT TimeStamp;
BYTE TimeFlag;
BYTE SendType;
BYTE RemoteFlag;//是否是远程帧
BYTE ExternFlag;//是否是扩展帧
BYTE DataLen;
BYTE Data[8];
BYTE Reserved[3];
}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
//3.定义初始化CAN的数据类型
typedef struct _INIT_CONFIG{
DWORD AccCode;
DWORD AccMask;
DWORD Reserved;
UCHAR Filter;
UCHAR Timing0;
UCHAR Timing1;
UCHAR Mode;
}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
///////// new add struct for filter /////////
typedef struct _VCI_FILTER_RECORD{
DWORD ExtFrame; //是否为扩展帧
DWORD Start;
DWORD End;
}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
#ifdef __cplusplus
#define EXTERN_C extern "C"
#else
#define EXTERN_C
#endif
EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
EXTERN_C DWORD VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
EXTERN_C DWORD VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
#endif // CONTROLCAN_H
sensors/can_driver/launch/can_driver.launch
+22
View File
@@ -0,0 +1,22 @@
<launch>
<!-- CAN驱动节点 -->
<node pkg="can_driver" type="can_ros_node" name="can_ros_node" output="screen">
<!-- CAN设备参数 -->
<param name="device_type" value="4" />
<param name="device_index" value="0" />
<param name="can_index_1" value="0" />
<param name="can_index_2" value="1" />
<!-- 波特率设置，单位为kbps 波特率修改在can_driver.cpp initCanChannel 函数内修改 -->
<!-- <param name="baud_rate_1" value="1000" />
<param name="baud_rate_2" value="250" /> -->
<!-- ROS主题配置 -->
<param name="vcu_topic" value="can_frame_topic" />
<param name="bms_topic" value="/sent_messages_bms" />
<param name="output_topic" value="/receive_canMessages" />
<!-- 接收缓冲区大小 -->
<param name="receive_buffer_size" value="1000" />
</node>
</launch>
sensors/can_driver/libcontrolcan.so
BIN
View File
Binary file not shown.
sensors/can_driver/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package format="2">
<name>can_driver</name>
<version>0.0.1</version>
<description>ROS1 package for CAN bus communication</description>
<maintainer email="your@email.com">Your Name</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>can_msgs</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>can_msgs</exec_depend>
<export>
<build_type>catkin</build_type>
</export>
</package>
sensors/can_driver/src/can_driver.cpp
+314
View File
@@ -0,0 +1,314 @@
#include "can_ros_package/can_ros_node.h"
#include <iostream>
#include <thread>
#include <chrono>
namespace can_ros_package {
CanDriver::CanDriver()
: is_running_(false),
device_type_(VCI_USBCAN2),
device_ind_(0),
can1_connected_(false),
can2_connected_(false),
reconnect_interval_ms_(600),
max_reconnect_attempts_(5),
new_data_available_(false) {
// 初始化互斥锁和原子变量
}
CanDriver::~CanDriver() {
stop();
}
bool CanDriver::init() {
// 打开设备
if (VCI_OpenDevice(device_type_, device_ind_, 0) != STATUS_OK) {
std::cerr << "Failed to open CAN device" << std::endl;
return false;
}
// 读取设备信息
if (VCI_ReadBoardInfo(device_type_, device_ind_, &board_info_) != STATUS_OK) {
std::cerr << "Failed to read board info" << std::endl;
VCI_CloseDevice(device_type_, device_ind_);
return false;
}
// 分别初始化两路CAN通道
bool can1_init = initCanChannel(0);
bool can2_init = initCanChannel(1);
can1_connected_ = can1_init;
can2_connected_ = can2_init;
if (!can1_init && !can2_init) {
VCI_CloseDevice(device_type_, device_ind_);
return false;
}
is_running_ = true;
new_data_available_ = false;
// 启动接收线程
receive_thread_ = std::thread(&CanDriver::receiveLoop, this);
return true;
}
// 波特率 Timing0 (BTR0) Timing1 (BTR1) 说明
// 1 Mbps 0x00 0x14 高速通信
// 500 Kbps 0x00 0x1C 最常用配置
// 250 Kbps 0x01 0x1C 中等速度通信
// 125 Kbps 0x03 0x1C 低速通信，你的原始配置
// 50 Kbps 0x09 0x1C 长距离或低干扰环境
// 10 Kbps 0x13 0x22 非常低速，特殊场景
bool CanDriver::initCanChannel(int channel) {
VCI_INIT_CONFIG config;
config.AccCode = 0;
config.AccMask = 0xFFFFFFFF;
config.Filter = 1; // 接收所有帧
if(channel == 0){
// 设置1000Kbps波特率
config.Timing0 = 0x00;
config.Timing1 = 0x14;
} else if(channel == 1) {
// 设置250Kbps波特率
config.Timing0 = 0x01;
config.Timing1 = 0x1C;
}
config.Mode = 0; // 正常模式
if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
std::cerr << "Failed to initialize CAN channel " << channel << std::endl;
return false;
}
// 启动前短暂延迟
std::this_thread::sleep_for(std::chrono::milliseconds(10));
if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
std::cerr << "Failed to start CAN channel " << channel << std::endl;
return false;
}
return true;
}
bool CanDriver::sendToCan1(VCI_CAN_OBJ& msg) {
if (!can1_connected_) {
return false;
}
std::lock_guard<std::mutex> send_lock(send_mutex_);
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
int result = VCI_Transmit(device_type_, device_ind_, 0, &msg, 1);
if (result != STATUS_OK) {
std::cerr << "Failed to send to CAN1, error code: " << result << std::endl;
can1_connected_ = false;
std::thread(&CanDriver::reconnect, this, 0).detach();
return false;
}
return true;
}
bool CanDriver::sendToCan2(VCI_CAN_OBJ& msg) {
if (!can2_connected_) {
return false;
}
std::lock_guard<std::mutex> send_lock(send_mutex_);
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
int result = VCI_Transmit(device_type_, device_ind_, 1, &msg, 1);
if (result != STATUS_OK) {
std::cerr << "Failed to send to CAN2, error code: " << result << std::endl;
can2_connected_ = false;
std::thread(&CanDriver::reconnect, this, 1).detach();
return false;
}
return true;
}
void CanDriver::receiveLoop() {
VCI_CAN_OBJ rec[3000];
int reclen = 0;
while (is_running_) {
// 接收CAN1数据
if (can1_connected_) {
{
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
reclen = VCI_Receive(device_type_, device_ind_, 0, rec, 3000, 0); // 0ms超时实现非阻塞读取
}
if (reclen < 0) {
std::cerr << "Error receiving from CAN1, error code: " << reclen << std::endl;
can1_connected_ = false;
std::thread(&CanDriver::reconnect, this, 0).detach();
} else if (reclen > 0) {
std::lock_guard<std::mutex> receive_lock(receive_mutex_);
for (int j = 0; j < reclen; j++) {
can_msgs_.push_back(rec[j]);
}
new_data_available_ = true;
receive_cv_.notify_one();
}
}
// 接收CAN2数据
if (can2_connected_) {
{
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
reclen = VCI_Receive(device_type_, device_ind_, 1, rec, 3000, 0); // 0ms超时实现非阻塞读取
}
if (reclen < 0) {
std::cerr << "Error receiving from CAN2, error code: " << reclen << std::endl;
can2_connected_ = false;
std::thread(&CanDriver::reconnect, this, 1).detach();
} else if (reclen > 0) {
std::lock_guard<std::mutex> receive_lock(receive_mutex_);
for (int j = 0; j < reclen; j++) {
can_msgs_.push_back(rec[j]);
}
new_data_available_ = true;
receive_cv_.notify_one();
}
}
// 短暂休眠，避免CPU占用过高
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
std::vector<VCI_CAN_OBJ> CanDriver::getReceivedMessages() {
std::unique_lock<std::mutex> lock(receive_mutex_);
// 等待新数据或超时
if (!new_data_available_) {
receive_cv_.wait_for(lock, std::chrono::milliseconds(10));
}
std::vector<VCI_CAN_OBJ> result = can_msgs_;
can_msgs_.clear();
new_data_available_ = false;
return result;
}
void CanDriver::reconnect(int channel) {
std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
int attempt = 0;
while (is_running_ && attempt < max_reconnect_attempts_ &&
!(channel == 0 ? can1_connected_ : can2_connected_))
{
std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
if (tryReconnectChannel(channel)) {
ROS_INFO_STREAM("Successfully reconnected to " << channel_name);
break;
} else {
ROS_WARN_STREAM("Reconnect attempt " << (attempt+1)
<< "/" << max_reconnect_attempts_
<< " failed for " << channel_name);
attempt++;
}
}
if (attempt >= max_reconnect_attempts_) {
ROS_ERROR_STREAM("Giving up on reconnecting " << channel_name
<< " after " << max_reconnect_attempts_ << " attempts");
}
}
bool CanDriver::tryReconnectChannel(int channel) {
std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
// 1. 重置通道
if (is_running_) {
VCI_ResetCAN(device_type_, device_ind_, channel);
VCI_ClearBuffer(device_type_, device_ind_, channel);
}
// 2. 重新初始化通道
VCI_INIT_CONFIG config;
config.AccCode = 0;
config.AccMask = 0xFFFFFFFF;
config.Filter = 1; // 接收所有帧
if(channel == 0){
// 设置1000Kbps波特率
config.Timing0 = 0x00;
config.Timing1 = 0x1C;
} else if(channel == 1) {
// 设置250Kbps波特率
config.Timing0 = 0x01;
config.Timing1 = 0x1C;
}
config.Mode = 0; // 正常模式
if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
return false;
}
// 启动前短暂延迟
std::this_thread::sleep_for(std::chrono::milliseconds(10));
if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
return false;
}
// 3. 更新连接状态
if (channel == 0) {
can1_connected_ = true;
} else {
can2_connected_ = true;
}
// 启动后延迟确保稳定
std::this_thread::sleep_for(std::chrono::milliseconds(10));
return true;
}
void CanDriver::stop() {
if (is_running_) {
is_running_ = false;
can1_connected_ = false;
can2_connected_ = false;
// 通知接收线程退出
{
std::lock_guard<std::mutex> lock(receive_mutex_);
receive_cv_.notify_all();
}
if (receive_thread_.joinable()) {
receive_thread_.join();
}
// 复位并关闭两路CAN通道
VCI_ResetCAN(device_type_, device_ind_, 0);
VCI_ResetCAN(device_type_, device_ind_, 1);
VCI_CloseDevice(device_type_, device_ind_);
}
}
bool CanDriver::isCan1Connected() {
return can1_connected_;
}
bool CanDriver::isCan2Connected() {
return can2_connected_;
}
} // namespace can_ros_package
sensors/can_driver/src/can_ros_node.cpp
+120
View File
@@ -0,0 +1,120 @@
#include "can_ros_package/can_ros_node.h"
#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include <boost/function.hpp>
#include <boost/bind.hpp>
int main(int argc, char** argv) {
ros::init(argc, argv, "can_ros_node");
ros::NodeHandle nh;
can_ros_package::CanDriver can_driver;
if (!can_driver.init()) {
ROS_FATAL("Failed to initialize CAN driver");
return -1;
}
ROS_INFO("CAN driver initialized successfully");
ros::Publisher can_pub = nh.advertise<can_msgs::Frame>("/receive_canMessages", 1000);
// 创建高优先级线程处理消息发布
ros::AsyncSpinner async_spinner(1); // 单线程处理
async_spinner.start();
// 使用boost::function包装lambda表达式
// VCU主题订阅 - 通过CAN1发送
boost::function<void(const can_msgs::Frame::ConstPtr&)> vcu_callback =
[&](const can_msgs::Frame::ConstPtr& msg) {
if (can_driver.isCan1Connected()) {
VCI_CAN_OBJ send_msg{};
send_msg.ID = msg->id;
send_msg.SendType = 0;
send_msg.ExternFlag = msg->is_extended ? 1 : 0;
send_msg.DataLen = msg->dlc;
for (int i = 0; i < msg->dlc && i < 8; i++) {
send_msg.Data[i] = msg->data[i];
}
if (!can_driver.sendToCan1(send_msg)) {
ROS_WARN_THROTTLE(1.0, "Failed to send VCU message to CAN1");
}
} else {
ROS_WARN_THROTTLE(1.0, "CAN1 not connected, VCU message dropped");
}
};
ros::Subscriber vcu_sub = nh.subscribe("can_frame_topic", 100, vcu_callback);
// BMS主题订阅 - 通过CAN2发送
boost::function<void(const can_msgs::Frame::ConstPtr&)> bms_callback =
[&](const can_msgs::Frame::ConstPtr& msg) {
if (can_driver.isCan2Connected()) {
VCI_CAN_OBJ send_msg{};
send_msg.ID = msg->id;
send_msg.SendType = 0;
send_msg.RemoteFlag = 0;
send_msg.ExternFlag = msg->is_extended ? 1 : 0;
send_msg.DataLen = msg->dlc;
for (int i = 0; i < msg->dlc && i < 8; i++) {
send_msg.Data[i] = msg->data[i];
}
if (!can_driver.sendToCan2(send_msg)) {
ROS_WARN_THROTTLE(1.0, "Failed to send BMS message to CAN2");
}
} else {
ROS_WARN_THROTTLE(1.0, "CAN2 not connected, BMS message dropped");
}
};
ros::Subscriber bms_sub = nh.subscribe("/sent_messages_bms", 1000, bms_callback);
// VCU Driver主题订阅 - 通过CAN1发送
boost::function<void(const can_msgs::Frame::ConstPtr&)> vcu_driver_callback =
[&](const can_msgs::Frame::ConstPtr& msg) {
if (can_driver.isCan1Connected()) {
VCI_CAN_OBJ send_msg{};
send_msg.ID = msg->id;
send_msg.SendType = 0;
send_msg.RemoteFlag = 0;
send_msg.ExternFlag = msg->is_extended ? 1 : 0;
send_msg.DataLen = msg->dlc;
for (int i = 0; i < msg->dlc && i < 8; i++) {
send_msg.Data[i] = msg->data[i];
}
if (!can_driver.sendToCan1(send_msg)) {
ROS_WARN_THROTTLE(1.0, "Failed to send VCU Driver message to CAN1");
}
} else {
ROS_WARN_THROTTLE(1.0, "CAN1 not connected, VCU Driver message dropped");
}
};
ros::Subscriber vcu_driver_sub = nh.subscribe("/sent_messages_vcuDriver", 1000, vcu_driver_callback);
ros::Rate loop_rate(100); // 100Hz
while (ros::ok()) {
// 发布接收到的CAN消息
std::vector<VCI_CAN_OBJ> received_msgs = can_driver.getReceivedMessages();
if (!received_msgs.empty()) {
for (const auto& msg : received_msgs) {
can_msgs::Frame can_msg;
can_msg.header.stamp = ros::Time::now();
can_msg.id = msg.ID;
can_msg.is_extended = msg.ExternFlag;
can_msg.dlc = msg.DataLen;
for (int i = 0; i < msg.DataLen && i < 8; i++) {
can_msg.data[i] = msg.Data[i];
}
can_pub.publish(can_msg);
}
}
loop_rate.sleep();
}
can_driver.stop();
return 0;
}
sensors/cockpit/CMakeLists.txt
+39
View File
@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(cockpit)
find_package(catkin REQUIRED COMPONENTS
autoware_msgs
roscpp
sensor_msgs
serial
message_generation
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES cockpit
# CATKIN_DEPENDS autoware_msgs roscpp sensor_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/msg
)
add_executable(cockpit_node
src/cockpit_node.cpp
src/cockpit_core.cpp
)
target_link_libraries(cockpit_node
${catkin_LIBRARIES}
)
add_dependencies(cockpit_node
autoware_msgs_generate_messages_cpp
)
sensors/cockpit/include/cockpit_core.h
+50
View File
@@ -0,0 +1,50 @@
#ifndef COCKPIT_CORE_H
#define COCKPIT_CORE_H
#include "ros/ros.h"
#include "serial/serial.h"
#include "rc_receiver/rc.h"
#include "sensor_msgs/BatteryState.h"
#define __APP_NAME__ "cockpit_driver"
class CockPit{
public:
CockPit();
~CockPit();
void run();
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
private:
/*串口号 */
std::string cockpit_com_;
/* 串口波特率 */
int baudrate_;
/* 驾驶模式控制的指令话题 */
std::string input_ctr_mode_topic_;
/* 电池状态的指令话题 */
std::string input_battery_state_topic_;
private:
uint16_t soc_ = 0;
uint8_t current_page_ = 0;
uint8_t drive_mode_ = 0;
private:
ros::Subscriber ctr_mode_sub_;
ros::Subscriber battery_state_sub_;
private:
serial::Serial serial_;
private:
void ctr_mode_callback(const rc_receiver::rc::ConstPtr &msg);
void battery_status_callback(const sensor_msgs::BatteryState::ConstPtr& msg);
void process(const ros::TimerEvent& e);
void serial_initial();
};
#endif
sensors/cockpit/launch/cockpit.launch
+13
View File
@@ -0,0 +1,13 @@
<launch>
<arg name="cockpit_com_" default="/dev/ttyUSB0"/>
<arg name="baudrate_" default="115200"/>
<arg name="input_ctr_mode_topic_" default="/remote_ctrl"/>
<arg name="input_battery_state_topic_" default="/sensor/battery_state"/>
<node name="cockpit_node" pkg="cockpit" type="cockpit_node" output="screen">
<param name="cockpit_com_" value="$(arg cockpit_com_)" type="string"/>
<param name="baudrate_" value="$(arg baudrate_)" type="int"/>
<param name="input_ctr_mode_topic_" value="$(arg input_ctr_mode_topic_)" type="string"/>
<param name="input_battery_state_topic_" value="$(arg input_battery_state_topic_)" type="string"/>
</node>
</launch>
sensors/cockpit/package.xml
+71
View File
@@ -0,0 +1,71 @@
<?xml version="1.0"?>
<package format="2">
<name>cockpit</name>
<version>0.0.0</version>
<description>The cockpit 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/cockpit</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>autoware_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>serial</build_depend>
<build_export_depend>autoware_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>serial</build_export_depend>
<exec_depend>autoware_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>serial</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/cockpit/src/cockpit_core.cpp
+156
View File
@@ -0,0 +1,156 @@
#include "cockpit_core.h"
CockPit::CockPit() : nh_(""), private_nh_("~")
{
private_nh_.param<std::string>("cockpit_com_", cockpit_com_, "");
private_nh_.param<int>("baudrate_", baudrate_, 115200);
private_nh_.param<std::string>("input_ctr_mode_topic_", input_ctr_mode_topic_, "");
private_nh_.param<std::string>("input_battery_state_topic_", input_battery_state_topic_, "");
}
CockPit::~CockPit() {}
void CockPit::run()
{
serial_initial();
ctr_mode_sub_ = nh_.subscribe<rc_receiver::rc>(input_ctr_mode_topic_, 1, &CockPit::ctr_mode_callback, this);
battery_state_sub_ = nh_.subscribe<sensor_msgs::BatteryState>(input_battery_state_topic_, 1, &CockPit::battery_status_callback, this);
ros::Timer timer = nh_.createTimer(ros::Duration(0.4), &CockPit::process, this, false, true);
ros::spin();
}
void CockPit::serial_initial()
{
serial::Timeout timeOut = serial::Timeout::simpleTimeout(1000);
try
{
serial_.setPort(cockpit_com_);
serial_.setBaudrate(baudrate_);
serial_.setTimeout(timeOut);
serial_.open();
}
catch (serial::IOException &e)
{
ROS_ERROR("[%s] Unable to open cockpit com", __APP_NAME__);
return;
}
}
void CockPit::ctr_mode_callback(const rc_receiver::rc::ConstPtr &msg)
{
drive_mode_ = msg->control_mode;
}
void CockPit::battery_status_callback(const sensor_msgs::BatteryState::ConstPtr &msg)
{
soc_ = msg->percentage * 10;
current_page_ = (msg->power_supply_status == 1) ? 1 : 0;
}
/* CRC16 数组 */
static const uint8_t aucCRCHi[] = {0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80,
0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00,
0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
static const uint8_t aucCRCLo[] = {0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D,
0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB,
0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2,
0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37,
0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8,
0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24,
0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63,
0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F,
0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C,
0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46,
0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
// CRC计算函数
uint16_t usMBCRC16(uint8_t *pucFrame, uint16_t usLen)
{
uint8_t ucCRCHi = 0xFF; // 初始值为0xFF（高字节）
uint8_t ucCRCLo = 0xFF; // 初始值为0xFF（低字节）
int iIndex;
while (usLen--)
{
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
}
return (uint16_t)(ucCRCHi << 8 | ucCRCLo); // 大端序结果（高字节在前）
}
uint8_t Intelligent_str_cmd[18] = {0x5A, 0xA5, 0x0F, 0x10, 0x00, 0xBA, 0xD7, 0xD4, 0xB6, 0xAF, 0xB5, 0xBC, 0xBA, 0xBD, 0x00, 0x00, 0x7A, 0xC1};
uint8_t remote_str_cmd[18] = {0x5A, 0xA5, 0x0F, 0x10, 0x00, 0xBA, 0xD2, 0xA3, 0xBF, 0xD8, 0xBC, 0xDD, 0xCA, 0xBB, 0x00, 0x00, 0xA9, 0xDE};
uint8_t page0_cmd[10] = {0x5A, 0xA5, 0x07, 0x10, 0x70, 0X00, 0X00, 0X00, 0XFF, 0X03};
uint8_t page1_charging_cmd[10] = {0x5A, 0xA5, 0x07, 0x10, 0x70, 0X00, 0X00, 0X01, 0X3E, 0XC3};
uint8_t buffer_battery[10] = {0x5A, 0xA5, 0x07, 0x10, 0x00, 0x5C, 0x03, 0xE8, 0x25, 0x6F};
void CockPit::process(const ros::TimerEvent &e)
{
// 发送页面显示
if (current_page_ == 0)
{
serial_.write(page0_cmd, sizeof(page0_cmd));
ros::Duration(0.01).sleep();//指令间隔延时
static int count = 0; // 用于命令发送计数
/* 智驾模式 */
if (drive_mode_ == 0)
{
if (count < 2)
{
serial_.write(Intelligent_str_cmd, sizeof(Intelligent_str_cmd));
}
else
{
count = 2;
}
count++;
}
else
{
/* 遥控模式 */
if (count > -2)
{
serial_.write(remote_str_cmd, sizeof(remote_str_cmd));
}
else
{
count = -2;
}
count--;
}
}
else
{
// 充电页面
serial_.write(page1_charging_cmd, sizeof(page1_charging_cmd));
}
ros::Duration(0.01).sleep();
// 发送电量显示
buffer_battery[6] = (soc_ & 0xFFFF) >> 8;
buffer_battery[7] = soc_ & 0xFF;
/* 校验 */
uint16_t Voltage_CRC = usMBCRC16(&buffer_battery[3], 5);
buffer_battery[8] = (uint8_t)(Voltage_CRC >> 0); // CRC校验值
buffer_battery[9] = (uint8_t)(Voltage_CRC >> 8);
// 串口发送
serial_.write(buffer_battery, sizeof(buffer_battery));
}
sensors/cockpit/src/cockpit_node.cpp
+8
View File
@@ -0,0 +1,8 @@
#include "cockpit_core.h"
int main(int argc, char **argv){
ros::init(argc, argv, "cockpit_node");
CockPit cock_pit;
cock_pit.run();
return 0;
}
sensors/e_stop_relay/CMakeLists.txt
+68
View File
@@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 3.0.2)
project(e_stop_relay)
# 查找必要的catkin组件
find_package(catkin REQUIRED COMPONENTS
roscpp
serial
std_msgs
message_generation
)
# 声明消息文件
add_message_files(
FILES
)
# 声明服务文件
add_service_files(
FILES
EStopService.srv
)
# 生成消息文件
generate_messages(
DEPENDENCIES
std_msgs
)
# 配置catkin包
catkin_package(
INCLUDE_DIRS include
LIBRARIES e_stop_relay
CATKIN_DEPENDS roscpp serial std_msgs message_runtime
)
# 包含头文件路径
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# 编译可执行文件
add_executable(e_stop_relay_node
src/e_stop_relay_node.cpp
)
# 添加消息生成依赖
add_dependencies(e_stop_relay_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# 链接依赖库
target_link_libraries(e_stop_relay_node
${catkin_LIBRARIES}
)
# 安装可执行文件
install(TARGETS e_stop_relay_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# 安装头文件
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
sensors/e_stop_relay/include/e_stop_relay.h
+47
View File
@@ -0,0 +1,47 @@
#ifndef E_STOP_RELAY_H
#define E_STOP_RELAY_H
#include <ros/ros.h>
#include <serial/serial.h>
#include "e_stop_relay/EStopService.h"
class EStopRelay {
public:
EStopRelay();
void e_stop_readSerialData();
void relay_readSerialData();
void e_stop_parseModbusFrame(const std::vector<uint8_t> &frame);
void relay_parseModbusFrame(const std::vector<uint8_t> &frame);
void run();
bool handleEStopService(e_stop_relay::EStopService::Request &req, e_stop_relay::EStopService::Response &res);
bool setLightStatus(bool status);
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
serial::Serial ser_;
ros::Publisher e_stop_pub_;
ros::ServiceServer e_stop_service_;
std::string port_name_;
int baudrate_;
std::string topic_name_;
std::vector<uint8_t> serial_buffer_;
ros::Time lastReceivedTime_;
std::string output_e_stop_topic_;
bool relay_status;
// 新增：重连控制参数（保留原有注释结构）
ros::Duration reconnect_interval_; // 重连间隔时间（秒）
ros::Time last_reconnect_attempt_; // 上次重连尝试的时间戳
// Modbus指令定义（原有注释保留）
const unsigned char e_stop_pressed_data[7] = {0x01, 0x04, 0x02 ,0x00, 0x01, 0x78, 0xF0};
const unsigned char e_stop_released_data[7] = {0x01 ,0x04 ,0x02 ,0x00 ,0x00 ,0xB9 ,0x30};
const unsigned char relay_turnon_cmd[13] = {0x01 ,0x10 ,0x00 ,0x01 ,0x00 ,0x02 ,0x04 ,0x00 ,0x01 ,0x00 ,0x01 ,0xA2 ,0x63};
const unsigned char relay_turnoff_cmd[13] = {0x01 ,0x10 ,0x00 ,0x01 ,0x00 ,0x02 ,0x04 ,0x00 ,0x00 ,0x00 ,0x00 ,0x32 ,0x63};
// 串口初始化函数（原有声明保留）
bool initSerial();
};
#endif // E_STOP_RELAY_H
sensors/e_stop_relay/launch/e_stop_relay.launch
+11
View File
@@ -0,0 +1,11 @@
<launch>
<arg name="e_stop_relay_com_" default="/dev/ttyS0"/>
<arg name="baudrate_" default="115200"/>
<arg name="output_e_stop_topic_" default="/e_stop_states"/>
<node name="e_stop_relay_node" pkg="e_stop_relay" type="e_stop_relay_node" output="screen">
<param name="e_stop_relay_com_" value="$(arg e_stop_relay_com_)" type="string"/>
<param name="baudrate_" value="$(arg baudrate_)" type="int"/>
<param name="output_e_stop_topic_" value="$(arg output_e_stop_topic_)" type="string" />
</node>
</launch>
sensors/e_stop_relay/msg/wheel_motor_states.msg
+2
View File
@@ -0,0 +1,2 @@
# 急停按钮状态（0: Released，1: Pressed）
uint8 emergencyStopButton_status
sensors/e_stop_relay/package.xml
+64
View File
@@ -0,0 +1,64 @@
<?xml version="1.0"?>
<package format="2">
<name>e_stop_relay</name>
<version>0.0.0</version>
<description>The e_stop_relay package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dreamdeck@todo.todo">dreamdeck</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/e_stop_relay</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>serial</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>serial</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>serial</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/e_stop_relay/src/e_stop_relay_node.cpp
+271
View File
@@ -0,0 +1,271 @@
#include "e_stop_relay.h"
#include <std_msgs/Bool.h>
#include <ros/console.h>
#include <boost/algorithm/string.hpp>
// 构造函数
EStopRelay::EStopRelay() : private_nh_("~"), lastReceivedTime_(ros::Time::now())
{
// 从参数服务器获取配置参数
private_nh_.param<std::string>("e_stop_relay_com_", port_name_, "/dev/usb485_1");
private_nh_.param<int>("baudrate_", baudrate_, 115200);
private_nh_.param<std::string>("output_e_stop_topic_", output_e_stop_topic_, "/e_stop_states");
// 创建急停状态发布者
e_stop_pub_ = nh_.advertise<std_msgs::Bool>(output_e_stop_topic_, 10);
relay_status = false;
// 初始化重连参数（5秒重连一次）
reconnect_interval_ = ros::Duration(5.0);
last_reconnect_attempt_ = ros::Time::now() - reconnect_interval_; // 允许立即首次尝试
// 初始化串口（失败不退出，后续重连）
if (!initSerial()) {
ROS_ERROR("Failed to initialize serial port on startup! Will attempt reconnection...");
} else {
ROS_INFO_STREAM("Serial port initialized: " << port_name_ << " @ " << baudrate_);
}
// 创建急停服务服务器
e_stop_service_ = nh_.advertiseService("e_stop_service", &EStopRelay::handleEStopService, this);
ROS_INFO("EStop service is ready.");
}
// 串口初始化（保留原有注释，新增异常处理）
bool EStopRelay::initSerial()
{
try {
// 先关闭已打开的串口（避免重复占用）
if (ser_.isOpen()) {
ser_.close();
}
// 配置串口参数
ser_.setPort(port_name_);
ser_.setBaudrate(baudrate_);
serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 超时时间1秒
ser_.setTimeout(to);
ser_.open();
} catch (serial::IOException &e) {
ROS_ERROR_STREAM("Unable to open serial port: " << e.what());
return false;
} catch (std::exception &e) {
ROS_ERROR_STREAM("Serial port error: " << e.what());
return false;
}
return ser_.isOpen();
}
// 读取急停串口数据（保留原有注释，新增重连逻辑）
void EStopRelay::e_stop_readSerialData()
{
// 新增：检查串口状态，未打开则尝试重连
if (!ser_.isOpen()) {
ros::Time now = ros::Time::now();
if (now - last_reconnect_attempt_ > reconnect_interval_) {
ROS_INFO_STREAM("Attempting to reconnect to " << port_name_ << "...");
if (initSerial()) {
ROS_INFO_STREAM("Successfully reconnected to " << port_name_);
serial_buffer_.clear(); // 清空缓冲区，避免旧数据干扰
} else {
ROS_ERROR_STREAM("Reconnection to " << port_name_ << " failed. Retrying...");
}
last_reconnect_attempt_ = now;
}
return; // 串口未就绪，退出本次读取
}
// 读取
try {
const size_t available = ser_.available();
if (available > 0) {
std::vector<uint8_t> buffer(available);
ser_.read(buffer.data(), available);
serial_buffer_.insert(serial_buffer_.end(), buffer.begin(), buffer.end());
lastReceivedTime_ = ros::Time::now(); // 更新最后接收时间
}
} catch (serial::IOException &e) {
// 新增：捕获读取异常（物理断开时触发）
ROS_ERROR_STREAM("Serial read error (e-stop): " << e.what() << " - forcing reconnection");
ser_.close(); // 标记为关闭，触发重连
return;
}
// 解析Modbus帧
if (serial_buffer_.size() == 7) {
std::vector<uint8_t> frame(serial_buffer_.begin(), serial_buffer_.begin() + 7);
e_stop_parseModbusFrame(frame);
serial_buffer_.erase(serial_buffer_.begin(), serial_buffer_.begin() + 7);
} else {
serial_buffer_.clear(); // 帧不完整时清空，避免堆积
}
}
// 读取继电器串口数据（保留原有注释，新增重连逻辑）
void EStopRelay::relay_readSerialData()
{
// 检查串口状态，未打开则尝试重连
if (!ser_.isOpen()) {
ros::Time now = ros::Time::now();
if (now - last_reconnect_attempt_ > reconnect_interval_) {
ROS_INFO_STREAM("Attempting to reconnect to " << port_name_ << "...");
if (initSerial()) {
ROS_INFO_STREAM("Successfully reconnected to " << port_name_);
serial_buffer_.clear(); // 清空缓冲区
} else {
ROS_ERROR_STREAM("Reconnection to " << port_name_ << " failed. Retrying...");
}
last_reconnect_attempt_ = now;
}
return; // 串口未就绪，退出本次读取
}
// 读取
try {
const size_t available = ser_.available();
if (available > 0) {
std::vector<uint8_t> buffer(available);
ser_.read(buffer.data(), available);
serial_buffer_.insert(serial_buffer_.end(), buffer.begin(), buffer.end());
lastReceivedTime_ = ros::Time::now(); // 更新最后接收时间
}
} catch (serial::IOException &e) {
// 捕获读取异常（物理断开时触发）
ROS_ERROR_STREAM("Serial read error (relay): " << e.what() << " - forcing reconnection");
ser_.close(); // 标记为关闭，触发重连
return;
}
// 解析Modbus帧
if (serial_buffer_.size() == 13) {
std::vector<uint8_t> frame(serial_buffer_.begin(), serial_buffer_.begin() + 13);
relay_parseModbusFrame(frame);
serial_buffer_.erase(serial_buffer_.begin(), serial_buffer_.begin() + 13);
} else {
serial_buffer_.clear(); // 帧不完整时清空
}
}
// 解析急停Modbus帧
void EStopRelay::e_stop_parseModbusFrame(const std::vector<uint8_t> &frame)
{
std_msgs::Bool e_stop_msg;
// 匹配急停按下指令
if (frame[0] == e_stop_pressed_data[0] && frame[1] == e_stop_pressed_data[1] && frame[2] == e_stop_pressed_data[2]) {
static uint8_t last_stat = 3; // 初始状态无效值
if (frame[4] == e_stop_pressed_data[4]) {
if (last_stat != 1) { // 状态变化时打印日志
ROS_INFO("Emergency stop button is pressed");
}
e_stop_msg.data = true;
e_stop_pub_.publish(e_stop_msg);
last_stat = 1;
} else if (frame[4] == e_stop_released_data[4]) {
if (last_stat != 0) { // 状态变化时打印日志
ROS_INFO("Emergency stop button is released");
}
e_stop_msg.data = false;
e_stop_pub_.publish(e_stop_msg);
last_stat = 0;
}
}
}
// 解析继电器Modbus帧
void EStopRelay::relay_parseModbusFrame(const std::vector<uint8_t> &frame)
{
// 匹配继电器指令帧头
if (frame[0] == 0x01 && frame[1] == 0x10) {
// 继电器打开指令响应
if (frame[8] == 0x01 && frame[10] == 0x01 && frame[11] == 0xA2 && frame[12] == 0x63) {
relay_status = true;
ROS_INFO("Relay turned on");
}
// 继电器关闭指令响应
else if (frame[8] == 0x00 && frame[10] == 0x00 && frame[11] == 0x32 && frame[12] == 0x63) {
relay_status = false;
ROS_INFO("Relay turned off");
}
}
}
// 主循环（超时检测）
void EStopRelay::run()
{
ros::Rate rate(10); // 10Hz循环频率
ros::Duration timeout(5.0); // 新增：5秒无数据判定为超时
while (ros::ok()) {
e_stop_readSerialData();
relay_readSerialData();
// 超时检测（长时间无数据强制重连）
if (ser_.isOpen() && (ros::Time::now() - lastReceivedTime_ > timeout)) {
ROS_WARN("No data received for %fs, forcing reconnection...", timeout.toSec());
ser_.close(); // 关闭串口触发重连
}
ros::spinOnce();
rate.sleep();
}
// 退出时关闭串口
if (ser_.isOpen()) {
ser_.close();
ROS_INFO("Serial port closed on exit");
}
}
// 急停服务处理
bool EStopRelay::handleEStopService(e_stop_relay::EStopService::Request &req, e_stop_relay::EStopService::Response &res)
{
std::string command = req.request_message;
boost::to_lower(command); // 指令转为小写，兼容大小写输入
if (command == "turn on") {
res.success = setLightStatus(true);
res.response_message = res.success ? "Relay turned on" : "Failed to turn on relay";
} else if (command == "turn off") {
res.success = setLightStatus(false);
res.response_message = res.success ? "Relay turned off" : "Failed to turn off relay";
} else {
res.success = false;
res.response_message = "Unknown command: " + req.request_message;
ROS_WARN_STREAM("Received unknown command: " << req.request_message);
}
return true; // 服务处理成功
}
// 控制灯光状态（异常处理）
bool EStopRelay::setLightStatus(bool status)
{
if (!ser_.isOpen()) {
ROS_ERROR("Cannot send command - serial port is not open");
return false;
}
try {
size_t written;
if (status) {
written = ser_.write(relay_turnon_cmd, 13); // 发送打开指令
} else {
written = ser_.write(relay_turnoff_cmd, 13); // 发送关闭指令
}
return written == 13; // 确认指令完整发送
} catch (serial::IOException &e) {
// 新增：发送异常时触发重连
ROS_ERROR_STREAM("Failed to write to serial port: " << e.what() << " - forcing reconnection");
ser_.close();
return false;
}
}
// 主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "e_stop_relay_node");
EStopRelay node;
node.run();
return 0;
}
sensors/e_stop_relay/srv/EStopService.srv
+6
View File
@@ -0,0 +1,6 @@
# 请求部分
string request_message
---
# 响应部分
bool success
string response_message
sensors/fdilink_ahrs/CMakeLists.txt
+50
View File
@@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 2.8)
project(fdilink_ahrs)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
find_package(Eigen3 REQUIRED)
set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
find_package(catkin REQUIRED COMPONENTS
message_generation
# genmsg
# geometry_msgs
roscpp
rospy
sensor_msgs
std_msgs
tf
# std_srvs
nav_msgs
serial
)
include_directories(
include
src
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
)
catkin_package(
CATKIN_DEPENDS message_runtime
# INCLUDE_DIRS include
# LIBRARIES xf_mic_api
# CATKIN_DEPENDS roscpp rospy senor_msgs std_msgs
# DEPENDS system_lib
)
# set(SERIAL_LIB /opt/ros/kinetic/lib/libserial.so)
# set(SERIAL_LIB_PATH /opt/ros/kinetic/lib)
# link_directories(${SERIAL_LIB_PATH})
## ahrs_driver
add_executable(ahrs_driver src/ahrs_driver.cpp)
target_link_libraries(ahrs_driver crc_table ${catkin_LIBRARIES})# ${SERIAL_LIB}
add_executable(imu_tf src/imu_tf.cpp)
target_link_libraries( imu_tf ${catkin_LIBRARIES})# ${SERIAL_LIB}
## crc_table
add_library(crc_table src/crc_table.cpp)
sensors/fdilink_ahrs/README.md
+81
View File
@@ -0,0 +1,81 @@
## fdilink的imu驱动包
Deta-10-ros-v0.0.1
### 依赖：
```bash
sudo apt install ros-melodic-serial
```
### 使用：
ahrs_driver.launch
```
<launch>
<node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
<!-- 串口设备，可通过rules.d配置固定 -->
<param name="port" value="/dev/ttyUSB0"/>
<!-- <param name="port" value="/dev/ttyTHS1"/> -->
<!-- 波特率 -->
<param name="baud" value="921600"/>
<!-- 发布的imu话题名 -->
<param name="imu_topic" value="/imu"/>
<!-- 发布的imu话题中的frame_id -->
<param name="imu_frame" value="imu"/>
<!-- 地磁北的yaw角 --> # 二维指北的朝向，北为0，逆时针增加，0~2π的取值范围。
<param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
<!-- 发布的数据基于不同设备有不同的坐标系 -->
<param name="device_type" value="1"/> <!-- 0: origin_data, 1: for single imu or ucar in ROS, 2:for Xiao in ROS -->
</node>
</launch>
```
其中`device_type`：
0. Deta-10的原始坐标系模式
1. 单独imu的坐标系模式
调用的ahrs_driver节点会发布`sensor_msgs/Imu`格式的imu topic。
```
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance
```
也会发布`geometry_msgs/Pose2D`格式的二维指北角话题，话题名默认为`/mag_pose_2d`。
```
float64 x
float64 y
float64 theta # 指北角
```
### 2020-1-15
维护了文件注释。
### 2020-10-20
添加了`device_type`参数，可以在`ahrs_data.launch`文件中指定设备类型，根据不同设备类型以不同的坐标系发布ROS的imu数据。
其中：
0. Deta-10的原始坐标系模式
1. 单独imu的坐标系模式
sensors/fdilink_ahrs/include/ahrs_driver.h
+97
View File
@@ -0,0 +1,97 @@
#ifndef BASE_DRIVER_H_
#define BASE_DRIVER_H_
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <iostream>
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
#include <fstream>
#include <fdilink_data_struct.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Pose2D.h>
#include <boost/thread.hpp>
#include <string>
#include <ros/package.h>
#include <crc_table.h>
using namespace std;
namespace FDILink
{
#define FRAME_HEAD 0xfc
#define FRAME_END 0xfd
#define TYPE_IMU 0x40
#define TYPE_AHRS 0x41
#define TYPE_INSGPS 0x42
#define TYPE_GEODETIC_POS 0x5c
#define TYPE_GROUND 0xf0
#define IMU_LEN 0x38 //56
#define AHRS_LEN 0x30 //48
#define INSGPS_LEN 0x48 //80
#define GEODETIC_POS_LEN 0x20 //32
#define PI 3.141592653589793
#define DEG_TO_RAD 0.017453292519943295
class ahrsBringup
{
public:
ahrsBringup();
~ahrsBringup();
void processLoop();
bool checkCS8(int len);
bool checkCS16(int len);
void checkSN(int type);
void magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz);
ros::NodeHandle nh_;
private:
bool if_debug_;
//sum info
int sn_lost_ = 0;
int crc_error_ = 0;
uint8_t read_sn_ = 0;
bool frist_sn_;
int device_type_ = 1;
//serial
serial::Serial serial_; //声明串口对象
std::string serial_port_;
int serial_baud_;
int serial_timeout_;
//data
FDILink::imu_frame_read imu_frame_;
FDILink::ahrs_frame_read ahrs_frame_;
FDILink::insgps_frame_read insgps_frame_;
//FDILink::lanlon_frame_read latlon_frame_;
FDILink::Geodetic_Position_frame_read Geodetic_Position_frame_;
//frame name
string imu_frame_id_;
string insgps_frame_id_;
string latlon_frame_id_;
//topic
string imu_topic_, mag_pose_2d_topic_;
string latlon_topic_;
string Euler_angles_topic_,Magnetic_topic_;
string gps_topic_,twist_topic_,NED_odom_topic_;
//Publisher
ros::Publisher imu_pub_;
ros::Publisher gps_pub_;
ros::Publisher mag_pose_pub_;
ros::Publisher Euler_angles_pub_;
ros::Publisher Magnetic_pub_;
ros::Publisher twist_pub_;
ros::Publisher NED_odom_pub_;
}; //ahrsBringup
} // namespace FDILink
#endif
sensors/fdilink_ahrs/include/crc_table.h
+10
View File
@@ -0,0 +1,10 @@
#ifndef CRC_TABLE_H
#define CRC_TABLE_H
#include <stdint.h>
uint8_t CRC8_Table(uint8_t* p, uint8_t counter);
uint16_t CRC16_Table(uint8_t *p, uint8_t counter);
uint32_t CRC32_Table(uint8_t *p, uint8_t counter);
#endif // CRC_TABLE_H
sensors/fdilink_ahrs/include/fdilink_data_struct.h
+189
View File
@@ -0,0 +1,189 @@
#ifndef FDILINK_DATA_STRUCT_H_
#define FDILINK_DATA_STRUCT_H_
#include <iostream>
namespace FDILink{
#pragma pack(1)
struct fdilink_header
{
uint8_t header_start;
uint8_t data_type;
uint8_t data_size;
uint8_t serial_num;
uint8_t header_crc8;
uint8_t header_crc16_h;
uint8_t header_crc16_l;
};
#pragma pack()
#pragma pack(1)
struct IMUData_Packet_t
{
float gyroscope_x; //unit: rad/s
float gyroscope_y; //unit: rad/s
float gyroscope_z; //unit: rad/s
float accelerometer_x; //m/s^2
float accelerometer_y; //m/s^2
float accelerometer_z; //m/s^2
float magnetometer_x; //mG
float magnetometer_y; //mG
float magnetometer_z; //mG
float imu_temperature; //C
float Pressure; //Pa
float pressure_temperature; //C
int64_t Timestamp; //us
};
#pragma pack()
struct AHRSData_Packet_t
{
float RollSpeed; //unit: rad/s
float PitchSpeed; //unit: rad/s
float HeadingSpeed;//unit: rad/s
float Roll; //unit: rad
float Pitch; //unit: rad
float Heading; //unit: rad
float Qw;//w //Quaternion
float Qx;//x
float Qy;//y
float Qz;//z
int64_t Timestamp; //unit: us
};
#pragma pack(1)
struct INSGPSData_Packet_t
{
float BodyVelocity_X;
float BodyVelocity_Y;
float BodyVelocity_Z;
float BodyAcceleration_X;
float BodyAcceleration_Y;
float BodyAcceleration_Z;
float Location_North;
float Location_East;
float Location_Down;
float Velocity_North;
float Velocity_East;
float Velocity_Down;
float Acceleration_North;
float Acceleration_East;
float Acceleration_Down;
float Pressure_Altitude;
int64_t Timestamp;
};
#pragma pack()
#pragma pack(1)
struct Geodetic_Position_Packet_t
{
double Latitude;
double Longitude;
double Height;
float hAcc;
float vAcc;
};
#pragma pack()
//for IMU=========================
#pragma pack(1)
struct read_imu_struct{
fdilink_header header; //7
union data
{
IMUData_Packet_t data_pack; //56
uint8_t data_buff[56]; //56
}data;
uint8_t frame_end; //1
};
struct read_imu_tmp{
uint8_t frame_header[7];
uint8_t read_msg[57];
};
union imu_frame_read{
struct read_imu_struct frame;
read_imu_tmp read_buf;
uint8_t read_tmp[64];
};
#pragma pack()
//for IMU------------------------
//for AHRS=========================
#pragma pack(1)
struct read_ahrs_struct{
fdilink_header header; //7
union data
{
AHRSData_Packet_t data_pack; //48
uint8_t data_buff[48]; //48
}data;
uint8_t frame_end; //1
};
struct read_ahrs_tmp{
uint8_t frame_header[7];
uint8_t read_msg[49];
};
union ahrs_frame_read{
struct read_ahrs_struct frame;
read_ahrs_tmp read_buf;
uint8_t read_tmp[56];
};
#pragma pack()
//for AHRS------------------------
//for INSGPS=========================
#pragma pack(1)
struct read_insgps_struct{
fdilink_header header; //7
union data
{
INSGPSData_Packet_t data_pack; //72
uint8_t data_buff[72]; //72
}data;
uint8_t frame_end; //1
};
struct read_insgps_tmp{
uint8_t frame_header[7];
uint8_t read_msg[73];
};
union insgps_frame_read{
struct read_insgps_struct frame;
read_insgps_tmp read_buf;
uint8_t read_tmp[80];
};
#pragma pack()
//for INSGPS------------------------
//for Geodetic_Position=========================
#pragma pack(1)
struct read_Geodetic_Position_struct{
fdilink_header header; //7
union data
{
Geodetic_Position_Packet_t data_pack; //40
uint8_t data_buff[32]; //40
}data;
uint8_t frame_end; //1
};
struct read_Geodetic_Position_tmp{
uint8_t frame_header[7];
uint8_t read_msg[33];
};
union Geodetic_Position_frame_read{
struct read_Geodetic_Position_struct frame;
read_Geodetic_Position_tmp read_buf;
uint8_t read_tmp[40];
};
#pragma pack()
}//namespace FDILink
#endif//FDILINK_DATA_STRUCT_H_
sensors/fdilink_ahrs/launch/ahrs_data.launch
+38
View File
@@ -0,0 +1,38 @@
<launch>
<node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
<!-- 串口设备，可通过rules.d配置固定。
若使用DETA100,则value="/dev/wheeltec_ch340"
若使用WHEELTEC N系列，则不需要改动 -->
<param name="port" value="/dev/ttyUSB0"/>
<!-- 波特率 -->
<param name="baud" value="921600"/>
<!-- 发布的imu话题名 -->
<param name="imu_topic" value="/imu/data"/>
<!-- 发布的imu话题中的frame_id -->
<param name="imu_frame" value="imu"/>
<!-- 地磁北的yaw角 -->
<param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
<!-- 欧拉角 -->
<param name="Euler_angles_pub_" value="/euler_angles"/>
<!-- 磁力计磁场强度 -->
<param name="Magnetic_pub_" value="/magnetic"/>
<!-- gps数据输出话题-->
<param name="gps_topic_" value="/gps/fix"/>
<!-- 机体系速度数据输出话题-->
<param name="twist_topic_" value="/system_speed"/>
<!-- NED系位移和速度数据输出话题-->
<param name="NED_odom_topic_" value="/NED_odometry"/>
<!-- 发布的数据基于不同设备有不同的坐标系-->
<param name="device_type" value="1"/> <!-- 0: origin_data, 1: for single imu or ucar in ROS, 2:for Xiao in ROS -->
</node>
</launch>
sensors/fdilink_ahrs/launch/tf.launch
+11
View File
@@ -0,0 +1,11 @@
<launch>
<node pkg="fdilink_ahrs" name="imu_tf" type="imu_tf" output="screen" >
<param name="imu_topic" value="/imu"/>
<param name="world_frame_id" value="/world"/>
<param name="imu_frame_id" value="/gyro_link"/>
<param name="position_x" value="1"/>
<param name="position_y" value="1"/>
<param name="position_z" value="0"/>
</node>
</launch>
sensors/fdilink_ahrs/package.xml
+54
View File
@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<package format="2">
<name>fdilink_ahrs</name>
<version>0.0.1</version>
<description>The fdilink_ahrs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="jwliang@iflytek.com">iflytek</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>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>genmsg</buildtool_depend>
<!-- <build_depend>geometry_msgs</build_depend> -->
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>tf</build_depend>
<build_depend>nav_msgs</build_depend>
<!-- <build_export_depend>geometry_msgs</build_export_depend> -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<build_export_depend>tf</build_export_depend>
<!-- <exec_depend>geometry_msgs</exec_depend> -->
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
<!-- <build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend> -->
</package>
sensors/fdilink_ahrs/src/ahrs_driver.cpp
+640
View File
@@ -0,0 +1,640 @@
#include <ahrs_driver.h>
#include <Eigen/Eigen>
namespace FDILink
{
ahrsBringup::ahrsBringup() :frist_sn_(false), serial_timeout_(20)
{
ros::NodeHandle pravite_nh("~");
//topic_name & frame_id 加载参数服务器
pravite_nh.param("debug", if_debug_, false);
pravite_nh.param("device_type", device_type_, 1); // default: single imu
pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
pravite_nh.param("gps_topic_", gps_topic_, std::string("/gps/fix"));
pravite_nh.param("twist_topic_", twist_topic_, std::string("/system_speed"));
pravite_nh.param("NED_odom_topic_", NED_odom_topic_, std::string("/NED_odometry"));
//serial
pravite_nh.param("port", serial_port_, std::string("/dev/ttyTHS1"));
pravite_nh.param("baud", serial_baud_, 115200);
//publisher 创建发布对象
imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic_.c_str(), 10);
mag_pose_pub_ = nh_.advertise<geometry_msgs::Pose2D>(mag_pose_2d_topic_.c_str(), 10);
Euler_angles_pub_ = nh_.advertise<geometry_msgs::Vector3>(Euler_angles_topic_.c_str(), 10);
Magnetic_pub_ = nh_.advertise<geometry_msgs::Vector3>(Magnetic_topic_.c_str(), 10);
gps_pub_ = nh_.advertise<sensor_msgs::NavSatFix>(gps_topic_.c_str(), 10);
twist_pub_ = nh_.advertise<geometry_msgs::Twist>(twist_topic_.c_str(), 10);
NED_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(NED_odom_topic_.c_str(), 10);
//setp up serial 设置串口参数并打开串口
try
{
serial_.setPort(serial_port_);
serial_.setBaudrate(serial_baud_);
serial_.setFlowcontrol(serial::flowcontrol_none);
serial_.setParity(serial::parity_none); //default is parity_none
serial_.setStopbits(serial::stopbits_one);
serial_.setBytesize(serial::eightbits);
serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
serial_.setTimeout(time_out);
serial_.open();
}
catch (serial::IOException &e) // 抓取异常
{
ROS_ERROR_STREAM("Unable to open port ");
exit(0);
}
if (serial_.isOpen())
{
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
ROS_ERROR_STREAM("Unable to initial Serial port ");
exit(0);
}
processLoop();
}
ahrsBringup::~ahrsBringup() // 析构函数关闭串口通道
{
if (serial_.isOpen())
serial_.close();
}
void ahrsBringup::processLoop() // 数据处理过程
{
ROS_INFO("ahrsBringup::processLoop: start");
while (ros::ok())
{
if (!serial_.isOpen())
{
ROS_WARN("serial unopen");
}
//check head start 检查起始 数据帧头
uint8_t check_head[1] = {0xff};
size_t head_s = serial_.read(check_head, 1);
if (if_debug_){
if (head_s != 1)
{
ROS_ERROR("Read serial port time out! can't read pack head.");
}
std::cout << std::endl;
std::cout << "check_head: " << std::hex << (int)check_head[0] << std::dec << std::endl;
}
if (check_head[0] != FRAME_HEAD)
{
continue;
}
// check head type 检查数据类型
uint8_t head_type[1] = {0xff};
size_t type_s = serial_.read(head_type, 1);
if (if_debug_){
std::cout << "head_type: " << std::hex << (int)head_type[0] << std::dec << std::endl;
}
if (head_type[0] != TYPE_IMU && head_type[0] != TYPE_AHRS && head_type[0] != TYPE_INSGPS && head_type[0] != TYPE_GEODETIC_POS && head_type[0] != 0x50 && head_type[0] != TYPE_GROUND&& head_type[0] != 0xff)
{
ROS_WARN("head_type error: %02X",head_type[0]);
continue;
}
//check head length 检查对应数据类型的长度是否符合
uint8_t check_len[1] = {0xff};
size_t len_s = serial_.read(check_len, 1);
if (if_debug_){
std::cout << "check_len: "<< std::dec << (int)check_len[0] << std::endl;
}
if (head_type[0] == TYPE_IMU && check_len[0] != IMU_LEN)
{
ROS_WARN("head_len error (imu)");
continue;
}else if (head_type[0] == TYPE_AHRS && check_len[0] != AHRS_LEN)
{
ROS_WARN("head_len error (ahrs)");
continue;
}else if (head_type[0] == TYPE_INSGPS && check_len[0] != INSGPS_LEN)
{
ROS_WARN("head_len error (insgps)");
continue;
}else if (head_type[0] == TYPE_GEODETIC_POS && check_len[0] != GEODETIC_POS_LEN)
{
ROS_WARN("head_len error (GEODETIC_POS)");
continue;
}
else if (head_type[0] == TYPE_GROUND || head_type[0] == 0x50) // 未知数据，防止记录失败
{
uint8_t ground_sn[1];
size_t ground_sn_s = serial_.read(ground_sn, 1);
if (++read_sn_ != ground_sn[0])
{
if ( ground_sn[0] < read_sn_)
{
if(if_debug_){
ROS_WARN("detected sn lost.");
}
sn_lost_ += 256 - (int)(read_sn_ - ground_sn[0]);
read_sn_ = ground_sn[0];
// continue;
}
else
{
if(if_debug_){
ROS_WARN("detected sn lost.");
}
sn_lost_ += (int)(ground_sn[0] - read_sn_);
read_sn_ = ground_sn[0];
// continue;
}
}
uint8_t ground_ignore[500];
size_t ground_ignore_s = serial_.read(ground_ignore, (check_len[0]+4));
continue;
}
//read head sn 检查sn 流水序号
uint8_t check_sn[1] = {0xff};
size_t sn_s = serial_.read(check_sn, 1);
uint8_t head_crc8[1] = {0xff};
size_t crc8_s = serial_.read(head_crc8, 1);
uint8_t head_crc16_H[1] = {0xff};
uint8_t head_crc16_L[1] = {0xff};
size_t crc16_H_s = serial_.read(head_crc16_H, 1);
size_t crc16_L_s = serial_.read(head_crc16_L, 1);
if (if_debug_){
std::cout << "check_sn: " << std::hex << (int)check_sn[0] << std::dec << std::endl;
std::cout << "head_crc8: " << std::hex << (int)head_crc8[0] << std::dec << std::endl;
std::cout << "head_crc16_H: " << std::hex << (int)head_crc16_H[0] << std::dec << std::endl;
std::cout << "head_crc16_L: " << std::hex << (int)head_crc16_L[0] << std::dec << std::endl;
}
// put header & check crc8 & count sn lost
// check crc8 进行crc8数据校验
if (head_type[0] == TYPE_IMU)
{
imu_frame_.frame.header.header_start = check_head[0];
imu_frame_.frame.header.data_type = head_type[0];
imu_frame_.frame.header.data_size = check_len[0];
imu_frame_.frame.header.serial_num = check_sn[0];
imu_frame_.frame.header.header_crc8 = head_crc8[0];
imu_frame_.frame.header.header_crc16_h = head_crc16_H[0];
imu_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(imu_frame_.read_buf.frame_header, 4);
if (CRC8 != imu_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = imu_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
//check sn
ahrsBringup::checkSN(TYPE_IMU);
}
else if (head_type[0] == TYPE_AHRS)
{
ahrs_frame_.frame.header.header_start = check_head[0];
ahrs_frame_.frame.header.data_type = head_type[0];
ahrs_frame_.frame.header.data_size = check_len[0];
ahrs_frame_.frame.header.serial_num = check_sn[0];
ahrs_frame_.frame.header.header_crc8 = head_crc8[0];
ahrs_frame_.frame.header.header_crc16_h = head_crc16_H[0];
ahrs_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(ahrs_frame_.read_buf.frame_header, 4);
if (CRC8 != ahrs_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = ahrs_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
//check sn
ahrsBringup::checkSN(TYPE_AHRS);
}
else if (head_type[0] == TYPE_INSGPS)
{
insgps_frame_.frame.header.header_start = check_head[0];
insgps_frame_.frame.header.data_type = head_type[0];
insgps_frame_.frame.header.data_size = check_len[0];
insgps_frame_.frame.header.serial_num = check_sn[0];
insgps_frame_.frame.header.header_crc8 = head_crc8[0];
insgps_frame_.frame.header.header_crc16_h = head_crc16_H[0];
insgps_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(insgps_frame_.read_buf.frame_header, 4);
if (CRC8 != insgps_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
else if(if_debug_)
{
std::cout << "header_crc8 matched." << std::endl;
}
ahrsBringup::checkSN(TYPE_INSGPS);
}
else if (head_type[0] == TYPE_GEODETIC_POS)
{
Geodetic_Position_frame_.frame.header.header_start = check_head[0];
Geodetic_Position_frame_.frame.header.data_type = head_type[0];
Geodetic_Position_frame_.frame.header.data_size = check_len[0];
Geodetic_Position_frame_.frame.header.serial_num = check_sn[0];
Geodetic_Position_frame_.frame.header.header_crc8 = head_crc8[0];
Geodetic_Position_frame_.frame.header.header_crc16_h = head_crc16_H[0];
Geodetic_Position_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(Geodetic_Position_frame_.read_buf.frame_header, 4);
if (CRC8 != Geodetic_Position_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
ahrsBringup::checkSN(TYPE_GEODETIC_POS);
}
// check crc16 进行crc16数据校验
if (head_type[0] == TYPE_IMU)
{
uint16_t head_crc16_l = imu_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = imu_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(imu_frame_.read_buf.read_msg, (IMU_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (IMU_LEN + 1); i++)
// {
// std::cout << std::hex << (int)imu_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(imu_frame_.frame.data.data_buff, IMU_LEN);
if (if_debug_)
{
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
bool if_right = ((int)head_crc16 == (int)CRC16);
std::cout << "if_right: " << if_right << std::endl;
}
if (head_crc16 != CRC16)
{
ROS_WARN("check crc16 faild(imu).");
continue;
}
else if(imu_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_AHRS)
{
uint16_t head_crc16_l = ahrs_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = ahrs_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(ahrs_frame_.read_buf.read_msg, (AHRS_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(ahrs_frame_.frame.data.data_buff, AHRS_LEN);
if (if_debug_){
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
bool if_right = ((int)head_crc16 == (int)CRC16);
std::cout << "if_right: " << if_right << std::endl;
}
if (head_crc16 != CRC16)
{
ROS_WARN("check crc16 faild(ahrs).");
continue;
}
else if(ahrs_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_INSGPS)
{
uint16_t head_crc16_l = insgps_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = insgps_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(insgps_frame_.read_buf.read_msg, (INSGPS_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(insgps_frame_.frame.data.data_buff, INSGPS_LEN);
if (if_debug_){
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
bool if_right = ((int)head_crc16 == (int)CRC16);
std::cout << "if_right: " << if_right << std::endl;
}
if (head_crc16 != CRC16)
{
ROS_WARN("check crc16 faild(ahrs).");
continue;
}
else if(insgps_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_GEODETIC_POS)
{
uint16_t head_crc16_l = Geodetic_Position_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = Geodetic_Position_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(Geodetic_Position_frame_.read_buf.read_msg, (GEODETIC_POS_LEN + 1)); //24+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(Geodetic_Position_frame_.frame.data.data_buff, GEODETIC_POS_LEN);
if (if_debug_){
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
bool if_right = ((int)head_crc16 == (int)CRC16);
std::cout << "if_right: " << if_right << std::endl;
}
if (head_crc16 != CRC16)
{
ROS_WARN("check crc16 faild(gps).");
continue;
}
else if(Geodetic_Position_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
//读取IMU数据进行解析，并发布相关话题
if (head_type[0] == TYPE_IMU)
{
// publish imu topic
sensor_msgs::Imu imu_data;
imu_data.header.stamp = ros::Time::now();
imu_data.header.frame_id = imu_frame_id_.c_str();
Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
ahrs_frame_.frame.data.data_pack.Qx,
ahrs_frame_.frame.data.data_pack.Qy,
ahrs_frame_.frame.data.data_pack.Qz);
Eigen::Quaterniond q_r =
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_rr =
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_xiao_rr =
Eigen::AngleAxisd( PI/2, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
if (device_type_ == 0) //未经变换的原始数据
{
imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qx;
imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qy;
imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;
imu_data.angular_velocity.x = ahrs_frame_.frame.data.data_pack.RollSpeed;
imu_data.angular_velocity.y = ahrs_frame_.frame.data.data_pack.PitchSpeed;
imu_data.angular_velocity.z = ahrs_frame_.frame.data.data_pack.HeadingSpeed;
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_y;
imu_data.linear_acceleration.z = imu_frame_.frame.data.data_pack.accelerometer_z;
}
else if (device_type_ == 1) //imu单品ROS标准下的坐标变换
{
Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr;
imu_data.orientation.w = q_out.w();
imu_data.orientation.x = q_out.x();
imu_data.orientation.y = q_out.y();
imu_data.orientation.z = q_out.z();
imu_data.angular_velocity.x = ahrs_frame_.frame.data.data_pack.RollSpeed;
imu_data.angular_velocity.y = -ahrs_frame_.frame.data.data_pack.PitchSpeed;
imu_data.angular_velocity.z = -ahrs_frame_.frame.data.data_pack.HeadingSpeed;
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
imu_data.linear_acceleration.y = -imu_frame_.frame.data.data_pack.accelerometer_y;
imu_data.linear_acceleration.z = -imu_frame_.frame.data.data_pack.accelerometer_z;
}
imu_pub_.publish(imu_data);
}
//读取AHRS数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_AHRS)
{
geometry_msgs::Pose2D pose_2d;
pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
mag_pose_pub_.publish(pose_2d);
//std::cout << "YAW: " << pose_2d.theta << std::endl;
geometry_msgs::Vector3 Euler_angles_2d,Magnetic;
Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
Euler_angles_pub_.publish(Euler_angles_2d);
Magnetic_pub_.publish(Magnetic);
}
//读取gps_pos数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_GEODETIC_POS)
{
sensor_msgs::NavSatFix gps_data;
gps_data.header.stamp = ros::Time::now();
gps_data.header.frame_id = "navsat_link";
gps_data.latitude = Geodetic_Position_frame_.frame.data.data_pack.Latitude / DEG_TO_RAD;
gps_data.longitude = Geodetic_Position_frame_.frame.data.data_pack.Longitude / DEG_TO_RAD;
gps_data.altitude = Geodetic_Position_frame_.frame.data.data_pack.Height;
//std::cout << "lat: " << Geodetic_Position_frame_.frame.data.data_pack.Latitude << std::endl;
//std::cout << "lon: " << Geodetic_Position_frame_.frame.data.data_pack.Longitude << std::endl;
//std::cout << "h: " << Geodetic_Position_frame_.frame.data.data_pack.Height << std::endl;
gps_pub_.publish(gps_data);
}
//读取INSGPS数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_INSGPS)
{
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = ros::Time::now();
// odom_msg.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
odom_msg.pose.pose.position.x = insgps_frame_.frame.data.data_pack.Location_North; //Position //位置
odom_msg.pose.pose.position.y = insgps_frame_.frame.data.data_pack.Location_East;
odom_msg.pose.pose.position.z = insgps_frame_.frame.data.data_pack.Location_Down;
// odom_msg.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
odom_msg.twist.twist.linear.x = insgps_frame_.frame.data.data_pack.Velocity_North; //Speed in the X direction //X方向速度
odom_msg.twist.twist.linear.y = insgps_frame_.frame.data.data_pack.Velocity_East; //Speed in the Y direction //Y方向速度
odom_msg.twist.twist.linear.z = insgps_frame_.frame.data.data_pack.Velocity_Down;
NED_odom_pub_.publish(odom_msg);
geometry_msgs::Twist speed_msg;
speed_msg.linear.x = insgps_frame_.frame.data.data_pack.BodyVelocity_X;
speed_msg.linear.y = insgps_frame_.frame.data.data_pack.BodyVelocity_Y;
speed_msg.linear.z = insgps_frame_.frame.data.data_pack.BodyVelocity_Z;
twist_pub_.publish(speed_msg);
// std::cout << "N: " << insgps_frame_.frame.data.data_pack.Location_North << std::endl;
// std::cout << "E: " << insgps_frame_.frame.data.data_pack.Location_East << std::endl;
// std::cout << "D: " << insgps_frame_.frame.data.data_pack.Location_Down << std::endl;
}
}
}
void ahrsBringup::magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz)
{
double temp1 = magy * cos(roll) + magz * sin(roll);
double temp2 = magx * cos(pitch) + magy * sin(pitch) * sin(roll) - magz * sin(pitch) * cos(roll);
magyaw = atan2(-temp1, temp2);
if(magyaw < 0)
{
magyaw = magyaw + 2 * PI;
}
// return magyaw;
}
void ahrsBringup::checkSN(int type)
{
switch (type)
{
case TYPE_IMU:
if (++read_sn_ != imu_frame_.frame.header.serial_num)
{
if ( imu_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - imu_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(imu_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = imu_frame_.frame.header.serial_num;
break;
case TYPE_AHRS:
if (++read_sn_ != ahrs_frame_.frame.header.serial_num)
{
if ( ahrs_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - ahrs_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(ahrs_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = ahrs_frame_.frame.header.serial_num;
break;
case TYPE_INSGPS:
if (++read_sn_ != insgps_frame_.frame.header.serial_num)
{
if ( insgps_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - insgps_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(insgps_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = insgps_frame_.frame.header.serial_num;
break;
case TYPE_GEODETIC_POS:
if (++read_sn_ != Geodetic_Position_frame_.frame.header.serial_num)
{
if ( Geodetic_Position_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - Geodetic_Position_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(Geodetic_Position_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num;
break;
default:
break;
}
}
} //namespace FDILink
int main(int argc, char **argv)
{
ros::init(argc, argv, "ahrs_bringup");
FDILink::ahrsBringup bp;
return 0;
}
sensors/fdilink_ahrs/src/crc_table.cpp
+159
View File
@@ -0,0 +1,159 @@
#include <stdint.h>
#include <crc_table.h>
static const uint8_t CRC8Table[] = {
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53};
static const uint16_t CRC16Table[256] =
{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
static const uint32_t CRC32Table[] = {
0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL,
0x076dc419L, 0x706af48fL, 0xe963a535L, 0x9e6495a3L,
0x0edb8832L, 0x79dcb8a4L, 0xe0d5e91eL, 0x97d2d988L,
0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L, 0x90bf1d91L,
0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L,
0x136c9856L, 0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL,
0x14015c4fL, 0x63066cd9L, 0xfa0f3d63L, 0x8d080df5L,
0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L, 0xa2677172L,
0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L,
0x32d86ce3L, 0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L,
0x26d930acL, 0x51de003aL, 0xc8d75180L, 0xbfd06116L,
0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L, 0xb8bda50fL,
0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL,
0x76dc4190L, 0x01db7106L, 0x98d220bcL, 0xefd5102aL,
0x71b18589L, 0x06b6b51fL, 0x9fbfe4a5L, 0xe8b8d433L,
0x7807c9a2L, 0x0f00f934L, 0x9609a88eL, 0xe10e9818L,
0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL,
0x6c0695edL, 0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L,
0x65b0d9c6L, 0x12b7e950L, 0x8bbeb8eaL, 0xfcb9887cL,
0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L, 0xfbd44c65L,
0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL,
0x4369e96aL, 0x346ed9fcL, 0xad678846L, 0xda60b8d0L,
0x44042d73L, 0x33031de5L, 0xaa0a4c5fL, 0xdd0d7cc9L,
0x5005713cL, 0x270241aaL, 0xbe0b1010L, 0xc90c2086L,
0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L,
0x59b33d17L, 0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL,
0xedb88320L, 0x9abfb3b6L, 0x03b6e20cL, 0x74b1d29aL,
0xead54739L, 0x9dd277afL, 0x04db2615L, 0x73dc1683L,
0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L,
0xf00f9344L, 0x8708a3d2L, 0x1e01f268L, 0x6906c2feL,
0xf762575dL, 0x806567cbL, 0x196c3671L, 0x6e6b06e7L,
0xfed41b76L, 0x89d32be0L, 0x10da7a5aL, 0x67dd4accL,
0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L,
0xd1bb67f1L, 0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL,
0xd80d2bdaL, 0xaf0a1b4cL, 0x36034af6L, 0x41047a60L,
0xdf60efc3L, 0xa867df55L, 0x316e8eefL, 0x4669be79L,
0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL,
0xc5ba3bbeL, 0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L,
0xc2d7ffa7L, 0xb5d0cf31L, 0x2cd99e8bL, 0x5bdeae1dL,
0x9b64c2b0L, 0xec63f226L, 0x756aa39cL, 0x026d930aL,
0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L,
0x92d28e9bL, 0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L,
0x86d3d2d4L, 0xf1d4e242L, 0x68ddb3f8L, 0x1fda836eL,
0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L, 0x18b74777L,
0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L,
0xa00ae278L, 0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L,
0xa7672661L, 0xd06016f7L, 0x4969474dL, 0x3e6e77dbL,
0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L, 0x37d83bf0L,
0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L,
0xbad03605L, 0xcdd70693L, 0x54de5729L, 0x23d967bfL,
0xb3667a2eL, 0xc4614ab8L, 0x5d681b02L, 0x2a6f2b94L,
0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL, 0x2d02ef8dL
};
uint8_t CRC8_Table(uint8_t* p, uint8_t counter)
{
uint8_t crc8 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
uint8_t new_index = crc8 ^ value;
crc8 = CRC8Table[new_index];
}
return (crc8);
}
uint16_t CRC16_Table(uint8_t *p, uint8_t counter)
{
uint16_t crc16 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
crc16 = CRC16Table[((crc16 >> 8) ^ value) & 0xff] ^ (crc16 << 8);
}
return (crc16);
}
uint32_t CRC32_Table(uint8_t *p, uint8_t counter)
{
uint16_t crc32 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
crc32 = CRC16Table[((crc32 >> 8) ^ value) & 0xff] ^ (crc32 << 8);
}
return (crc32);
}
sensors/fdilink_ahrs/src/imu_tf.cpp
+53
View File
@@ -0,0 +1,53 @@
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <string>
/* 参考ROS wiki
* http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
* */
int position_x ;
int position_y ;
int position_z ;
std::string imu_frame_id, world_frame_id;
void ImuCallback(const sensor_msgs::ImuConstPtr& imu_data) {
static tf::TransformBroadcaster br;//广播器
tf::Transform transform;
transform.setOrigin(tf::Vector3(position_x, position_y, position_z));//设置平移部分
//从IMU消息包中获取四元数数据
tf::Quaternion q;
q.setX(imu_data->orientation.x);
q.setY(imu_data->orientation.y);
q.setZ(imu_data->orientation.z);
q.setW(imu_data->orientation.w);
q.normalized();//归一化
transform.setRotation(q);//设置旋转部分
//广播出去
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), world_frame_id, imu_frame_id));
}
int main (int argc, char ** argv) {
ros::init(argc, argv, "imu_data_to_tf");
ros::NodeHandle node;
std::string imu_topic;
node.param("/imu_tf/imu_topic", imu_topic, std::string("/imu"));
node.param("/imu_tf/position_x", position_x, 0);
node.param("/imu_tf/position_y", position_y, 0);
node.param("/imu_tf/position_z", position_z, 0);
node.param("/imu_tf/world_frame_id", world_frame_id, std::string("/world"));
node.param("/imu_tf/imu_frame_id", imu_frame_id, std::string("/imu"));
ros::Subscriber sub = node.subscribe(imu_topic.c_str(), 10, &ImuCallback);
ros::spin();
return 0;
}
sensors/hunter_se_description/CMakeLists.txt
+14
View File
@@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(hunter_se_description)
find_package(catkin REQUIRED)
catkin_package()
find_package(roslaunch)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)
sensors/hunter_se_description/config/joint_names_hunter_se_description_2.yaml
+1
View File
@@ -0,0 +1 @@
controller_joint_names: ['', 'fr_left_joint', 'fr_right_joint', 're_left_jiont', 're_right_jiont', ]
sensors/hunter_se_description/launch/display.launch
+9
View File
@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<launch>
<arg name="model" default="$(find hunter_se_description)/urdf/hunter_se_description.urdf"/>
<arg name="gui" default="true"/>
<param name="robot_description" textfile="$(arg model)"/>
<para name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>
sensors/hunter_se_description/launch/gazebo.launch
+20
View File
@@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- <node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" /> -->
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find hunter_se_description)/urdf/hunter_se_description.urdf -urdf -model hunter_se_description"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>
sensors/hunter_se_description/meshes/base_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/fr_left_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/fr_right_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/fr_steer_left_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/fr_steer_right_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/re_left_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/meshes/re_right_link.STL
BIN
View File
Binary file not shown.
sensors/hunter_se_description/package.xml
+21
View File
@@ -0,0 +1,21 @@
<package format="2">
<name>hunter_se_description</name>
<version>1.0.0</version>
<description>
<p>URDF Description package for hunter_se_description</p>
<p>This package contains configuration data, 3D models and launch files
for hunter_se_description robot</p>
</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
<export>
<architecture_independent />
</export>
</package>
sensors/hunter_se_description/rviz.rviz
+210
View File
@@ -0,0 +1,210 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 549
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_steer_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_steer_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
re_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
re_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
fr_left_link:
Value: true
fr_right_link:
Value: true
fr_steer_left_link:
Value: true
fr_steer_right_link:
Value: true
re_left_link:
Value: true
re_right_link:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
base_link:
fr_steer_left_link:
fr_left_link:
{}
fr_steer_right_link:
fr_right_link:
{}
re_left_link:
{}
re_right_link:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.7850098609924316
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5403982400894165
Target Frame: <Fixed Frame>
Yaw: 5.213582515716553
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1267
X: 211
Y: 27
sensors/hunter_se_description/urdf/hunter_se_description.csv
+8
View File
@@ -0,0 +1,8 @@
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
base_link,0.0374140021041951,-0.000373005187591258,-0.0771282894414029,0,0,0,14.7373774918344,0.123215829336501,-2.51201031615063E-06,0.00480100638469038,0.219144715482931,-1.0329581476775E-05,0.313606030126045,0,0,0,0,0,0,package://hunter_se_description_2/meshes/base_link.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/base_link.STL,,M-HUTM-010014-A-1;M-HUTM-010001-A-2;电池外盖_20211022_B(3)-1;LP20-4PSX座子_3_V21_V22_V3_1-2;电池盖插销-2;内装式电量显示-1;LANBOO蓝波钥匙开关-开孔19-1;航插充电头-1;电池盖插销-1;平行四边形减震模组-1/减震连接小件-4;尾部侧板-1;平行四边形减震模组-1/减震竖直板-1;平行四边形减震模组-1/减震连接小件-2;平行四边形减震模组-1/减震连接小件-6;平行四边形减震模组-1/减速器车架连接件-1;平行四边形减震模组-1/减震连接小件-3;镜向尾部侧板-2;平行四边形减震模组-2/减速器车架连接件-1;平行四边形减震模组-2/减震连接小件-3;平行四边形减震模组-2/-孔距100初始150磅电动车弹簧-1/避震杆01-1;平行四边形减震模组-2/-孔距100初始150磅电动车弹簧-1/避震杆02-1;平行四边形减震模组-2/减震连接小件-6;平行四边形减震模组-2/减震竖直板-1;平行四边形减震模组-2/减震连接小件-2;平行四边形减震模组-2/减震连接小件-4;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震杆02-1;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震杆01-1;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震弹簧-1;M-HUTM-010011-A-1;M-HUTM-010012-A-1;M-HUTM-010008-A-1;前桥模组2-2/M-HUTM-010204-A-1;前桥模组2-2/M-HUTM-010206-A-1;灯带-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-2/RTLBL10R_2-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-2/RTLBL10R_3-1;前桥模组2-2/AKM-001-00-1/球头1-2;前桥模组2-2/AKM-001-00-1/RTLBL10R-1/RTLBL10R_2-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-1/RTLBL10R_3-1;前桥模组2-2/M-HUTM-010201-B-2;前桥模组2-2/塞打螺钉16_ghx_11_1_3_V20_V21_V22_V3_1-2;前桥模组2-2/M-HUTM-010203-A-2;前桥模组2-2/M-HUTM-010203-A-4;前桥模组2-2/M-HUTM-010201-B-4;M-HUTM-010013-A-1,坐标系base,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
fr_steer_left_link,-0.0002776492198312,0.0163539773588368,4.97346169803237E-09,0,0,0,3.14923108719031,0.0176012120277572,-0.000255822069720042,4.89135197289253E-08,0.0326650592194903,-5.05595058544686E-10,0.0175968940276803,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_steer_left_link.STL,0.223529411764706,0.223529411764706,0.223529411764706,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_steer_left_link.STL,,前车轮-1;前车轮外壳2-2,坐标系5,,fr_steer_left_jiont,fixed,0.34058,0.24619,-0.1535,0,0,0,base_link,0,0,0,,,,,,,,,,,,
fr_left_link,-3.5837499634539E-10,4.97346158701006E-09,-0.016356334080185,0,0,0,3.14923108719031,0.0175968687768806,4.88978895069836E-08,2.59363668202492E-09,0.0175968940276803,1.33582951478864E-09,0.0326694024703669,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_left_link.STL,0.223529411764706,0.223529411764706,0.223529411764706,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_left_link.STL,,前车轮-1;前车轮外壳2-2,坐标系1,基准轴1,fr_left_joint,continuous,0,0,0,1.5708,0,0.016976,fr_steer_left_link,0,0,-1,,,,,,,,,,,,
fr_steer_right_link,0.000274981985673328,-0.0163540222836661,-3.24802407192237E-11,0,0,0,3.14923106523906,0.0176011208398464,-0.000253357559513646,8.00593138443467E-09,0.0326650290732704,5.65898650063874E-11,0.0175967847225325,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_steer_right_link.STL,1,1,1,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_steer_right_link.STL,,前车轮外壳2-1;前车轮-2,坐标系6,,fr_steer_right_link,fixed,0.34219,-0.24619,-0.1535,0,0,0,base_link,0,0,0,,,,,,,,,,,,
fr_right_link,0.00027498198567355,3.24801296969213E-11,-0.0163540222836661,0,0,0,3.14923106523906,0.0176011208398464,-8.00593138518773E-09,-0.000253357559513648,0.0175967847225325,-5.65898177386586E-11,0.0326650290732703,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_right_link.STL,0.223529411764706,0.223529411764706,0.223529411764706,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/fr_right_link.STL,,前车轮-2;前车轮外壳2-1,坐标系2,基准轴2,fr_right_joint,continuous,0,0,0,-1.5708,0,0,fr_steer_right_link,0.016812,0,-0.99986,,,,,,,,,,,,
re_left_link,1.245E-09,1.7252E-06,-0.010284,0,0,0,3.7665,0.018208,-1.1815E-10,-4.2619E-10,0.018208,-5.0277E-07,0.03306,0,0,0,0,0,0,package://hunter_se_description_2/meshes/re_left_link.STL,0.37647,0.37647,0.37647,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/re_left_link.STL,,后车轮-1;后车轮外壳-1,坐标系3,基准轴3,re_left_jiont,continuous,-0.2078,0.252,-0.158,1.5708,0,0,base_link,0,0,-1,,,,,,,,,,,,
re_right_link,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,package://hunter_se_description_2/meshes/re_right_link.STL,1,1,1,1,0,0,0,0,0,0,package://hunter_se_description_2/meshes/re_right_link.STL,,后车轮-2;后车轮外壳-2,坐标系4,基准轴4,re_right_jiont,continuous,-0.2078,-0.252,-0.158,-1.5708,0,0,base_link,0,0,-1,,,,,,,,,,,,
1 Link Name Center of Mass X Center of Mass Y Center of Mass Z Center of Mass Roll Center of Mass Pitch Center of Mass Yaw Mass Moment Ixx Moment Ixy Moment Ixz Moment Iyy Moment Iyz Moment Izz Visual X Visual Y Visual Z Visual Roll Visual Pitch Visual Yaw Mesh Filename Color Red Color Green Color Blue Color Alpha Collision X Collision Y Collision Z Collision Roll Collision Pitch Collision Yaw Collision Mesh Filename Material Name SW Components Coordinate System Axis Name Joint Name Joint Type Joint Origin X Joint Origin Y Joint Origin Z Joint Origin Roll Joint Origin Pitch Joint Origin Yaw Parent Joint Axis X Joint Axis Y Joint Axis Z Limit Effort Limit Velocity Limit Lower Limit Upper Calibration rising Calibration falling Dynamics Damping Dynamics Friction Safety Soft Upper Safety Soft Lower Safety K Position Safety K Velocity
2 base_link 0.0374140021041951 -0.000373005187591258 -0.0771282894414029 0 0 0 14.7373774918344 0.123215829336501 -2.51201031615063E-06 0.00480100638469038 0.219144715482931 -1.0329581476775E-05 0.313606030126045 0 0 0 0 0 0 package://hunter_se_description_2/meshes/base_link.STL 0.752941176470588 0.752941176470588 0.752941176470588 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/base_link.STL M-HUTM-010014-A-1;M-HUTM-010001-A-2;电池外盖_20211022_B(3)-1;LP20-4PSX座子_3_V21_V22_V3_1-2;电池盖插销-2;内装式电量显示-1;LANBOO蓝波钥匙开关-开孔19-1;航插充电头-1;电池盖插销-1;平行四边形减震模组-1/减震连接小件-4;尾部侧板-1;平行四边形减震模组-1/减震竖直板-1;平行四边形减震模组-1/减震连接小件-2;平行四边形减震模组-1/减震连接小件-6;平行四边形减震模组-1/减速器车架连接件-1;平行四边形减震模组-1/减震连接小件-3;镜向尾部侧板-2;平行四边形减震模组-2/减速器车架连接件-1;平行四边形减震模组-2/减震连接小件-3;平行四边形减震模组-2/-孔距100初始150磅电动车弹簧-1/避震杆01-1;平行四边形减震模组-2/-孔距100初始150磅电动车弹簧-1/避震杆02-1;平行四边形减震模组-2/减震连接小件-6;平行四边形减震模组-2/减震竖直板-1;平行四边形减震模组-2/减震连接小件-2;平行四边形减震模组-2/减震连接小件-4;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震杆02-1;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震杆01-1;平行四边形减震模组-1/-孔距100初始150磅电动车弹簧-1/避震弹簧-1;M-HUTM-010011-A-1;M-HUTM-010012-A-1;M-HUTM-010008-A-1;前桥模组2-2/M-HUTM-010204-A-1;前桥模组2-2/M-HUTM-010206-A-1;灯带-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-2/RTLBL10R_2-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-2/RTLBL10R_3-1;前桥模组2-2/AKM-001-00-1/球头1-2;前桥模组2-2/AKM-001-00-1/RTLBL10R-1/RTLBL10R_2-1;前桥模组2-2/AKM-001-00-1/RTLBL10R-1/RTLBL10R_3-1;前桥模组2-2/M-HUTM-010201-B-2;前桥模组2-2/塞打螺钉16_ghx_11_1_3_V20_V21_V22_V3_1-2;前桥模组2-2/M-HUTM-010203-A-2;前桥模组2-2/M-HUTM-010203-A-4;前桥模组2-2/M-HUTM-010201-B-4;M-HUTM-010013-A-1 坐标系base 0 0 0 0 0 0 0 0 0
3 fr_steer_left_link -0.0002776492198312 0.0163539773588368 4.97346169803237E-09 0 0 0 3.14923108719031 0.0176012120277572 -0.000255822069720042 4.89135197289253E-08 0.0326650592194903 -5.05595058544686E-10 0.0175968940276803 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_steer_left_link.STL 0.223529411764706 0.223529411764706 0.223529411764706 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_steer_left_link.STL 前车轮-1;前车轮外壳2-2 坐标系5 fr_steer_left_jiont fixed 0.34058 0.24619 -0.1535 0 0 0 base_link 0 0 0
4 fr_left_link -3.5837499634539E-10 4.97346158701006E-09 -0.016356334080185 0 0 0 3.14923108719031 0.0175968687768806 4.88978895069836E-08 2.59363668202492E-09 0.0175968940276803 1.33582951478864E-09 0.0326694024703669 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_left_link.STL 0.223529411764706 0.223529411764706 0.223529411764706 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_left_link.STL 前车轮-1;前车轮外壳2-2 坐标系1 基准轴1 fr_left_joint continuous 0 0 0 1.5708 0 0.016976 fr_steer_left_link 0 0 -1
5 fr_steer_right_link 0.000274981985673328 -0.0163540222836661 -3.24802407192237E-11 0 0 0 3.14923106523906 0.0176011208398464 -0.000253357559513646 8.00593138443467E-09 0.0326650290732704 5.65898650063874E-11 0.0175967847225325 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_steer_right_link.STL 1 1 1 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_steer_right_link.STL 前车轮外壳2-1;前车轮-2 坐标系6 fr_steer_right_link fixed 0.34219 -0.24619 -0.1535 0 0 0 base_link 0 0 0
6 fr_right_link 0.00027498198567355 3.24801296969213E-11 -0.0163540222836661 0 0 0 3.14923106523906 0.0176011208398464 -8.00593138518773E-09 -0.000253357559513648 0.0175967847225325 -5.65898177386586E-11 0.0326650290732703 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_right_link.STL 0.223529411764706 0.223529411764706 0.223529411764706 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/fr_right_link.STL 前车轮-2;前车轮外壳2-1 坐标系2 基准轴2 fr_right_joint continuous 0 0 0 -1.5708 0 0 fr_steer_right_link 0.016812 0 -0.99986
7 re_left_link 1.245E-09 1.7252E-06 -0.010284 0 0 0 3.7665 0.018208 -1.1815E-10 -4.2619E-10 0.018208 -5.0277E-07 0.03306 0 0 0 0 0 0 package://hunter_se_description_2/meshes/re_left_link.STL 0.37647 0.37647 0.37647 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/re_left_link.STL 后车轮-1;后车轮外壳-1 坐标系3 基准轴3 re_left_jiont continuous -0.2078 0.252 -0.158 1.5708 0 0 base_link 0 0 -1
8 re_right_link 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 package://hunter_se_description_2/meshes/re_right_link.STL 1 1 1 1 0 0 0 0 0 0 package://hunter_se_description_2/meshes/re_right_link.STL 后车轮-2;后车轮外壳-2 坐标系4 基准轴4 re_right_jiont continuous -0.2078 -0.252 -0.158 -1.5708 0 0 base_link 0 0 -1
sensors/hunter_se_description/urdf/hunter_se_description.urdf
+537
View File
@@ -0,0 +1,537 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="hunter_se_description">
<link
name="base_link">
<inertial>
<origin
xyz="0.0374140021041951 -0.000373005187591258 -0.0771282894414029"
rpy="0 0 0" />
<mass
value="24.73" />
<inertia
ixx="0.123215829336501"
ixy="-2.51201031615063E-06"
ixz="0.00480100638469038"
iyy="0.219144715482931"
iyz="-1.0329581476775E-05"
izz="0.313606030126045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="fr_steer_left_link">
<inertial>
<origin
xyz="-0.0002776492198312 0.0163539773588368 4.97346169803237E-09"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint
name="fr_steer_left_joint"
type="revolute">
<origin
xyz="0.34058 0.24619 -0.1535"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="fr_steer_left_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="fr_left_link">
<inertial>
<origin
xyz="-3.5837499634539E-10 4.97346158701006E-09 -0.016356334080185"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/fr_left_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/fr_left_link.STL" />
</geometry>
</collision>
</link>
<joint
name="fr_left_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="1.5708 0 0.016976" />
<parent
link="fr_steer_left_link" />
<child
link="fr_left_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="fr_steer_right_link">
<inertial>
<origin
xyz="0.000274981985673328 -0.0163540222836661 -3.24802407192237E-11"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint
name="fr_steer_right_joint"
type="revolute">
<origin
xyz="0.34219 -0.24619 -0.1535"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="fr_steer_right_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="fr_right_link">
<inertial>
<origin
xyz="0.00027498198567355 3.24801296969213E-11 -0.0163540222836661"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/fr_right_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/fr_right_link.STL" />
</geometry>
</collision>
</link>
<joint
name="fr_right_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="fr_steer_right_link" />
<child
link="fr_right_link" />
<axis
xyz="0 0 1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="re_left_link">
<inertial>
<origin
xyz="1.245E-09 1.7252E-06 -0.010284"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/re_left_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.37647 0.37647 0.37647 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/re_left_link.STL" />
</geometry>
</collision>
</link>
<joint
name="re_left_joint"
type="continuous">
<origin
xyz="-0.2078 0.252 -0.158"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="re_left_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="re_right_link">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="3.149" />
<inertia
ixx="0.0222"
ixy="0.0"
ixz="0.0"
iyy="0.0222"
iyz="0.0"
izz="0.0378075" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/re_right_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description/meshes/re_right_link.STL" />
</geometry>
</collision>
</link>
<joint
name="re_right_joint"
type="continuous">
<origin
xyz="-0.2078 -0.252 -0.158"
rpy="-1.5708 0 0" />
<parent
link="base_link" />
<child
link="re_right_link" />
<axis
xyz="0 0 1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="front_steer_link">
<inertial>
<origin
xyz="0.049836 8.1046E-15 0.017912"
rpy="0 0 0" />
<mass
value="0.0049179" />
<inertia
ixx="1.6846E-07"
ixy="7.6396E-23"
ixz="-1.0974E-08"
iyy="7.549E-07"
iyz="-1.8957E-22"
izz="7.334E-07" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="front_steer_joint"
type="revolute">
<origin
xyz="0.40792 -1.1431E-05 -0.085581"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="front_steer_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.72"
upper="0.72"
effort="0"
velocity="0" />
</joint>
<link
name="rear_wheel_link">
<inertial>
<origin
xyz="-0.02 0.0031269 -0.12735"
rpy="0 0 0" />
<mass
value="0.0049179" />
<inertia
ixx="1.1821E-06"
ixy="9.6965E-13"
ixz="-3.1553E-12"
iyy="9.5276E-07"
iyz="1.2428E-08"
izz="1.0913E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="rear_wheel_joint"
type="continuous">
<origin
xyz="-0.30408 -1.1431E-05 -0.085581"
rpy="0.0032313 0 0" />
<parent
link="base_link" />
<child
link="rear_wheel_link" />
<axis
xyz="0 0 1" />
</joint>
<gazebo reference="fr_left_link">
<mu1>0.8</mu1>
<mu2>0.8</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="fr_right_link">
<mu1>0.8</mu1>
<mu2>0.8</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="re_left_link">
<mu1>0.8</mu1>
<mu2>10</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="re_right_link">
<mu1>0.8</mu1>
<mu2>10</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType>
<legacyModeNS>false</legacyModeNS>
</plugin>
</gazebo>
</robot>
sensors/hunter_se_description/urdf/hunter_se_description.xacro
+372
View File
@@ -0,0 +1,372 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="hunter_se_description_2">
<link
name="base_link">
<inertial>
<origin
xyz="0.0374140021041951 -0.000373005187591258 -0.0771282894414029"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.123215829336501"
ixy="-2.51201031615063E-06"
ixz="0.00480100638469038"
iyy="0.219144715482931"
iyz="-1.0329581476775E-05"
izz="0.313606030126045" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="fr_steer_left_link">
<inertial>
<origin
xyz="-0.0002776492198312 0.0163539773588368 4.97346169803237E-09"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.0176012120277572"
ixy="-0.000255822069720042"
ixz="4.89135197289253E-08"
iyy="0.0326650592194903"
iyz="-5.05595058544686E-10"
izz="0.0175968940276803" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint
name="fr_steer_left_jiont"
type="revolute">
<origin
xyz="0.34058 0.24619 -0.1535"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="fr_steer_left_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="fr_left_link">
<inertial>
<origin
xyz="-3.5837499634539E-10 4.97346158701006E-09 -0.016356334080185"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.0175968687768806"
ixy="4.88978895069836E-08"
ixz="2.59363668202492E-09"
iyy="0.0175968940276803"
iyz="1.33582951478864E-09"
izz="0.0326694024703669" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/fr_left_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/fr_left_link.STL" />
</geometry>
</collision>
</link>
<joint
name="fr_left_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="1.5708 0 0.016976" />
<parent
link="fr_steer_left_link" />
<child
link="fr_left_link" />
<axis
xyz="0 0 -1" />
</joint>
<link
name="fr_steer_right_link">
<inertial>
<origin
xyz="0.000274981985673328 -0.0163540222836661 -3.24802407192237E-11"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.0176011208398464"
ixy="-0.000253357559513646"
ixz="8.00593138443467E-09"
iyy="0.0326650290732704"
iyz="5.65898650063874E-11"
izz="0.0175967847225325" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint
name="fr_steer_right_link"
type="revolute">
<origin
xyz="0.34219 -0.24619 -0.1535"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="fr_steer_right_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="fr_right_link">
<inertial>
<origin
xyz="0.00027498198567355 3.24801296969213E-11 -0.0163540222836661"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.0176011208398464"
ixy="-8.00593138518773E-09"
ixz="-0.000253357559513648"
iyy="0.0175967847225325"
iyz="-5.65898177386586E-11"
izz="0.0326650290732703" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/fr_right_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/fr_right_link.STL" />
</geometry>
</collision>
</link>
<joint
name="fr_right_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="fr_steer_right_link" />
<child
link="fr_right_link" />
<axis
xyz="0 0 1" />
</joint>
<link
name="re_left_link">
<inertial>
<origin
xyz="1.245E-09 1.7252E-06 -0.010284"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.018208"
ixy="-1.1815E-10"
ixz="-4.2619E-10"
iyy="0.018208"
iyz="-5.0277E-07"
izz="0.03306" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/re_left_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.37647 0.37647 0.37647 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/re_left_link.STL" />
</geometry>
</collision>
</link>
<joint
name="re_left_jiont"
type="continuous">
<origin
xyz="-0.2078 0.252 -0.158"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="re_left_link" />
<axis
xyz="0 0 -1" />
</joint>
<link
name="re_right_link">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="0.01" />
<inertia
ixx="0.0176011208398464"
ixy="-8.00593138518773E-09"
ixz="-0.000253357559513648"
iyy="0.0175967847225325"
iyz="-5.65898177386586E-11"
izz="0.0326650290732703" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/re_right_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter_se_description_2/meshes/re_right_link.STL" />
</geometry>
</collision>
</link>
<joint
name="re_right_jiont"
type="continuous">
<origin
xyz="-0.2078 -0.252 -0.158"
rpy="-1.5708 0 0" />
<parent
link="base_link" />
<child
link="re_right_link" />
<axis
xyz="0 0 1" />
</joint>
</robot>
sensors/livox_ros_driver2/3rdparty/rapidjson/allocators.h
Vendored
+308
View File
@@ -0,0 +1,308 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ALLOCATORS_H_
#define RAPIDJSON_ALLOCATORS_H_
#include "rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Allocator
/*! \class rapidjson::Allocator
\brief Concept for allocating, resizing and freeing memory block.
Note that Malloc() and Realloc() are non-static but Free() is static.
So if an allocator need to support Free(), it needs to put its pointer in
the header of memory block.
\code
concept Allocator {
static const bool kNeedFree; //!< Whether this allocator needs to call
Free().
// Allocate a memory block.
// \param size of the memory block in bytes.
// \returns pointer to the memory block.
void* Malloc(size_t size);
// Resize a memory block.
// \param originalPtr The pointer to current memory block. Null pointer is
permitted.
// \param originalSize The current size in bytes. (Design issue: since some
allocator may not book-keep this, explicitly pass to it can save memory.)
// \param newSize the new size in bytes.
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
// Free a memory block.
// \param pointer to the memory block. Null pointer is permitted.
static void Free(void *ptr);
};
\endcode
*/
/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
\ingroup RAPIDJSON_CONFIG
\brief User-defined kDefaultChunkCapacity definition.
User can define this as any \c size that is a power of 2.
*/
#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
#endif
///////////////////////////////////////////////////////////////////////////////
// CrtAllocator
//! C-runtime library allocator.
/*! This class is just wrapper for standard C library memory routines.
\note implements Allocator concept
*/
class CrtAllocator {
public:
static const bool kNeedFree = true;
void *Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size);
else
return NULL; // standardize to returning NULL.
}
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
}
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
};
///////////////////////////////////////////////////////////////////////////////
// MemoryPoolAllocator
//! Default memory allocator used by the parser and DOM.
/*! This allocator allocate memory blocks from pre-allocated memory chunks.
It does not free memory blocks. And Realloc() only allocate new memory.
The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
default.
User may also supply a buffer as the first chunk.
If the user-buffer is full then additional chunks are allocated by
BaseAllocator.
The user-buffer is not deallocated by this allocator.
\tparam BaseAllocator the allocator type for allocating memory chunks.
Default is CrtAllocator. \note implements Allocator concept
*/
template <typename BaseAllocator = CrtAllocator>
class MemoryPoolAllocator {
public:
static const bool kNeedFree =
false; //!< Tell users that no need to call Free() with this allocator.
//!< (concept Allocator)
//! Constructor with chunkSize.
/*! \param chunkSize The size of memory chunk. The default is
kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
chunks.
*/
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 0)
: chunkHead_(0),
chunk_capacity_(chunkSize),
userBuffer_(0),
baseAllocator_(baseAllocator),
ownBaseAllocator_(0) {}
//! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool
allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than
sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
is kDefaultChunkSize. \param baseAllocator The allocator for allocating
memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size,
size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 0)
: chunkHead_(0),
chunk_capacity_(chunkSize),
userBuffer_(buffer),
baseAllocator_(baseAllocator),
ownBaseAllocator_(0) {
RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader *next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void *Malloc(size_t size) {
if (!size) return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0) return Malloc(newSize);
if (newSize == 0) return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize) return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient
// space
if (originalPtr ==
reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
return originalPtr;
}
}
// Realloc process: allocate and copy memory, do not free original buffer.
if (void *newBuffer = Malloc(newSize)) {
if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
} else
return NULL;
}
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
private:
//! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
//! Copy assignment operator is not permitted.
MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
//! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes.
\return true if success.
*/
bool AddChunk(size_t capacity) {
if (!baseAllocator_)
ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
if (ChunkHeader *chunk =
reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
} else
return false;
}
static const int kDefaultChunkCapacity =
RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
//! Chunk header for perpending to each chunk.
/*! Chunks are stored as a singly linked list.
*/
struct ChunkHeader {
size_t capacity; //!< Capacity of the chunk in bytes (excluding the header
//!< itself).
size_t size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list.
};
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head
//!< chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are
//!< allocated.
void *userBuffer_; //!< User supplied buffer.
BaseAllocator
*baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ENCODINGS_H_
sensors/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h
Vendored
+81
View File
@@ -0,0 +1,81 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
#include "stream.h"
#if defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists.
/*!
\tparam InputStream Any stream that implements Stream Concept
*/
template <typename InputStream, typename Encoding = UTF8<>>
class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public:
typedef typename Encoding::Ch Ch;
CursorStreamWrapper(InputStream &is)
: GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
// counting line and column number
Ch Take() {
Ch ch = this->is_.Take();
if (ch == '\n') {
line_++;
col_ = 0;
} else {
col_++;
}
return ch;
}
//! Get the error line number, if error exists.
size_t GetLine() const { return line_; }
//! Get the error column number, if error exists.
size_t GetColumn() const { return col_; }
private:
size_t line_; //!< Current Line
size_t col_; //!< Current Column
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
#if defined(__GNUC__)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_
sensors/livox_ros_driver2/3rdparty/rapidjson/document.h
Vendored
+3309
View File
File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More