创建了单电机驱动
This commit is contained in:
parent
30d3d2fb3a
commit
bca5ab3a3b
69
.vscode/settings.json
vendored
Normal file
69
.vscode/settings.json
vendored
Normal file
@ -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"
|
||||
}
|
||||
}
|
||||
80
src/hardware/fdilink_ahrs_ROS2/CMakeLists.txt
Normal file
80
src/hardware/fdilink_ahrs_ROS2/CMakeLists.txt
Normal file
@ -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()
|
||||
BIN
src/hardware/fdilink_ahrs_ROS2/data/gps_data.bag
Normal file
BIN
src/hardware/fdilink_ahrs_ROS2/data/gps_data.bag
Normal file
Binary file not shown.
111
src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h
Normal file
111
src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h
Normal file
@ -0,0 +1,111 @@
|
||||
#ifndef BASE_DRIVER_H_
|
||||
#define BASE_DRIVER_H_
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <memory>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
|
||||
#include <math.h>
|
||||
#include <fstream>
|
||||
#include <fdilink_data_struct.h>
|
||||
//#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||
#include <geometry_msgs/msg/pose2_d.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
|
||||
#include <string>
|
||||
//#include <ros/package.h>
|
||||
#include <ament_index_cpp/get_package_prefix.hpp>
|
||||
#include <crc_table.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/Core>
|
||||
//#include <boost/thread.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
namespace FDILink
|
||||
{
|
||||
#define FRAME_HEAD 0xfc
|
||||
#define FRAME_END 0xfd
|
||||
#define TYPE_IMU 0x40
|
||||
#define TYPE_AHRS 0x41
|
||||
#define TYPE_INSGPS 0x42
|
||||
#define TYPE_GEODETIC_POS 0x5c
|
||||
#define TYPE_GROUND 0xf0
|
||||
|
||||
#define IMU_LEN 0x38 //56
|
||||
#define AHRS_LEN 0x30 //48
|
||||
#define INSGPS_LEN 0x48 //80
|
||||
#define GEODETIC_POS_LEN 0x20 //32
|
||||
#define PI 3.141592653589793
|
||||
#define DEG_TO_RAD 0.017453292519943295
|
||||
|
||||
|
||||
class ahrsBringup : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ahrsBringup();
|
||||
~ahrsBringup();
|
||||
void processLoop();
|
||||
bool checkCS8(int len);
|
||||
bool checkCS16(int len);
|
||||
void checkSN(int type);
|
||||
void magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz);
|
||||
|
||||
private:
|
||||
bool if_debug_;
|
||||
//sum info
|
||||
int sn_lost_ = 0;
|
||||
int crc_error_ = 0;
|
||||
uint8_t read_sn_ = 0;
|
||||
bool frist_sn_;
|
||||
int device_type_ = 1;
|
||||
|
||||
//serial
|
||||
serial::Serial serial_; //声明串口对象
|
||||
std::string serial_port_;
|
||||
int serial_baud_;
|
||||
int serial_timeout_;
|
||||
//data
|
||||
FDILink::imu_frame_read imu_frame_;
|
||||
FDILink::ahrs_frame_read ahrs_frame_;
|
||||
FDILink::insgps_frame_read insgps_frame_;
|
||||
//FDILink::lanlon_frame_read latlon_frame_;
|
||||
FDILink::Geodetic_Position_frame_read Geodetic_Position_frame_;
|
||||
//frame name
|
||||
//std::string imu_frame_id="gyro_link";
|
||||
std::string imu_frame_id_;
|
||||
std::string insgps_frame_id_;
|
||||
std::string latlon_frame_id_;
|
||||
//topic
|
||||
std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
|
||||
std::string latlon_topic="latlon";
|
||||
std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
|
||||
std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
|
||||
|
||||
|
||||
//Publisher
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr mag_pose_pub_;
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr NED_odom_pub_;
|
||||
|
||||
|
||||
|
||||
}; //ahrsBringup
|
||||
} // namespace FDILink
|
||||
|
||||
#endif
|
||||
10
src/hardware/fdilink_ahrs_ROS2/include/crc_table.h
Normal file
10
src/hardware/fdilink_ahrs_ROS2/include/crc_table.h
Normal file
@ -0,0 +1,10 @@
|
||||
#ifndef CRC_TABLE_H
|
||||
#define CRC_TABLE_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
uint8_t CRC8_Table(uint8_t* p, uint8_t counter);
|
||||
uint16_t CRC16_Table(uint8_t *p, uint8_t counter);
|
||||
uint32_t CRC32_Table(uint8_t *p, uint8_t counter);
|
||||
|
||||
#endif // CRC_TABLE_H
|
||||
189
src/hardware/fdilink_ahrs_ROS2/include/fdilink_data_struct.h
Normal file
189
src/hardware/fdilink_ahrs_ROS2/include/fdilink_data_struct.h
Normal file
@ -0,0 +1,189 @@
|
||||
#ifndef FDILINK_DATA_STRUCT_H_
|
||||
#define FDILINK_DATA_STRUCT_H_
|
||||
|
||||
#include <iostream>
|
||||
namespace FDILink{
|
||||
#pragma pack(1)
|
||||
struct fdilink_header
|
||||
{
|
||||
uint8_t header_start;
|
||||
uint8_t data_type;
|
||||
uint8_t data_size;
|
||||
uint8_t serial_num;
|
||||
uint8_t header_crc8;
|
||||
uint8_t header_crc16_h;
|
||||
uint8_t header_crc16_l;
|
||||
};
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
struct IMUData_Packet_t
|
||||
{
|
||||
float gyroscope_x; //unit: rad/s
|
||||
float gyroscope_y; //unit: rad/s
|
||||
float gyroscope_z; //unit: rad/s
|
||||
float accelerometer_x; //m/s^2
|
||||
float accelerometer_y; //m/s^2
|
||||
float accelerometer_z; //m/s^2
|
||||
float magnetometer_x; //mG
|
||||
float magnetometer_y; //mG
|
||||
float magnetometer_z; //mG
|
||||
float imu_temperature; //C
|
||||
float Pressure; //Pa
|
||||
float pressure_temperature; //C
|
||||
int64_t Timestamp; //us
|
||||
};
|
||||
#pragma pack()
|
||||
|
||||
struct AHRSData_Packet_t
|
||||
{
|
||||
float RollSpeed; //unit: rad/s
|
||||
float PitchSpeed; //unit: rad/s
|
||||
float HeadingSpeed;//unit: rad/s
|
||||
float Roll; //unit: rad
|
||||
float Pitch; //unit: rad
|
||||
float Heading; //unit: rad
|
||||
float Qw;//w //Quaternion
|
||||
float Qx;//x
|
||||
float Qy;//y
|
||||
float Qz;//z
|
||||
int64_t Timestamp; //unit: us
|
||||
};
|
||||
#pragma pack(1)
|
||||
struct INSGPSData_Packet_t
|
||||
{
|
||||
float BodyVelocity_X;
|
||||
float BodyVelocity_Y;
|
||||
float BodyVelocity_Z;
|
||||
float BodyAcceleration_X;
|
||||
float BodyAcceleration_Y;
|
||||
float BodyAcceleration_Z;
|
||||
float Location_North;
|
||||
float Location_East;
|
||||
float Location_Down;
|
||||
float Velocity_North;
|
||||
float Velocity_East;
|
||||
float Velocity_Down;
|
||||
float Acceleration_North;
|
||||
float Acceleration_East;
|
||||
float Acceleration_Down;
|
||||
float Pressure_Altitude;
|
||||
int64_t Timestamp;
|
||||
};
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
struct Geodetic_Position_Packet_t
|
||||
{
|
||||
double Latitude;
|
||||
double Longitude;
|
||||
double Height;
|
||||
float hAcc;
|
||||
float vAcc;
|
||||
};
|
||||
#pragma pack()
|
||||
|
||||
//for IMU=========================
|
||||
#pragma pack(1)
|
||||
struct read_imu_struct{
|
||||
fdilink_header header; //7
|
||||
union data
|
||||
{
|
||||
IMUData_Packet_t data_pack; //56
|
||||
uint8_t data_buff[56]; //56
|
||||
}data;
|
||||
uint8_t frame_end; //1
|
||||
};
|
||||
|
||||
struct read_imu_tmp{
|
||||
uint8_t frame_header[7];
|
||||
uint8_t read_msg[57];
|
||||
};
|
||||
|
||||
union imu_frame_read{
|
||||
struct read_imu_struct frame;
|
||||
read_imu_tmp read_buf;
|
||||
uint8_t read_tmp[64];
|
||||
};
|
||||
#pragma pack()
|
||||
//for IMU------------------------
|
||||
|
||||
//for AHRS=========================
|
||||
#pragma pack(1)
|
||||
struct read_ahrs_struct{
|
||||
fdilink_header header; //7
|
||||
union data
|
||||
{
|
||||
AHRSData_Packet_t data_pack; //48
|
||||
uint8_t data_buff[48]; //48
|
||||
}data;
|
||||
uint8_t frame_end; //1
|
||||
};
|
||||
|
||||
|
||||
struct read_ahrs_tmp{
|
||||
uint8_t frame_header[7];
|
||||
uint8_t read_msg[49];
|
||||
};
|
||||
|
||||
union ahrs_frame_read{
|
||||
struct read_ahrs_struct frame;
|
||||
read_ahrs_tmp read_buf;
|
||||
uint8_t read_tmp[56];
|
||||
};
|
||||
#pragma pack()
|
||||
//for AHRS------------------------
|
||||
|
||||
//for INSGPS=========================
|
||||
#pragma pack(1)
|
||||
struct read_insgps_struct{
|
||||
fdilink_header header; //7
|
||||
union data
|
||||
{
|
||||
INSGPSData_Packet_t data_pack; //72
|
||||
uint8_t data_buff[72]; //72
|
||||
}data;
|
||||
uint8_t frame_end; //1
|
||||
};
|
||||
|
||||
|
||||
struct read_insgps_tmp{
|
||||
uint8_t frame_header[7];
|
||||
uint8_t read_msg[73];
|
||||
};
|
||||
|
||||
union insgps_frame_read{
|
||||
struct read_insgps_struct frame;
|
||||
read_insgps_tmp read_buf;
|
||||
uint8_t read_tmp[80];
|
||||
};
|
||||
#pragma pack()
|
||||
//for INSGPS------------------------
|
||||
|
||||
//for Geodetic_Position=========================
|
||||
#pragma pack(1)
|
||||
struct read_Geodetic_Position_struct{
|
||||
fdilink_header header; //7
|
||||
union data
|
||||
{
|
||||
Geodetic_Position_Packet_t data_pack; //40
|
||||
uint8_t data_buff[32]; //40
|
||||
}data;
|
||||
uint8_t frame_end; //1
|
||||
};
|
||||
|
||||
struct read_Geodetic_Position_tmp{
|
||||
uint8_t frame_header[7];
|
||||
uint8_t read_msg[33];
|
||||
};
|
||||
|
||||
union Geodetic_Position_frame_read{
|
||||
struct read_Geodetic_Position_struct frame;
|
||||
read_Geodetic_Position_tmp read_buf;
|
||||
uint8_t read_tmp[40];
|
||||
};
|
||||
|
||||
#pragma pack()
|
||||
|
||||
}//namespace FDILink
|
||||
#endif//FDILINK_DATA_STRUCT_H_
|
||||
37
src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py
Normal file
37
src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py
Normal file
@ -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
|
||||
18
src/hardware/fdilink_ahrs_ROS2/launch/imu_tf.launch.py
Normal file
18
src/hardware/fdilink_ahrs_ROS2/launch/imu_tf.launch.py
Normal file
@ -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
|
||||
@ -1,24 +1,25 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mdog_hardware_interface</name>
|
||||
<name>fdilink_ahrs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<maintainer email="noah@todo.todo">noah</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gmock</test_depend>
|
||||
<test_depend>ros2_control_test_assets</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
675
src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp
Normal file
675
src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp
Normal file
@ -0,0 +1,675 @@
|
||||
#include <ahrs_driver.h>
|
||||
//#include <Eigen/Eigen>
|
||||
rclcpp::Node::SharedPtr nh_=nullptr;
|
||||
|
||||
|
||||
namespace FDILink
|
||||
{
|
||||
ahrsBringup::ahrsBringup()
|
||||
: rclcpp::Node ("ahrs_bringup")
|
||||
{
|
||||
//topic_name & frame_id 加载参数服务器
|
||||
this->declare_parameter("if_debug_",false);
|
||||
this->get_parameter("if_debug_", if_debug_);
|
||||
|
||||
this->declare_parameter<std::int8_t>("device_type_",1);
|
||||
this->get_parameter("device_type_", device_type_);
|
||||
|
||||
this->declare_parameter<std::string>("imu_topic","/imu");
|
||||
this->get_parameter("imu_topic", imu_topic);
|
||||
|
||||
this->declare_parameter<std::string>("imu_frame_id_","gyro_link");
|
||||
this->get_parameter("imu_frame_id_", imu_frame_id_);
|
||||
|
||||
this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
|
||||
this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
|
||||
|
||||
this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
|
||||
this->get_parameter("Euler_angles_topic", Euler_angles_topic);
|
||||
|
||||
this->declare_parameter<std::string>("gps_topic","/gps/fix");
|
||||
this->get_parameter("gps_topic", gps_topic);
|
||||
|
||||
|
||||
this->declare_parameter<std::string>("Magnetic_topic","/magnetic");
|
||||
this->get_parameter("Magnetic_topic", Magnetic_topic);
|
||||
|
||||
this->declare_parameter<std::string>("twist_topic","/system_speed");
|
||||
this->get_parameter("twist_topic", twist_topic);
|
||||
|
||||
this->declare_parameter<std::string>("NED_odom_topic","/NED_odometry");
|
||||
this->get_parameter("NED_odom_topic", NED_odom_topic);
|
||||
|
||||
this->declare_parameter<std::string>("serial_port_","/dev/fdilink_ahrs");
|
||||
this->get_parameter("serial_port_", serial_port_);
|
||||
|
||||
this->declare_parameter<std::int64_t>("serial_baud_",921600);
|
||||
this->get_parameter("serial_baud_", serial_baud_);
|
||||
|
||||
//pravite_nh.param("debug", if_debug_, false);
|
||||
//pravite_nh.param("device_type", device_type_, 1); // default: single imu
|
||||
// pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
|
||||
// pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
|
||||
// pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
|
||||
// pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
|
||||
// pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
|
||||
|
||||
//serial
|
||||
//pravite_nh.param("port", serial_port_, std::string("/dev/ttyTHS1"));
|
||||
//pravite_nh.param("baud", serial_baud_, 115200);
|
||||
|
||||
//publisher
|
||||
imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10);
|
||||
gps_pub_ = create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic.c_str(), 10);
|
||||
|
||||
mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
|
||||
|
||||
Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
|
||||
Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
|
||||
|
||||
twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
|
||||
NED_odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(NED_odom_topic.c_str(), 10);
|
||||
|
||||
|
||||
//setp up serial 设置串口参数并打开串口
|
||||
try
|
||||
{
|
||||
serial_.setPort(serial_port_);
|
||||
serial_.setBaudrate(serial_baud_);
|
||||
serial_.setFlowcontrol(serial::flowcontrol_none);
|
||||
serial_.setParity(serial::parity_none); //default is parity_none
|
||||
serial_.setStopbits(serial::stopbits_one);
|
||||
serial_.setBytesize(serial::eightbits);
|
||||
serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
|
||||
serial_.setTimeout(time_out);
|
||||
serial_.open();
|
||||
}
|
||||
catch (serial::IOException &e) // 抓取异常
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(),"Unable to open port ");
|
||||
exit(0);
|
||||
}
|
||||
if (serial_.isOpen())
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(),"Serial Port initialized");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(),"Unable to initial Serial port ");
|
||||
exit(0);
|
||||
}
|
||||
processLoop();
|
||||
}
|
||||
|
||||
ahrsBringup::~ahrsBringup() // 析构函数关闭串口通道
|
||||
{
|
||||
if (serial_.isOpen())
|
||||
serial_.close();
|
||||
}
|
||||
|
||||
void ahrsBringup::processLoop() // 数据处理过程
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(),"ahrsBringup::processLoop: start");
|
||||
while (rclcpp::ok())
|
||||
{
|
||||
if (!serial_.isOpen())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"serial unopen");
|
||||
}
|
||||
//check head start 检查起始 数据帧头
|
||||
uint8_t check_head[1] = {0xff};
|
||||
size_t head_s = serial_.read(check_head, 1);
|
||||
if (if_debug_){
|
||||
if (head_s != 1)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(),"Read serial port time out! can't read pack head.");
|
||||
}
|
||||
std::cout << std::endl;
|
||||
std::cout << "check_head: " << std::hex << (int)check_head[0] << std::dec << std::endl;
|
||||
}
|
||||
if (check_head[0] != FRAME_HEAD)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//check head type 检查数据类型
|
||||
uint8_t head_type[1] = {0xff};
|
||||
size_t type_s = serial_.read(head_type, 1);
|
||||
if (if_debug_){
|
||||
std::cout << "head_type: " << std::hex << (int)head_type[0] << std::dec << std::endl;
|
||||
}
|
||||
if (head_type[0] != TYPE_IMU && head_type[0] != TYPE_AHRS && head_type[0] != TYPE_INSGPS && head_type[0] != TYPE_GEODETIC_POS && head_type[0] != 0x50 && head_type[0] != TYPE_GROUND&& head_type[0] != 0xff)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"head_type error: %02X",head_type[0]);
|
||||
continue;
|
||||
}
|
||||
//check head length 检查对应数据类型的长度是否符合
|
||||
uint8_t check_len[1] = {0xff};
|
||||
size_t len_s = serial_.read(check_len, 1);
|
||||
if (if_debug_){
|
||||
std::cout << "check_len: "<< std::dec << (int)check_len[0] << std::endl;
|
||||
}
|
||||
if (head_type[0] == TYPE_IMU && check_len[0] != IMU_LEN)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"head_len error (imu)");
|
||||
continue;
|
||||
}else if (head_type[0] == TYPE_AHRS && check_len[0] != AHRS_LEN)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"head_len error (ahrs)");
|
||||
continue;
|
||||
}else if (head_type[0] == TYPE_INSGPS && check_len[0] != INSGPS_LEN)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"head_len error (insgps)");
|
||||
continue;
|
||||
}else if (head_type[0] == TYPE_GEODETIC_POS && check_len[0] != GEODETIC_POS_LEN)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"head_len error (GEODETIC_POS)");
|
||||
continue;
|
||||
}
|
||||
else if (head_type[0] == TYPE_GROUND || head_type[0] == 0x50) // 未知数据，防止记录失败
|
||||
{
|
||||
uint8_t ground_sn[1];
|
||||
size_t ground_sn_s = serial_.read(ground_sn, 1);
|
||||
if (++read_sn_ != ground_sn[0])
|
||||
{
|
||||
if ( ground_sn[0] < read_sn_)
|
||||
{
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
sn_lost_ += 256 - (int)(read_sn_ - ground_sn[0]);
|
||||
read_sn_ = ground_sn[0];
|
||||
// continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
sn_lost_ += (int)(ground_sn[0] - read_sn_);
|
||||
read_sn_ = ground_sn[0];
|
||||
// continue;
|
||||
}
|
||||
}
|
||||
uint8_t ground_ignore[500];
|
||||
size_t ground_ignore_s = serial_.read(ground_ignore, (check_len[0]+4));
|
||||
continue;
|
||||
}
|
||||
//read head sn
|
||||
uint8_t check_sn[1] = {0xff};
|
||||
size_t sn_s = serial_.read(check_sn, 1);
|
||||
uint8_t head_crc8[1] = {0xff};
|
||||
size_t crc8_s = serial_.read(head_crc8, 1);
|
||||
uint8_t head_crc16_H[1] = {0xff};
|
||||
uint8_t head_crc16_L[1] = {0xff};
|
||||
size_t crc16_H_s = serial_.read(head_crc16_H, 1);
|
||||
size_t crc16_L_s = serial_.read(head_crc16_L, 1);
|
||||
if (if_debug_){
|
||||
std::cout << "check_sn: " << std::hex << (int)check_sn[0] << std::dec << std::endl;
|
||||
std::cout << "head_crc8: " << std::hex << (int)head_crc8[0] << std::dec << std::endl;
|
||||
std::cout << "head_crc16_H: " << std::hex << (int)head_crc16_H[0] << std::dec << std::endl;
|
||||
std::cout << "head_crc16_L: " << std::hex << (int)head_crc16_L[0] << std::dec << std::endl;
|
||||
}
|
||||
// put header & check crc8 & count sn lost
|
||||
// check crc8 进行crc8数据校验
|
||||
if (head_type[0] == TYPE_IMU)
|
||||
{
|
||||
imu_frame_.frame.header.header_start = check_head[0];
|
||||
imu_frame_.frame.header.data_type = head_type[0];
|
||||
imu_frame_.frame.header.data_size = check_len[0];
|
||||
imu_frame_.frame.header.serial_num = check_sn[0];
|
||||
imu_frame_.frame.header.header_crc8 = head_crc8[0];
|
||||
imu_frame_.frame.header.header_crc16_h = head_crc16_H[0];
|
||||
imu_frame_.frame.header.header_crc16_l = head_crc16_L[0];
|
||||
uint8_t CRC8 = CRC8_Table(imu_frame_.read_buf.frame_header, 4);
|
||||
if (CRC8 != imu_frame_.frame.header.header_crc8)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"header_crc8 error");
|
||||
continue;
|
||||
}
|
||||
if(!frist_sn_){
|
||||
read_sn_ = imu_frame_.frame.header.serial_num - 1;
|
||||
frist_sn_ = true;
|
||||
}
|
||||
//check sn
|
||||
ahrsBringup::checkSN(TYPE_IMU);
|
||||
}
|
||||
else if (head_type[0] == TYPE_AHRS)
|
||||
{
|
||||
ahrs_frame_.frame.header.header_start = check_head[0];
|
||||
ahrs_frame_.frame.header.data_type = head_type[0];
|
||||
ahrs_frame_.frame.header.data_size = check_len[0];
|
||||
ahrs_frame_.frame.header.serial_num = check_sn[0];
|
||||
ahrs_frame_.frame.header.header_crc8 = head_crc8[0];
|
||||
ahrs_frame_.frame.header.header_crc16_h = head_crc16_H[0];
|
||||
ahrs_frame_.frame.header.header_crc16_l = head_crc16_L[0];
|
||||
uint8_t CRC8 = CRC8_Table(ahrs_frame_.read_buf.frame_header, 4);
|
||||
if (CRC8 != ahrs_frame_.frame.header.header_crc8)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"header_crc8 error");
|
||||
continue;
|
||||
}
|
||||
if(!frist_sn_){
|
||||
read_sn_ = ahrs_frame_.frame.header.serial_num - 1;
|
||||
frist_sn_ = true;
|
||||
}
|
||||
//check sn
|
||||
ahrsBringup::checkSN(TYPE_AHRS);
|
||||
}
|
||||
else if (head_type[0] == TYPE_INSGPS)
|
||||
{
|
||||
insgps_frame_.frame.header.header_start = check_head[0];
|
||||
insgps_frame_.frame.header.data_type = head_type[0];
|
||||
insgps_frame_.frame.header.data_size = check_len[0];
|
||||
insgps_frame_.frame.header.serial_num = check_sn[0];
|
||||
insgps_frame_.frame.header.header_crc8 = head_crc8[0];
|
||||
insgps_frame_.frame.header.header_crc16_h = head_crc16_H[0];
|
||||
insgps_frame_.frame.header.header_crc16_l = head_crc16_L[0];
|
||||
uint8_t CRC8 = CRC8_Table(insgps_frame_.read_buf.frame_header, 4);
|
||||
if (CRC8 != insgps_frame_.frame.header.header_crc8)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"header_crc8 error");
|
||||
continue;
|
||||
}
|
||||
else if(if_debug_)
|
||||
{
|
||||
std::cout << "header_crc8 matched." << std::endl;
|
||||
}
|
||||
|
||||
ahrsBringup::checkSN(TYPE_INSGPS);
|
||||
}
|
||||
else if (head_type[0] == TYPE_GEODETIC_POS)
|
||||
{
|
||||
Geodetic_Position_frame_.frame.header.header_start = check_head[0];
|
||||
Geodetic_Position_frame_.frame.header.data_type = head_type[0];
|
||||
Geodetic_Position_frame_.frame.header.data_size = check_len[0];
|
||||
Geodetic_Position_frame_.frame.header.serial_num = check_sn[0];
|
||||
Geodetic_Position_frame_.frame.header.header_crc8 = head_crc8[0];
|
||||
Geodetic_Position_frame_.frame.header.header_crc16_h = head_crc16_H[0];
|
||||
Geodetic_Position_frame_.frame.header.header_crc16_l = head_crc16_L[0];
|
||||
uint8_t CRC8 = CRC8_Table(Geodetic_Position_frame_.read_buf.frame_header, 4);
|
||||
if (CRC8 != Geodetic_Position_frame_.frame.header.header_crc8)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"header_crc8 error");
|
||||
continue;
|
||||
}
|
||||
if(!frist_sn_){
|
||||
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num - 1;
|
||||
frist_sn_ = true;
|
||||
}
|
||||
|
||||
ahrsBringup::checkSN(TYPE_GEODETIC_POS);
|
||||
}
|
||||
// check crc16 进行crc16数据校验
|
||||
if (head_type[0] == TYPE_IMU)
|
||||
{
|
||||
uint16_t head_crc16_l = imu_frame_.frame.header.header_crc16_l;
|
||||
uint16_t head_crc16_h = imu_frame_.frame.header.header_crc16_h;
|
||||
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
|
||||
size_t data_s = serial_.read(imu_frame_.read_buf.read_msg, (IMU_LEN + 1)); //48+1
|
||||
// if (if_debug_){
|
||||
// for (size_t i = 0; i < (IMU_LEN + 1); i++)
|
||||
// {
|
||||
// std::cout << std::hex << (int)imu_frame_.read_buf.read_msg[i] << " ";
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
// }
|
||||
uint16_t CRC16 = CRC16_Table(imu_frame_.frame.data.data_buff, IMU_LEN);
|
||||
if (if_debug_)
|
||||
{
|
||||
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
|
||||
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
|
||||
bool if_right = ((int)head_crc16 == (int)CRC16);
|
||||
std::cout << "if_right: " << if_right << std::endl;
|
||||
}
|
||||
|
||||
if (head_crc16 != CRC16)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(imu).");
|
||||
continue;
|
||||
}
|
||||
else if(imu_frame_.frame.frame_end != FRAME_END)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check frame end.");
|
||||
continue;
|
||||
}
|
||||
|
||||
}
|
||||
else if (head_type[0] == TYPE_AHRS)
|
||||
{
|
||||
uint16_t head_crc16_l = ahrs_frame_.frame.header.header_crc16_l;
|
||||
uint16_t head_crc16_h = ahrs_frame_.frame.header.header_crc16_h;
|
||||
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
|
||||
size_t data_s = serial_.read(ahrs_frame_.read_buf.read_msg, (AHRS_LEN + 1)); //48+1
|
||||
// if (if_debug_){
|
||||
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
|
||||
// {
|
||||
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
// }
|
||||
uint16_t CRC16 = CRC16_Table(ahrs_frame_.frame.data.data_buff, AHRS_LEN);
|
||||
if (if_debug_){
|
||||
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
|
||||
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
|
||||
bool if_right = ((int)head_crc16 == (int)CRC16);
|
||||
std::cout << "if_right: " << if_right << std::endl;
|
||||
}
|
||||
|
||||
if (head_crc16 != CRC16)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
|
||||
continue;
|
||||
}
|
||||
else if(ahrs_frame_.frame.frame_end != FRAME_END)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check frame end.");
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if (head_type[0] == TYPE_INSGPS)
|
||||
{
|
||||
uint16_t head_crc16_l = insgps_frame_.frame.header.header_crc16_l;
|
||||
uint16_t head_crc16_h = insgps_frame_.frame.header.header_crc16_h;
|
||||
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
|
||||
size_t data_s = serial_.read(insgps_frame_.read_buf.read_msg, (INSGPS_LEN + 1)); //48+1
|
||||
// if (if_debug_){
|
||||
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
|
||||
// {
|
||||
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
// }
|
||||
uint16_t CRC16 = CRC16_Table(insgps_frame_.frame.data.data_buff, INSGPS_LEN);
|
||||
if (if_debug_){
|
||||
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
|
||||
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
|
||||
bool if_right = ((int)head_crc16 == (int)CRC16);
|
||||
std::cout << "if_right: " << if_right << std::endl;
|
||||
}
|
||||
|
||||
if (head_crc16 != CRC16)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(ahrs).");
|
||||
continue;
|
||||
}
|
||||
else if(insgps_frame_.frame.frame_end != FRAME_END)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check frame end.");
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if (head_type[0] == TYPE_GEODETIC_POS)
|
||||
{
|
||||
uint16_t head_crc16_l = Geodetic_Position_frame_.frame.header.header_crc16_l;
|
||||
uint16_t head_crc16_h = Geodetic_Position_frame_.frame.header.header_crc16_h;
|
||||
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
|
||||
size_t data_s = serial_.read(Geodetic_Position_frame_.read_buf.read_msg, (GEODETIC_POS_LEN + 1)); //24+1
|
||||
// if (if_debug_){
|
||||
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
|
||||
// {
|
||||
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
|
||||
// }
|
||||
// std::cout << std::dec << std::endl;
|
||||
// }
|
||||
uint16_t CRC16 = CRC16_Table(Geodetic_Position_frame_.frame.data.data_buff, GEODETIC_POS_LEN);
|
||||
if (if_debug_){
|
||||
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
|
||||
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
|
||||
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
|
||||
bool if_right = ((int)head_crc16 == (int)CRC16);
|
||||
std::cout << "if_right: " << if_right << std::endl;
|
||||
}
|
||||
|
||||
if (head_crc16 != CRC16)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check crc16 faild(gps).");
|
||||
continue;
|
||||
}
|
||||
else if(Geodetic_Position_frame_.frame.frame_end != FRAME_END)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"check frame end.");
|
||||
continue;
|
||||
}
|
||||
}
|
||||
// publish magyaw topic
|
||||
//读取IMU数据进行解析，并发布相关话题
|
||||
|
||||
if (head_type[0] == TYPE_IMU)
|
||||
{
|
||||
// publish imu topic
|
||||
sensor_msgs::msg::Imu imu_data;
|
||||
imu_data.header.stamp = rclcpp::Node::now();
|
||||
imu_data.header.frame_id = imu_frame_id_.c_str();
|
||||
Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
|
||||
ahrs_frame_.frame.data.data_pack.Qx,
|
||||
ahrs_frame_.frame.data.data_pack.Qy,
|
||||
ahrs_frame_.frame.data.data_pack.Qz);
|
||||
Eigen::Quaterniond q_r =
|
||||
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) *
|
||||
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) *
|
||||
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX());
|
||||
Eigen::Quaterniond q_rr =
|
||||
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) *
|
||||
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
|
||||
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
|
||||
Eigen::Quaterniond q_xiao_rr =
|
||||
Eigen::AngleAxisd( PI/2, Eigen::Vector3d::UnitZ()) *
|
||||
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
|
||||
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
|
||||
if (device_type_ == 0) //未经变换的原始数据
|
||||
{
|
||||
imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
|
||||
imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qx;
|
||||
imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qy;
|
||||
imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;
|
||||
imu_data.angular_velocity.x = imu_frame_.frame.data.data_pack.gyroscope_x;
|
||||
imu_data.angular_velocity.y = imu_frame_.frame.data.data_pack.gyroscope_y;
|
||||
imu_data.angular_velocity.z = imu_frame_.frame.data.data_pack.gyroscope_z;
|
||||
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
|
||||
imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_y;
|
||||
imu_data.linear_acceleration.z = imu_frame_.frame.data.data_pack.accelerometer_z;
|
||||
}
|
||||
else if (device_type_ == 1) //imu单品rclcpp标准下的坐标变换
|
||||
{
|
||||
|
||||
Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr;
|
||||
imu_data.orientation.w = q_out.w();
|
||||
imu_data.orientation.x = q_out.x();
|
||||
imu_data.orientation.y = q_out.y();
|
||||
imu_data.orientation.z = q_out.z();
|
||||
imu_data.angular_velocity.x = imu_frame_.frame.data.data_pack.gyroscope_x;
|
||||
imu_data.angular_velocity.y = -imu_frame_.frame.data.data_pack.gyroscope_y;
|
||||
imu_data.angular_velocity.z = -imu_frame_.frame.data.data_pack.gyroscope_z;
|
||||
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
|
||||
imu_data.linear_acceleration.y = -imu_frame_.frame.data.data_pack.accelerometer_y;
|
||||
imu_data.linear_acceleration.z = -imu_frame_.frame.data.data_pack.accelerometer_z;
|
||||
}
|
||||
imu_pub_->publish(imu_data);
|
||||
}
|
||||
//读取AHRS数据进行解析，并发布相关话题
|
||||
else if (head_type[0] == TYPE_AHRS)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D pose_2d;
|
||||
pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
|
||||
mag_pose_pub_->publish(pose_2d);
|
||||
//std::cout << "YAW: " << pose_2d.theta << std::endl;
|
||||
geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;
|
||||
Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
|
||||
Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
|
||||
Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
|
||||
Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
|
||||
Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
|
||||
Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
|
||||
|
||||
Euler_angles_pub_->publish(Euler_angles_2d);
|
||||
Magnetic_pub_->publish(Magnetic);
|
||||
|
||||
}
|
||||
|
||||
//读取gps_pos数据进行解析，并发布相关话题
|
||||
else if (head_type[0] == TYPE_GEODETIC_POS)
|
||||
{
|
||||
sensor_msgs::msg::NavSatFix gps_data;
|
||||
gps_data.header.stamp = rclcpp::Node::now();
|
||||
gps_data.header.frame_id = "navsat_link";
|
||||
gps_data.latitude = Geodetic_Position_frame_.frame.data.data_pack.Latitude / DEG_TO_RAD;
|
||||
gps_data.longitude = Geodetic_Position_frame_.frame.data.data_pack.Longitude / DEG_TO_RAD;
|
||||
gps_data.altitude = Geodetic_Position_frame_.frame.data.data_pack.Height;
|
||||
|
||||
//std::cout << "lat: " << Geodetic_Position_frame_.frame.data.data_pack.Latitude << std::endl;
|
||||
//std::cout << "lon: " << Geodetic_Position_frame_.frame.data.data_pack.Longitude << std::endl;
|
||||
//std::cout << "h: " << Geodetic_Position_frame_.frame.data.data_pack.Height << std::endl;
|
||||
|
||||
gps_pub_->publish(gps_data);
|
||||
}
|
||||
//读取INSGPS数据进行解析，并发布相关话题
|
||||
else if (head_type[0] == TYPE_INSGPS)
|
||||
{
|
||||
nav_msgs::msg::Odometry odom_msg;
|
||||
odom_msg.header.stamp = rclcpp::Node::now();
|
||||
// odom_msg.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
|
||||
odom_msg.pose.pose.position.x = insgps_frame_.frame.data.data_pack.Location_North; //Position //位置
|
||||
odom_msg.pose.pose.position.y = insgps_frame_.frame.data.data_pack.Location_East;
|
||||
odom_msg.pose.pose.position.z = insgps_frame_.frame.data.data_pack.Location_Down;
|
||||
|
||||
// odom_msg.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
|
||||
odom_msg.twist.twist.linear.x = insgps_frame_.frame.data.data_pack.Velocity_North; //Speed in the X direction //X方向速度
|
||||
odom_msg.twist.twist.linear.y = insgps_frame_.frame.data.data_pack.Velocity_East; //Speed in the Y direction //Y方向速度
|
||||
odom_msg.twist.twist.linear.z = insgps_frame_.frame.data.data_pack.Velocity_Down;
|
||||
NED_odom_pub_->publish(odom_msg);
|
||||
|
||||
geometry_msgs::msg::Twist speed_msg;
|
||||
speed_msg.linear.x = insgps_frame_.frame.data.data_pack.BodyVelocity_X;
|
||||
speed_msg.linear.y = insgps_frame_.frame.data.data_pack.BodyVelocity_Y;
|
||||
speed_msg.linear.z = insgps_frame_.frame.data.data_pack.BodyVelocity_Z;
|
||||
twist_pub_->publish(speed_msg);
|
||||
|
||||
// std::cout << "N: " << insgps_frame_.frame.data.data_pack.Location_North << std::endl;
|
||||
// std::cout << "E: " << insgps_frame_.frame.data.data_pack.Location_East << std::endl;
|
||||
// std::cout << "D: " << insgps_frame_.frame.data.data_pack.Location_Down << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ahrsBringup::magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz)
|
||||
{
|
||||
double temp1 = magy * cos(roll) + magz * sin(roll);
|
||||
double temp2 = magx * cos(pitch) + magy * sin(pitch) * sin(roll) - magz * sin(pitch) * cos(roll);
|
||||
magyaw = atan2(-temp1, temp2);
|
||||
if(magyaw < 0)
|
||||
{
|
||||
magyaw = magyaw + 2 * PI;
|
||||
}
|
||||
// return magyaw;
|
||||
}
|
||||
|
||||
void ahrsBringup::checkSN(int type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case TYPE_IMU:
|
||||
if (++read_sn_ != imu_frame_.frame.header.serial_num)
|
||||
{
|
||||
if ( imu_frame_.frame.header.serial_num < read_sn_)
|
||||
{
|
||||
sn_lost_ += 256 - (int)(read_sn_ - imu_frame_.frame.header.serial_num);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sn_lost_ += (int)(imu_frame_.frame.header.serial_num - read_sn_);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
}
|
||||
read_sn_ = imu_frame_.frame.header.serial_num;
|
||||
break;
|
||||
|
||||
case TYPE_AHRS:
|
||||
if (++read_sn_ != ahrs_frame_.frame.header.serial_num)
|
||||
{
|
||||
if ( ahrs_frame_.frame.header.serial_num < read_sn_)
|
||||
{
|
||||
sn_lost_ += 256 - (int)(read_sn_ - ahrs_frame_.frame.header.serial_num);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sn_lost_ += (int)(ahrs_frame_.frame.header.serial_num - read_sn_);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
}
|
||||
read_sn_ = ahrs_frame_.frame.header.serial_num;
|
||||
break;
|
||||
|
||||
case TYPE_INSGPS:
|
||||
if (++read_sn_ != insgps_frame_.frame.header.serial_num)
|
||||
{
|
||||
if ( insgps_frame_.frame.header.serial_num < read_sn_)
|
||||
{
|
||||
sn_lost_ += 256 - (int)(read_sn_ - insgps_frame_.frame.header.serial_num);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sn_lost_ += (int)(insgps_frame_.frame.header.serial_num - read_sn_);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
}
|
||||
read_sn_ = insgps_frame_.frame.header.serial_num;
|
||||
break;
|
||||
|
||||
case TYPE_GEODETIC_POS:
|
||||
if (++read_sn_ != Geodetic_Position_frame_.frame.header.serial_num)
|
||||
{
|
||||
if ( Geodetic_Position_frame_.frame.header.serial_num < read_sn_)
|
||||
{
|
||||
sn_lost_ += 256 - (int)(read_sn_ - Geodetic_Position_frame_.frame.header.serial_num);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sn_lost_ += (int)(Geodetic_Position_frame_.frame.header.serial_num - read_sn_);
|
||||
if(if_debug_){
|
||||
RCLCPP_WARN(this->get_logger(),"detected sn lost.");
|
||||
}
|
||||
}
|
||||
}
|
||||
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace FDILink
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
FDILink::ahrsBringup bp;
|
||||
|
||||
return 0;
|
||||
}
|
||||
159
src/hardware/fdilink_ahrs_ROS2/src/crc_table.cpp
Normal file
159
src/hardware/fdilink_ahrs_ROS2/src/crc_table.cpp
Normal file
@ -0,0 +1,159 @@
|
||||
#include <stdint.h>
|
||||
#include <crc_table.h>
|
||||
|
||||
static const uint8_t CRC8Table[] = {
|
||||
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
|
||||
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
|
||||
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
|
||||
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
|
||||
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
|
||||
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
|
||||
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
|
||||
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
|
||||
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
|
||||
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
|
||||
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
|
||||
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
|
||||
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
|
||||
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
|
||||
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
|
||||
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53};
|
||||
|
||||
static const uint16_t CRC16Table[256] =
|
||||
{
|
||||
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
|
||||
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
|
||||
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
|
||||
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
|
||||
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
|
||||
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
|
||||
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
|
||||
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
|
||||
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
|
||||
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
|
||||
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
|
||||
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
|
||||
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
|
||||
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
|
||||
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
|
||||
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
|
||||
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
|
||||
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
|
||||
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
|
||||
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
|
||||
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
|
||||
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
|
||||
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
|
||||
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
|
||||
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
|
||||
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
|
||||
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
|
||||
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
|
||||
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
|
||||
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
|
||||
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
|
||||
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
|
||||
};
|
||||
|
||||
static const uint32_t CRC32Table[] = {
|
||||
0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL,
|
||||
0x076dc419L, 0x706af48fL, 0xe963a535L, 0x9e6495a3L,
|
||||
0x0edb8832L, 0x79dcb8a4L, 0xe0d5e91eL, 0x97d2d988L,
|
||||
0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L, 0x90bf1d91L,
|
||||
0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
|
||||
0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L,
|
||||
0x136c9856L, 0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL,
|
||||
0x14015c4fL, 0x63066cd9L, 0xfa0f3d63L, 0x8d080df5L,
|
||||
0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L, 0xa2677172L,
|
||||
0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
|
||||
0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L,
|
||||
0x32d86ce3L, 0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L,
|
||||
0x26d930acL, 0x51de003aL, 0xc8d75180L, 0xbfd06116L,
|
||||
0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L, 0xb8bda50fL,
|
||||
0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
|
||||
0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL,
|
||||
0x76dc4190L, 0x01db7106L, 0x98d220bcL, 0xefd5102aL,
|
||||
0x71b18589L, 0x06b6b51fL, 0x9fbfe4a5L, 0xe8b8d433L,
|
||||
0x7807c9a2L, 0x0f00f934L, 0x9609a88eL, 0xe10e9818L,
|
||||
0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
|
||||
0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL,
|
||||
0x6c0695edL, 0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L,
|
||||
0x65b0d9c6L, 0x12b7e950L, 0x8bbeb8eaL, 0xfcb9887cL,
|
||||
0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L, 0xfbd44c65L,
|
||||
0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
|
||||
0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL,
|
||||
0x4369e96aL, 0x346ed9fcL, 0xad678846L, 0xda60b8d0L,
|
||||
0x44042d73L, 0x33031de5L, 0xaa0a4c5fL, 0xdd0d7cc9L,
|
||||
0x5005713cL, 0x270241aaL, 0xbe0b1010L, 0xc90c2086L,
|
||||
0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
|
||||
0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L,
|
||||
0x59b33d17L, 0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL,
|
||||
0xedb88320L, 0x9abfb3b6L, 0x03b6e20cL, 0x74b1d29aL,
|
||||
0xead54739L, 0x9dd277afL, 0x04db2615L, 0x73dc1683L,
|
||||
0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
|
||||
0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L,
|
||||
0xf00f9344L, 0x8708a3d2L, 0x1e01f268L, 0x6906c2feL,
|
||||
0xf762575dL, 0x806567cbL, 0x196c3671L, 0x6e6b06e7L,
|
||||
0xfed41b76L, 0x89d32be0L, 0x10da7a5aL, 0x67dd4accL,
|
||||
0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
|
||||
0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L,
|
||||
0xd1bb67f1L, 0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL,
|
||||
0xd80d2bdaL, 0xaf0a1b4cL, 0x36034af6L, 0x41047a60L,
|
||||
0xdf60efc3L, 0xa867df55L, 0x316e8eefL, 0x4669be79L,
|
||||
0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
|
||||
0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL,
|
||||
0xc5ba3bbeL, 0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L,
|
||||
0xc2d7ffa7L, 0xb5d0cf31L, 0x2cd99e8bL, 0x5bdeae1dL,
|
||||
0x9b64c2b0L, 0xec63f226L, 0x756aa39cL, 0x026d930aL,
|
||||
0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
|
||||
0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L,
|
||||
0x92d28e9bL, 0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L,
|
||||
0x86d3d2d4L, 0xf1d4e242L, 0x68ddb3f8L, 0x1fda836eL,
|
||||
0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L, 0x18b74777L,
|
||||
0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
|
||||
0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L,
|
||||
0xa00ae278L, 0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L,
|
||||
0xa7672661L, 0xd06016f7L, 0x4969474dL, 0x3e6e77dbL,
|
||||
0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L, 0x37d83bf0L,
|
||||
0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
|
||||
0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L,
|
||||
0xbad03605L, 0xcdd70693L, 0x54de5729L, 0x23d967bfL,
|
||||
0xb3667a2eL, 0xc4614ab8L, 0x5d681b02L, 0x2a6f2b94L,
|
||||
0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL, 0x2d02ef8dL
|
||||
};
|
||||
|
||||
|
||||
uint8_t CRC8_Table(uint8_t* p, uint8_t counter)
|
||||
{
|
||||
uint8_t crc8 = 0;
|
||||
for (int i = 0; i < counter; i++)
|
||||
{
|
||||
uint8_t value = p[i];
|
||||
uint8_t new_index = crc8 ^ value;
|
||||
crc8 = CRC8Table[new_index];
|
||||
}
|
||||
return (crc8);
|
||||
}
|
||||
|
||||
|
||||
uint16_t CRC16_Table(uint8_t *p, uint8_t counter)
|
||||
{
|
||||
uint16_t crc16 = 0;
|
||||
for (int i = 0; i < counter; i++)
|
||||
{
|
||||
uint8_t value = p[i];
|
||||
crc16 = CRC16Table[((crc16 >> 8) ^ value) & 0xff] ^ (crc16 << 8);
|
||||
}
|
||||
return (crc16);
|
||||
}
|
||||
|
||||
uint32_t CRC32_Table(uint8_t *p, uint8_t counter)
|
||||
{
|
||||
uint16_t crc32 = 0;
|
||||
for (int i = 0; i < counter; i++)
|
||||
{
|
||||
uint8_t value = p[i];
|
||||
crc32 = CRC16Table[((crc32 >> 8) ^ value) & 0xff] ^ (crc32 << 8);
|
||||
}
|
||||
return (crc32);
|
||||
}
|
||||
107
src/hardware/fdilink_ahrs_ROS2/src/imu_tf.cpp
Normal file
107
src/hardware/fdilink_ahrs_ROS2/src/imu_tf.cpp
Normal file
@ -0,0 +1,107 @@
|
||||
#include <memory>
|
||||
#include <inttypes.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <string>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
using std::placeholders::_1;
|
||||
using namespace std;
|
||||
|
||||
/* 参考ROS wiki
|
||||
* http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
|
||||
* */
|
||||
|
||||
int position_x ;
|
||||
int position_y ;
|
||||
int position_z ;
|
||||
std::string imu_topic;
|
||||
std::string imu_frame_id, world_frame_id;
|
||||
|
||||
static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
|
||||
rclcpp::Node::SharedPtr nh_=nullptr;
|
||||
class imu_data_to_tf : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
imu_data_to_tf():Node("imu_data_to_tf")
|
||||
{
|
||||
|
||||
// node.param("/imu_tf/imu_topic", imu_topic, std::string("/imu"));
|
||||
// node.param("/imu_tf/position_x", position_x, 0);
|
||||
// node.param("/imu_tf/position_y", position_y, 0);
|
||||
// node.param("/imu_tf/position_z", position_z, 0);
|
||||
|
||||
this->declare_parameter<std::string>("world_frame_id","/world");
|
||||
this->get_parameter("world_frame_id", world_frame_id);
|
||||
|
||||
this->declare_parameter<std::string>("imu_frame_id","/imu");
|
||||
this->get_parameter("imu_frame_id", imu_frame_id);
|
||||
|
||||
this->declare_parameter<std::string>("imu_topic","/imu");
|
||||
this->get_parameter("imu_topic", imu_topic);
|
||||
|
||||
this->declare_parameter<std::int16_t>("position_x",1);
|
||||
this->get_parameter("position_x", position_x);
|
||||
|
||||
this->declare_parameter<std::int16_t>("position_y",1);
|
||||
this->get_parameter("position_y", position_y);
|
||||
|
||||
this->declare_parameter<std::int16_t>("position_z",1);
|
||||
this->get_parameter("position_z", position_z);
|
||||
br =std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||
sub_ = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic.c_str(), 10, std::bind(&imu_data_to_tf::ImuCallback,this,_1));
|
||||
|
||||
}
|
||||
private:
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
|
||||
|
||||
//rclcpp::Subscriber sub = node.subscribe(imu_topic.c_str(), 10, &ImuCallback);
|
||||
|
||||
|
||||
void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_data) {
|
||||
|
||||
//static tf2_ros::TransformBroadcaster br;//广播器
|
||||
|
||||
// tf2::Transform transform;
|
||||
// transform.setOrigin(tf2::Vector3(position_x, position_y, position_z));//设置平移部分
|
||||
|
||||
// 从IMU消息包中获取四元数数据
|
||||
tf2::Quaternion q;
|
||||
q.setX(imu_data->orientation.x);
|
||||
q.setY(imu_data->orientation.y);
|
||||
q.setZ(imu_data->orientation.z);
|
||||
q.setW(imu_data->orientation.w);
|
||||
q.normalized();//归一化
|
||||
|
||||
// transform.setRotation(q);//设置旋转部分
|
||||
//广播出去
|
||||
//br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "imu"));
|
||||
geometry_msgs::msg::TransformStamped tfs;
|
||||
tfs.header.stamp=rclcpp::Node::now();
|
||||
tfs.header.frame_id ="world";
|
||||
tfs.child_frame_id="imu";
|
||||
tfs.transform.translation.x=position_x;
|
||||
tfs.transform.translation.y=position_y;
|
||||
tfs.transform.translation.z=position_z;
|
||||
tfs.transform.rotation.x=q.getX();
|
||||
tfs.transform.rotation.y=q.getY();
|
||||
tfs.transform.rotation.z=q.getZ();
|
||||
tfs.transform.rotation.w=q.getW();
|
||||
br->sendTransform(tfs);
|
||||
//tf2::(transform, rclcpp::Node::now(), "world", "imu")
|
||||
}
|
||||
};
|
||||
|
||||
int main (int argc, char ** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
//ros::NodeHandle node;, "imu_data_to_tf"
|
||||
//auto node =std::make_shared<imu_data_to_tf>();
|
||||
rclcpp::spin(std::make_shared<imu_data_to_tf>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
14
src/hardware/fdilink_ahrs_ROS2/wheeltec_udev.sh
Normal file
14
src/hardware/fdilink_ahrs_ROS2/wheeltec_udev.sh
Normal file
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<double> hw_commands_;
|
||||
std::vector<double> hw_states_;
|
||||
std::vector<double> imu_states_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
|
||||
|
||||
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace mdog_hardware
|
||||
|
||||
43
src/hardware/mdog_hardware/launch/mdog_hardware.launch.py
Normal file
43
src/hardware/mdog_hardware/launch/mdog_hardware.launch.py
Normal file
@ -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
|
||||
])
|
||||
@ -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<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
|
||||
// 初始化硬件状态和命令
|
||||
hw_states_.resize(info_.joints.size() * 3, std::numeric_limits<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits<double>::quiet_NaN());
|
||||
imu_states_.resize(10, std::numeric_limits<double>::quiet_NaN());
|
||||
|
||||
// 创建 ROS 2 节点和订阅器
|
||||
node_ = std::make_shared<rclcpp::Node>("mdog_hardware_interface");
|
||||
imu_subscription_ = node_->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/imu", 10, std::bind(&MDogHardwareInterface::imu_callback, this, std::placeholders::_1));
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
@ -50,9 +74,34 @@ std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> 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;
|
||||
}
|
||||
|
||||
|
||||
@ -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()
|
||||
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
#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<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> 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<double> hw_commands_;
|
||||
std::vector<double> hw_states_;
|
||||
};
|
||||
|
||||
} // namespace mdog_hardware_interface
|
||||
|
||||
#endif // MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
@ -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_
|
||||
@ -1,9 +0,0 @@
|
||||
<library path="mdog_hardware_interface">
|
||||
<class name="mdog_hardware_interface/MDogHardwareInterface"
|
||||
type="mdog_hardware_interface::MDogHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ros2_control hardware interface.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@ -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 <limits>
|
||||
#include <vector>
|
||||
|
||||
#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<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::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<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::CommandInterface> 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)
|
||||
@ -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 <gmock/gmock.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#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"(
|
||||
<ros2_control name="MDogHardwareInterface2dof" type="system">
|
||||
<hardware>
|
||||
<plugin>mdog_hardware_interface/MDogHardwareInterface</plugin>
|
||||
</hardware>
|
||||
<joint name="joint1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">1.57</param>
|
||||
</joint>
|
||||
<joint name="joint2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">0.7854</param>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
)";
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
65
src/hardware/unitree_leg_serial_driver/CMakeLists.txt
Normal file
65
src/hardware/unitree_leg_serial_driver/CMakeLists.txt
Normal file
@ -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()
|
||||
@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
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
|
||||
@ -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()
|
||||
@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include <serial/serial.h>
|
||||
#include <atomic>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#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::Serial> serial_port_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
std::thread read_thread_;
|
||||
std::atomic<bool> running_{false};
|
||||
|
||||
// 状态管理
|
||||
enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 };
|
||||
std::atomic<StatusFlag> status_flag_{OFFLINE};
|
||||
std::atomic<int8_t> tick_{0};
|
||||
|
||||
// 串口参数
|
||||
std::string serial_port_name_;
|
||||
int baud_rate_;
|
||||
|
||||
// 电机数据
|
||||
MotorCmd_t motor_cmd_;
|
||||
MotorData_t motor_fbk_;
|
||||
};
|
||||
|
||||
} // namespace unitree_leg_serial
|
||||
@ -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"
|
||||
)
|
||||
])
|
||||
@ -0,0 +1,5 @@
|
||||
float32 torque_des
|
||||
float32 speed_des
|
||||
float32 pos_des
|
||||
float32 kp
|
||||
float32 kd
|
||||
@ -0,0 +1,3 @@
|
||||
float32 torque
|
||||
float32 speed
|
||||
float32 pos
|
||||
34
src/hardware/unitree_leg_serial_driver/package.xml
Normal file
34
src/hardware/unitree_leg_serial_driver/package.xml
Normal file
@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>unitree_leg_serial_driver</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Unitree Leg Serial Driver for ROS 2</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<!-- Build dependencies -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rclcpp_components</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>serial</build_depend>
|
||||
|
||||
<!-- Execution dependencies -->
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rclcpp_components</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>serial</exec_depend>
|
||||
|
||||
<!-- Test dependencies -->
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<!-- Required for packages with interfaces -->
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
68
src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp
Normal file
68
src/hardware/unitree_leg_serial_driver/src/crc_ccitt.cpp
Normal file
@ -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
|
||||
@ -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::Serial>(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<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
|
||||
}
|
||||
|
||||
void UnitreeLegSerial::receive_data()
|
||||
{
|
||||
size_t packet_size = sizeof(RIS_MotorData_t);
|
||||
std::vector<uint8_t> 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)
|
||||
11
src/robot_descriptions/qut/mdog_description/CMakeLists.txt
Normal file
11
src/robot_descriptions/qut/mdog_description/CMakeLists.txt
Normal file
@ -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()
|
||||
@ -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
|
||||
@ -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: <Fixed 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: <Fixed 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
|
||||
@ -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',
|
||||
)
|
||||
])
|
||||
18
src/robot_descriptions/qut/mdog_description/package.xml
Normal file
18
src/robot_descriptions/qut/mdog_description/package.xml
Normal file
@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mdog_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>imu_sensor_broadcaster</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
555
src/robot_descriptions/qut/mdog_description/robot.urdf
Normal file
555
src/robot_descriptions/qut/mdog_description/robot.urdf
Normal file
@ -0,0 +1,555 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from /home/robofish/CM_DOG/src/robot_descriptions/qut/mdog_description/xacro/robot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="mdog">
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="deep-grey">
|
||||
<color rgba="0.1 0.1 0.1 1.0"/>
|
||||
</material>
|
||||
<material name="grey">
|
||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||
</material>
|
||||
<material name="light-grey">
|
||||
<color rgba="0.35 0.35 0.35 1.0"/>
|
||||
</material>
|
||||
<material name="silver">
|
||||
<color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
|
||||
</material>
|
||||
<material name="brown">
|
||||
<color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<ros2_control name="MdogSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>hardware_mdog/hardware_mdog_real</plugin>
|
||||
</hardware>
|
||||
<joint name="FR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<sensor name="imu_sensor">
|
||||
<state_interface name="orientation.w"/>
|
||||
<state_interface name="orientation.x"/>
|
||||
<state_interface name="orientation.y"/>
|
||||
<state_interface name="orientation.z"/>
|
||||
<state_interface name="angular_velocity.x"/>
|
||||
<state_interface name="angular_velocity.y"/>
|
||||
<state_interface name="angular_velocity.z"/>
|
||||
<state_interface name="linear_acceleration.x"/>
|
||||
<state_interface name="linear_acceleration.y"/>
|
||||
<state_interface name="linear_acceleration.z"/>
|
||||
</sensor>
|
||||
</ros2_control>
|
||||
<link name="base">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="trunk">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.32 0.3 0.12"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.3 0.3 0.12"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_trunk" type="fixed">
|
||||
<parent link="base"/>
|
||||
<child link="trunk"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
<child link="imu_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.05"/>
|
||||
</joint>
|
||||
<link name="imu_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="FR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.23 0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="FR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="FR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
|
||||
<parent link="FR_hip"/>
|
||||
<child link="FR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="FR_calf_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
|
||||
<parent link="FR_thigh"/>
|
||||
<child link="FR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="FL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.23 -0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="FL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="FL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
|
||||
<parent link="FL_hip"/>
|
||||
<child link="FL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="FL_calf_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
|
||||
<parent link="FL_thigh"/>
|
||||
<child link="FL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="RR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.23 0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="RR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="RR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
|
||||
<parent link="RR_hip"/>
|
||||
<child link="RR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="RR_calf_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
|
||||
<parent link="RR_thigh"/>
|
||||
<child link="RR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="RL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.23 -0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="RL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="RL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
|
||||
<parent link="RL_hip"/>
|
||||
<child link="RL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="RL_calf_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
|
||||
<parent link="RL_thigh"/>
|
||||
<child link="RL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
555
src/robot_descriptions/qut/mdog_description/urdf/robot.urdf
Normal file
555
src/robot_descriptions/qut/mdog_description/urdf/robot.urdf
Normal file
@ -0,0 +1,555 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from /home/robofish/CM_DOG/src/robot_descriptions/qut/mdog_description/xacro/robot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="mdog">
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="deep-grey">
|
||||
<color rgba="0.1 0.1 0.1 1.0"/>
|
||||
</material>
|
||||
<material name="grey">
|
||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||
</material>
|
||||
<material name="light-grey">
|
||||
<color rgba="0.35 0.35 0.35 1.0"/>
|
||||
</material>
|
||||
<material name="silver">
|
||||
<color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
|
||||
</material>
|
||||
<material name="brown">
|
||||
<color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<ros2_control name="MdogSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>hardware_mdog/hardware_mdog_real</plugin>
|
||||
</hardware>
|
||||
<joint name="FR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="FL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<joint name="RL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
<sensor name="imu_sensor">
|
||||
<state_interface name="orientation.w"/>
|
||||
<state_interface name="orientation.x"/>
|
||||
<state_interface name="orientation.y"/>
|
||||
<state_interface name="orientation.z"/>
|
||||
<state_interface name="angular_velocity.x"/>
|
||||
<state_interface name="angular_velocity.y"/>
|
||||
<state_interface name="angular_velocity.z"/>
|
||||
<state_interface name="linear_acceleration.x"/>
|
||||
<state_interface name="linear_acceleration.y"/>
|
||||
<state_interface name="linear_acceleration.z"/>
|
||||
</sensor>
|
||||
</ros2_control>
|
||||
<link name="base">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="trunk">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.32 0.3 0.12"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.3 0.3 0.12"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_trunk" type="fixed">
|
||||
<parent link="base"/>
|
||||
<child link="trunk"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
<child link="imu_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.05"/>
|
||||
</joint>
|
||||
<link name="imu_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="FR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.23 0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="FR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="FR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0.05 0"/>
|
||||
<parent link="FR_hip"/>
|
||||
<child link="FR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="FR_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.25 0 0"/>
|
||||
<parent link="FR_thigh"/>
|
||||
<child link="FR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FR_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="FL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.23 -0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="FL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="FL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/>
|
||||
<parent link="FL_hip"/>
|
||||
<child link="FL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="FL_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.25 0 0"/>
|
||||
<parent link="FL_thigh"/>
|
||||
<child link="FL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="FL_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="RR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.23 0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="RR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 -0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="RR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="0 0.05 0"/>
|
||||
<parent link="RR_hip"/>
|
||||
<child link="RR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="RR_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.25 0 0"/>
|
||||
<parent link="RR_thigh"/>
|
||||
<child link="RR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RR_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="RL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.23 -0.15 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="RL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 0.03 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="RL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/>
|
||||
<parent link="RL_hip"/>
|
||||
<child link="RL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="RL_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.25 0 0"/>
|
||||
<parent link="RL_thigh"/>
|
||||
<child link="RL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" lower="-1.57" upper="1.57" velocity="10"/>
|
||||
</joint>
|
||||
<link name="RL_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
159
src/robot_descriptions/qut/mdog_description/xacro/const.xacro
Normal file
159
src/robot_descriptions/qut/mdog_description/xacro/const.xacro
Normal file
@ -0,0 +1,159 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
<xacro:property name="stick_mass" value="0.00001"/>
|
||||
|
||||
<!-- simplified collision value -->
|
||||
<xacro:property name="trunk_width" value="0.0935"/>
|
||||
<xacro:property name="trunk_length" value="0.3762"/>
|
||||
<xacro:property name="trunk_height" value="0.114"/>
|
||||
<xacro:property name="hip_radius" value="0.046"/>
|
||||
<xacro:property name="hip_length" value="0.04"/>
|
||||
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
|
||||
<xacro:property name="thigh_shoulder_length" value="0.032"/>
|
||||
<xacro:property name="thigh_width" value="0.0245"/>
|
||||
<xacro:property name="thigh_height" value="0.034"/>
|
||||
<xacro:property name="calf_width" value="0.016"/>
|
||||
<xacro:property name="calf_height" value="0.016"/>
|
||||
<xacro:property name="foot_radius" value="0.02"/>
|
||||
<xacro:property name="stick_radius" value="0.01"/>
|
||||
<xacro:property name="stick_length" value="0.2"/>
|
||||
|
||||
<!-- kinematic value -->
|
||||
<xacro:property name="thigh_offset" value="0.08"/>
|
||||
<xacro:property name="thigh_length" value="0.213"/>
|
||||
<xacro:property name="calf_length" value="0.213"/>
|
||||
|
||||
<!-- leg offset from trunk center value -->
|
||||
<xacro:property name="leg_offset_x" value="0.1881"/>
|
||||
<xacro:property name="leg_offset_y" value="0.04675"/>
|
||||
<!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
|
||||
<xacro:property name="hip_offset" value="0.08"/>
|
||||
|
||||
<!-- offset of link and rotor locations (left front) -->
|
||||
<xacro:property name="hip_offset_x" value="0.1881"/>
|
||||
<xacro:property name="hip_offset_y" value="0.04675"/>
|
||||
<xacro:property name="hip_offset_z" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_offset_x" value="0.11215"/>
|
||||
<xacro:property name="hip_rotor_offset_y" value="0.04675"/>
|
||||
<xacro:property name="hip_rotor_offset_z" value="0.0"/>
|
||||
|
||||
<xacro:property name="thigh_offset_x" value="0"/>
|
||||
<xacro:property name="thigh_offset_y" value="0.08"/>
|
||||
<xacro:property name="thigh_offset_z" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_offset_x" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_offset_y" value="-0.00015"/>
|
||||
<xacro:property name="thigh_rotor_offset_z" value="0.0"/>
|
||||
|
||||
<xacro:property name="calf_offset_x" value="0.0"/>
|
||||
<xacro:property name="calf_offset_y" value="0.0"/>
|
||||
<xacro:property name="calf_offset_z" value="-0.213"/>
|
||||
<xacro:property name="calf_rotor_offset_x" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_offset_y" value="-0.03235"/>
|
||||
<xacro:property name="calf_rotor_offset_z" value="0.0"/>
|
||||
|
||||
<!-- joint limits -->
|
||||
<xacro:property name="damping" value="0.1"/>
|
||||
<xacro:property name="friction" value="0.1"/>
|
||||
<xacro:property name="hip_position_max" value="0.863"/>
|
||||
<xacro:property name="hip_position_min" value="-0.863"/>
|
||||
<xacro:property name="hip_velocity_max" value="30.1"/>
|
||||
<xacro:property name="hip_torque_max" value="23.7"/>
|
||||
<xacro:property name="thigh_position_max" value="4.501"/>
|
||||
<xacro:property name="thigh_position_min" value="-0.686"/>
|
||||
<xacro:property name="thigh_velocity_max" value="30.1"/>
|
||||
<xacro:property name="thigh_torque_max" value="23.7"/>
|
||||
<xacro:property name="calf_position_max" value="-0.888"/>
|
||||
<xacro:property name="calf_position_min" value="-2.818"/>
|
||||
<xacro:property name="calf_velocity_max" value="20.06"/>
|
||||
<xacro:property name="calf_torque_max" value="35.55"/>
|
||||
|
||||
<!-- dynamics inertial value total 12.84kg -->
|
||||
<!-- trunk -->
|
||||
<xacro:property name="trunk_mass" value="5.204"/>
|
||||
<xacro:property name="trunk_com_x" value="0.0223"/>
|
||||
<xacro:property name="trunk_com_y" value="0.002"/>
|
||||
<xacro:property name="trunk_com_z" value="-0.0005"/>
|
||||
<xacro:property name="trunk_ixx" value="0.0168128557"/>
|
||||
<xacro:property name="trunk_ixy" value="-0.0002296769"/>
|
||||
<xacro:property name="trunk_ixz" value="-0.0002945293"/>
|
||||
<xacro:property name="trunk_iyy" value="0.063009565"/>
|
||||
<xacro:property name="trunk_iyz" value="-0.0000418731"/>
|
||||
<xacro:property name="trunk_izz" value="0.0716547275"/>
|
||||
|
||||
<!-- hip (left front) -->
|
||||
<xacro:property name="hip_mass" value="0.591"/>
|
||||
<xacro:property name="hip_com_x" value="-0.005657"/>
|
||||
<xacro:property name="hip_com_y" value="-0.008752"/>
|
||||
<xacro:property name="hip_com_z" value="-0.000102"/>
|
||||
<xacro:property name="hip_ixx" value="0.000334008405"/>
|
||||
<xacro:property name="hip_ixy" value="-0.000010826066"/>
|
||||
<xacro:property name="hip_ixz" value="0.000001290732"/>
|
||||
<xacro:property name="hip_iyy" value="0.000619101213"/>
|
||||
<xacro:property name="hip_iyz" value="0.000001643194"/>
|
||||
<xacro:property name="hip_izz" value="0.00040057614"/>
|
||||
|
||||
<xacro:property name="hip_rotor_mass" value="0.089"/>
|
||||
<xacro:property name="hip_rotor_com_x" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_com_y" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_com_z" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_ixx" value="0.000111842"/>
|
||||
<xacro:property name="hip_rotor_ixy" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_ixz" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_iyy" value="0.000059647"/>
|
||||
<xacro:property name="hip_rotor_iyz" value="0.0"/>
|
||||
<xacro:property name="hip_rotor_izz" value="0.000059647"/>
|
||||
|
||||
<!-- thigh -->
|
||||
<xacro:property name="thigh_mass" value="0.92"/>
|
||||
<xacro:property name="thigh_com_x" value="-0.003342"/>
|
||||
<xacro:property name="thigh_com_y" value="-0.018054"/>
|
||||
<xacro:property name="thigh_com_z" value="-0.033451"/>
|
||||
<xacro:property name="thigh_ixx" value="0.004431760472"/>
|
||||
<xacro:property name="thigh_ixy" value="0.000057496807"/>
|
||||
<xacro:property name="thigh_ixz" value="-0.000218457134"/>
|
||||
<xacro:property name="thigh_iyy" value="0.004485671726"/>
|
||||
<xacro:property name="thigh_iyz" value="0.000572001265"/>
|
||||
<xacro:property name="thigh_izz" value="0.000740309489"/>
|
||||
|
||||
<xacro:property name="thigh_rotor_mass" value="0.089"/>
|
||||
<xacro:property name="thigh_rotor_com_x" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_com_y" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_com_z" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_ixx" value="0.000059647"/>
|
||||
<xacro:property name="thigh_rotor_ixy" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_ixz" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_iyy" value="0.000111842"/>
|
||||
<xacro:property name="thigh_rotor_iyz" value="0.0"/>
|
||||
<xacro:property name="thigh_rotor_izz" value="0.000059647"/>
|
||||
|
||||
<!-- calf -->
|
||||
<xacro:property name="calf_mass" value="0.135862"/>
|
||||
<xacro:property name="calf_com_x" value="0.006197"/>
|
||||
<xacro:property name="calf_com_y" value="0.001408"/>
|
||||
<xacro:property name="calf_com_z" value="-0.116695"/>
|
||||
<xacro:property name="calf_ixx" value="0.001088793059"/>
|
||||
<xacro:property name="calf_ixy" value="-0.000000255679"/>
|
||||
<xacro:property name="calf_ixz" value="0.000007117814"/>
|
||||
<xacro:property name="calf_iyy" value="0.001100428748"/>
|
||||
<xacro:property name="calf_iyz" value="0.000002077264"/>
|
||||
<xacro:property name="calf_izz" value="0.000024787446"/>
|
||||
|
||||
<xacro:property name="calf_rotor_mass" value="0.089"/>
|
||||
<xacro:property name="calf_rotor_com_x" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_com_y" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_com_z" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_ixx" value="0.000059647"/>
|
||||
<xacro:property name="calf_rotor_ixy" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_ixz" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_iyy" value="0.000111842"/>
|
||||
<xacro:property name="calf_rotor_iyz" value="0.0"/>
|
||||
<xacro:property name="calf_rotor_izz" value="0.000059647"/>
|
||||
|
||||
<!-- foot -->
|
||||
<xacro:property name="foot_mass" value="0.06"/>
|
||||
|
||||
</robot>
|
||||
98
src/robot_descriptions/qut/mdog_description/xacro/leg.xacro
Normal file
98
src/robot_descriptions/qut/mdog_description/xacro/leg.xacro
Normal file
@ -0,0 +1,98 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="leg" params="name mirror front_hind">
|
||||
|
||||
<!-- 定义hip_joint -->
|
||||
<joint name="${name}_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="${front_hind * 0.23} ${mirror * 0.15} 0"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="${name}_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_hip">
|
||||
<visual>
|
||||
<origin rpy="1.5708 0 0" xyz="0 ${mirror * -0.03} 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.05"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5708 0 0" xyz="0 ${mirror * -0.03} 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.10"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="${name}_thigh_joint" type="revolute">
|
||||
<origin rpy="0 -0.7854 0" xyz="0 ${mirror * 0.05} 0"/>
|
||||
<parent link="${name}_hip"/>
|
||||
<child link="${name}_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- 定义calf_joint -->
|
||||
<joint name="${name}_calf_joint" type="revolute">
|
||||
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
|
||||
<parent link="${name}_thigh"/>
|
||||
<child link="${name}_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.25 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
@ -0,0 +1,48 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="deep-grey">
|
||||
<color rgba="0.1 0.1 0.1 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="grey">
|
||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="light-grey">
|
||||
<color rgba="0.35 0.35 0.35 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="silver">
|
||||
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="orange">
|
||||
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="brown">
|
||||
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
|
||||
</robot>
|
||||
@ -0,0 +1,62 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="mdog" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:include filename="$(find mdog_description)/xacro/const.xacro"/>
|
||||
<xacro:include filename="$(find mdog_description)/xacro/materials.xacro"/>
|
||||
<xacro:include filename="$(find mdog_description)/xacro/leg.xacro"/>
|
||||
<xacro:include filename="$(find mdog_description)/xacro/ros2_control.xacro"/>
|
||||
|
||||
<link name="base">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="trunk">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.32 0.3 0.12"/>
|
||||
</geometry>
|
||||
<material name="light-grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.3 0.3 0.12"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_trunk" type="fixed">
|
||||
<parent link="base"/>
|
||||
<child link="trunk"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
<child link="imu_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.05"/>
|
||||
</joint>
|
||||
|
||||
<link name="imu_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<xacro:leg name="FR" mirror="1" front_hind="1"/>
|
||||
<xacro:leg name="FL" mirror="-1" front_hind="1"/>
|
||||
<xacro:leg name="RR" mirror="1" front_hind="-1"/>
|
||||
<xacro:leg name="RL" mirror="-1" front_hind="-1"/>
|
||||
|
||||
</robot>
|
||||
@ -0,0 +1,169 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<ros2_control name="MdogSystem" type="system">
|
||||
|
||||
<hardware>
|
||||
<plugin>mdog_hardware/mdog_hardware_interface</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="FR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_hip_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_thigh_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_calf_joint">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<command_interface name="effort"/>
|
||||
<command_interface name="kp"/>
|
||||
<command_interface name="kd"/>
|
||||
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<sensor name="imu_sensor">
|
||||
<state_interface name="orientation.w"/>
|
||||
<state_interface name="orientation.x"/>
|
||||
<state_interface name="orientation.y"/>
|
||||
<state_interface name="orientation.z"/>
|
||||
<state_interface name="angular_velocity.x"/>
|
||||
<state_interface name="angular_velocity.y"/>
|
||||
<state_interface name="angular_velocity.z"/>
|
||||
<state_interface name="linear_acceleration.x"/>
|
||||
<state_interface name="linear_acceleration.y"/>
|
||||
<state_interface name="linear_acceleration.z"/>
|
||||
</sensor>
|
||||
|
||||
</ros2_control>
|
||||
|
||||
</robot>
|
||||
Loading…
Reference in New Issue
Block a user