diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..02e32c6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,69 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/src/hardware/fdilink_ahrs_ROS2/CMakeLists.txt b/src/hardware/fdilink_ahrs_ROS2/CMakeLists.txt new file mode 100644 index 0000000..cceefa0 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/CMakeLists.txt @@ -0,0 +1,80 @@ +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() diff --git a/src/hardware/fdilink_ahrs_ROS2/data/gps_data.bag b/src/hardware/fdilink_ahrs_ROS2/data/gps_data.bag new file mode 100644 index 0000000..76ec1bf Binary files /dev/null and b/src/hardware/fdilink_ahrs_ROS2/data/gps_data.bag differ diff --git a/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h b/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h new file mode 100644 index 0000000..baaa193 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h @@ -0,0 +1,111 @@ +#ifndef BASE_DRIVER_H_ +#define BASE_DRIVER_H_ + + +#include +#include +#include +#include +#include +#include +#include //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html +#include +#include +#include +//#include +#include +#include +#include +#include +#include + +#include +//#include +#include +#include +#include +#include +#include +//#include + +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::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr mag_pose_pub_; + + rclcpp::Publisher::SharedPtr gps_pub_; + + rclcpp::Publisher::SharedPtr Euler_angles_pub_; + rclcpp::Publisher::SharedPtr Magnetic_pub_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr NED_odom_pub_; + + + +}; //ahrsBringup +} // namespace FDILink + +#endif diff --git a/src/hardware/fdilink_ahrs_ROS2/include/crc_table.h b/src/hardware/fdilink_ahrs_ROS2/include/crc_table.h new file mode 100644 index 0000000..02875ba --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/include/crc_table.h @@ -0,0 +1,10 @@ +#ifndef CRC_TABLE_H +#define CRC_TABLE_H + +#include + +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 diff --git a/src/hardware/fdilink_ahrs_ROS2/include/fdilink_data_struct.h b/src/hardware/fdilink_ahrs_ROS2/include/fdilink_data_struct.h new file mode 100644 index 0000000..6ef6384 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/include/fdilink_data_struct.h @@ -0,0 +1,189 @@ +#ifndef FDILINK_DATA_STRUCT_H_ +#define FDILINK_DATA_STRUCT_H_ + +#include +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_ diff --git a/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py b/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py new file mode 100644 index 0000000..02a4662 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py @@ -0,0 +1,37 @@ +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 diff --git a/src/hardware/fdilink_ahrs_ROS2/launch/imu_tf.launch.py b/src/hardware/fdilink_ahrs_ROS2/launch/imu_tf.launch.py new file mode 100644 index 0000000..46dd488 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/launch/imu_tf.launch.py @@ -0,0 +1,18 @@ +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 diff --git a/src/hardware/mdog_hardware_interface/package.xml b/src/hardware/fdilink_ahrs_ROS2/package.xml similarity index 63% rename from src/hardware/mdog_hardware_interface/package.xml rename to src/hardware/fdilink_ahrs_ROS2/package.xml index 31a398f..fa6885c 100644 --- a/src/hardware/mdog_hardware_interface/package.xml +++ b/src/hardware/fdilink_ahrs_ROS2/package.xml @@ -1,24 +1,25 @@ - mdog_hardware_interface + fdilink_ahrs 0.0.0 TODO: Package description - robofish + noah TODO: License declaration ament_cmake - rclcpp_lifecycle - rclcpp - hardware_interface - pluginlib + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + nav_msgs + geometry_msgs ament_lint_auto ament_lint_common - ament_cmake_gmock - ros2_control_test_assets ament_cmake diff --git a/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp b/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp new file mode 100644 index 0000000..f08193e --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp @@ -0,0 +1,675 @@ +#include +//#include +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("device_type_",1); + this->get_parameter("device_type_", device_type_); + + this->declare_parameter("imu_topic","/imu"); + this->get_parameter("imu_topic", imu_topic); + + this->declare_parameter("imu_frame_id_","gyro_link"); + this->get_parameter("imu_frame_id_", imu_frame_id_); + + this->declare_parameter("mag_pose_2d_topic","/mag_pose_2d"); + this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic); + + this->declare_parameter("Euler_angles_topic","/euler_angles"); + this->get_parameter("Euler_angles_topic", Euler_angles_topic); + + this->declare_parameter("gps_topic","/gps/fix"); + this->get_parameter("gps_topic", gps_topic); + + + this->declare_parameter("Magnetic_topic","/magnetic"); + this->get_parameter("Magnetic_topic", Magnetic_topic); + + this->declare_parameter("twist_topic","/system_speed"); + this->get_parameter("twist_topic", twist_topic); + + this->declare_parameter("NED_odom_topic","/NED_odometry"); + this->get_parameter("NED_odom_topic", NED_odom_topic); + + this->declare_parameter("serial_port_","/dev/fdilink_ahrs"); + this->get_parameter("serial_port_", serial_port_); + + this->declare_parameter("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(imu_topic.c_str(), 10); + gps_pub_ = create_publisher(gps_topic.c_str(), 10); + + mag_pose_pub_ = create_publisher(mag_pose_2d_topic.c_str(), 10); + + Euler_angles_pub_ = create_publisher(Euler_angles_topic.c_str(), 10); + Magnetic_pub_ = create_publisher(Magnetic_topic.c_str(), 10); + + twist_pub_ = create_publisher(twist_topic.c_str(), 10); + NED_odom_pub_ = create_publisher(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; +} diff --git a/src/hardware/fdilink_ahrs_ROS2/src/crc_table.cpp b/src/hardware/fdilink_ahrs_ROS2/src/crc_table.cpp new file mode 100644 index 0000000..5d8b041 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/src/crc_table.cpp @@ -0,0 +1,159 @@ +#include +#include + +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); +} diff --git a/src/hardware/fdilink_ahrs_ROS2/src/imu_tf.cpp b/src/hardware/fdilink_ahrs_ROS2/src/imu_tf.cpp new file mode 100644 index 0000000..d469707 --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/src/imu_tf.cpp @@ -0,0 +1,107 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +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 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("world_frame_id","/world"); + this->get_parameter("world_frame_id", world_frame_id); + + this->declare_parameter("imu_frame_id","/imu"); + this->get_parameter("imu_frame_id", imu_frame_id); + + this->declare_parameter("imu_topic","/imu"); + this->get_parameter("imu_topic", imu_topic); + + this->declare_parameter("position_x",1); + this->get_parameter("position_x", position_x); + + this->declare_parameter("position_y",1); + this->get_parameter("position_y", position_y); + + this->declare_parameter("position_z",1); + this->get_parameter("position_z", position_z); + br =std::make_shared(this); + sub_ = this->create_subscription(imu_topic.c_str(), 10, std::bind(&imu_data_to_tf::ImuCallback,this,_1)); + + } + private: + rclcpp::Subscription::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(); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/hardware/fdilink_ahrs_ROS2/wheeltec_udev.sh b/src/hardware/fdilink_ahrs_ROS2/wheeltec_udev.sh new file mode 100644 index 0000000..06d9b2b --- /dev/null +++ b/src/hardware/fdilink_ahrs_ROS2/wheeltec_udev.sh @@ -0,0 +1,14 @@ +#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 + + diff --git a/src/hardware/mdog_hardware/CMakeLists.txt b/src/hardware/mdog_hardware/CMakeLists.txt index d5d1337..19448ab 100644 --- a/src/hardware/mdog_hardware/CMakeLists.txt +++ b/src/hardware/mdog_hardware/CMakeLists.txt @@ -49,6 +49,11 @@ install( DESTINATION include ) +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 diff --git a/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp b/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp index b2c57fd..b66f801 100644 --- a/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp +++ b/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" namespace mdog_hardware { @@ -64,6 +66,12 @@ public: private: std::vector hw_commands_; std::vector hw_states_; + std::vector imu_states_; + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr imu_subscription_; + + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); }; } // namespace mdog_hardware diff --git a/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py b/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py new file mode 100644 index 0000000..f82d709 --- /dev/null +++ b/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py @@ -0,0 +1,43 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro # 添加 xacro 模块 + + +def process_xacro(): + # 获取 xacro 文件路径 + pkg_path = os.path.join(get_package_share_directory('mdog_description')) + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + # 处理 xacro 文件 + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() + + +def generate_launch_description(): + # 生成机器人描述 + robot_description = {'robot_description': process_xacro()} + + # 启动机器人状态发布器 + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + # 启动控制器管理器 + controller_manager_node = Node( + package='controller_manager', + executable='ros2_control_node', + output='screen', + parameters=[robot_description], + arguments=['--ros-args', '--log-level', 'info'] + ) + + return LaunchDescription([ + robot_state_publisher_node, + controller_manager_node + ]) \ No newline at end of file diff --git a/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp b/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp index 2929155..d80ada8 100644 --- a/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp +++ b/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp @@ -18,9 +18,27 @@ #include "mdog_hardware/mdog_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" namespace mdog_hardware { + +void MDogHardwareInterface::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + imu_states_[0] = msg->orientation.w; + imu_states_[1] = msg->orientation.x; + imu_states_[2] = msg->orientation.y; + imu_states_[3] = msg->orientation.z; + + imu_states_[4] = msg->angular_velocity.x; + imu_states_[5] = msg->angular_velocity.y; + imu_states_[6] = msg->angular_velocity.z; + + imu_states_[7] = msg->linear_acceleration.x; + imu_states_[8] = msg->linear_acceleration.y; + imu_states_[9] = msg->linear_acceleration.z; +} + hardware_interface::CallbackReturn MDogHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { @@ -29,9 +47,15 @@ hardware_interface::CallbackReturn MDogHardwareInterface::on_init( return CallbackReturn::ERROR; } - // TODO(anyone): read parameters and initialize the hardware - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + // 初始化硬件状态和命令 + hw_states_.resize(info_.joints.size() * 3, std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits::quiet_NaN()); + imu_states_.resize(10, std::numeric_limits::quiet_NaN()); + + // 创建 ROS 2 节点和订阅器 + node_ = std::make_shared("mdog_hardware_interface"); + imu_subscription_ = node_->create_subscription( + "/imu", 10, std::bind(&MDogHardwareInterface::imu_callback, this, std::placeholders::_1)); return CallbackReturn::SUCCESS; } @@ -50,9 +74,34 @@ std::vector MDogHardwareInterface::export_st for (size_t i = 0; i < info_.joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - // TODO(anyone): insert correct interfaces - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i * 3])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i * 3 + 1])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_[i * 3 + 2])); + } + + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.w", &imu_states_[0])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.x", &imu_states_[1])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.y", &imu_states_[2])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.z", &imu_states_[3])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.x", &imu_states_[4])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.y", &imu_states_[5])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.z", &imu_states_[6])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.x", &imu_states_[7])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.y", &imu_states_[8])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.z", &imu_states_[9])); return state_interfaces; } @@ -63,10 +112,16 @@ std::vector MDogHardwareInterface::export_ for (size_t i = 0; i < info_.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - // TODO(anyone): insert correct interfaces - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i * 5])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i * 5 + 1])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i * 5 + 2])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "kp", &hw_commands_[i * 5 + 3])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "kd", &hw_commands_[i * 5 + 4])); } - return command_interfaces; } diff --git a/src/hardware/mdog_hardware_interface/CMakeLists.txt b/src/hardware/mdog_hardware_interface/CMakeLists.txt deleted file mode 100644 index 87ba48d..0000000 --- a/src/hardware/mdog_hardware_interface/CMakeLists.txt +++ /dev/null @@ -1,86 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mdog_hardware_interface) - -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_lifecycle REQUIRED) -find_package(rclcpp REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) - -add_library( - mdog_hardware_interface - SHARED - src/mdog_hardware_interface.cpp -) -target_include_directories( - mdog_hardware_interface - PUBLIC - include -) -ament_target_dependencies( - mdog_hardware_interface - hardware_interface - rclcpp - rclcpp_lifecycle -) -# prevent pluginlib from using boost -target_compile_definitions(mdog_hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file( - hardware_interface mdog_hardware_interface.xml) - -install( - TARGETS - mdog_hardware_interface - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - -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() - find_package(ament_cmake_gmock REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp) - target_include_directories(test_mdog_hardware_interface PRIVATE include) - ament_target_dependencies( - test_mdog_hardware_interface - hardware_interface - pluginlib - ros2_control_test_assets - ) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - mdog_hardware_interface -) -ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle -) - -ament_package() diff --git a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp deleted file mode 100644 index 58b9061..0000000 --- a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ -#define MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ - -#include -#include - -#include "mdog_hardware_interface/visibility_control.h" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace mdog_hardware_interface -{ -class MDogHardwareInterface : public hardware_interface::SystemInterface -{ -public: - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_state_interfaces() override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_command_interfaces() override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -private: - std::vector hw_commands_; - std::vector hw_states_; -}; - -} // namespace mdog_hardware_interface - -#endif // MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h deleted file mode 100644 index 15be442..0000000 --- a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ -#define MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT -#endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL -#endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ diff --git a/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml b/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml deleted file mode 100644 index 48f25f8..0000000 --- a/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - ros2_control hardware interface. - - - diff --git a/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp b/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp deleted file mode 100644 index 89b2b0b..0000000 --- a/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "mdog_hardware_interface/mdog_hardware_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace mdog_hardware_interface -{ -hardware_interface::CallbackReturn MDogHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) -{ - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - // TODO(anyone): read parameters and initialize the hardware - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces - - return CallbackReturn::SUCCESS; -} - -std::vector MDogHardwareInterface::export_state_interfaces() -{ - std::vector state_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - // TODO(anyone): insert correct interfaces - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector MDogHardwareInterface::export_command_interfaces() -{ - std::vector command_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - // TODO(anyone): insert correct interfaces - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - } - - return command_interfaces; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to receive commands - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to stop receiving commands - - return CallbackReturn::SUCCESS; -} - -hardware_interface::return_type MDogHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // TODO(anyone): read robot states - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type MDogHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // TODO(anyone): write robot's commands' - - return hardware_interface::return_type::OK; -} - -} // namespace mdog_hardware_interface - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - mdog_hardware_interface::MDogHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp b/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp deleted file mode 100644 index 7144ae6..0000000 --- a/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include "hardware_interface/resource_manager.hpp" -#include "ros2_control_test_assets/components_urdfs.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -class TestMDogHardwareInterface : public ::testing::Test -{ -protected: - void SetUp() override - { - // TODO(anyone): Extend this description to your robot - mdog_hardware_interface_2dof_ = - R"( - - - mdog_hardware_interface/MDogHardwareInterface - - - - - 1.57 - - - - - 0.7854 - - - )"; - } - - std::string mdog_hardware_interface_2dof_; -}; - -TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof) -{ - auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ + - ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); -} diff --git a/src/hardware/unitree_leg_serial_driver/CMakeLists.txt b/src/hardware/unitree_leg_serial_driver/CMakeLists.txt new file mode 100644 index 0000000..52e408d --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(unitree_leg_serial_driver) + +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(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(serial REQUIRED) +find_package(rosidl_default_generators REQUIRED) # 添加消息生成依赖 + +# 添加头文件目录 +include_directories(include) + +# 添加消息文件 +set(msg_files + msgs/GoMotorFdb.msg + msgs/GoMotorCmd.msg +) + +# 生成消息 +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES std_msgs +) + +# 添加源文件并生成共享库 +add_library(${PROJECT_NAME}_lib SHARED # 修改目标名称，避免冲突 + src/unitree_leg_serial.cpp + src/crc_ccitt.cpp +) + +# 链接依赖库 +ament_target_dependencies(${PROJECT_NAME}_lib + rclcpp + rclcpp_components + std_msgs + serial +) + +# 注册组件 +rclcpp_components_register_nodes(${PROJECT_NAME}_lib "unitree_leg_serial::UnitreeLegSerial") + +# 安装目标文件和头文件 +install(TARGETS ${PROJECT_NAME}_lib + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/launch +) + +ament_package() \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/crc_ccitt.hpp b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/crc_ccitt.hpp new file mode 100644 index 0000000..cdee9a6 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/crc_ccitt.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include + + +namespace crc_ccitt { + +uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c); + +/** + * crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data + * buffer + * @crc: previous CRC value + * @buffer: data pointer + * @len: number of bytes in the buffer + */ +uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len); + +} // namespace crc_ccitt diff --git a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/gom_protocol.hpp b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/gom_protocol.hpp new file mode 100644 index 0000000..f7f9613 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/gom_protocol.hpp @@ -0,0 +1,109 @@ +#pragma once + +#include "crc_ccitt.hpp" + +#pragma pack(1) +/** + * @brief 电机模式控制信息 + * + */ +typedef struct +{ + uint8_t id : 4; // 电机ID: 0,1...,13,14 15表示向所有电机广播数据(此时无返回) + uint8_t status : 3; // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留 + uint8_t reserve : 1; // 保留位 +} RIS_Mode_t; // 控制模式 1Byte + +/** + * @brief 电机状态控制信息 + * + */ +typedef struct +{ + int16_t tor_des; // 期望关节输出扭矩 unit: N.m (q8) + int16_t spd_des; // 期望关节输出速度 unit: rad/s (q8) + int32_t pos_des; // 期望关节输出位置 unit: rad (q15) + int16_t k_pos; // 期望关节刚度系数 unit: -1.0-1.0 (q15) + int16_t k_spd; // 期望关节阻尼系数 unit: -1.0-1.0 (q15) + +} RIS_Comd_t; // 控制参数 12Byte + +/** + * @brief 电机状态反馈信息 + * + */ +typedef struct +{ + int16_t torque; // 实际关节输出扭矩 unit: N.m (q8) + int16_t speed; // 实际关节输出速度 unit: rad/s (q8) + int32_t pos; // 实际关节输出位置 unit: rad (q15) + int8_t temp; // 电机温度: -128~127°C + uint8_t MError : 3; // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留 + uint16_t force : 12; // 足端气压传感器数据 12bit (0-4095) + uint8_t none : 1; // 保留位 +} RIS_Fbk_t; // 状态数据 11Byte + +/** + * @brief 控制数据包格式 + * + */ +typedef struct +{ + uint8_t head[2]; // 包头 2Byte + RIS_Mode_t mode; // 电机控制模式 1Byte + RIS_Comd_t comd; // 电机期望数据 12Byte + uint16_t CRC16; // CRC 2Byte + +} RIS_ControlData_t; // 主机控制命令 17Byte + +/** + * @brief 电机反馈数据包格式 + * + */ +typedef struct +{ + uint8_t head[2]; // 包头 2Byte + RIS_Mode_t mode; // 电机控制模式 1Byte + RIS_Fbk_t fbk; // 电机反馈数据 11Byte + uint16_t CRC16; // CRC 2Byte + +} RIS_MotorData_t; // 电机返回数据 16Byte + +#pragma pack() + +/// @brief 电机指令结构体 +typedef struct +{ + unsigned short id; // 电机ID，15代表广播数据包 + unsigned short mode; // 0:空闲 1:FOC控制 2:电机标定 + float T; // 期望关节的输出力矩(电机本身的力矩)(Nm) + float W; // 期望关节速度(电机本身的速度)(rad/s) + float Pos; // 期望关节位置(rad) + float K_P; // 关节刚度系数(0-25.599) + float K_W; // 关节速度系数(0-25.599) + + RIS_ControlData_t motor_send_data; + +} MotorCmd_t; + +/// @brief 电机反馈结构体 +typedef struct +{ + unsigned char motor_id; // 电机ID + unsigned char mode; // 0:空闲 1:FOC控制 2:电机标定 + int Temp; // 温度 + int MError; // 错误码 + float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm) + float W; // 当前实际电机速度(电机本身的速度)(rad/s) + float Pos; // 当前电机位置(rad) + int correct; // 接收数据是否完整(1完整，0不完整) + int footForce; // 足端力传感器原始数值 + + uint16_t calc_crc; + uint32_t timeout; // 通讯超时 数量 + uint32_t bad_msg; // CRC校验错误 数量 + RIS_MotorData_t motor_recv_data; // 电机接收数据结构体 + +} MotorData_t; + +#pragma pack() diff --git a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp new file mode 100644 index 0000000..a2b4b0b --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "unitree_leg_serial_driver/crc_ccitt.hpp" +#include "unitree_leg_serial_driver/gom_protocol.hpp" + +namespace unitree_leg_serial +{ + +class UnitreeLegSerial : public rclcpp::Node +{ +public: + explicit UnitreeLegSerial(const rclcpp::NodeOptions &options); + ~UnitreeLegSerial() override; + +private: + void receive_data(); + void reopen_port(); + void send_motor_data(const MotorCmd_t &cmd); + void motor_update(); + void motor_cmd_reset(); + bool open_serial_port(); + void close_serial_port(); + + std::unique_ptr serial_port_; + rclcpp::TimerBase::SharedPtr timer_; + std::thread read_thread_; + std::atomic running_{false}; + + // 状态管理 + enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; + std::atomic status_flag_{OFFLINE}; + std::atomic tick_{0}; + + // 串口参数 + std::string serial_port_name_; + int baud_rate_; + + // 电机数据 + MotorCmd_t motor_cmd_; + MotorData_t motor_fbk_; +}; + +} // namespace unitree_leg_serial \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py b/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py new file mode 100644 index 0000000..241d684 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return LaunchDescription([ + ComposableNodeContainer( + name="component_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="unitree_leg_serial_driver", + plugin="unitree_leg_serial::UnitreeLegSerial", + name="unitree_leg_serial" + ) + ], + output="screen" + ) + ]) \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/msgs/GoMotorCmd.msg b/src/hardware/unitree_leg_serial_driver/msgs/GoMotorCmd.msg new file mode 100644 index 0000000..af779a1 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/msgs/GoMotorCmd.msg @@ -0,0 +1,5 @@ +float32 torque_des +float32 speed_des +float32 pos_des +float32 kp +float32 kd \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/msgs/GoMotorFdb.msg b/src/hardware/unitree_leg_serial_driver/msgs/GoMotorFdb.msg new file mode 100644 index 0000000..2d44cd6 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/msgs/GoMotorFdb.msg @@ -0,0 +1,3 @@ +float32 torque +float32 speed +float32 pos \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/package.xml b/src/hardware/unitree_leg_serial_driver/package.xml new file mode 100644 index 0000000..8595382 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/package.xml @@ -0,0 +1,34 @@ + + + + unitree_leg_serial_driver + 0.0.0 + Unitree Leg Serial Driver for ROS 2 + robofish + Apache-2.0 + + + ament_cmake + rosidl_default_generators + rclcpp + rclcpp_components + std_msgs + serial + + + rclcpp + rclcpp_components + std_msgs + serial + + + ament_lint_auto + ament_lint_common + + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp b/src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp new file mode 100644 index 0000000..22ea09c --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp @@ -0,0 +1,68 @@ +#include "unitree_leg_serial_driver/crc_ccitt.hpp" + +namespace crc_ccitt +{ + +// CRC-CCITT 预计算表 +const uint16_t crc_ccitt_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + +/** + * @brief 计算单字节的 CRC-CCITT 值 + * @param crc 当前的 CRC 值 + * @param c 输入字节 + * @return 更新后的 CRC 值 + */ + uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c) + { + return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff]; + } + +/** + * @brief 计算数据缓冲区的 CRC-CCITT 值 + * @param crc 初始 CRC 值 + * @param buffer 数据缓冲区指针 + * @param len 缓冲区长度 + * @return 计算后的 CRC 值 + */ + uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len) + { + while (len--) + crc = crc_ccitt_byte(crc, *buffer++); + return crc; + } + + +} // namespace crc_ccitt \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp new file mode 100644 index 0000000..a492324 --- /dev/null +++ b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp @@ -0,0 +1,185 @@ +#include "unitree_leg_serial_driver/unitree_leg_serial.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace unitree_leg_serial +{ + +UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) + : Node("unitree_leg_serial", options) +{ + serial_port_name_ = "/dev/ttyACM3"; + baud_rate_ = 4000000; + + if (!open_serial_port()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str()); + return; + } + + running_ = true; + status_flag_ = OFFLINE; + tick_ = 0; + read_thread_ = std::thread(&UnitreeLegSerial::receive_data, this); + + timer_ = this->create_wall_timer( + std::chrono::microseconds(500), + [this]() { motor_update(); }); +} + +UnitreeLegSerial::~UnitreeLegSerial() +{ + running_ = false; + if (read_thread_.joinable()) { + read_thread_.join(); + } + close_serial_port(); +} + +bool UnitreeLegSerial::open_serial_port() +{ + try { + serial_port_ = std::make_unique(serial_port_name_, baud_rate_, serial::Timeout::simpleTimeout(1000)); + return serial_port_->isOpen(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Serial open exception: %s", e.what()); + return false; + } +} + +void UnitreeLegSerial::close_serial_port() +{ + if (serial_port_ && serial_port_->isOpen()) { + serial_port_->close(); + } +} + +void UnitreeLegSerial::motor_update() +{ + if (tick_ < 10) { + ++tick_; + } else { + status_flag_ = OFFLINE; + } + + if (status_flag_ != CONTROLED) { + motor_cmd_reset(); + } + send_motor_data(motor_cmd_); +} + +void UnitreeLegSerial::motor_cmd_reset() +{ + motor_cmd_ = MotorCmd_t{}; + motor_cmd_.id = 2; + motor_cmd_.mode = 1; +} + +void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd) +{ + if (!serial_port_ || !serial_port_->isOpen()) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Serial port is not open."); + return; + } + + auto &out = motor_cmd_.motor_send_data; + out.head[0] = 0xFE; + out.head[1] = 0xEE; + + auto saturate = [](auto &val, auto min, auto max) { + if (val < min) val = min; + else if (val > max) val = max; + }; + + MotorCmd_t cmd_copy = cmd; + saturate(cmd_copy.id, 0, 15); + saturate(cmd_copy.mode, 0, 7); + saturate(cmd_copy.K_P, 0.0f, 25.599f); + saturate(cmd_copy.K_W, 0.0f, 25.599f); + saturate(cmd_copy.T, -127.99f, 127.99f); + saturate(cmd_copy.W, -804.00f, 804.00f); + saturate(cmd_copy.Pos, -411774.0f, 411774.0f); + + out.mode.id = cmd_copy.id; + out.mode.status = cmd_copy.mode; + out.comd.k_pos = cmd_copy.K_P / 25.6f * 32768.0f; + out.comd.k_spd = cmd_copy.K_W / 25.6f * 32768.0f; + out.comd.pos_des = cmd_copy.Pos / 6.28318f * 32768.0f; + out.comd.spd_des = cmd_copy.W / 6.28318f * 256.0f; + out.comd.tor_des = cmd_copy.T * 256.0f; + out.CRC16 = crc_ccitt::crc_ccitt( + 0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16)); + + serial_port_->write(reinterpret_cast(&out), sizeof(RIS_ControlData_t)); +} + +void UnitreeLegSerial::receive_data() +{ + size_t packet_size = sizeof(RIS_MotorData_t); + std::vector buffer(packet_size * 2); + size_t buffer_offset = 0; + + while (running_) { + try { + size_t bytes_read = serial_port_->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset); + if (bytes_read == 0) continue; + buffer_offset += bytes_read; + + size_t header_index = 0; + while (header_index < buffer_offset - 1) { + if (buffer[header_index] == 0xFD && buffer[header_index + 1] == 0xEE) break; + ++header_index; + } + if (header_index >= buffer_offset - 1) { + buffer_offset = 0; + continue; + } + if (header_index > 0) { + std::memmove(buffer.data(), buffer.data() + header_index, buffer_offset - header_index); + buffer_offset -= header_index; + } + if (buffer_offset < packet_size) continue; + + std::memcpy(&motor_fbk_.motor_recv_data, buffer.data(), packet_size); + + if (motor_fbk_.motor_recv_data.head[0] != 0xFD || motor_fbk_.motor_recv_data.head[1] != 0xEE) { + motor_fbk_.correct = 0; + } else { + motor_fbk_.calc_crc = crc_ccitt::crc_ccitt( + 0, (uint8_t *)&motor_fbk_.motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_fbk_.motor_recv_data.CRC16)); + if (motor_fbk_.motor_recv_data.CRC16 != motor_fbk_.calc_crc) { + memset(&motor_fbk_.motor_recv_data, 0, sizeof(RIS_MotorData_t)); + motor_fbk_.correct = 0; + motor_fbk_.bad_msg++; + } else { + motor_fbk_.motor_id = motor_fbk_.motor_recv_data.mode.id; + motor_fbk_.mode = motor_fbk_.motor_recv_data.mode.status; + motor_fbk_.Temp = motor_fbk_.motor_recv_data.fbk.temp; + motor_fbk_.MError = motor_fbk_.motor_recv_data.fbk.MError; + motor_fbk_.W = ((float)motor_fbk_.motor_recv_data.fbk.speed / 256.0f) * 6.28318f; + motor_fbk_.T = ((float)motor_fbk_.motor_recv_data.fbk.torque) / 256.0f; + motor_fbk_.Pos = 6.28318f * ((float)motor_fbk_.motor_recv_data.fbk.pos) / 32768.0f; + motor_fbk_.footForce = motor_fbk_.motor_recv_data.fbk.force; + motor_fbk_.correct = 1; + } + } + if (motor_fbk_.correct) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Motor ID: %d, Position: %f", motor_fbk_.motor_id, motor_fbk_.Pos); + } + std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size); + buffer_offset -= packet_size; + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Serial read exception: %s", e.what()); + reopen_port(); + } + } +} + +void UnitreeLegSerial::reopen_port() +{ + close_serial_port(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + open_serial_port(); +} + +} // namespace unitree_leg_serial + +RCLCPP_COMPONENTS_REGISTER_NODE(unitree_leg_serial::UnitreeLegSerial) \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/CMakeLists.txt b/src/robot_descriptions/qut/mdog_description/CMakeLists.txt new file mode 100644 index 0000000..9a6750a --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) +project(mdog_description) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY meshes xacro launch config urdf + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/config/robot_control.yaml b/src/robot_descriptions/qut/mdog_description/config/robot_control.yaml new file mode 100644 index 0000000..168156e --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/config/robot_control.yaml @@ -0,0 +1,63 @@ +# Controller Manager configuration +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + # Define the available controllers + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + + mdog_simpal_controller: + type: mdog_simpal_controller/MGogSimpalController + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: "imu_sensor" + frame_id: "imu_link" + +mdog_simpal_controller: + ros__parameters: + update_rate: 500 # Hz + joints: + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + imu_name: "imu_sensor" + base_name: "base" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z diff --git a/src/robot_descriptions/qut/mdog_description/config/visualize_urdf.rviz b/src/robot_descriptions/qut/mdog_description/config/visualize_urdf.rviz new file mode 100644 index 0000000..1317835 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/config/visualize_urdf.rviz @@ -0,0 +1,383 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 303 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_calf_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_calf_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh_rotor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_chin: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_face: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_laserscan_link_left: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_laserscan_link_right: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_optical_chin: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_face: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_left: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_rearDown: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_optical_right: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_rearDown: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + trunk: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ultraSound_face: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ultraSound_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ultraSound_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.8687032461166382 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.017344314604997635 + Y: -0.0033522010780870914 + Z: -0.09058035165071487 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.49539798498153687 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8403961062431335 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 630 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003130000005efc0100000002fb0000000800540069006d0065010000000000000313000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000313000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 787 + X: 763 + Y: 351 diff --git a/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py b/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py new file mode 100644 index 0000000..5780e6d --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/launch/visualize.launch.py @@ -0,0 +1,49 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + +package_description = "mdog_description" + +def process_xacro(): + pkg_path = os.path.join(get_package_share_directory(package_description)) + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + robot_description = process_xacro() + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + { + 'publish_frequency': 10.0, + 'use_tf_static': True, + 'robot_description': robot_description + } + ], + ), + Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher', + output='screen', + ) + ]) \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/package.xml b/src/robot_descriptions/qut/mdog_description/package.xml new file mode 100644 index 0000000..f49f9d8 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/package.xml @@ -0,0 +1,18 @@ + + + + mdog_description + 0.0.0 + TODO: Package description + robofish + TODO: License declaration + + xacro + joint_state_publisher + robot_state_publisher + imu_sensor_broadcaster + + + ament_cmake + + diff --git a/src/robot_descriptions/qut/mdog_description/robot.urdf b/src/robot_descriptions/qut/mdog_description/robot.urdf new file mode 100644 index 0000000..b8ff698 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/robot.urdf @@ -0,0 +1,555 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + hardware_mdog/hardware_mdog_real + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf b/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf new file mode 100644 index 0000000..5d72935 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf @@ -0,0 +1,555 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + hardware_mdog/hardware_mdog_real + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_descriptions/qut/mdog_description/xacro/const.xacro b/src/robot_descriptions/qut/mdog_description/xacro/const.xacro new file mode 100644 index 0000000..5fc5410 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/xacro/const.xacro @@ -0,0 +1,159 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro new file mode 100644 index 0000000..47761e2 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/xacro/materials.xacro b/src/robot_descriptions/qut/mdog_description/xacro/materials.xacro new file mode 100644 index 0000000..36e08d5 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/xacro/materials.xacro @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_descriptions/qut/mdog_description/xacro/robot.xacro b/src/robot_descriptions/qut/mdog_description/xacro/robot.xacro new file mode 100644 index 0000000..63af0d1 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/xacro/robot.xacro @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro b/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro new file mode 100644 index 0000000..e21aa01 --- /dev/null +++ b/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro @@ -0,0 +1,169 @@ + + + + + + + mdog_hardware/mdog_hardware_interface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +