Compare commits
No commits in common. "dff4d0c2ed7b9c8095ef61b4a15f288be60e772a" and "7f377533a19bd11d90cd1bc163dcc911d126ed97" have entirely different histories.
dff4d0c2ed
...
7f377533a1
18
1.txt
18
1.txt
@ -1,18 +0,0 @@
|
|||||||
ros2 topic pub --rate 10 /LF/motor_cmds rc_msgs/msg/MotorCmds "motor_cmd_0:
|
|
||||||
tau: 0.0
|
|
||||||
vw: 0.0
|
|
||||||
pos: 0.0
|
|
||||||
kp: 0.0
|
|
||||||
kd: 0.0
|
|
||||||
motor_cmd_1:
|
|
||||||
tau: 0.0
|
|
||||||
vw: 0.0
|
|
||||||
pos: 0.0
|
|
||||||
kp: 0.0
|
|
||||||
kd: 0.0
|
|
||||||
motor_cmd_2:
|
|
||||||
tau: 0.0
|
|
||||||
vw: 0.0
|
|
||||||
pos: 0.0
|
|
||||||
kp: 0.0
|
|
||||||
kd: 0.0"
|
|
||||||
@ -1,80 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.5)
|
|
||||||
project(fdilink_ahrs)
|
|
||||||
|
|
||||||
|
|
||||||
# Default to C99
|
|
||||||
if(NOT CMAKE_C_STANDARD)
|
|
||||||
set(CMAKE_C_STANDARD 99)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# Default to C++14
|
|
||||||
if(NOT CMAKE_CXX_STANDARD)
|
|
||||||
set(CMAKE_CXX_STANDARD 14)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
find_package(Eigen3 REQUIRED)
|
|
||||||
set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
|
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
find_package(rclcpp REQUIRED)
|
|
||||||
find_package(rclpy REQUIRED)
|
|
||||||
find_package(sensor_msgs REQUIRED)
|
|
||||||
find_package(std_msgs REQUIRED)
|
|
||||||
find_package(tf2 REQUIRED)
|
|
||||||
find_package(tf2_ros REQUIRED)
|
|
||||||
find_package(serial REQUIRED)
|
|
||||||
find_package(nav_msgs REQUIRED)
|
|
||||||
find_package(geometry_msgs REQUIRED)
|
|
||||||
find_package(tf2_geometry_msgs REQUIRED)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
# the following line skips the linter which checks for copyrights
|
|
||||||
# uncomment the line when a copyright and license is not present in all source files
|
|
||||||
#set(ament_cmake_copyright_FOUND TRUE)
|
|
||||||
# the following line skips cpplint (only works in a git repo)
|
|
||||||
# uncomment the line when this package is not in a git repo
|
|
||||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
include_directories(
|
|
||||||
include
|
|
||||||
src
|
|
||||||
${catkin_INCLUDE_DIRS}
|
|
||||||
${Eigen3_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(ahrs_driver_node src/ahrs_driver.cpp src/crc_table.cpp)
|
|
||||||
ament_target_dependencies(ahrs_driver_node rclcpp rclpy std_msgs sensor_msgs serial tf2_ros tf2 nav_msgs geometry_msgs)
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(imu_tf_node src/imu_tf.cpp)
|
|
||||||
ament_target_dependencies(imu_tf_node rclcpp rclpy std_msgs sensor_msgs serial tf2_ros tf2 tf2_geometry_msgs)
|
|
||||||
|
|
||||||
# find_package(Boost 1.55.0 REQUIRED COMPONENTS system filesystem)
|
|
||||||
# include_directories(ahrs_driver_node ${Boost_INCLOUDE_DIRS})
|
|
||||||
# link_directories(ahrs_driver_node ${Boost_LIBRARY_DIRS})
|
|
||||||
# target_link_libraries(ahrs_driver_node ${Boost_LIBRSRIES})
|
|
||||||
|
|
||||||
install(TARGETS
|
|
||||||
ahrs_driver_node
|
|
||||||
imu_tf_node
|
|
||||||
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(
|
|
||||||
DIRECTORY launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
Binary file not shown.
@ -1,111 +0,0 @@
|
|||||||
#ifndef BASE_DRIVER_H_
|
|
||||||
#define BASE_DRIVER_H_
|
|
||||||
|
|
||||||
|
|
||||||
#include <inttypes.h>
|
|
||||||
#include <memory>
|
|
||||||
#include <rclcpp/rclcpp.hpp>
|
|
||||||
#include <tf2_ros/transform_broadcaster.h>
|
|
||||||
#include <iostream>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
|
|
||||||
#include <math.h>
|
|
||||||
#include <fstream>
|
|
||||||
#include <fdilink_data_struct.h>
|
|
||||||
//#include <sensor_msgs/Imu.h>
|
|
||||||
#include <sensor_msgs/msg/imu.hpp>
|
|
||||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
|
||||||
#include <geometry_msgs/msg/pose2_d.hpp>
|
|
||||||
#include <geometry_msgs/msg/twist.hpp>
|
|
||||||
#include <nav_msgs/msg/odometry.hpp>
|
|
||||||
|
|
||||||
#include <string>
|
|
||||||
//#include <ros/package.h>
|
|
||||||
#include <ament_index_cpp/get_package_prefix.hpp>
|
|
||||||
#include <crc_table.h>
|
|
||||||
#include <Eigen/Eigen>
|
|
||||||
#include <Eigen/Geometry>
|
|
||||||
#include <Eigen/Core>
|
|
||||||
//#include <boost/thread.h>
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace Eigen;
|
|
||||||
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 rclcpp::Node
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
|
|
||||||
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
|
|
||||||
//std::string imu_frame_id="gyro_link";
|
|
||||||
std::string imu_frame_id_;
|
|
||||||
std::string insgps_frame_id_;
|
|
||||||
std::string latlon_frame_id_;
|
|
||||||
//topic
|
|
||||||
std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
|
|
||||||
std::string latlon_topic="latlon";
|
|
||||||
std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
|
|
||||||
std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
|
|
||||||
|
|
||||||
|
|
||||||
//Publisher
|
|
||||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
|
|
||||||
rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr mag_pose_pub_;
|
|
||||||
|
|
||||||
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
|
|
||||||
|
|
||||||
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
|
|
||||||
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
|
|
||||||
|
|
||||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
|
|
||||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr NED_odom_pub_;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}; //ahrsBringup
|
|
||||||
} // namespace FDILink
|
|
||||||
|
|
||||||
#endif
|
|
||||||
@ -1,10 +0,0 @@
|
|||||||
#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
|
|
||||||
@ -1,189 +0,0 @@
|
|||||||
#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_
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
import os
|
|
||||||
from pathlib import Path
|
|
||||||
from ament_index_python.packages import get_package_share_directory
|
|
||||||
from launch import LaunchDescription
|
|
||||||
from launch.actions import (DeclareLaunchArgument, GroupAction,
|
|
||||||
IncludeLaunchDescription, SetEnvironmentVariable)
|
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
# bringup_dir = get_package_share_directory('fdilink_ahrs')
|
|
||||||
# launch_dir = os.path.join(bringup_dir, 'launch')
|
|
||||||
# imu_tf = IncludeLaunchDescription(
|
|
||||||
# PythonLaunchDescriptionSource(os.path.join(launch_dir, 'imu_tf.launch.py')),
|
|
||||||
# )
|
|
||||||
def generate_launch_description():
|
|
||||||
ahrs_driver=Node(
|
|
||||||
package="fdilink_ahrs",
|
|
||||||
executable="ahrs_driver_node",
|
|
||||||
parameters=[{'if_debug_': False,
|
|
||||||
# 'serial_port_':'/dev/wheeltec_FDI_IMU_GNSS',
|
|
||||||
'serial_port_':'/dev/ttyUSB0',
|
|
||||||
'serial_baud_':921600,
|
|
||||||
'imu_topic':'/imu',
|
|
||||||
'imu_frame_id_':'gyro_link',
|
|
||||||
'mag_pose_2d_topic':'/mag_pose_2d',
|
|
||||||
'Magnetic_topic':'/magnetic',
|
|
||||||
'Euler_angles_topic':'/euler_angles',
|
|
||||||
'gps_topic':'/gps/fix',
|
|
||||||
'twist_topic':'/system_speed',
|
|
||||||
'NED_odom_topic':'/NED_odometry',
|
|
||||||
'device_type_':1}],
|
|
||||||
output="screen"
|
|
||||||
)
|
|
||||||
|
|
||||||
launch_description =LaunchDescription()
|
|
||||||
launch_description.add_action(ahrs_driver)
|
|
||||||
# launch_description.add_action(imu_tf)
|
|
||||||
return launch_description
|
|
||||||
@ -1,18 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
imu_tf=Node(
|
|
||||||
package="fdilink_ahrs",
|
|
||||||
#node_executable="imu_tf_node",
|
|
||||||
parameters=[{'imu_topic':'/imu',
|
|
||||||
'world_frame_id':'/world',
|
|
||||||
'imu_frame_id':'/gyro_link',
|
|
||||||
'position_x':1,
|
|
||||||
'position_y':1,
|
|
||||||
'position_z':1,
|
|
||||||
}],
|
|
||||||
)
|
|
||||||
|
|
||||||
launch_description =LaunchDescription([imu_tf])
|
|
||||||
return launch_description
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>fdilink_ahrs</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="noah@todo.todo">noah</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>rclpy</depend>
|
|
||||||
<depend>sensor_msgs</depend>
|
|
||||||
<depend>std_msgs</depend>
|
|
||||||
<depend>tf2</depend>
|
|
||||||
<depend>tf2_ros</depend>
|
|
||||||
<depend>nav_msgs</depend>
|
|
||||||
<depend>geometry_msgs</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,675 +0,0 @@
|
|||||||
#include <ahrs_driver.h>
|
|
||||||
//#include <Eigen/Eigen>
|
|
||||||
rclcpp::Node::SharedPtr nh_=nullptr;
|
|
||||||
|
|
||||||
|
|
||||||
namespace FDILink
|
|
||||||
{
|
|
||||||
ahrsBringup::ahrsBringup()
|
|
||||||
: rclcpp::Node ("ahrs_bringup")
|
|
||||||
{
|
|
||||||
//topic_name & frame_id 加载参数服务器
|
|
||||||
this->declare_parameter("if_debug_",false);
|
|
||||||
this->get_parameter("if_debug_", if_debug_);
|
|
||||||
|
|
||||||
this->declare_parameter<std::int8_t>("device_type_",1);
|
|
||||||
this->get_parameter("device_type_", device_type_);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("imu_topic","/imu");
|
|
||||||
this->get_parameter("imu_topic", imu_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("imu_frame_id_","gyro_link");
|
|
||||||
this->get_parameter("imu_frame_id_", imu_frame_id_);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
|
|
||||||
this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
|
|
||||||
this->get_parameter("Euler_angles_topic", Euler_angles_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("gps_topic","/gps/fix");
|
|
||||||
this->get_parameter("gps_topic", gps_topic);
|
|
||||||
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("Magnetic_topic","/magnetic");
|
|
||||||
this->get_parameter("Magnetic_topic", Magnetic_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("twist_topic","/system_speed");
|
|
||||||
this->get_parameter("twist_topic", twist_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("NED_odom_topic","/NED_odometry");
|
|
||||||
this->get_parameter("NED_odom_topic", NED_odom_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("serial_port_","/dev/fdilink_ahrs");
|
|
||||||
this->get_parameter("serial_port_", serial_port_);
|
|
||||||
|
|
||||||
this->declare_parameter<std::int64_t>("serial_baud_",921600);
|
|
||||||
this->get_parameter("serial_baud_", serial_baud_);
|
|
||||||
|
|
||||||
//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"));
|
|
||||||
|
|
||||||
//serial
|
|
||||||
//pravite_nh.param("port", serial_port_, std::string("/dev/ttyTHS1"));
|
|
||||||
//pravite_nh.param("baud", serial_baud_, 115200);
|
|
||||||
|
|
||||||
//publisher
|
|
||||||
imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10);
|
|
||||||
gps_pub_ = create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic.c_str(), 10);
|
|
||||||
|
|
||||||
mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
|
|
||||||
|
|
||||||
Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
|
|
||||||
Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
|
|
||||||
|
|
||||||
twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
|
|
||||||
NED_odom_pub_ = create_publisher<nav_msgs::msg::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) // 抓取异常
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(),"Unable to open port ");
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
if (serial_.isOpen())
|
|
||||||
{
|
|
||||||
RCLCPP_INFO(this->get_logger(),"Serial Port initialized");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(),"Unable to initial Serial port ");
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
processLoop();
|
|
||||||
}
|
|
||||||
|
|
||||||
ahrsBringup::~ahrsBringup() // 析构函数关闭串口通道
|
|
||||||
{
|
|
||||||
if (serial_.isOpen())
|
|
||||||
serial_.close();
|
|
||||||
}
|
|
||||||
|
|
||||||
void ahrsBringup::processLoop() // 数据处理过程
|
|
||||||
{
|
|
||||||
RCLCPP_INFO(this->get_logger(),"ahrsBringup::processLoop: start");
|
|
||||||
while (rclcpp::ok())
|
|
||||||
{
|
|
||||||
if (!serial_.isOpen())
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"head_len error (imu)");
|
|
||||||
continue;
|
|
||||||
}else if (head_type[0] == TYPE_AHRS && check_len[0] != AHRS_LEN)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"head_len error (ahrs)");
|
|
||||||
continue;
|
|
||||||
}else if (head_type[0] == TYPE_INSGPS && check_len[0] != INSGPS_LEN)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"head_len error (insgps)");
|
|
||||||
continue;
|
|
||||||
}else if (head_type[0] == TYPE_GEODETIC_POS && check_len[0] != GEODETIC_POS_LEN)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
sn_lost_ += 256 - (int)(read_sn_ - ground_sn[0]);
|
|
||||||
read_sn_ = ground_sn[0];
|
|
||||||
// continue;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if(if_debug_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(imu).");
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
else if(imu_frame_.frame.frame_end != FRAME_END)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
else if(ahrs_frame_.frame.frame_end != FRAME_END)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
else if(insgps_frame_.frame.frame_end != FRAME_END)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(gps).");
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
else if(Geodetic_Position_frame_.frame.frame_end != FRAME_END)
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(),"check frame end.");
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// publish magyaw topic
|
|
||||||
//读取IMU数据进行解析，并发布相关话题
|
|
||||||
|
|
||||||
if (head_type[0] == TYPE_IMU)
|
|
||||||
{
|
|
||||||
// publish imu topic
|
|
||||||
sensor_msgs::msg::Imu imu_data;
|
|
||||||
imu_data.header.stamp = rclcpp::Node::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 = imu_frame_.frame.data.data_pack.gyroscope_x;
|
|
||||||
imu_data.angular_velocity.y = imu_frame_.frame.data.data_pack.gyroscope_y;
|
|
||||||
imu_data.angular_velocity.z = imu_frame_.frame.data.data_pack.gyroscope_z;
|
|
||||||
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单品rclcpp标准下的坐标变换
|
|
||||||
{
|
|
||||||
|
|
||||||
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 = imu_frame_.frame.data.data_pack.gyroscope_x;
|
|
||||||
imu_data.angular_velocity.y = -imu_frame_.frame.data.data_pack.gyroscope_y;
|
|
||||||
imu_data.angular_velocity.z = -imu_frame_.frame.data.data_pack.gyroscope_z;
|
|
||||||
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::msg::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::msg::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::msg::NavSatFix gps_data;
|
|
||||||
gps_data.header.stamp = rclcpp::Node::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::msg::Odometry odom_msg;
|
|
||||||
odom_msg.header.stamp = rclcpp::Node::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::msg::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_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
sn_lost_ += (int)(imu_frame_.frame.header.serial_num - read_sn_);
|
|
||||||
if(if_debug_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
sn_lost_ += (int)(ahrs_frame_.frame.header.serial_num - read_sn_);
|
|
||||||
if(if_debug_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
sn_lost_ += (int)(insgps_frame_.frame.header.serial_num - read_sn_);
|
|
||||||
if(if_debug_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"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_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
sn_lost_ += (int)(Geodetic_Position_frame_.frame.header.serial_num - read_sn_);
|
|
||||||
if(if_debug_){
|
|
||||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num;
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} //namespace FDILink
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
FDILink::ahrsBringup bp;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,159 +0,0 @@
|
|||||||
#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);
|
|
||||||
}
|
|
||||||
@ -1,107 +0,0 @@
|
|||||||
#include <memory>
|
|
||||||
#include <inttypes.h>
|
|
||||||
#include <rclcpp/rclcpp.hpp>
|
|
||||||
#include <sensor_msgs/msg/imu.hpp>
|
|
||||||
#include <tf2_ros/transform_broadcaster.h>
|
|
||||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
||||||
#include <tf2/LinearMath/Transform.h>
|
|
||||||
#include <tf2/LinearMath/Quaternion.h>
|
|
||||||
#include <string>
|
|
||||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
|
||||||
#include <rclcpp/time.hpp>
|
|
||||||
using std::placeholders::_1;
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
/* 参考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_topic;
|
|
||||||
std::string imu_frame_id, world_frame_id;
|
|
||||||
|
|
||||||
static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
|
|
||||||
rclcpp::Node::SharedPtr nh_=nullptr;
|
|
||||||
class imu_data_to_tf : public rclcpp::Node
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
imu_data_to_tf():Node("imu_data_to_tf")
|
|
||||||
{
|
|
||||||
|
|
||||||
// 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);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("world_frame_id","/world");
|
|
||||||
this->get_parameter("world_frame_id", world_frame_id);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("imu_frame_id","/imu");
|
|
||||||
this->get_parameter("imu_frame_id", imu_frame_id);
|
|
||||||
|
|
||||||
this->declare_parameter<std::string>("imu_topic","/imu");
|
|
||||||
this->get_parameter("imu_topic", imu_topic);
|
|
||||||
|
|
||||||
this->declare_parameter<std::int16_t>("position_x",1);
|
|
||||||
this->get_parameter("position_x", position_x);
|
|
||||||
|
|
||||||
this->declare_parameter<std::int16_t>("position_y",1);
|
|
||||||
this->get_parameter("position_y", position_y);
|
|
||||||
|
|
||||||
this->declare_parameter<std::int16_t>("position_z",1);
|
|
||||||
this->get_parameter("position_z", position_z);
|
|
||||||
br =std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
|
||||||
sub_ = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10, std::bind(&imu_data_to_tf::ImuCallback,this,_1));
|
|
||||||
|
|
||||||
}
|
|
||||||
private:
|
|
||||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
|
|
||||||
|
|
||||||
//rclcpp::Subscriber sub = node.subscribe(imu_topic.c_str(), 10, &ImuCallback);
|
|
||||||
|
|
||||||
|
|
||||||
void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_data) {
|
|
||||||
|
|
||||||
//static tf2_ros::TransformBroadcaster br;//广播器
|
|
||||||
|
|
||||||
// tf2::Transform transform;
|
|
||||||
// transform.setOrigin(tf2::Vector3(position_x, position_y, position_z));//设置平移部分
|
|
||||||
|
|
||||||
// 从IMU消息包中获取四元数数据
|
|
||||||
tf2::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", "imu"));
|
|
||||||
geometry_msgs::msg::TransformStamped tfs;
|
|
||||||
tfs.header.stamp=rclcpp::Node::now();
|
|
||||||
tfs.header.frame_id ="world";
|
|
||||||
tfs.child_frame_id="imu";
|
|
||||||
tfs.transform.translation.x=position_x;
|
|
||||||
tfs.transform.translation.y=position_y;
|
|
||||||
tfs.transform.translation.z=position_z;
|
|
||||||
tfs.transform.rotation.x=q.getX();
|
|
||||||
tfs.transform.rotation.y=q.getY();
|
|
||||||
tfs.transform.rotation.z=q.getZ();
|
|
||||||
tfs.transform.rotation.w=q.getW();
|
|
||||||
br->sendTransform(tfs);
|
|
||||||
//tf2::(transform, rclcpp::Node::now(), "world", "imu")
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
int main (int argc, char ** argv) {
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
|
|
||||||
//ros::NodeHandle node;, "imu_data_to_tf"
|
|
||||||
//auto node =std::make_shared<imu_data_to_tf>();
|
|
||||||
rclcpp::spin(std::make_shared<imu_data_to_tf>());
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,14 +0,0 @@
|
|||||||
#CP2102 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
|
|
||||||
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss.rules
|
|
||||||
#CH9102，同时系统安装了对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
|
|
||||||
echo 'KERNEL=="ttyCH343USB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss2.rules
|
|
||||||
#CH9102，同时系统没有安装对应驱动 串口号0003 设置别名为wheeltec_FDI_IMU_GNSS
|
|
||||||
echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdi_imu_gnss3.rules
|
|
||||||
#CH340，直接设置别名为wheeltec_FDI_IMU_GNSS
|
|
||||||
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", GROUP:="dialout", SYMLINK+="wheeltec_FDI_IMU_GNSS"' >/etc/udev/rules.d/wheeltec_fdcontroller_340.rules
|
|
||||||
|
|
||||||
service udev reload
|
|
||||||
sleep 2
|
|
||||||
service udev restart
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.5)
|
|
||||||
project(ps2_controller)
|
|
||||||
|
|
||||||
# Default to C++17
|
|
||||||
if(NOT CMAKE_CXX_STANDARD)
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# Find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
find_package(rclcpp REQUIRED)
|
|
||||||
find_package(rc_msgs REQUIRED) # 添加对 rc_msgs 的依赖
|
|
||||||
|
|
||||||
add_executable(ps2_reader src/ps2_reader.cpp)
|
|
||||||
|
|
||||||
ament_target_dependencies(ps2_reader rclcpp rc_msgs) # 添加 rc_msgs 依赖
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}/
|
|
||||||
)
|
|
||||||
|
|
||||||
install(TARGETS
|
|
||||||
ps2_reader
|
|
||||||
DESTINATION lib/${PROJECT_NAME})
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
return LaunchDescription([
|
|
||||||
Node(
|
|
||||||
package='ps2_controller',
|
|
||||||
executable='ps2_reader',
|
|
||||||
name='ps2_controller_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[
|
|
||||||
{'device': '/dev/input/js0'} # 可根据需要修改设备路径
|
|
||||||
]
|
|
||||||
)
|
|
||||||
])
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>ps2_controller</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>rc_msgs</depend>
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,123 +0,0 @@
|
|||||||
#include <rclcpp/rclcpp.hpp>
|
|
||||||
#include <linux/joystick.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <iostream>
|
|
||||||
#include <string>
|
|
||||||
#include "rc_msgs/msg/ps2_data.hpp" // 引入 Ps2Data 消息类型
|
|
||||||
|
|
||||||
class PS2ControllerNode : public rclcpp::Node
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
PS2ControllerNode() : Node("ps2_controller_node")
|
|
||||||
{
|
|
||||||
this->declare_parameter<std::string>("device", "/dev/input/js0");
|
|
||||||
device_path_ = this->get_parameter("device").as_string();
|
|
||||||
|
|
||||||
joystick_fd_ = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK);
|
|
||||||
if (joystick_fd_ < 0)
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "Failed to open device: %s", device_path_.c_str());
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Connected to device: %s", device_path_.c_str());
|
|
||||||
|
|
||||||
ps2_data_ = std::make_shared<rc_msgs::msg::Ps2Data>();
|
|
||||||
publisher_ = this->create_publisher<rc_msgs::msg::Ps2Data>("ps2_data", 10);
|
|
||||||
|
|
||||||
timer_ = this->create_wall_timer(
|
|
||||||
std::chrono::milliseconds(10), // 100Hz
|
|
||||||
std::bind(&PS2ControllerNode::publishData, this));
|
|
||||||
}
|
|
||||||
|
|
||||||
~PS2ControllerNode()
|
|
||||||
{
|
|
||||||
if (joystick_fd_ >= 0)
|
|
||||||
{
|
|
||||||
close(joystick_fd_);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
void publishData()
|
|
||||||
{
|
|
||||||
readJoystick();
|
|
||||||
publisher_->publish(*ps2_data_);
|
|
||||||
}
|
|
||||||
|
|
||||||
void readJoystick()
|
|
||||||
{
|
|
||||||
struct js_event event;
|
|
||||||
while (read(joystick_fd_, &event, sizeof(event)) > 0)
|
|
||||||
{
|
|
||||||
switch (event.type & ~JS_EVENT_INIT)
|
|
||||||
{
|
|
||||||
case JS_EVENT_BUTTON:
|
|
||||||
handleButtonEvent(event);
|
|
||||||
break;
|
|
||||||
case JS_EVENT_AXIS:
|
|
||||||
handleAxisEvent(event);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void handleButtonEvent(const js_event &event)
|
|
||||||
{
|
|
||||||
switch (event.number)
|
|
||||||
{
|
|
||||||
case 10: ps2_data_->select = event.value; break;
|
|
||||||
case 11: ps2_data_->start = event.value; break;
|
|
||||||
case 6: ps2_data_->l1 = event.value; break;
|
|
||||||
case 8: ps2_data_->l2 = event.value; break;
|
|
||||||
case 7: ps2_data_->r1 = event.value; break;
|
|
||||||
case 9: ps2_data_->r2 = event.value; break;
|
|
||||||
case 3: ps2_data_->x = event.value; break;
|
|
||||||
case 4: ps2_data_->y = event.value; break;
|
|
||||||
case 0: ps2_data_->a = event.value; break;
|
|
||||||
case 1: ps2_data_->b = event.value; break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void handleAxisEvent(const js_event &event)
|
|
||||||
{
|
|
||||||
constexpr float MAX_AXIS_VALUE = 32767.0f; // 最大轴值
|
|
||||||
constexpr float MIN_AXIS_VALUE = -32768.0f; // 最小轴值
|
|
||||||
|
|
||||||
auto normalize = [](int value) -> float {
|
|
||||||
return static_cast<float>(value) / MAX_AXIS_VALUE;
|
|
||||||
};
|
|
||||||
|
|
||||||
switch (event.number)
|
|
||||||
{
|
|
||||||
case 0: ps2_data_->lx = normalize(event.value); break;
|
|
||||||
case 1: ps2_data_->ly = normalize(event.value); break;
|
|
||||||
case 2: ps2_data_->rx = normalize(event.value); break;
|
|
||||||
case 3: ps2_data_->ry = normalize(event.value); break;
|
|
||||||
case 6: ps2_data_->left_right = normalize(event.value); break;
|
|
||||||
case 7: ps2_data_->up_down = normalize(event.value); break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string device_path_;
|
|
||||||
int joystick_fd_;
|
|
||||||
rclcpp::TimerBase::SharedPtr timer_;
|
|
||||||
rclcpp::Publisher<rc_msgs::msg::Ps2Data>::SharedPtr publisher_;
|
|
||||||
std::shared_ptr<rc_msgs::msg::Ps2Data> ps2_data_;
|
|
||||||
};
|
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
auto node = std::make_shared<PS2ControllerNode>();
|
|
||||||
rclcpp::spin(node);
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -6,10 +6,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||||||
"msg/Ps2Data.msg"
|
"msg/Ps2Data.msg"
|
||||||
"msg/DataControl.msg"
|
"msg/DataControl.msg"
|
||||||
"msg/DataMotor.msg"
|
"msg/DataMotor.msg"
|
||||||
"msg/DataLeg.msg"
|
|
||||||
"msg/DataMotors.msg"
|
|
||||||
"msg/MotorCmd.msg"
|
|
||||||
"msg/MotorCmds.msg"
|
|
||||||
)
|
)
|
||||||
|
|
||||||
ament_package()
|
ament_package()
|
||||||
|
|||||||
@ -1,5 +1,5 @@
|
|||||||
float32 tau
|
int16 tau
|
||||||
float32 vw
|
int16 vw
|
||||||
float32 pos
|
int32 pos
|
||||||
float32 kp
|
uint16 kp
|
||||||
float32 kd
|
uint16 kd
|
||||||
@ -1,7 +0,0 @@
|
|||||||
DataControl data_control_0
|
|
||||||
DataControl data_control_1
|
|
||||||
DataControl data_control_2
|
|
||||||
|
|
||||||
DataMotor data_motor_0
|
|
||||||
DataMotor data_motor_1
|
|
||||||
DataMotor data_motor_2
|
|
||||||
@ -1,3 +1,3 @@
|
|||||||
float32 tau
|
int16 tau
|
||||||
float32 vw
|
int16 vw
|
||||||
float32 pos
|
int32 pos
|
||||||
@ -1,3 +0,0 @@
|
|||||||
DataMotor data_motor_0
|
|
||||||
DataMotor data_motor_1
|
|
||||||
DataMotor data_motor_2
|
|
||||||
@ -1,5 +0,0 @@
|
|||||||
float32 tau
|
|
||||||
float32 vw
|
|
||||||
float32 pos
|
|
||||||
float32 kp
|
|
||||||
float32 kd
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
MotorCmd motor_cmd_0
|
|
||||||
MotorCmd motor_cmd_1
|
|
||||||
MotorCmd motor_cmd_2
|
|
||||||
@ -15,11 +15,6 @@ bool r2
|
|||||||
# 四种模式
|
# 四种模式
|
||||||
uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
|
uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
|
||||||
|
|
||||||
bool x
|
|
||||||
bool y
|
|
||||||
bool a
|
|
||||||
bool b
|
|
||||||
|
|
||||||
bool select
|
bool select
|
||||||
bool start
|
bool start
|
||||||
|
|
||||||
|
|||||||
@ -1,27 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.8)
|
|
||||||
project(robot_bringup)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}/
|
|
||||||
)
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
# the following line skips the linter which checks for copyrights
|
|
||||||
# comment the line when a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_copyright_FOUND TRUE)
|
|
||||||
# the following line skips cpplint (only works in a git repo)
|
|
||||||
# comment the line when this package is in a git repo and when
|
|
||||||
# a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,128 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
from launch_ros.actions import ComposableNodeContainer
|
|
||||||
from launch_ros.descriptions import ComposableNode
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# 定义 leg_ctrl 的四个容器节点
|
|
||||||
lf_container = ComposableNodeContainer(
|
|
||||||
name='lf_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_lf',
|
|
||||||
parameters=[{'serial_port': '/dev/ttyACM0'}],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'LF/motor_cmds'),
|
|
||||||
('data_motors', 'LF/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
rf_container = ComposableNodeContainer(
|
|
||||||
name='rf_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_rf',
|
|
||||||
parameters=[{'serial_port': '/dev/ttyACM1'}],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'RF/motor_cmds'),
|
|
||||||
('data_motors', 'RF/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
lr_container = ComposableNodeContainer(
|
|
||||||
name='lr_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_lr',
|
|
||||||
parameters=[{'serial_port': '/dev/ttyACM2'}],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'LR/motor_cmds'),
|
|
||||||
('data_motors', 'LR/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
rr_container = ComposableNodeContainer(
|
|
||||||
name='rr_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_rr',
|
|
||||||
parameters=[{'serial_port': '/dev/ttyACM3'}],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'RR/motor_cmds'),
|
|
||||||
('data_motors', 'RR/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
# 定义 ahrs_driver 节点
|
|
||||||
ahrs_driver = Node(
|
|
||||||
package="fdilink_ahrs",
|
|
||||||
executable="ahrs_driver_node",
|
|
||||||
parameters=[{
|
|
||||||
'if_debug_': False,
|
|
||||||
'serial_port_': '/dev/ttyUSB0',
|
|
||||||
'serial_baud_': 921600,
|
|
||||||
'imu_topic': '/imu',
|
|
||||||
'imu_frame_id_': 'gyro_link',
|
|
||||||
'mag_pose_2d_topic': '/mag_pose_2d',
|
|
||||||
'Magnetic_topic': '/magnetic',
|
|
||||||
'Euler_angles_topic': '/euler_angles',
|
|
||||||
'gps_topic': '/gps/fix',
|
|
||||||
'twist_topic': '/system_speed',
|
|
||||||
'NED_odom_topic': '/NED_odometry',
|
|
||||||
'device_type_': 1
|
|
||||||
}],
|
|
||||||
output="screen"
|
|
||||||
)
|
|
||||||
|
|
||||||
# 定义 ps2_controller 节点
|
|
||||||
ps2_controller_node = Node(
|
|
||||||
package='ps2_controller',
|
|
||||||
executable='ps2_reader',
|
|
||||||
name='ps2_controller_node',
|
|
||||||
output='screen',
|
|
||||||
parameters=[
|
|
||||||
{'device': '/dev/input/js0'} # 可根据需要修改设备路径
|
|
||||||
]
|
|
||||||
)
|
|
||||||
|
|
||||||
# 返回主启动文件的描述
|
|
||||||
return LaunchDescription([
|
|
||||||
lf_container,
|
|
||||||
rf_container,
|
|
||||||
lr_container,
|
|
||||||
rr_container,
|
|
||||||
ahrs_driver,
|
|
||||||
ps2_controller_node # 添加 ps2_controller 节点
|
|
||||||
])
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>robot_bringup</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>launch</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,38 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.8)
|
|
||||||
project(simple_quadruped_control)
|
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
|
||||||
find_package(rclcpp REQUIRED)
|
|
||||||
find_package(rc_msgs REQUIRED)
|
|
||||||
|
|
||||||
# 添加头文件目录
|
|
||||||
include_directories(include)
|
|
||||||
|
|
||||||
add_executable(simple_control src/simple_control.cpp)
|
|
||||||
ament_target_dependencies(simple_control rclcpp rc_msgs)
|
|
||||||
|
|
||||||
install(TARGETS
|
|
||||||
simple_control
|
|
||||||
DESTINATION lib/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY include/
|
|
||||||
DESTINATION include/${PROJECT_NAME}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
|
||||||
DESTINATION share/${PROJECT_NAME}/
|
|
||||||
)
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,100 +0,0 @@
|
|||||||
#ifndef SIMPLE_QUADRUPED_CONTROL_HPP
|
|
||||||
#define SIMPLE_QUADRUPED_CONTROL_HPP
|
|
||||||
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
|
||||||
#include "rc_msgs/msg/ps2_data.hpp"
|
|
||||||
#include "rc_msgs/msg/motor_cmds.hpp"
|
|
||||||
#include "rc_msgs/msg/data_motors.hpp"
|
|
||||||
|
|
||||||
struct ps2_t
|
|
||||||
{
|
|
||||||
float lx;
|
|
||||||
float ly;
|
|
||||||
float rx;
|
|
||||||
float ry;
|
|
||||||
|
|
||||||
float up_down;
|
|
||||||
float left_right;
|
|
||||||
|
|
||||||
bool l1;
|
|
||||||
bool l2;
|
|
||||||
bool r1;
|
|
||||||
bool r2;
|
|
||||||
|
|
||||||
bool x;
|
|
||||||
bool y;
|
|
||||||
bool a;
|
|
||||||
bool b;
|
|
||||||
|
|
||||||
bool select;
|
|
||||||
bool start;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct motor_cmd_t
|
|
||||||
{
|
|
||||||
float pos;
|
|
||||||
float vw;
|
|
||||||
float tau;
|
|
||||||
float kp;
|
|
||||||
float kd;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct motor_cmds_t
|
|
||||||
{
|
|
||||||
motor_cmd_t motors[3]; // 使用数组替代单独的字段
|
|
||||||
};
|
|
||||||
|
|
||||||
struct data_motor_t
|
|
||||||
{
|
|
||||||
float pos;
|
|
||||||
float vw;
|
|
||||||
float tau;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct data_motors_t
|
|
||||||
{
|
|
||||||
data_motor_t motors[3]; // 使用数组替代单独的字段
|
|
||||||
};
|
|
||||||
|
|
||||||
struct mech_zero_t
|
|
||||||
{
|
|
||||||
float motors[3]; // 使用数组替代单独的字段
|
|
||||||
};
|
|
||||||
|
|
||||||
struct cmd_t
|
|
||||||
{
|
|
||||||
float pos;
|
|
||||||
float vw;
|
|
||||||
float tau;
|
|
||||||
float kp;
|
|
||||||
float kd;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct cmds_t
|
|
||||||
{
|
|
||||||
cmd_t motors[3]; // 使用数组替代单独的字段
|
|
||||||
};
|
|
||||||
|
|
||||||
ps2_t ps2;
|
|
||||||
motor_cmds_t motor_cmds;
|
|
||||||
data_motors_t data_motors;
|
|
||||||
mech_zero_t mech_zero;
|
|
||||||
cmds_t cmds;
|
|
||||||
|
|
||||||
class QuadrupedSubscriber : public rclcpp::Node
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
QuadrupedSubscriber();
|
|
||||||
|
|
||||||
private:
|
|
||||||
void ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg);
|
|
||||||
void data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg);
|
|
||||||
float limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit);
|
|
||||||
void publish_motor_cmds();
|
|
||||||
|
|
||||||
rclcpp::Subscription<rc_msgs::msg::Ps2Data>::SharedPtr ps2_data_subscriber_;
|
|
||||||
rclcpp::Publisher<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmds_publisher_;
|
|
||||||
rclcpp::Subscription<rc_msgs::msg::DataMotors>::SharedPtr data_motors_subscriber_;
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif // SIMPLE_QUADRUPED_CONTROL_HPP
|
|
||||||
@ -1,12 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch_ros.actions import Node
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
return LaunchDescription([
|
|
||||||
Node(
|
|
||||||
package='simple_quadruped_control',
|
|
||||||
executable='simple_control',
|
|
||||||
name='simple_control_node',
|
|
||||||
output='screen'
|
|
||||||
)
|
|
||||||
])
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
||||||
<package format="3">
|
|
||||||
<name>simple_quadruped_control</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>TODO: Package description</description>
|
|
||||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
|
||||||
<license>TODO: License declaration</license>
|
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
|
||||||
<depend>rc_msgs</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,208 +0,0 @@
|
|||||||
#include "simple_quadruped_control/simple_control.hpp"
|
|
||||||
|
|
||||||
// tau = tff + kp(q - q0) + kd(dq - dq0)
|
|
||||||
// tau :输出力矩
|
|
||||||
// tff :前馈力矩
|
|
||||||
// q：期望角度位置
|
|
||||||
// q0：当前角度位置
|
|
||||||
// dq 期望角速度
|
|
||||||
// dq0 当前角速度
|
|
||||||
// kp：位置刚度
|
|
||||||
// kd：速度阻尼
|
|
||||||
|
|
||||||
QuadrupedSubscriber::QuadrupedSubscriber() : Node("simple_control")
|
|
||||||
{
|
|
||||||
// 订阅 Ps2Data 话题
|
|
||||||
ps2_data_subscriber_ = this->create_subscription<rc_msgs::msg::Ps2Data>(
|
|
||||||
"ps2_data", 10,
|
|
||||||
std::bind(&QuadrupedSubscriber::ps2_data_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
// 发布 MotorCmds 话题
|
|
||||||
motor_cmds_publisher_ = this->create_publisher<rc_msgs::msg::MotorCmds>(
|
|
||||||
"/LF/motor_cmds", 10);
|
|
||||||
|
|
||||||
// 订阅 DataMotors 话题
|
|
||||||
data_motors_subscriber_ = this->create_subscription<rc_msgs::msg::DataMotors>(
|
|
||||||
"/LF/data_motors", 10,
|
|
||||||
std::bind(&QuadrupedSubscriber::data_motors_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
// 初始化机械零点
|
|
||||||
mech_zero.motors[0] = 0.7438;
|
|
||||||
mech_zero.motors[1] = 0.5693;
|
|
||||||
mech_zero.motors[2] = 0.6547;
|
|
||||||
}
|
|
||||||
|
|
||||||
void QuadrupedSubscriber::ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg)
|
|
||||||
{
|
|
||||||
ps2.lx = msg->lx;
|
|
||||||
ps2.ly = msg->ly;
|
|
||||||
ps2.rx = msg->rx;
|
|
||||||
ps2.ry = msg->ry;
|
|
||||||
|
|
||||||
ps2.up_down = msg->up_down;
|
|
||||||
ps2.left_right = msg->left_right;
|
|
||||||
|
|
||||||
ps2.l1 = msg->l1;
|
|
||||||
ps2.l2 = msg->l2;
|
|
||||||
ps2.r1 = msg->r1;
|
|
||||||
ps2.r2 = msg->r2;
|
|
||||||
|
|
||||||
ps2.x = msg->x;
|
|
||||||
ps2.y = msg->y;
|
|
||||||
ps2.a = msg->a;
|
|
||||||
ps2.b = msg->b;
|
|
||||||
|
|
||||||
ps2.select = msg->select;
|
|
||||||
ps2.start = msg->start;
|
|
||||||
|
|
||||||
// 回零
|
|
||||||
if (ps2.l1)
|
|
||||||
{
|
|
||||||
for (int i = 0; i < 3; ++i)
|
|
||||||
{ // 重置commonds
|
|
||||||
cmds.motors[i].pos = mech_zero.motors[i];
|
|
||||||
cmds.motors[i].vw = 0.0;
|
|
||||||
cmds.motors[i].tau = 0.0;
|
|
||||||
cmds.motors[i].kp = 1.2;
|
|
||||||
cmds.motors[i].kd = 0.05;
|
|
||||||
// 限制力矩输出
|
|
||||||
motor_cmds.motors[i].pos = cmds.motors[i].pos;
|
|
||||||
motor_cmds.motors[i].vw = cmds.motors[i].vw;
|
|
||||||
motor_cmds.motors[i].tau = cmds.motors[i].tau;
|
|
||||||
motor_cmds.motors[i].kp = cmds.motors[i].kp;
|
|
||||||
motor_cmds.motors[i].kd = cmds.motors[i].kd;
|
|
||||||
motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
|
|
||||||
}
|
|
||||||
publish_motor_cmds();
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Zeroing motors");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (ps2.l2)
|
|
||||||
{
|
|
||||||
|
|
||||||
cmds.motors[0].pos = cmds.motors[0].pos + ps2.lx * 0.01;
|
|
||||||
cmds.motors[0].vw = 0.0;
|
|
||||||
cmds.motors[0].tau = 0.0;
|
|
||||||
cmds.motors[0].kp = 1.2;
|
|
||||||
cmds.motors[0].kd = 0.05;
|
|
||||||
|
|
||||||
cmds.motors[1].pos = cmds.motors[1].pos + ps2.ly * 0.01;
|
|
||||||
cmds.motors[1].vw = 0.0;
|
|
||||||
cmds.motors[1].tau = 0.0;
|
|
||||||
cmds.motors[1].kp = 1.2;
|
|
||||||
cmds.motors[1].kd = 0.05;
|
|
||||||
|
|
||||||
cmds.motors[2].pos = cmds.motors[2].pos + ps2.ly * 0.01;
|
|
||||||
cmds.motors[2].vw = 0.0;
|
|
||||||
cmds.motors[2].tau = 0.0;
|
|
||||||
cmds.motors[2].kp = 1.2;
|
|
||||||
cmds.motors[2].kd = 0.05;
|
|
||||||
// 限制力矩输出
|
|
||||||
for (int i = 0; i < 3; ++i)
|
|
||||||
{
|
|
||||||
motor_cmds.motors[i].pos = cmds.motors[i].pos;
|
|
||||||
motor_cmds.motors[i].vw = cmds.motors[i].vw;
|
|
||||||
motor_cmds.motors[i].tau = cmds.motors[i].tau;
|
|
||||||
motor_cmds.motors[i].kp = cmds.motors[i].kp;
|
|
||||||
motor_cmds.motors[i].kd = cmds.motors[i].kd;
|
|
||||||
motor_cmds.motors[i].tau = limit_torque(motor_cmds.motors[i], data_motors.motors[i], 0.5);
|
|
||||||
}
|
|
||||||
publish_motor_cmds();
|
|
||||||
// RCLCPP_INFO(this->get_logger(), "Zeroing motors");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void QuadrupedSubscriber::data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg)
|
|
||||||
{
|
|
||||||
data_motors.motors[0].pos = msg->data_motor_0.pos;
|
|
||||||
data_motors.motors[0].vw = msg->data_motor_0.vw;
|
|
||||||
data_motors.motors[0].tau = msg->data_motor_0.tau;
|
|
||||||
|
|
||||||
data_motors.motors[1].pos = msg->data_motor_1.pos;
|
|
||||||
data_motors.motors[1].vw = msg->data_motor_1.vw;
|
|
||||||
data_motors.motors[1].tau = msg->data_motor_1.tau;
|
|
||||||
|
|
||||||
data_motors.motors[2].pos = msg->data_motor_2.pos;
|
|
||||||
data_motors.motors[2].vw = msg->data_motor_2.vw;
|
|
||||||
data_motors.motors[2].tau = msg->data_motor_2.tau;
|
|
||||||
|
|
||||||
// RCLCPP_INFO(this->get_logger(), "Updated DataMotors for motors 0, 1, 2");
|
|
||||||
}
|
|
||||||
|
|
||||||
void QuadrupedSubscriber::publish_motor_cmds()
|
|
||||||
{
|
|
||||||
auto motor_cmds_msg = rc_msgs::msg::MotorCmds();
|
|
||||||
|
|
||||||
// // 设置每个电机的命令
|
|
||||||
// for (int i = 0; i < 3; ++i)
|
|
||||||
// {
|
|
||||||
// motor_cmds_msg.motor_cmds[i].tau = motor_cmds.motors[i].tau;
|
|
||||||
// motor_cmds_msg.motor_cmds[i].vw = motor_cmds.motors[i].vw;
|
|
||||||
// motor_cmds_msg.motor_cmds[i].pos = motor_cmds.motors[i].pos;
|
|
||||||
// motor_cmds_msg.motor_cmds[i].kp = motor_cmds.motors[i].kp;
|
|
||||||
// motor_cmds_msg.motor_cmds[i].kd = motor_cmds.motors[i].kd;
|
|
||||||
// }
|
|
||||||
// 设置每个电机的命令
|
|
||||||
motor_cmds_msg.motor_cmd_0.tau = motor_cmds.motors[0].tau;
|
|
||||||
motor_cmds_msg.motor_cmd_0.vw = motor_cmds.motors[0].vw;
|
|
||||||
motor_cmds_msg.motor_cmd_0.pos = motor_cmds.motors[0].pos;
|
|
||||||
motor_cmds_msg.motor_cmd_0.kp = motor_cmds.motors[0].kp;
|
|
||||||
motor_cmds_msg.motor_cmd_0.kd = motor_cmds.motors[0].kd;
|
|
||||||
|
|
||||||
motor_cmds_msg.motor_cmd_1.tau = motor_cmds.motors[1].tau;
|
|
||||||
motor_cmds_msg.motor_cmd_1.vw = motor_cmds.motors[1].vw;
|
|
||||||
motor_cmds_msg.motor_cmd_1.pos = motor_cmds.motors[1].pos;
|
|
||||||
motor_cmds_msg.motor_cmd_1.kp = motor_cmds.motors[1].kp;
|
|
||||||
motor_cmds_msg.motor_cmd_1.kd = motor_cmds.motors[1].kd;
|
|
||||||
|
|
||||||
motor_cmds_msg.motor_cmd_2.tau = motor_cmds.motors[2].tau;
|
|
||||||
motor_cmds_msg.motor_cmd_2.vw = motor_cmds.motors[2].vw;
|
|
||||||
motor_cmds_msg.motor_cmd_2.pos = motor_cmds.motors[2].pos;
|
|
||||||
motor_cmds_msg.motor_cmd_2.kp = motor_cmds.motors[2].kp;
|
|
||||||
motor_cmds_msg.motor_cmd_2.kd = motor_cmds.motors[2].kd;
|
|
||||||
|
|
||||||
|
|
||||||
// 发布 MotorCmds 消息
|
|
||||||
motor_cmds_publisher_->publish(motor_cmds_msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 限制输出力矩的函数
|
|
||||||
float QuadrupedSubscriber::limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit)
|
|
||||||
{
|
|
||||||
// 读取结构体中的参数
|
|
||||||
float tff = motor_cmd.tau; // 前馈力矩
|
|
||||||
float kp = motor_cmd.kp; // 位置刚度
|
|
||||||
float kd = motor_cmd.kd; // 速度阻尼
|
|
||||||
float q = motor_cmd.pos*6.33; // 期望角度位置
|
|
||||||
float q0 = data_motor.pos*6.33; // 当前角度位置
|
|
||||||
float dq = motor_cmd.vw*6.33; // 期望角速度
|
|
||||||
float dq0 = data_motor.vw*6.33; // 当前角速度
|
|
||||||
|
|
||||||
// 计算输出力矩
|
|
||||||
float tau = tff + kp * (q - q0) + kd * (dq - dq0);
|
|
||||||
|
|
||||||
// 如果力矩超过限制值，调整前馈力矩
|
|
||||||
if (tau > limit)
|
|
||||||
{
|
|
||||||
tff -= (tau - limit) / 2;
|
|
||||||
tau = limit;
|
|
||||||
tff = tau - kp * (q - q0) - kd * (dq - dq0);
|
|
||||||
return tff;
|
|
||||||
}else if (tau < -limit)
|
|
||||||
{
|
|
||||||
tff += (tau + limit) / 2;
|
|
||||||
tau = -limit;
|
|
||||||
tff = tau - kp * (q - q0) - kd * (dq - dq0);
|
|
||||||
return tff;
|
|
||||||
}
|
|
||||||
return tff;
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
rclcpp::init(argc, argv);
|
|
||||||
rclcpp::spin(std::make_shared<QuadrupedSubscriber>());
|
|
||||||
rclcpp::shutdown();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -8,9 +8,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
|
|||||||
# 包含 ROS 2 依赖
|
# 包含 ROS 2 依赖
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
find_package(rclcpp REQUIRED)
|
find_package(rclcpp REQUIRED)
|
||||||
find_package(rclcpp_components REQUIRED)
|
|
||||||
find_package(std_msgs REQUIRED)
|
find_package(std_msgs REQUIRED)
|
||||||
find_package(rc_msgs REQUIRED)
|
|
||||||
|
|
||||||
# 包含目录和库路径
|
# 包含目录和库路径
|
||||||
include_directories(include)
|
include_directories(include)
|
||||||
@ -27,38 +25,20 @@ else()
|
|||||||
set(EXTRA_LIBS libUnitreeMotorSDK_Linux64.so)
|
set(EXTRA_LIBS libUnitreeMotorSDK_Linux64.so)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
# 创建组件库
|
# 添加可执行文件和链接库
|
||||||
add_library(unitree_motor_components SHARED
|
add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
|
||||||
src/unitree_motor_serial_driver.cpp
|
ament_target_dependencies(goM8010_6_motor rclcpp)
|
||||||
)
|
target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
|
||||||
ament_target_dependencies(unitree_motor_components
|
|
||||||
rclcpp
|
|
||||||
rclcpp_components
|
|
||||||
rc_msgs
|
|
||||||
)
|
|
||||||
target_link_libraries(unitree_motor_components ${EXTRA_LIBS})
|
|
||||||
rclcpp_components_register_nodes(unitree_motor_components
|
|
||||||
"unitree_motor_serial_driver::MotorControlNode"
|
|
||||||
)
|
|
||||||
|
|
||||||
# # 创建独立可执行文件
|
add_executable(unitree_motor_serial_driver src/unitree_motor_serial_driver.cpp)
|
||||||
# add_executable(goM8010_6_motor src/goM8010_6_motor.cpp)
|
ament_target_dependencies(unitree_motor_serial_driver rclcpp)
|
||||||
# ament_target_dependencies(goM8010_6_motor rclcpp rc_msgs)
|
target_link_libraries(unitree_motor_serial_driver ${EXTRA_LIBS})
|
||||||
# target_link_libraries(goM8010_6_motor ${EXTRA_LIBS})
|
|
||||||
|
|
||||||
# 创建节点可执行文件（非组件方式）
|
|
||||||
# add_executable(unitree_motor_serial_driver_node src/unitree_motor_serial_driver_node.cpp)
|
|
||||||
# target_link_libraries(unitree_motor_serial_driver_node unitree_motor_components)
|
|
||||||
# ament_target_dependencies(unitree_motor_serial_driver_node rclcpp)
|
|
||||||
|
|
||||||
# 安装目标
|
# 安装目标
|
||||||
install(TARGETS
|
install(TARGETS
|
||||||
# goM8010_6_motor
|
goM8010_6_motor
|
||||||
# unitree_motor_serial_driver_node
|
unitree_motor_serial_driver
|
||||||
unitree_motor_components
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
|
||||||
ARCHIVE DESTINATION lib
|
|
||||||
LIBRARY DESTINATION lib
|
|
||||||
)
|
)
|
||||||
|
|
||||||
install(DIRECTORY launch/
|
install(DIRECTORY launch/
|
||||||
|
|||||||
@ -6,8 +6,7 @@
|
|||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "serialPort/SerialPort.h"
|
#include "serialPort/SerialPort.h"
|
||||||
#include "unitreeMotor/unitreeMotor.h"
|
#include "unitreeMotor/unitreeMotor.h"
|
||||||
#include "rc_msgs/msg/motor_cmds.hpp"
|
|
||||||
#include "rc_msgs/msg/data_motors.hpp"
|
|
||||||
|
|
||||||
namespace unitree_motor_serial_driver
|
namespace unitree_motor_serial_driver
|
||||||
{
|
{
|
||||||
@ -18,22 +17,23 @@ struct Motor_t
|
|||||||
MotorData data;
|
MotorData data;
|
||||||
};
|
};
|
||||||
|
|
||||||
Motor_t motor[3]; // 修改为数组以支持多个电机
|
// struct Leg_Motor_t
|
||||||
|
// {
|
||||||
|
// Motor_t foot;
|
||||||
|
// Motor_t small;
|
||||||
|
// Motor_t big;
|
||||||
|
// };
|
||||||
|
|
||||||
class MotorControlNode : public rclcpp::Node
|
class MotorControlNode : public rclcpp::Node
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
explicit MotorControlNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
MotorControlNode();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void control_motor();
|
void control_motor();
|
||||||
void motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg);
|
|
||||||
void Motor_Ctrl_Offline(Motor_t *motor_s);
|
|
||||||
SerialPort serial_;
|
SerialPort serial_;
|
||||||
rclcpp::TimerBase::SharedPtr timer_;
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
rclcpp::Subscription<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmd_sub_;
|
|
||||||
rclcpp::Publisher<rc_msgs::msg::DataMotors>::SharedPtr data_motor_pub_;
|
|
||||||
int timer_count_ = 0;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,103 +0,0 @@
|
|||||||
from launch import LaunchDescription
|
|
||||||
from launch_ros.actions import ComposableNodeContainer
|
|
||||||
from launch_ros.descriptions import ComposableNode
|
|
||||||
|
|
||||||
def generate_launch_description():
|
|
||||||
# 左前腿容器 - LF (Left Front)
|
|
||||||
lf_container = ComposableNodeContainer(
|
|
||||||
name='lf_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_lf',
|
|
||||||
parameters=[
|
|
||||||
{'serial_port': '/dev/ttyACM0'}
|
|
||||||
],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'LF/motor_cmds'),
|
|
||||||
('data_motors', 'LF/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
# 右前腿容器 - RF (Right Front)
|
|
||||||
rf_container = ComposableNodeContainer(
|
|
||||||
name='rf_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_rf',
|
|
||||||
parameters=[
|
|
||||||
{'serial_port': '/dev/ttyACM1'}
|
|
||||||
],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'RF/motor_cmds'),
|
|
||||||
('data_motors', 'RF/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
# 左后腿容器 - LR (Left Rear)
|
|
||||||
lr_container = ComposableNodeContainer(
|
|
||||||
name='lr_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_lr',
|
|
||||||
parameters=[
|
|
||||||
{'serial_port': '/dev/ttyACM2'}
|
|
||||||
],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'LR/motor_cmds'),
|
|
||||||
('data_motors', 'LR/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
# 右后腿容器 - RR (Right Rear)
|
|
||||||
rr_container = ComposableNodeContainer(
|
|
||||||
name='rr_motor_container',
|
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
|
||||||
name='unitree_motor_serial_driver_rr',
|
|
||||||
parameters=[
|
|
||||||
{'serial_port': '/dev/ttyACM3'}
|
|
||||||
],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'RR/motor_cmds'),
|
|
||||||
('data_motors', 'RR/data_motors')
|
|
||||||
]
|
|
||||||
)
|
|
||||||
],
|
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
return LaunchDescription([
|
|
||||||
lf_container,
|
|
||||||
rf_container,
|
|
||||||
lr_container,
|
|
||||||
rr_container
|
|
||||||
])
|
|
||||||
@ -1,30 +1,15 @@
|
|||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch_ros.actions import ComposableNodeContainer
|
from launch_ros.actions import Node
|
||||||
from launch_ros.descriptions import ComposableNode
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
container = ComposableNodeContainer(
|
return LaunchDescription([
|
||||||
name='motor_container',
|
Node(
|
||||||
namespace='',
|
|
||||||
package='rclcpp_components',
|
|
||||||
executable='component_container',
|
|
||||||
composable_node_descriptions=[
|
|
||||||
ComposableNode(
|
|
||||||
package='unitree_motor_serial_driver',
|
package='unitree_motor_serial_driver',
|
||||||
plugin='unitree_motor_serial_driver::MotorControlNode',
|
executable='unitree_motor_serial_driver',
|
||||||
name='unitree_motor_serial_driver',
|
name='unitree_motor_serial_driver',
|
||||||
|
output='screen',
|
||||||
parameters=[
|
parameters=[
|
||||||
{'serial_port': '/dev/ttyACM0'}, # 修改默认串口路径
|
|
||||||
{'serial_retry_count': 5},
|
|
||||||
{'serial_retry_delay_ms': 1000}
|
|
||||||
],
|
|
||||||
remappings=[
|
|
||||||
('motor_cmds', 'motor_cmds'),
|
|
||||||
('data_motors', 'data_motors')
|
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
])
|
||||||
output='screen',
|
|
||||||
)
|
|
||||||
|
|
||||||
return LaunchDescription([container])
|
|
||||||
@ -10,9 +10,7 @@
|
|||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
<depend>rclcpp</depend>
|
<depend>rclcpp</depend>
|
||||||
<depend>rclcpp_components</depend>
|
|
||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
<depend>rc_msgs</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|||||||
@ -1,145 +1,42 @@
|
|||||||
#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
|
#include "unitree_motor_serial_driver/unitree_motor_serial_driver.hpp"
|
||||||
#include "rclcpp_components/register_node_macro.hpp"
|
|
||||||
|
|
||||||
namespace unitree_motor_serial_driver
|
namespace unitree_motor_serial_driver
|
||||||
{
|
{
|
||||||
|
|
||||||
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
|
MotorControlNode::MotorControlNode()
|
||||||
: Node("unitree_motor_serial_driver", options),
|
: Node("unitree_motor_serial_driver"), serial_("/dev/ttyACM0")
|
||||||
serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
|
|
||||||
{
|
{
|
||||||
// 声明其他参数
|
|
||||||
auto retry_count = this->declare_parameter("serial_retry_count", 5); // 默认重试5次
|
|
||||||
auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000); // 默认延迟1秒
|
|
||||||
|
|
||||||
bool connection_success = false;
|
|
||||||
int attempts = 0;
|
|
||||||
timer_count_ = 0;
|
|
||||||
|
|
||||||
// 定时器
|
|
||||||
timer_ = this->create_wall_timer(
|
timer_ = this->create_wall_timer(
|
||||||
std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
|
std::chrono::milliseconds(1), std::bind(&MotorControlNode::control_motor, this));
|
||||||
|
|
||||||
// 订阅 MotorCmds 消息
|
|
||||||
motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::MotorCmds>(
|
|
||||||
"motor_cmds", 10, std::bind(&MotorControlNode::motor_cmd_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
// 发布 DataMotors 消息
|
|
||||||
data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataMotors>("data_motors", 10);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorControlNode::motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg)
|
|
||||||
{
|
|
||||||
// 将接收到的 MotorCmds 消息分别赋值给 motor_t 数组
|
|
||||||
motor[0].cmd.tau = msg->motor_cmd_0.tau;
|
|
||||||
motor[0].cmd.dq = msg->motor_cmd_0.vw;
|
|
||||||
motor[0].cmd.q = msg->motor_cmd_0.pos;
|
|
||||||
motor[0].cmd.kp = msg->motor_cmd_0.kp;
|
|
||||||
motor[0].cmd.kd = msg->motor_cmd_0.kd;
|
|
||||||
|
|
||||||
motor[1].cmd.tau = msg->motor_cmd_1.tau;
|
|
||||||
motor[1].cmd.dq = msg->motor_cmd_1.vw;
|
|
||||||
motor[1].cmd.q = msg->motor_cmd_1.pos;
|
|
||||||
motor[1].cmd.kp = msg->motor_cmd_1.kp;
|
|
||||||
motor[1].cmd.kd = msg->motor_cmd_1.kd;
|
|
||||||
|
|
||||||
motor[2].cmd.tau = msg->motor_cmd_2.tau;
|
|
||||||
motor[2].cmd.dq = msg->motor_cmd_2.vw;
|
|
||||||
motor[2].cmd.q = msg->motor_cmd_2.pos;
|
|
||||||
motor[2].cmd.kp = msg->motor_cmd_2.kp;
|
|
||||||
motor[2].cmd.kd = msg->motor_cmd_2.kd;
|
|
||||||
|
|
||||||
timer_count_ = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotorControlNode::control_motor()
|
void MotorControlNode::control_motor()
|
||||||
{
|
|
||||||
rc_msgs::msg::DataMotors motor_msg;
|
|
||||||
|
|
||||||
timer_count_++;
|
|
||||||
|
|
||||||
for (int i = 0; i < 3; ++i)
|
|
||||||
{
|
{
|
||||||
MotorCmd cmd;
|
MotorCmd cmd;
|
||||||
MotorData data;
|
MotorData data;
|
||||||
|
|
||||||
cmd.motorType = MotorType::GO_M8010_6;
|
cmd.motorType = MotorType::GO_M8010_6;
|
||||||
data.motorType = MotorType::GO_M8010_6;
|
data.motorType = MotorType::GO_M8010_6;
|
||||||
cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
|
cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
|
||||||
cmd.id = i;
|
cmd.id = 0;
|
||||||
cmd.kp = motor[i].cmd.kp;
|
cmd.q = 0.0*queryGearRatio(MotorType::GO_M8010_6);;
|
||||||
cmd.kd = motor[i].cmd.kd;
|
cmd.dq = 0.0*queryGearRatio(MotorType::GO_M8010_6);;
|
||||||
cmd.q = motor[i].cmd.q * queryGearRatio(MotorType::GO_M8010_6);
|
cmd.kp = 0.0;
|
||||||
cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
|
cmd.kd = 0.0;
|
||||||
cmd.tau = motor[i].cmd.tau;
|
cmd.tau = 0.0;
|
||||||
|
|
||||||
if (timer_count_ > 50)
|
serial_.sendRecv(&cmd, &data);
|
||||||
{
|
|
||||||
Motor_Ctrl_Offline(&motor[i]);
|
|
||||||
timer_count_ = 200;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 添加异常处理以防止程序崩溃
|
RCLCPP_INFO(this->get_logger(), "\nMotor.q: %f\nMotor.temp: %d\nMotor.W: %f\nMotor.tau: %f\n",
|
||||||
try {
|
data.q, data.temp, data.dq, data.tau);
|
||||||
// 优化串口通信，减少阻塞
|
|
||||||
if (!serial_.sendRecv(&cmd, &data))
|
|
||||||
{
|
|
||||||
RCLCPP_WARN(this->get_logger(), "Failed to communicate with motor %d", i);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 填充 DataMotors 消息
|
|
||||||
if (i == 0)
|
|
||||||
{
|
|
||||||
motor_msg.data_motor_0.tau = data.tau;
|
|
||||||
motor_msg.data_motor_0.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
motor_msg.data_motor_0.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
}
|
|
||||||
else if (i == 1)
|
|
||||||
{
|
|
||||||
motor_msg.data_motor_1.tau = data.tau;
|
|
||||||
motor_msg.data_motor_1.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
motor_msg.data_motor_1.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
}
|
|
||||||
else if (i == 2)
|
|
||||||
{
|
|
||||||
motor_msg.data_motor_2.tau = data.tau;
|
|
||||||
motor_msg.data_motor_2.vw = data.dq / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
motor_msg.data_motor_2.pos = data.q / queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
}
|
|
||||||
} catch (const std::exception &e) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(),
|
|
||||||
"Exception during communication with motor %d: %s", i, e.what());
|
|
||||||
|
|
||||||
// 尝试重新初始化串口连接
|
|
||||||
try {
|
|
||||||
auto serial_port = this->get_parameter("serial_port").as_string();
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Attempting to reopen serial port: %s", serial_port.c_str());
|
|
||||||
serial_ = SerialPort(serial_port);
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Successfully reopened serial port: %s", serial_port.c_str());
|
|
||||||
} catch (const std::exception &e2) {
|
|
||||||
RCLCPP_ERROR(this->get_logger(),
|
|
||||||
"Failed to reopen serial port: %s", e2.what());
|
|
||||||
}
|
|
||||||
|
|
||||||
// 将电机设置为安全状态
|
|
||||||
Motor_Ctrl_Offline(&motor[i]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 发布 DataMotors 消息
|
|
||||||
data_motor_pub_->publish(motor_msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorControlNode::Motor_Ctrl_Offline(Motor_t *motor_s)
|
|
||||||
{
|
|
||||||
motor_s->cmd.kp = 0.0;
|
|
||||||
motor_s->cmd.kd = 0.01;
|
|
||||||
motor_s->cmd.q = 0.0;
|
|
||||||
motor_s->cmd.dq = 0.0;
|
|
||||||
motor_s->cmd.tau = 0.0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace unitree_motor_serial_driver
|
} // namespace unitree_motor_serial_driver
|
||||||
|
|
||||||
// 注册组件
|
int main(int argc, char **argv)
|
||||||
RCLCPP_COMPONENTS_REGISTER_NODE(unitree_motor_serial_driver::MotorControlNode)
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<unitree_motor_serial_driver::MotorControlNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user