Compare commits
4 Commits
main
...
ros2_contr
| Author | SHA1 | Date | |
|---|---|---|---|
| 7d2230b092 | |||
| bca5ab3a3b | |||
| 30d3d2fb3a | |||
|
078a375ac5 |
5
.vscode/settings.json
vendored
Normal file
5
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,5 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"functional": "cpp"
|
||||
}
|
||||
}
|
||||
@ -1,3 +1,4 @@
|
||||
# CM_DOG
|
||||
|
||||
A simple ros2 program for legged robot . Robocon2025
|
||||
A simple ros2 program for legged robot . Robocon2025
|
||||
|
||||
|
||||
3
src/commands/README.md
Normal file
3
src/commands/README.md
Normal file
@ -0,0 +1,3 @@
|
||||
# Control Command Inputs
|
||||
|
||||
This folder contains the ros2 node for control input, like keyboard or gamepad.
|
||||
9
src/commands/control_input_msgs/CMakeLists.txt
Normal file
9
src/commands/control_input_msgs/CMakeLists.txt
Normal file
@ -0,0 +1,9 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(control_input_msgs)
|
||||
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/Inputs.msg"
|
||||
)
|
||||
|
||||
ament_package()
|
||||
8
src/commands/control_input_msgs/msg/Inputs.msg
Normal file
8
src/commands/control_input_msgs/msg/Inputs.msg
Normal file
@ -0,0 +1,8 @@
|
||||
# control input message
|
||||
|
||||
int32 command
|
||||
|
||||
float32 lx
|
||||
float32 ly
|
||||
float32 rx
|
||||
float32 ry
|
||||
17
src/commands/control_input_msgs/package.xml
Normal file
17
src/commands/control_input_msgs/package.xml
Normal file
@ -0,0 +1,17 @@
|
||||
<?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>control_input_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="biao876990970@hotmail.com">biao</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
49
src/commands/joystick_input/CMakeLists.txt
Normal file
49
src/commands/joystick_input/CMakeLists.txt
Normal file
@ -0,0 +1,49 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(joystick_input)
|
||||
|
||||
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif ()
|
||||
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(control_input_msgs REQUIRED)
|
||||
|
||||
add_executable(joystick_input src/JoystickInput.cpp)
|
||||
target_include_directories(joystick_input
|
||||
PUBLIC
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
|
||||
ament_target_dependencies(
|
||||
joystick_input PUBLIC
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
control_input_msgs
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
joystick_input
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(
|
||||
DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
if (BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif ()
|
||||
|
||||
ament_package()
|
||||
34
src/commands/joystick_input/README.md
Normal file
34
src/commands/joystick_input/README.md
Normal file
@ -0,0 +1,34 @@
|
||||
# Joystick Input
|
||||
|
||||
This node will listen to the joystick topic and publish a control_input_msgs/Input message.
|
||||
|
||||
Tested environment:
|
||||
* Ubuntu 24.04
|
||||
* ROS2 Jazzy
|
||||
* Logitech F310 Gamepad
|
||||
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to joystick_input
|
||||
```
|
||||
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch joystick_input joystick.launch.py
|
||||
```
|
||||
|
||||
## 1. Use Instructions for Unitree Guide
|
||||
|
||||
### 1.1 Control Mode
|
||||
|
||||
* Passive Mode: LB + B
|
||||
* Fixed Stand: LB + A
|
||||
* Free Stand: LB + X
|
||||
* Trot: LB + Y
|
||||
* SwingTest: LT + B
|
||||
* Balance: LT + A
|
||||
|
||||
### 1.2 Control Input
|
||||
|
||||
* WASD IJKL: Move robot
|
||||
* Space: Reset Speed Input
|
||||
@ -0,0 +1,27 @@
|
||||
//
|
||||
// Created by tlab-uav on 24-9-13.
|
||||
//
|
||||
|
||||
#ifndef JOYSTICKINPUT_H
|
||||
#define JOYSTICKINPUT_H
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/joy.hpp>
|
||||
#include <control_input_msgs/msg/inputs.hpp>
|
||||
|
||||
|
||||
class JoystickInput final : public rclcpp::Node {
|
||||
public:
|
||||
JoystickInput();
|
||||
|
||||
~JoystickInput() override = default;
|
||||
|
||||
private:
|
||||
void joy_callback(sensor_msgs::msg::Joy::SharedPtr msg);
|
||||
|
||||
control_input_msgs::msg::Inputs inputs_;
|
||||
rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
|
||||
};
|
||||
|
||||
|
||||
#endif //JOYSTICKINPUT_H
|
||||
20
src/commands/joystick_input/launch/joystick.launch.py
Normal file
20
src/commands/joystick_input/launch/joystick.launch.py
Normal file
@ -0,0 +1,20 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='joy',
|
||||
executable='joy_node',
|
||||
name='joynode',
|
||||
parameters=[{
|
||||
'dev': '/dev/input/js0'
|
||||
}]
|
||||
),
|
||||
Node(
|
||||
package='joystick_input',
|
||||
executable='joystick_input',
|
||||
name='joystick_input_node'
|
||||
)
|
||||
])
|
||||
19
src/commands/joystick_input/package.xml
Normal file
19
src/commands/joystick_input/package.xml
Normal file
@ -0,0 +1,19 @@
|
||||
<?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>joystick_input</name>
|
||||
<version>0.0.0</version>
|
||||
<description>A package to convert joystick signal to control input</description>
|
||||
<maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>control_input_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
50
src/commands/joystick_input/src/JoystickInput.cpp
Normal file
50
src/commands/joystick_input/src/JoystickInput.cpp
Normal file
@ -0,0 +1,50 @@
|
||||
//
|
||||
// Created by tlab-uav on 24-9-13.
|
||||
//
|
||||
|
||||
#include "joystick_input/JoystickInput.h"
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
JoystickInput::JoystickInput() : Node("joysick_input_node") {
|
||||
publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
|
||||
subscription_ = create_subscription<
|
||||
sensor_msgs::msg::Joy>("joy", 10, std::bind(&JoystickInput::joy_callback, this, _1));
|
||||
}
|
||||
|
||||
void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
|
||||
if (msg->buttons[1] && msg->buttons[4]) {
|
||||
inputs_.command = 1; // LB + B
|
||||
} else if (msg->buttons[0] && msg->buttons[4]) {
|
||||
inputs_.command = 2; // LB + A
|
||||
} else if (msg->buttons[2] && msg->buttons[4]) {
|
||||
inputs_.command = 3; // LB + X
|
||||
} else if (msg->buttons[3] && msg->buttons[4]) {
|
||||
inputs_.command = 4; // LB + Y
|
||||
} else if (msg->axes[2] != 1 && msg->buttons[1]) {
|
||||
inputs_.command = 5; // LT + B
|
||||
} else if (msg->axes[2] != 1 && msg->buttons[0]) {
|
||||
inputs_.command = 6; // LT + A
|
||||
} else if (msg->axes[2] != 1 && msg->buttons[2]) {
|
||||
inputs_.command = 7; // LT + X
|
||||
} else if (msg->axes[2] != 1 && msg->buttons[3]) {
|
||||
inputs_.command = 8; // LT + Y
|
||||
} else if (msg->buttons[7]) {
|
||||
inputs_.command = 9; // START
|
||||
} else {
|
||||
inputs_.command = 0;
|
||||
inputs_.lx = -msg->axes[0];
|
||||
inputs_.ly = msg->axes[1];
|
||||
inputs_.rx = -msg->axes[3];
|
||||
inputs_.ry = msg->axes[4];
|
||||
}
|
||||
publisher_->publish(inputs_);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<JoystickInput>();
|
||||
spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
30
src/commands/keyboard_input/CMakeLists.txt
Normal file
30
src/commands/keyboard_input/CMakeLists.txt
Normal file
@ -0,0 +1,30 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(keyboard_input)
|
||||
|
||||
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif ()
|
||||
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(control_input_msgs REQUIRED)
|
||||
|
||||
add_executable(keyboard_input src/KeyboardInput.cpp)
|
||||
target_include_directories(keyboard_input
|
||||
PUBLIC
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
|
||||
ament_target_dependencies(
|
||||
keyboard_input PUBLIC
|
||||
rclcpp
|
||||
control_input_msgs
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
keyboard_input
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
33
src/commands/keyboard_input/README.md
Normal file
33
src/commands/keyboard_input/README.md
Normal file
@ -0,0 +1,33 @@
|
||||
# Keyboard Input
|
||||
|
||||
This node will read the keyboard input and publish a control_input_msgs/Input message.
|
||||
|
||||
Tested environment:
|
||||
* Ubuntu 24.04
|
||||
* ROS2 Jazzy
|
||||
* Ubuntu 22.04
|
||||
* ROS2 Humble
|
||||
|
||||
### Build Command
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to keyboard_input
|
||||
```
|
||||
|
||||
### Launch Command
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 run keyboard_input keyboard_input
|
||||
```
|
||||
|
||||
## 1. Use Instructions for Unitree Guide
|
||||
### 1.1 Control Mode
|
||||
* Passive Mode: Keyboard 1
|
||||
* Fixed Stand: Keyboard 2
|
||||
* Free Stand: Keyboard 3
|
||||
* Trot: Keyboard 4
|
||||
* SwingTest: Keyboard 5
|
||||
* Balance: Keyboard 6
|
||||
### 1.2 Control Input
|
||||
* WASD IJKL: Move robot
|
||||
* Space: Reset Speed Input
|
||||
@ -0,0 +1,53 @@
|
||||
//
|
||||
// Created by biao on 24-9-11.
|
||||
//
|
||||
|
||||
|
||||
#ifndef KEYBOARDINPUT_H
|
||||
#define KEYBOARDINPUT_H
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <control_input_msgs/msg/inputs.hpp>
|
||||
#include <termios.h>
|
||||
|
||||
|
||||
template <typename T1, typename T2>
|
||||
T1 max(const T1 a, const T2 b) {
|
||||
return (a > b ? a : b);
|
||||
}
|
||||
|
||||
template <typename T1, typename T2>
|
||||
T1 min(const T1 a, const T2 b) {
|
||||
return (a < b ? a : b);
|
||||
}
|
||||
|
||||
|
||||
class KeyboardInput final : public rclcpp::Node {
|
||||
public:
|
||||
KeyboardInput();
|
||||
|
||||
~KeyboardInput() override {
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &old_tio_);
|
||||
}
|
||||
|
||||
private:
|
||||
void timer_callback();
|
||||
|
||||
void check_command(char key);
|
||||
void check_value(char key);
|
||||
|
||||
static bool kbhit();
|
||||
|
||||
control_input_msgs::msg::Inputs inputs_;
|
||||
rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
bool just_published_ = false;
|
||||
int reset_count_ = 0;
|
||||
|
||||
float sensitivity_left_ = 0.05;
|
||||
float sensitivity_right_ = 0.05;
|
||||
termios old_tio_{}, new_tio_{};
|
||||
};
|
||||
|
||||
|
||||
#endif //KEYBOARDINPUT_H
|
||||
18
src/commands/keyboard_input/package.xml
Normal file
18
src/commands/keyboard_input/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>keyboard_input</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="biao876990970@hotmail.com">biao</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>control_input_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
150
src/commands/keyboard_input/src/KeyboardInput.cpp
Normal file
150
src/commands/keyboard_input/src/KeyboardInput.cpp
Normal file
@ -0,0 +1,150 @@
|
||||
//
|
||||
// Created by biao on 24-9-11.
|
||||
//
|
||||
|
||||
#include <keyboard_input/KeyboardInput.h>
|
||||
|
||||
KeyboardInput::KeyboardInput() : Node("keyboard_input_node") {
|
||||
publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
|
||||
timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&KeyboardInput::timer_callback, this));
|
||||
inputs_ = control_input_msgs::msg::Inputs();
|
||||
|
||||
tcgetattr(STDIN_FILENO, &old_tio_);
|
||||
new_tio_ = old_tio_;
|
||||
new_tio_.c_lflag &= (~ICANON & ~ECHO);
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &new_tio_);
|
||||
RCLCPP_INFO(get_logger(), "Keyboard input node started.");
|
||||
RCLCPP_INFO(get_logger(), "Press 1-0 to switch between different modes");
|
||||
RCLCPP_INFO(get_logger(), "Use W/S/A/D and I/K/J/L to move the robot.");
|
||||
RCLCPP_INFO(get_logger(), "Please input keys, press Ctrl+C to quit.");
|
||||
}
|
||||
|
||||
void KeyboardInput::timer_callback() {
|
||||
if (kbhit()) {
|
||||
char key = getchar();
|
||||
check_command(key);
|
||||
if (inputs_.command == 0) check_value(key);
|
||||
else {
|
||||
inputs_.lx = 0;
|
||||
inputs_.ly = 0;
|
||||
inputs_.rx = 0;
|
||||
inputs_.ry = 0;
|
||||
reset_count_ = 100;
|
||||
}
|
||||
publisher_->publish(inputs_);
|
||||
just_published_ = true;
|
||||
} else {
|
||||
if (just_published_) {
|
||||
reset_count_ -= 1;
|
||||
if (reset_count_ == 0) {
|
||||
just_published_ = false;
|
||||
if (inputs_.command != 0) {
|
||||
inputs_.command = 0;
|
||||
publisher_->publish(inputs_);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardInput::check_command(const char key) {
|
||||
switch (key) {
|
||||
case '1':
|
||||
inputs_.command = 1; // L2_B
|
||||
break;
|
||||
case '2':
|
||||
inputs_.command = 2; // L2_A
|
||||
break;
|
||||
case '3':
|
||||
inputs_.command = 3; // L2_X
|
||||
break;
|
||||
case '4':
|
||||
inputs_.command = 4; // L2_Y
|
||||
break;
|
||||
case '5':
|
||||
inputs_.command = 5; // L1_A
|
||||
break;
|
||||
case '6':
|
||||
inputs_.command = 6; // L1_B
|
||||
break;
|
||||
case '7':
|
||||
inputs_.command = 7; // L1_X
|
||||
break;
|
||||
case '8':
|
||||
inputs_.command = 8; // L1_Y
|
||||
break;
|
||||
case '9':
|
||||
inputs_.command = 9;
|
||||
break;
|
||||
case '0':
|
||||
inputs_.command = 10;
|
||||
break;
|
||||
case ' ':
|
||||
inputs_.lx = 0;
|
||||
inputs_.ly = 0;
|
||||
inputs_.rx = 0;
|
||||
inputs_.ry = 0;
|
||||
inputs_.command = 0;
|
||||
break;
|
||||
default:
|
||||
inputs_.command = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardInput::check_value(char key) {
|
||||
switch (key) {
|
||||
case 'w':
|
||||
case 'W':
|
||||
inputs_.ly = min<float>(inputs_.ly + sensitivity_left_, 1.0);
|
||||
break;
|
||||
case 's':
|
||||
case 'S':
|
||||
inputs_.ly = max<float>(inputs_.ly - sensitivity_left_, -1.0);
|
||||
break;
|
||||
case 'd':
|
||||
case 'D':
|
||||
inputs_.lx = min<float>(inputs_.lx + sensitivity_left_, 1.0);
|
||||
break;
|
||||
case 'a':
|
||||
case 'A':
|
||||
inputs_.lx = max<float>(inputs_.lx - sensitivity_left_, -1.0);
|
||||
break;
|
||||
|
||||
case 'i':
|
||||
case 'I':
|
||||
inputs_.ry = min<float>(inputs_.ry + sensitivity_right_, 1.0);
|
||||
break;
|
||||
case 'k':
|
||||
case 'K':
|
||||
inputs_.ry = max<float>(inputs_.ry - sensitivity_right_, -1.0);
|
||||
break;
|
||||
case 'l':
|
||||
case 'L':
|
||||
inputs_.rx = min<float>(inputs_.rx + sensitivity_right_, 1.0);
|
||||
break;
|
||||
case 'j':
|
||||
case 'J':
|
||||
inputs_.rx = max<float>(inputs_.rx - sensitivity_right_, -1.0);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool KeyboardInput::kbhit() {
|
||||
timeval tv = {0L, 0L};
|
||||
fd_set fds;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(STDIN_FILENO, &fds);
|
||||
return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<KeyboardInput>();
|
||||
spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
0
src/controllers/trot.py
Normal file
0
src/controllers/trot.py
Normal file
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
|
||||
27
src/hardware/fdilink_ahrs_ROS2/package.xml
Normal file
27
src/hardware/fdilink_ahrs_ROS2/package.xml
Normal file
@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fdilink_ahrs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="noah@todo.todo">noah</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
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
|
||||
|
||||
|
||||
93
src/hardware/mdog_hardware/CMakeLists.txt
Normal file
93
src/hardware/mdog_hardware/CMakeLists.txt
Normal file
@ -0,0 +1,93 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(mdog_hardware)
|
||||
|
||||
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(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
|
||||
add_library(
|
||||
mdog_hardware
|
||||
SHARED
|
||||
src/mdog_hardware_interface.cpp
|
||||
)
|
||||
target_include_directories(
|
||||
mdog_hardware
|
||||
PUBLIC
|
||||
include
|
||||
)
|
||||
ament_target_dependencies(
|
||||
mdog_hardware
|
||||
hardware_interface
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
)
|
||||
# prevent pluginlib from using boost
|
||||
target_compile_definitions(mdog_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
|
||||
|
||||
pluginlib_export_plugin_description_file(
|
||||
hardware_interface mdog_hardware.xml)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
mdog_hardware
|
||||
RUNTIME DESTINATION bin
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
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
|
||||
# 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
|
||||
)
|
||||
ament_export_dependencies(
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -0,0 +1,79 @@
|
||||
// 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__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
#define MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "mdog_hardware/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"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
|
||||
namespace mdog_hardware
|
||||
{
|
||||
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_;
|
||||
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
|
||||
|
||||
#endif // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
@ -0,0 +1,49 @@
|
||||
// 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__VISIBILITY_CONTROL_H_
|
||||
#define MDOG_HARDWARE__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__VISIBILITY_CONTROL_H_
|
||||
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
|
||||
])
|
||||
9
src/hardware/mdog_hardware/mdog_hardware.xml
Normal file
9
src/hardware/mdog_hardware/mdog_hardware.xml
Normal file
@ -0,0 +1,9 @@
|
||||
<library path="mdog_hardware">
|
||||
<class name="mdog_hardware/MDogHardwareInterface"
|
||||
type="mdog_hardware::MDogHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ros2_control hardware interface.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
30
src/hardware/mdog_hardware/package.xml
Normal file
30
src/hardware/mdog_hardware/package.xml
Normal file
@ -0,0 +1,30 @@
|
||||
<?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</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_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>
|
||||
</export>
|
||||
</package>
|
||||
165
src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp
Normal file
165
src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp
Normal file
@ -0,0 +1,165 @@
|
||||
// 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/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)
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// 初始化硬件状态和命令
|
||||
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;
|
||||
}
|
||||
|
||||
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(
|
||||
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;
|
||||
}
|
||||
|
||||
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(
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mdog_hardware::MDogHardwareInterface, hardware_interface::SystemInterface)
|
||||
@ -0,0 +1,57 @@
|
||||
// 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/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));
|
||||
}
|
||||
53
src/hardware/unitree_leg_serial_driver/CMakeLists.txt
Normal file
53
src/hardware/unitree_leg_serial_driver/CMakeLists.txt
Normal file
@ -0,0 +1,53 @@
|
||||
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(rc_msgs REQUIRED)
|
||||
# 添加头文件目录
|
||||
include_directories(include)
|
||||
|
||||
# 添加源文件并生成共享库
|
||||
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
|
||||
rc_msgs
|
||||
)
|
||||
|
||||
# 注册组件
|
||||
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,58 @@
|
||||
#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"
|
||||
#include "rc_msgs/msg/go_motor_cmd.hpp"
|
||||
#include "rc_msgs/msg/go_motor_fdb.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();
|
||||
void motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg);
|
||||
|
||||
int send_count_;
|
||||
int recv_count_;
|
||||
rclcpp::Time last_freq_time_;
|
||||
|
||||
rclcpp::Publisher<rc_msgs::msg::GoMotorFdb>::SharedPtr motor_fdb_pub_;
|
||||
rclcpp::Subscription<rc_msgs::msg::GoMotorCmd>::SharedPtr motor_cmd_sub_;
|
||||
|
||||
|
||||
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"
|
||||
)
|
||||
])
|
||||
35
src/hardware/unitree_leg_serial_driver/package.xml
Normal file
35
src/hardware/unitree_leg_serial_driver/package.xml
Normal file
@ -0,0 +1,35 @@
|
||||
<?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>
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rclcpp_components</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>serial</build_depend>
|
||||
<build_depend>rc_msgs</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>
|
||||
<exec_depend>rc_msgs</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,226 @@
|
||||
#include "unitree_leg_serial_driver/unitree_leg_serial.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
#include "rc_msgs/msg/go_motor_cmd.hpp"
|
||||
#include "rc_msgs/msg/go_motor_fdb.hpp"
|
||||
namespace unitree_leg_serial
|
||||
{
|
||||
|
||||
UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
|
||||
: Node("unitree_leg_serial", options)
|
||||
{
|
||||
serial_port_name_ = "/dev/ttyACM0";
|
||||
baud_rate_ = 4000000;
|
||||
|
||||
send_count_ = 0;
|
||||
recv_count_ = 0;
|
||||
last_freq_time_ = this->now();
|
||||
|
||||
// 发布器
|
||||
motor_fdb_pub_ = this->create_publisher<rc_msgs::msg::GoMotorFdb>("motor_fdb", 10);
|
||||
// 订阅器
|
||||
motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::GoMotorCmd>(
|
||||
"motor_cmd", 10,
|
||||
std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1));
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
void UnitreeLegSerial::motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg)
|
||||
{
|
||||
// 填充motor_cmd_结构体
|
||||
motor_cmd_.T = msg->torque_des;
|
||||
motor_cmd_.W = msg->speed_des;
|
||||
motor_cmd_.Pos = msg->pos_des;
|
||||
motor_cmd_.K_P = msg->kp;
|
||||
motor_cmd_.K_W = msg->kd;
|
||||
status_flag_ = CONTROLED;
|
||||
tick_ = 0;
|
||||
}
|
||||
|
||||
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_);
|
||||
send_count_++;
|
||||
|
||||
// 每秒打印一次频率
|
||||
auto now = this->now();
|
||||
if ((now - last_freq_time_).seconds() >= 1.0) {
|
||||
RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_);
|
||||
send_count_ = 0;
|
||||
recv_count_ = 0;
|
||||
last_freq_time_ = now;
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
// 发布反馈消息
|
||||
rc_msgs::msg::GoMotorFdb msg;
|
||||
msg.torque = motor_fbk_.T;
|
||||
msg.speed = motor_fbk_.W;
|
||||
msg.pos = motor_fbk_.Pos;
|
||||
motor_fdb_pub_->publish(msg);
|
||||
recv_count_++;
|
||||
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)
|
||||
51
src/rc_msgs/.gitignore
vendored
Normal file
51
src/rc_msgs/.gitignore
vendored
Normal file
@ -0,0 +1,51 @@
|
||||
devel/
|
||||
logs/
|
||||
build/
|
||||
bin/
|
||||
lib/
|
||||
msg_gen/
|
||||
srv_gen/
|
||||
msg/*Action.msg
|
||||
msg/*ActionFeedback.msg
|
||||
msg/*ActionGoal.msg
|
||||
msg/*ActionResult.msg
|
||||
msg/*Feedback.msg
|
||||
msg/*Goal.msg
|
||||
msg/*Result.msg
|
||||
msg/_*.py
|
||||
build_isolated/
|
||||
devel_isolated/
|
||||
|
||||
# Generated by dynamic reconfigure
|
||||
*.cfgc
|
||||
/cfg/cpp/
|
||||
/cfg/*.py
|
||||
|
||||
# Ignore generated docs
|
||||
*.dox
|
||||
*.wikidoc
|
||||
|
||||
# eclipse stuff
|
||||
.project
|
||||
.cproject
|
||||
|
||||
# qcreator stuff
|
||||
CMakeLists.txt.user
|
||||
|
||||
srv/_*.py
|
||||
*.pcd
|
||||
*.pyc
|
||||
qtcreator-*
|
||||
*.user
|
||||
|
||||
/planning/cfg
|
||||
/planning/docs
|
||||
/planning/src
|
||||
|
||||
*~
|
||||
|
||||
# Emacs
|
||||
.#*
|
||||
|
||||
# Catkin custom files
|
||||
CATKIN_IGNORE
|
||||
17
src/rc_msgs/CMakeLists.txt
Normal file
17
src/rc_msgs/CMakeLists.txt
Normal file
@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(rc_msgs)
|
||||
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/DataMCU.msg"
|
||||
"msg/DataRef.msg"
|
||||
"msg/DataAI.msg"
|
||||
"msg/Ps2Data.msg"
|
||||
"msg/GoalPose.msg"
|
||||
"msg/DataNav.msg"
|
||||
"msg/GoMotorCmd.msg"
|
||||
"msg/GoMotorFdb.msg"
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
||||
21
src/rc_msgs/LICENSE
Normal file
21
src/rc_msgs/LICENSE
Normal file
@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 zucheng Lv
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
6
src/rc_msgs/README.md
Normal file
6
src/rc_msgs/README.md
Normal file
@ -0,0 +1,6 @@
|
||||
# rc_msg
|
||||
Some ROS 2 custom messages for Robocon
|
||||
|
||||
Usage
|
||||
Modify or add files in the /msg directory as needed
|
||||
colcon build
|
||||
7
src/rc_msgs/msg/DataAI.msg
Normal file
7
src/rc_msgs/msg/DataAI.msg
Normal file
@ -0,0 +1,7 @@
|
||||
float32 yaw
|
||||
float32 pit
|
||||
float32 rol
|
||||
float32 vx
|
||||
float32 vy
|
||||
float32 wz
|
||||
uint8 notice
|
||||
8
src/rc_msgs/msg/DataMCU.msg
Normal file
8
src/rc_msgs/msg/DataMCU.msg
Normal file
@ -0,0 +1,8 @@
|
||||
float32 q0
|
||||
float32 q1
|
||||
float32 q2
|
||||
float32 q3
|
||||
float32 yaw
|
||||
float32 pit
|
||||
float32 rol
|
||||
uint8 notice
|
||||
5
src/rc_msgs/msg/DataNav.msg
Normal file
5
src/rc_msgs/msg/DataNav.msg
Normal file
@ -0,0 +1,5 @@
|
||||
bool reached
|
||||
|
||||
float32 x
|
||||
float32 y
|
||||
float32 yaw
|
||||
3
src/rc_msgs/msg/DataRef.msg
Normal file
3
src/rc_msgs/msg/DataRef.msg
Normal file
@ -0,0 +1,3 @@
|
||||
uint16 remain_hp
|
||||
uint8 game_progress
|
||||
uint16 stage_remain_time
|
||||
5
src/rc_msgs/msg/GoMotorCmd.msg
Normal file
5
src/rc_msgs/msg/GoMotorCmd.msg
Normal file
@ -0,0 +1,5 @@
|
||||
float32 torque_des
|
||||
float32 speed_des
|
||||
float32 pos_des
|
||||
float32 kp
|
||||
float32 kd
|
||||
3
src/rc_msgs/msg/GoMotorFdb.msg
Normal file
3
src/rc_msgs/msg/GoMotorFdb.msg
Normal file
@ -0,0 +1,3 @@
|
||||
float32 torque
|
||||
float32 speed
|
||||
float32 pos
|
||||
8
src/rc_msgs/msg/GoalPose.msg
Normal file
8
src/rc_msgs/msg/GoalPose.msg
Normal file
@ -0,0 +1,8 @@
|
||||
float32 x
|
||||
float32 y
|
||||
float32 angle
|
||||
|
||||
float32 max_speed
|
||||
float32 tolerance
|
||||
|
||||
bool rotor
|
||||
20
src/rc_msgs/msg/Ps2Data.msg
Normal file
20
src/rc_msgs/msg/Ps2Data.msg
Normal file
@ -0,0 +1,20 @@
|
||||
# control input message
|
||||
float32 lx
|
||||
float32 ly
|
||||
float32 rx
|
||||
float32 ry
|
||||
|
||||
float32 up_down
|
||||
float32 left_right
|
||||
|
||||
bool l1
|
||||
bool l2
|
||||
bool r1
|
||||
bool r2
|
||||
|
||||
# 四种模式
|
||||
uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
|
||||
|
||||
bool select
|
||||
bool start
|
||||
|
||||
16
src/rc_msgs/package.xml
Normal file
16
src/rc_msgs/package.xml
Normal file
@ -0,0 +1,16 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>rc_msgs</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Describe custom messages</description>
|
||||
<maintainer email="1683502971@qq.com">biao</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
68
src/robot_descriptions/README.md
Normal file
68
src/robot_descriptions/README.md
Normal file
@ -0,0 +1,68 @@
|
||||
# Robot Descriptions
|
||||
|
||||
This folder contains the URDF and SRDF files for the quadruped robot.
|
||||
|
||||
* Unitree
|
||||
* [Go1](unitree/go1_description/)
|
||||
* [Go2](unitree/go2_description/)
|
||||
* [A1](unitree/a1_description/)
|
||||
* [Aliengo](unitree/aliengo_description/)
|
||||
* [B2](unitree/b2_description/)
|
||||
* Xiaomi
|
||||
* [Cyberdog](xiaomi/cyberdog_description/)
|
||||
* Deep Robotics
|
||||
* [Lite 3](deep_robotics/lite3_description/)
|
||||
* [X30](deep_robotics/x30_description/)
|
||||
* Anybotics
|
||||
* [Anymal C](anybotics/anymal_c_description/)
|
||||
|
||||
## 1. Steps to transfer urdf to Mujoco model
|
||||
|
||||
* Install [Mujoco](https://github.com/google-deepmind/mujoco)
|
||||
* Transfer the mesh files to mujoco supported format, like stl.
|
||||
* Adjust the urdf tile to match the mesh file. Transfer the mesh file from .dae to .stl may change the scale size of the
|
||||
mesh file.
|
||||
* use `xacro` to generate the urdf file.
|
||||
```
|
||||
xacro robot.xacro > ../urdf/robot.urdf
|
||||
```
|
||||
* use mujoco to convert the urdf file to mujoco model.
|
||||
```
|
||||
compile robot.urdf robot.xml
|
||||
```
|
||||
|
||||
## 2. Dependencies for Gazebo Simulation
|
||||
|
||||
Gazebo Simulation only tested on ROS2 Jazzy. It add support for ROS2 Humble because the package name is different.
|
||||
|
||||
* Gazebo Harmonic
|
||||
```bash
|
||||
sudo apt-get install ros-jazzy-ros-gz
|
||||
```
|
||||
* Ros2-Control for Gazebo
|
||||
```bash
|
||||
sudo apt-get install ros-jazzy-gz-ros2-control
|
||||
```
|
||||
* Legged PD Controller
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to leg_pd_controller
|
||||
```
|
||||
|
||||
## 2. Dependencies for Gazebo Classic Simulation
|
||||
|
||||
Gazebo Classic (Gazebo11) Simulation only tested on ROS2 Humble.
|
||||
|
||||
* Gazebo Classic
|
||||
```bash
|
||||
sudo apt-get install ros-humble-gazebo-ros
|
||||
```
|
||||
* Ros2-Control for Gazebo
|
||||
```bash
|
||||
sudo apt-get install ros-humble-gazebo-ros2-control
|
||||
```
|
||||
* Legged PD Controller
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to leg_pd_controller
|
||||
```
|
||||
@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(anymal_c_description)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY meshes xacro launch config urdf
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -0,0 +1,29 @@
|
||||
Copyright 2020, ANYbotics AG.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
|
||||
3. Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived
|
||||
from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@ -0,0 +1,39 @@
|
||||
# Anybotics Anymal_C Description
|
||||
This repository contains the urdf model of lite3.
|
||||
|
||||
|
||||
|
||||
Tested environment:
|
||||
* Ubuntu 24.04
|
||||
* ROS2 Jazzy
|
||||
|
||||
## Build
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to anymal_c_description --symlink-install
|
||||
```
|
||||
|
||||
## Visualize the robot
|
||||
To visualize and check the configuration of the robot in rviz, simply launch:
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch anymal_c_description visualize.launch.py
|
||||
```
|
||||
|
||||
## Launch ROS2 Control
|
||||
### Mujoco Simulator
|
||||
* Unitree Guide Controller
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=anymal_c_description
|
||||
```
|
||||
* OCS2 Quadruped Controller
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
|
||||
```
|
||||
* Legged Gym Controller
|
||||
```bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
|
||||
```
|
||||
@ -0,0 +1,255 @@
|
||||
list
|
||||
{
|
||||
[0] stance
|
||||
[1] trot
|
||||
[2] standing_trot
|
||||
[3] flying_trot
|
||||
[4] pace
|
||||
[5] standing_pace
|
||||
[6] dynamic_walk
|
||||
[7] static_walk
|
||||
[8] amble
|
||||
[9] lindyhop
|
||||
[10] skipping
|
||||
[11] pawup
|
||||
}
|
||||
|
||||
stance
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] STANCE
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.5
|
||||
}
|
||||
}
|
||||
|
||||
trot
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RH
|
||||
[1] RF_LH
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.35
|
||||
[2] 0.70
|
||||
}
|
||||
}
|
||||
|
||||
standing_trot
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RH
|
||||
[1] STANCE
|
||||
[2] RF_LH
|
||||
[3] STANCE
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.00
|
||||
[1] 0.30
|
||||
[2] 0.35
|
||||
[3] 0.65
|
||||
[4] 0.70
|
||||
}
|
||||
}
|
||||
|
||||
flying_trot
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RH
|
||||
[1] FLY
|
||||
[2] RF_LH
|
||||
[3] FLY
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.00
|
||||
[1] 0.27
|
||||
[2] 0.30
|
||||
[3] 0.57
|
||||
[4] 0.60
|
||||
}
|
||||
}
|
||||
|
||||
pace
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_LH
|
||||
[1] FLY
|
||||
[2] RF_RH
|
||||
[3] FLY
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.28
|
||||
[2] 0.30
|
||||
[3] 0.58
|
||||
[4] 0.60
|
||||
}
|
||||
}
|
||||
|
||||
standing_pace
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_LH
|
||||
[1] STANCE
|
||||
[2] RF_RH
|
||||
[3] STANCE
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.30
|
||||
[2] 0.35
|
||||
[3] 0.65
|
||||
[4] 0.70
|
||||
}
|
||||
}
|
||||
|
||||
dynamic_walk
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RF_RH
|
||||
[1] RF_RH
|
||||
[2] RF_LH_RH
|
||||
[3] LF_RF_LH
|
||||
[4] LF_LH
|
||||
[5] LF_LH_RH
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.2
|
||||
[2] 0.3
|
||||
[3] 0.5
|
||||
[4] 0.7
|
||||
[5] 0.8
|
||||
[6] 1.0
|
||||
}
|
||||
}
|
||||
|
||||
static_walk
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RF_RH
|
||||
[1] RF_LH_RH
|
||||
[2] LF_RF_LH
|
||||
[3] LF_LH_RH
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.3
|
||||
[2] 0.6
|
||||
[3] 0.9
|
||||
[4] 1.2
|
||||
}
|
||||
}
|
||||
|
||||
amble
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] RF_LH
|
||||
[1] LF_LH
|
||||
[2] LF_RH
|
||||
[3] RF_RH
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 0.15
|
||||
[2] 0.40
|
||||
[3] 0.55
|
||||
[4] 0.80
|
||||
}
|
||||
}
|
||||
|
||||
lindyhop
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RH
|
||||
[1] STANCE
|
||||
[2] RF_LH
|
||||
[3] STANCE
|
||||
[4] LF_LH
|
||||
[5] RF_RH
|
||||
[6] LF_LH
|
||||
[7] STANCE
|
||||
[8] RF_RH
|
||||
[9] LF_LH
|
||||
[10] RF_RH
|
||||
[11] STANCE
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.00 ; Step 1
|
||||
[1] 0.35 ; Stance
|
||||
[2] 0.45 ; Step 2
|
||||
[3] 0.80 ; Stance
|
||||
[4] 0.90 ; Tripple step
|
||||
[5] 1.125 ;
|
||||
[6] 1.35 ;
|
||||
[7] 1.70 ; Stance
|
||||
[8] 1.80 ; Tripple step
|
||||
[9] 2.025 ;
|
||||
[10] 2.25 ;
|
||||
[11] 2.60 ; Stance
|
||||
[12] 2.70 ;
|
||||
}
|
||||
}
|
||||
|
||||
skipping
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] LF_RH
|
||||
[1] FLY
|
||||
[2] LF_RH
|
||||
[3] FLY
|
||||
[4] RF_LH
|
||||
[5] FLY
|
||||
[6] RF_LH
|
||||
[7] FLY
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.00
|
||||
[1] 0.27
|
||||
[2] 0.30
|
||||
[3] 0.57
|
||||
[4] 0.60
|
||||
[5] 0.87
|
||||
[6] 0.90
|
||||
[7] 1.17
|
||||
[8] 1.20
|
||||
}
|
||||
}
|
||||
|
||||
pawup
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] RF_LH_RH
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 2.0
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,46 @@
|
||||
targetDisplacementVelocity 0.5;
|
||||
targetRotationVelocity 1.57;
|
||||
|
||||
comHeight 0.6;
|
||||
|
||||
defaultJointState
|
||||
{
|
||||
(0,0) -0.25 ; LF_HAA
|
||||
(1,0) 0.60 ; LF_HFE
|
||||
(2,0) -0.85 ; LF_KFE
|
||||
(3,0) -0.25 ; LH_HAA
|
||||
(4,0) -0.60 ; LH_HFE
|
||||
(5,0) 0.85 ; LH_KFE
|
||||
(6,0) 0.25 ; RF_HAA
|
||||
(7,0) 0.60 ; RF_HFE
|
||||
(8,0) -0.85 ; RF_KFE
|
||||
(9,0) 0.25 ; RH_HAA
|
||||
(10,0) -0.60 ; RH_HFE
|
||||
(11,0) 0.85 ; RH_KFE
|
||||
}
|
||||
|
||||
initialModeSchedule
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] STANCE
|
||||
[1] STANCE
|
||||
}
|
||||
eventTimes
|
||||
{
|
||||
[0] 0.5
|
||||
}
|
||||
}
|
||||
|
||||
defaultModeSequenceTemplate
|
||||
{
|
||||
modeSequence
|
||||
{
|
||||
[0] STANCE
|
||||
}
|
||||
switchingTimes
|
||||
{
|
||||
[0] 0.0
|
||||
[1] 1.0
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,318 @@
|
||||
centroidalModelType 0 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
|
||||
|
||||
legged_robot_interface
|
||||
{
|
||||
verbose false // show the loaded parameters
|
||||
}
|
||||
|
||||
model_settings
|
||||
{
|
||||
positionErrorGain 0.0 ; 20.0
|
||||
phaseTransitionStanceTime 0.4
|
||||
|
||||
verboseCppAd true
|
||||
recompileLibrariesCppAd false
|
||||
modelFolderCppAd /tmp/ocs2_quadruped_controller/anymal_c
|
||||
}
|
||||
|
||||
swing_trajectory_config
|
||||
{
|
||||
liftOffVelocity 0.2
|
||||
touchDownVelocity -0.4
|
||||
swingHeight 0.1
|
||||
touchdownAfterHorizon 0.2
|
||||
swingTimeScale 0.15
|
||||
}
|
||||
|
||||
; Multiple_Shooting SQP settings
|
||||
sqp
|
||||
{
|
||||
nThreads 3
|
||||
dt 0.015
|
||||
sqpIteration 1
|
||||
deltaTol 1e-4
|
||||
g_max 1e-2
|
||||
g_min 1e-6
|
||||
inequalityConstraintMu 0.1
|
||||
inequalityConstraintDelta 5.0
|
||||
projectStateInputEqualityConstraints true
|
||||
printSolverStatistics true
|
||||
printSolverStatus false
|
||||
printLinesearch false
|
||||
useFeedbackPolicy true
|
||||
integratorType RK2
|
||||
threadPriority 50
|
||||
}
|
||||
|
||||
; Multiple_Shooting IPM settings
|
||||
ipm
|
||||
{
|
||||
nThreads 3
|
||||
dt 0.015
|
||||
ipmIteration 1
|
||||
deltaTol 1e-4
|
||||
g_max 10.0
|
||||
g_min 1e-6
|
||||
computeLagrangeMultipliers true
|
||||
printSolverStatistics true
|
||||
printSolverStatus false
|
||||
printLinesearch false
|
||||
useFeedbackPolicy true
|
||||
integratorType RK2
|
||||
threadPriority 50
|
||||
|
||||
initialBarrierParameter 1e-4
|
||||
targetBarrierParameter 1e-4
|
||||
barrierLinearDecreaseFactor 0.2
|
||||
barrierSuperlinearDecreasePower 1.5
|
||||
barrierReductionCostTol 1e-3
|
||||
barrierReductionConstraintTol 1e-3
|
||||
|
||||
fractionToBoundaryMargin 0.995
|
||||
usePrimalStepSizeForDual false
|
||||
|
||||
initialSlackLowerBound 1e-4
|
||||
initialDualLowerBound 1e-4
|
||||
initialSlackMarginRate 1e-2
|
||||
initialDualMarginRate 1e-2
|
||||
}
|
||||
|
||||
; DDP settings
|
||||
ddp
|
||||
{
|
||||
algorithm SLQ
|
||||
|
||||
nThreads 3
|
||||
threadPriority 50
|
||||
|
||||
maxNumIterations 1
|
||||
minRelCost 1e-1
|
||||
constraintTolerance 5e-3
|
||||
|
||||
displayInfo false
|
||||
displayShortSummary false
|
||||
checkNumericalStability false
|
||||
debugPrintRollout false
|
||||
|
||||
AbsTolODE 1e-5
|
||||
RelTolODE 1e-3
|
||||
maxNumStepsPerSecond 10000
|
||||
timeStep 0.015
|
||||
backwardPassIntegratorType ODE45
|
||||
|
||||
constraintPenaltyInitialValue 20.0
|
||||
constraintPenaltyIncreaseRate 2.0
|
||||
|
||||
preComputeRiccatiTerms true
|
||||
|
||||
useFeedbackPolicy false
|
||||
|
||||
strategy LINE_SEARCH
|
||||
lineSearch
|
||||
{
|
||||
minStepLength 1e-2
|
||||
maxStepLength 1.0
|
||||
hessianCorrectionStrategy DIAGONAL_SHIFT
|
||||
hessianCorrectionMultiple 1e-5
|
||||
}
|
||||
}
|
||||
|
||||
; Rollout settings
|
||||
rollout
|
||||
{
|
||||
AbsTolODE 1e-5
|
||||
RelTolODE 1e-3
|
||||
timeStep 0.015
|
||||
integratorType ODE45
|
||||
maxNumStepsPerSecond 10000
|
||||
checkNumericalStability false
|
||||
}
|
||||
|
||||
mpc
|
||||
{
|
||||
timeHorizon 1.0 ; [s]
|
||||
solutionTimeWindow -1 ; maximum [s]
|
||||
coldStart false
|
||||
|
||||
debugPrint false
|
||||
|
||||
mpcDesiredFrequency 100 ; [Hz]
|
||||
mrtDesiredFrequency 500 ; [Hz]
|
||||
}
|
||||
|
||||
initialState
|
||||
{
|
||||
;; Normalized Centroidal Momentum: [linear, angular] ;;
|
||||
(0,0) 0.0 ; vcom_x
|
||||
(1,0) 0.0 ; vcom_y
|
||||
(2,0) 0.0 ; vcom_z
|
||||
(3,0) 0.0 ; L_x / robotMass
|
||||
(4,0) 0.0 ; L_y / robotMass
|
||||
(5,0) 0.0 ; L_z / robotMass
|
||||
|
||||
;; Base Pose: [position, orientation] ;;
|
||||
(6,0) 0.0 ; p_base_x
|
||||
(7,0) 0.0 ; p_base_y
|
||||
(8,0) 0.575 ; p_base_z
|
||||
(9,0) 0.0 ; theta_base_z
|
||||
(10,0) 0.0 ; theta_base_y
|
||||
(11,0) 0.0 ; theta_base_x
|
||||
|
||||
;; Leg Joint Positions: [LF, LH, RF, RH] ;;
|
||||
(12,0) -0.25 ; LF_HAA
|
||||
(13,0) 0.60 ; LF_HFE
|
||||
(14,0) -0.85 ; LF_KFE
|
||||
(15,0) -0.25 ; LH_HAA
|
||||
(16,0) -0.60 ; LH_HFE
|
||||
(17,0) 0.85 ; LH_KFE
|
||||
(18,0) 0.25 ; RF_HAA
|
||||
(19,0) 0.60 ; RF_HFE
|
||||
(20,0) -0.85 ; RF_KFE
|
||||
(21,0) 0.25 ; RH_HAA
|
||||
(22,0) -0.60 ; RH_HFE
|
||||
(23,0) 0.85 ; RH_KFE
|
||||
}
|
||||
|
||||
; standard state weight matrix
|
||||
Q
|
||||
{
|
||||
scaling 1e+0
|
||||
|
||||
;; Normalized Centroidal Momentum: [linear, angular] ;;
|
||||
(0,0) 15.0 ; vcom_x
|
||||
(1,1) 15.0 ; vcom_y
|
||||
(2,2) 30.0 ; vcom_z
|
||||
(3,3) 5.0 ; L_x / robotMass
|
||||
(4,4) 10.0 ; L_y / robotMass
|
||||
(5,5) 10.0 ; L_z / robotMass
|
||||
|
||||
;; Base Pose: [position, orientation] ;;
|
||||
(6,6) 500.0 ; p_base_x
|
||||
(7,7) 500.0 ; p_base_y
|
||||
(8,8) 500.0 ; p_base_z
|
||||
(9,9) 100.0 ; theta_base_z
|
||||
(10,10) 200.0 ; theta_base_y
|
||||
(11,11) 200.0 ; theta_base_x
|
||||
|
||||
;; Leg Joint Positions: [LF, LH, RF, RH] ;;
|
||||
(12,12) 20.0 ; LF_HAA
|
||||
(13,13) 20.0 ; LF_HFE
|
||||
(14,14) 20.0 ; LF_KFE
|
||||
(15,15) 20.0 ; LH_HAA
|
||||
(16,16) 20.0 ; LH_HFE
|
||||
(17,17) 20.0 ; LH_KFE
|
||||
(18,18) 20.0 ; RF_HAA
|
||||
(19,19) 20.0 ; RF_HFE
|
||||
(20,20) 20.0 ; RF_KFE
|
||||
(21,21) 20.0 ; RH_HAA
|
||||
(22,22) 20.0 ; RH_HFE
|
||||
(23,23) 20.0 ; RH_KFE
|
||||
}
|
||||
|
||||
; control weight matrix
|
||||
R
|
||||
{
|
||||
scaling 1e-3
|
||||
|
||||
;; Feet Contact Forces: [LF, RF, LH, RH] ;;
|
||||
(0,0) 1.0 ; left_front_force
|
||||
(1,1) 1.0 ; left_front_force
|
||||
(2,2) 1.0 ; left_front_force
|
||||
(3,3) 1.0 ; right_front_force
|
||||
(4,4) 1.0 ; right_front_force
|
||||
(5,5) 1.0 ; right_front_force
|
||||
(6,6) 1.0 ; left_hind_force
|
||||
(7,7) 1.0 ; left_hind_force
|
||||
(8,8) 1.0 ; left_hind_force
|
||||
(9,9) 1.0 ; right_hind_force
|
||||
(10,10) 1.0 ; right_hind_force
|
||||
(11,11) 1.0 ; right_hind_force
|
||||
|
||||
;; foot velocity relative to base: [LF, LH, RF, RH] (uses the Jacobian at nominal configuration) ;;
|
||||
(12,12) 5000.0 ; x
|
||||
(13,13) 5000.0 ; y
|
||||
(14,14) 5000.0 ; z
|
||||
(15,15) 5000.0 ; x
|
||||
(16,16) 5000.0 ; y
|
||||
(17,17) 5000.0 ; z
|
||||
(18,18) 5000.0 ; x
|
||||
(19,19) 5000.0 ; y
|
||||
(20,20) 5000.0 ; z
|
||||
(21,21) 5000.0 ; x
|
||||
(22,22) 5000.0 ; y
|
||||
(23,23) 5000.0 ; z
|
||||
}
|
||||
|
||||
frictionConeSoftConstraint
|
||||
{
|
||||
frictionCoefficient 0.5
|
||||
|
||||
; relaxed log barrier parameters
|
||||
mu 0.1
|
||||
delta 5.0
|
||||
}
|
||||
|
||||
selfCollision
|
||||
{
|
||||
; Self Collision raw object pairs
|
||||
collisionObjectPairs
|
||||
{
|
||||
}
|
||||
|
||||
; Self Collision pairs
|
||||
collisionLinkPairs
|
||||
{
|
||||
[0] "LF_shank_fixed, RF_shank_fixed"
|
||||
[1] "LH_shank_fixed, RH_shank_fixed"
|
||||
[2] "LF_shank_fixed, LH_shank_fixed"
|
||||
[3] "RF_shank_fixed, RH_shank_fixed"
|
||||
[4] "LF_FOOT, RF_FOOT"
|
||||
[5] "LH_FOOT, RH_FOOT"
|
||||
[6] "LF_FOOT, LH_FOOT"
|
||||
[7] "RF_FOOT, RH_FOOT"
|
||||
}
|
||||
|
||||
minimumDistance 0.05
|
||||
|
||||
; relaxed log barrier parameters
|
||||
mu 1e-2
|
||||
delta 1e-3
|
||||
}
|
||||
|
||||
; Whole body control
|
||||
torqueLimitsTask
|
||||
{
|
||||
(0,0) 80.0 ; HAA
|
||||
(1,0) 80.0 ; HFE
|
||||
(2,0) 80.0 ; KFE
|
||||
}
|
||||
|
||||
frictionConeTask
|
||||
{
|
||||
frictionCoefficient 0.3
|
||||
}
|
||||
|
||||
swingLegTask
|
||||
{
|
||||
kp 350
|
||||
kd 37
|
||||
}
|
||||
|
||||
weight
|
||||
{
|
||||
swingLeg 100
|
||||
baseAccel 1
|
||||
contactForce 0.01
|
||||
}
|
||||
|
||||
; State Estimation
|
||||
kalmanFilter
|
||||
{
|
||||
footRadius 0.015
|
||||
imuProcessNoisePosition 0.02
|
||||
imuProcessNoiseVelocity 0.02
|
||||
footProcessNoisePosition 0.002
|
||||
footSensorNoisePosition 0.005
|
||||
footSensorNoiseVelocity 0.1
|
||||
footHeightSensorNoise 0.01
|
||||
}
|
||||
@ -0,0 +1,249 @@
|
||||
# Controller Manager configuration
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 200 # Hz
|
||||
|
||||
# Define the available controllers
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
type: imu_sensor_broadcaster/IMUSensorBroadcaster
|
||||
|
||||
unitree_guide_controller:
|
||||
type: unitree_guide_controller/UnitreeGuideController
|
||||
|
||||
ocs2_quadruped_controller:
|
||||
type: ocs2_quadruped_controller/Ocs2QuadrupedController
|
||||
|
||||
rl_quadruped_controller:
|
||||
type: rl_quadruped_controller/LeggedGymController
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
frame_id: "imu_link"
|
||||
|
||||
unitree_guide_controller:
|
||||
ros__parameters:
|
||||
update_rate: 200 # Hz
|
||||
joints:
|
||||
- RF_HAA
|
||||
- RF_HFE
|
||||
- RF_KFE
|
||||
- LF_HAA
|
||||
- LF_HFE
|
||||
- LF_KFE
|
||||
- RH_HAA
|
||||
- RH_HFE
|
||||
- RH_KFE
|
||||
- LH_HAA
|
||||
- LH_HFE
|
||||
- LH_KFE
|
||||
|
||||
down_pos:
|
||||
- -0.0
|
||||
- 1.41
|
||||
- -2.58
|
||||
- 0.0
|
||||
- 1.41
|
||||
- -2.58
|
||||
- -0.0
|
||||
- -1.41
|
||||
- 2.58
|
||||
- 0.0
|
||||
- -1.41
|
||||
- 2.58
|
||||
|
||||
stand_pos:
|
||||
- 0.2
|
||||
- 0.6
|
||||
- -0.85
|
||||
- -0.2
|
||||
- 0.6
|
||||
- -0.85
|
||||
- 0.2
|
||||
- -0.6
|
||||
- 0.85
|
||||
- -0.2
|
||||
- -0.6
|
||||
- 0.85
|
||||
|
||||
command_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
- kp
|
||||
- kd
|
||||
|
||||
state_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
|
||||
feet_names:
|
||||
- LF_FOOT
|
||||
- RF_FOOT
|
||||
- LH_FOOT
|
||||
- RH_FOOT
|
||||
|
||||
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
|
||||
|
||||
ocs2_quadruped_controller:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
default_kd: 1.5
|
||||
joints:
|
||||
- LF_HAA
|
||||
- LF_HFE
|
||||
- LF_KFE
|
||||
- LH_HAA
|
||||
- LH_HFE
|
||||
- LH_KFE
|
||||
- RF_HAA
|
||||
- RF_HFE
|
||||
- RF_KFE
|
||||
- RH_HAA
|
||||
- RH_HFE
|
||||
- RH_KFE
|
||||
|
||||
command_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
- kp
|
||||
- kd
|
||||
|
||||
state_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
|
||||
feet:
|
||||
- LF_FOOT
|
||||
- RF_FOOT
|
||||
- LH_FOOT
|
||||
- RH_FOOT
|
||||
|
||||
|
||||
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
|
||||
|
||||
foot_force_name: "foot_force"
|
||||
foot_force_interfaces:
|
||||
- LF
|
||||
- RF
|
||||
- LH
|
||||
- RH
|
||||
|
||||
rl_quadruped_controller:
|
||||
ros__parameters:
|
||||
update_rate: 200 # Hz
|
||||
robot_pkg: "anymal_c_description"
|
||||
model_folder: "legged_gym"
|
||||
joints:
|
||||
- LF_HAA
|
||||
- LF_HFE
|
||||
- LF_KFE
|
||||
- RF_HAA
|
||||
- RF_HFE
|
||||
- RF_KFE
|
||||
- LH_HAA
|
||||
- LH_HFE
|
||||
- LH_KFE
|
||||
- RH_HAA
|
||||
- RH_HFE
|
||||
- RH_KFE
|
||||
|
||||
down_pos:
|
||||
- -0.0
|
||||
- 1.41
|
||||
- -2.58
|
||||
- 0.0
|
||||
- 1.41
|
||||
- -2.58
|
||||
- -0.0
|
||||
- -1.41
|
||||
- 2.58
|
||||
- 0.0
|
||||
- -1.41
|
||||
- 2.58
|
||||
|
||||
stand_pos:
|
||||
- 0.2
|
||||
- 0.6
|
||||
- -0.85
|
||||
- -0.2
|
||||
- 0.6
|
||||
- -0.85
|
||||
- 0.2
|
||||
- -0.6
|
||||
- 0.85
|
||||
- -0.2
|
||||
- -0.6
|
||||
- 0.85
|
||||
|
||||
command_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
- kp
|
||||
- kd
|
||||
|
||||
state_interfaces:
|
||||
- effort
|
||||
- position
|
||||
- velocity
|
||||
|
||||
feet_names:
|
||||
- LF_FOOT
|
||||
- RF_FOOT
|
||||
- LH_FOOT
|
||||
- RH_FOOT
|
||||
|
||||
foot_force_name: "foot_force"
|
||||
foot_force_interfaces:
|
||||
- LF
|
||||
- RF
|
||||
- LH
|
||||
- RH
|
||||
|
||||
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,112 @@
|
||||
import os
|
||||
|
||||
import xacro
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
package_description = "anymal_c_description"
|
||||
|
||||
|
||||
def process_xacro(context):
|
||||
robot_type_value = context.launch_configurations['robot_type']
|
||||
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, mappings={'robot_type': robot_type_value})
|
||||
return (robot_description_config.toxml(), robot_type_value)
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
(robot_description, robot_type) = process_xacro(context)
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare(package_description),
|
||||
"config",
|
||||
"robot_control.yaml",
|
||||
]
|
||||
)
|
||||
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
parameters=[
|
||||
{
|
||||
'publish_frequency': 20.0,
|
||||
'use_tf_static': True,
|
||||
'robot_description': robot_description,
|
||||
'ignore_timestamp': True
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
controller_manager = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[robot_controllers],
|
||||
output="both",
|
||||
)
|
||||
|
||||
joint_state_publisher = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster",
|
||||
"--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
imu_sensor_broadcaster = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["imu_sensor_broadcaster",
|
||||
"--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
unitree_guide_controller = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
return [
|
||||
robot_state_publisher,
|
||||
controller_manager,
|
||||
joint_state_publisher,
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_publisher,
|
||||
on_exit=[imu_sensor_broadcaster],
|
||||
)
|
||||
),
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=imu_sensor_broadcaster,
|
||||
on_exit=[unitree_guide_controller],
|
||||
)
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
robot_type_arg = DeclareLaunchArgument(
|
||||
'robot_type',
|
||||
default_value='a1',
|
||||
description='Type of the robot'
|
||||
)
|
||||
|
||||
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
|
||||
|
||||
return LaunchDescription([
|
||||
robot_type_arg,
|
||||
OpaqueFunction(function=launch_setup),
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz_ocs2',
|
||||
output='screen',
|
||||
arguments=["-d", rviz_config_file]
|
||||
)
|
||||
])
|
||||
@ -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 = "anymal_c_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': 100.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',
|
||||
)
|
||||
])
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 194 KiB |
@ -0,0 +1,115 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author>Blender User</author>
|
||||
<authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
|
||||
</contributor>
|
||||
<created>2020-01-07T14:47:32</created>
|
||||
<modified>2020-01-07T14:47:32</modified>
|
||||
<unit name="meter" meter="1"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_effects>
|
||||
<effect id="battery-effect">
|
||||
<profile_COMMON>
|
||||
<newparam sid="battery-surface">
|
||||
<surface type="2D">
|
||||
<init_from>battery</init_from>
|
||||
</surface>
|
||||
</newparam>
|
||||
<newparam sid="battery-sampler">
|
||||
<sampler2D>
|
||||
<source>battery-surface</source>
|
||||
</sampler2D>
|
||||
</newparam>
|
||||
<technique sid="common">
|
||||
<lambert>
|
||||
<emission>
|
||||
<color sid="emission">0 0 0 1</color>
|
||||
</emission>
|
||||
<diffuse>
|
||||
<texture texture="battery-sampler" texcoord="UVMap"/>
|
||||
</diffuse>
|
||||
<index_of_refraction>
|
||||
<float sid="ior">1.45</float>
|
||||
</index_of_refraction>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_images>
|
||||
<image id="battery" name="battery">
|
||||
<init_from>battery.jpg</init_from>
|
||||
</image>
|
||||
</library_images>
|
||||
<library_materials>
|
||||
<material id="battery-material" name="battery">
|
||||
<instance_effect url="#battery-effect"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_geometries>
|
||||
<geometry id="Cube-mesh" name="Cube">
|
||||
<mesh>
|
||||
<source id="Cube-mesh-positions">
|
||||
<float_array id="Cube-mesh-positions-array" count="24">-0.225881 -0.06249898 -0.03684729 -0.225881 -0.06249898 0.03725165 -0.225881 0.06249898 -0.03684729 -0.225881 0.06249898 0.03725165 0.232213 -0.06249898 -0.03684729 0.232213 -0.06249898 0.03725165 0.232213 0.06249898 -0.03684729 0.232213 0.06249898 0.03725165</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-positions-array" count="8" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Cube-mesh-normals">
|
||||
<float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-normals-array" count="6" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Cube-mesh-map">
|
||||
<float_array id="Cube-mesh-map-array" count="72">0.8431081 0.2728654 1 0 1 0.2728654 0.6862162 1 0.8431079 0 0.8431081 1 0.8431081 0.5457308 1 0.2728654 1 0.5457308 0.6862159 0 0.5293241 1 0.5293239 0 0.264662 0 0.5293239 0.9999999 0.2646622 0.9999999 0.264662 0.9999999 0 0 0.2646618 0 0.8431081 0.2728654 0.8431081 0 1 0 0.6862162 1 0.6862161 0 0.8431079 0 0.8431081 0.5457308 0.8431081 0.2728654 1 0.2728654 0.6862159 0 0.6862161 1 0.5293241 1 0.264662 0 0.5293238 0 0.5293239 0.9999999 0.264662 0.9999999 1.73529e-7 0.9999999 0 0</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-map-array" count="36" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="Cube-mesh-vertices">
|
||||
<input semantic="POSITION" source="#Cube-mesh-positions"/>
|
||||
</vertices>
|
||||
<triangles material="battery-material" count="12">
|
||||
<input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
|
||||
<input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
|
||||
<input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
|
||||
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="Scene" name="Scene">
|
||||
<node id="AM3_Battery_LP" name="AM3 Battery LP" type="NODE">
|
||||
<matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 -0.03329167 0 0 0 1</matrix>
|
||||
<instance_geometry url="#Cube-mesh" name="AM3 Battery LP">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="battery-material" target="#battery-material">
|
||||
<bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#Scene"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 111 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 165 KiB |
@ -0,0 +1,115 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author>Blender User</author>
|
||||
<authoring_tool>Blender 2.82.7 commit date:2020-02-12, commit time:16:20, hash:77d23b0bd76f</authoring_tool>
|
||||
</contributor>
|
||||
<created>2020-02-20T10:55:16</created>
|
||||
<modified>2020-02-20T10:55:16</modified>
|
||||
<unit name="meter" meter="1"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_effects>
|
||||
<effect id="depth_camera-effect">
|
||||
<profile_COMMON>
|
||||
<newparam sid="depth_camera-surface">
|
||||
<surface type="2D">
|
||||
<init_from>depth_camera</init_from>
|
||||
</surface>
|
||||
</newparam>
|
||||
<newparam sid="depth_camera-sampler">
|
||||
<sampler2D>
|
||||
<source>depth_camera-surface</source>
|
||||
</sampler2D>
|
||||
</newparam>
|
||||
<technique sid="common">
|
||||
<lambert>
|
||||
<emission>
|
||||
<color sid="emission">0 0 0 1</color>
|
||||
</emission>
|
||||
<diffuse>
|
||||
<texture texture="depth_camera-sampler" texcoord="UVMap"/>
|
||||
</diffuse>
|
||||
<index_of_refraction>
|
||||
<float sid="ior">1.45</float>
|
||||
</index_of_refraction>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_images>
|
||||
<image id="depth_camera" name="depth_camera">
|
||||
<init_from>depth_camera.jpg</init_from>
|
||||
</image>
|
||||
</library_images>
|
||||
<library_materials>
|
||||
<material id="depth_camera-material" name="depth_camera">
|
||||
<instance_effect url="#depth_camera-effect"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_geometries>
|
||||
<geometry id="Cube-mesh" name="Cube">
|
||||
<mesh>
|
||||
<source id="Cube-mesh-positions">
|
||||
<float_array id="Cube-mesh-positions-array" count="24">0 -0.04699999 -0.01968461 0 -0.04699999 0.01951932 0 0.04699999 -0.01968461 0 0.04699999 0.01951932 0.03039997 -0.04699999 -0.01968461 0.03039997 -0.04699999 0.01951932 0.03039997 0.04699999 -0.01968461 0.03039997 0.04699999 0.01951932</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-positions-array" count="8" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Cube-mesh-normals">
|
||||
<float_array id="Cube-mesh-normals-array" count="18">-1 0 0 1 0 0 0 0 1 0 -1 0 0 1 0 0 0 -1</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-normals-array" count="6" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="Cube-mesh-map">
|
||||
<float_array id="Cube-mesh-map-array" count="72">0.2816218 0.7056847 0.5632433 0 0.5632433 0.7056847 1.60564e-7 0.7056847 0.2816215 0 0.2816217 0.7056847 0.7816217 0.7056846 1 0 1 0.7056846 0.5632433 0.7056847 0.344865 1 0.344865 0.7056847 0.7816216 0.7056847 0.5632434 1 0.5632433 0.7056847 0.7816217 0 0.5632433 0.7056846 0.5632434 0 0.2816218 0.7056847 0.2816217 0 0.5632433 0 1.60564e-7 0.7056847 0 0 0.2816215 0 0.7816217 0.7056846 0.7816217 0 1 0 0.5632433 0.7056847 0.5632433 1 0.344865 1 0.7816216 0.7056847 0.7816218 1 0.5632434 1 0.7816217 0 0.7816216 0.7056846 0.5632433 0.7056846</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#Cube-mesh-map-array" count="36" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="Cube-mesh-vertices">
|
||||
<input semantic="POSITION" source="#Cube-mesh-positions"/>
|
||||
</vertices>
|
||||
<triangles material="depth_camera-material" count="12">
|
||||
<input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
|
||||
<input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
|
||||
<input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
|
||||
<p>1 0 0 2 0 1 0 0 2 7 1 3 4 1 4 6 1 5 3 2 6 5 2 7 7 2 8 1 3 9 4 3 10 5 3 11 7 4 12 2 4 13 3 4 14 6 5 15 0 5 16 2 5 17 1 0 18 3 0 19 2 0 20 7 1 21 5 1 22 4 1 23 3 2 24 1 2 25 5 2 26 1 3 27 0 3 28 4 3 29 7 4 30 6 4 31 2 4 32 6 5 33 4 5 34 0 5 35</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="Scene" name="Scene">
|
||||
<node id="Realsense_LP" name="Realsense LP" type="NODE">
|
||||
<matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
|
||||
<instance_geometry url="#Cube-mesh" name="Realsense LP">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="depth_camera-material" target="#depth_camera-material">
|
||||
<bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#Scene"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 186 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 174 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 295 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 185 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 166 KiB |
@ -0,0 +1,115 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author>Blender User</author>
|
||||
<authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
|
||||
</contributor>
|
||||
<created>2020-01-13T19:28:23</created>
|
||||
<modified>2020-01-13T19:28:23</modified>
|
||||
<unit name="meter" meter="1"/>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_effects>
|
||||
<effect id="hatch-effect">
|
||||
<profile_COMMON>
|
||||
<newparam sid="hatch-surface">
|
||||
<surface type="2D">
|
||||
<init_from>hatch</init_from>
|
||||
</surface>
|
||||
</newparam>
|
||||
<newparam sid="hatch-sampler">
|
||||
<sampler2D>
|
||||
<source>hatch-surface</source>
|
||||
</sampler2D>
|
||||
</newparam>
|
||||
<technique sid="common">
|
||||
<lambert>
|
||||
<emission>
|
||||
<color sid="emission">0 0 0 1</color>
|
||||
</emission>
|
||||
<diffuse>
|
||||
<texture texture="hatch-sampler" texcoord="UVMap"/>
|
||||
</diffuse>
|
||||
<index_of_refraction>
|
||||
<float sid="ior">1.45</float>
|
||||
</index_of_refraction>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_images>
|
||||
<image id="hatch" name="hatch">
|
||||
<init_from>hatch.jpg</init_from>
|
||||
</image>
|
||||
</library_images>
|
||||
<library_materials>
|
||||
<material id="hatch-material" name="hatch">
|
||||
<instance_effect url="#hatch-effect"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_geometries>
|
||||
<geometry id="AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch Detailed HD.002">
|
||||
<mesh>
|
||||
<source id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions">
|
||||
<float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="96">-76.90127 -64.71254 0 -76.90127 -64.71254 5.500071 -76.90127 64.71254 0 -76.90127 64.71254 5.500071 76.90133 -64.71254 0 76.90133 -64.71254 5.500071 76.90133 64.71254 0 76.90133 64.71254 5.500071 -80.46286 11.54165 0 -91.00833 3.847216 0 -91.00833 -3.847217 0 -80.46286 -11.54165 0 -80.46286 -11.54165 5.500071 -91.00833 -3.847217 5.500071 -91.00833 3.847217 5.500071 -80.46286 11.54165 5.500071 80.46292 -11.54165 0 91.00839 -3.847217 0 91.00839 3.847217 0 80.46292 11.54165 0 80.46292 11.54165 5.500071 91.00839 3.847216 5.500071 91.00839 -3.847217 5.500071 80.46292 -11.54165 5.500071 -80.46286 38.12709 5.500071 80.46292 38.12709 0 -80.46286 38.12709 0 80.46292 38.12709 5.500071 -80.46286 -38.12709 0 80.46292 -38.12709 5.500071 -80.46286 -38.12709 5.500071 80.46292 -38.12709 0</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="32" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals">
|
||||
<float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="63">-0.9911455 0.1327809 0 0 1 0 0.9911455 -0.1327809 0 0 -1 0 0 0 1 0 0 1 1 0 0 0.5894234 0.8078244 0 0.5894234 -0.8078244 0 -1 0 0 -0.5894234 -0.8078244 0 -0.5894234 0.8078243 -2.12521e-7 0.9911454 0.1327813 0 -0.9911454 -0.1327813 0 -0.9911454 0.1327813 0 0.9911454 -0.1327813 0 0 0 1 0.5894235 0.8078243 2.12521e-7 -0.5894234 0.8078244 0 0.9911455 0.1327809 0 -0.9911455 -0.1327809 0</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="21" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="AM3_Application_Hatch_Detailed_HD_002-mesh-map">
|
||||
<float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="276">0.8547119 0.7845032 0.8910338 0.6826047 0.8910338 0.7845032 0.8547118 0.6826048 0.8910338 0 0.8910338 0.6826047 0.963678 0.5650011 0.927356 0.7110616 0.927356 0.5650011 0.891034 0.7845032 0.927356 0.1018984 0.927356 0.7845032 0.1755681 0.9420632 0 0.07750421 0.1755678 0.05793678 0.679144 0.9420632 0.5035759 0.05793678 0.6791438 0.05793678 0.503576 0.9420632 0.4527625 0 0.5035759 0.05793678 0.4527627 1 0.4019491 0 0.4527625 0 0.4019494 1 0.3511358 0.05793678 0.4019491 0 0.963678 0.1460605 0.927356 0.2921209 0.927356 0.1460605 0.963678 0.2921209 0.927356 0.3343942 0.927356 0.2921209 0.963678 0.3343942 0.927356 0.3766674 0.927356 0.3343942 0.963678 0.3766674 0.927356 0.4189407 0.927356 0.3766674 0.927356 0.7971531 0.963678 0.7110617 0.963678 0.7971532 0.963678 0.07171952 1 0 1 0.07171952 0.8547119 0.9672312 0.8910339 0.9423143 0.8910339 0.9672312 0.8547119 0.9423143 0.8910339 0.8705947 0.8910339 0.9423143 0.963678 0 0.927356 0.1460605 0.9273561 0 0.8547118 0.9224958 0.6791438 0.05793678 0.8547117 0.07750409 0.8547119 0.8705947 0.8910338 0.7845032 0.8910339 0.8705947 0.891034 0.1018984 0.927356 0 0.927356 0.1018984 0.351136 0.9420632 0.1755678 0.05793678 0.3511358 0.05793678 0.963678 0.4189407 0.927356 0.5650011 0.927356 0.4189407 0.8547119 0.7845032 0.8547118 0.6826048 0.8910338 0.6826047 0.8547118 0.6826048 0.8547118 0 0.8910338 0 0.963678 0.5650011 0.963678 0.7110616 0.927356 0.7110616 0.891034 0.7845032 0.891034 0.1018984 0.927356 0.1018984 0.1755681 0.9420632 0 0.9224959 0 0.07750421 0.679144 0.9420632 0.503576 0.9420632 0.5035759 0.05793678 0.503576 0.9420632 0.4527627 1 0.4527625 0 0.4527627 1 0.4019494 1 0.4019491 0 0.4019494 1 0.351136 0.9420632 0.3511358 0.05793678 0.963678 0.1460605 0.963678 0.2921209 0.927356 0.2921209 0.963678 0.2921209 0.963678 0.3343942 0.927356 0.3343942 0.963678 0.3343942 0.963678 0.3766674 0.927356 0.3766674 0.963678 0.3766674 0.963678 0.4189407 0.927356 0.4189407 0.927356 0.7971531 0.927356 0.7110616 0.963678 0.7110617 0.963678 0.07171952 0.963678 0 1 0 0.8547119 0.9672312 0.8547119 0.9423143 0.8910339 0.9423143 0.8547119 0.9423143 0.8547119 0.8705947 0.8910339 0.8705947 0.963678 0 0.963678 0.1460605 0.927356 0.1460605 0.8547118 0.9224958 0.679144 0.9420632 0.6791438 0.05793678 0.8547119 0.8705947 0.8547119 0.7845032 0.8910338 0.7845032 0.891034 0.1018984 0.891034 0 0.927356 0 0.351136 0.9420632 0.1755681 0.9420632 0.1755678 0.05793678 0.963678 0.4189407 0.963678 0.5650011 0.927356 0.5650011</float_array>
|
||||
<technique_common>
|
||||
<accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="138" stride="2">
|
||||
<param name="S" type="float"/>
|
||||
<param name="T" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="AM3_Application_Hatch_Detailed_HD_002-mesh-vertices">
|
||||
<input semantic="POSITION" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions"/>
|
||||
</vertices>
|
||||
<triangles material="hatch-material" count="46">
|
||||
<input semantic="VERTEX" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-vertices" offset="0"/>
|
||||
<input semantic="NORMAL" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals" offset="1"/>
|
||||
<input semantic="TEXCOORD" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map" offset="2" set="0"/>
|
||||
<p>24 0 0 2 0 1 26 0 2 3 1 3 6 1 4 2 1 5 29 2 6 4 2 7 31 2 8 5 3 9 0 3 10 4 3 11 30 4 12 5 4 13 29 4 14 24 4 15 20 4 16 27 4 17 15 4 18 21 4 19 20 4 20 14 5 21 22 5 22 21 5 23 13 4 24 23 4 25 22 4 26 27 6 27 19 6 28 25 6 29 20 7 30 18 7 31 19 7 32 21 6 33 17 6 34 18 6 35 22 8 36 16 8 37 17 8 38 30 9 39 11 9 40 28 9 41 12 10 42 10 10 43 11 10 44 13 9 45 9 9 46 10 9 47 14 11 48 8 11 49 9 11 50 7 12 51 25 12 52 6 12 53 3 4 54 27 4 55 7 4 56 15 9 57 26 9 58 8 9 59 1 13 60 28 13 61 0 13 62 12 4 63 29 4 64 23 4 65 23 6 66 31 6 67 16 6 68 24 14 69 3 14 70 2 14 71 3 1 72 7 1 73 6 1 74 29 15 75 5 15 76 4 15 77 5 3 78 1 3 79 0 3 80 30 4 81 1 4 82 5 4 83 24 4 84 15 4 85 20 4 86 15 16 87 14 16 88 21 16 89 14 4 90 13 4 91 22 4 92 13 4 93 12 4 94 23 4 95 27 6 96 20 6 97 19 6 98 20 17 99 21 17 100 18 17 101 21 6 102 22 6 103 17 6 104 22 8 105 23 8 106 16 8 107 30 9 108 12 9 109 11 9 110 12 10 111 13 10 112 10 10 113 13 9 114 14 9 115 9 9 116 14 18 117 15 18 118 8 18 119 7 19 120 27 19 121 25 19 122 3 4 123 24 4 124 27 4 125 15 9 126 24 9 127 26 9 128 1 20 129 30 20 130 28 20 131 12 4 132 30 4 133 29 4 134 23 6 135 29 6 136 31 6 137</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="Scene" name="Scene">
|
||||
<node id="AM3_Application_Hatch_LP" name="AM3 Application Hatch LP" type="NODE">
|
||||
<matrix sid="transform">0.001006675 0 0 0 0 9.99987e-4 0 0 0 0 9.99987e-4 0 0 0 0 1</matrix>
|
||||
<instance_geometry url="#AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch LP">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="hatch-material" target="#hatch-material">
|
||||
<bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#Scene"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 227 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 180 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 276 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 107 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 165 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user