Skip to content

Commit 68785e0

Browse files
mrspmrsp
authored
Feature/owdy ros2 driver (#31)
* wip * fix * nitpick
1 parent b99a8ac commit 68785e0

File tree

10 files changed

+1252
-97
lines changed

10 files changed

+1252
-97
lines changed

‎config/h1.json‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
{
2+
"base_frame": "torso_link",
3+
"point_feet": false,
4+
"foot_frames": {
5+
"0": "left_ankle_link",
6+
"1": "right_ankle_link"
7+
},
8+
"model_path": "h1.urdf",
9+
"mass": 65.5,
10+
"g": 9.81,
11+
"joint_rate": 150.0,
12+
"estimate_joint_velocity": false,
13+
"joint_cutoff_frequency": 10.0,
14+
"joint_position_variance": 0.1,
15+
"angular_momentum_cutoff_frequency": 5.0,
16+
"tau_0": 1.0,
17+
"tau_1": 0.1,
18+
"imu_rate": 150.0,
19+
"R_base_to_gyro": [1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0],
20+
"R_base_to_acc": [1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0],
21+
"calibrate_initial_imu_bias": false,
22+
"max_imu_calibration_cycles": 300,
23+
"bias_acc": [0.0, 0.0, 0.0],
24+
"bias_gyro": [0.0, 0.0, 0.0],
25+
"gyro_cutoff_frequency": 5.0,
26+
"force_torque_rate": 150.0,
27+
"R_foot_to_force": {
28+
"0": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
29+
"1": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
30+
},
31+
"R_foot_to_torque": {
32+
"0": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
33+
"1": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
34+
},
35+
"attitude_estimator_proportional_gain": 0.02,
36+
"attitude_estimator_integral_gain": 0.0,
37+
"estimate_contact_status": true,
38+
"high_threshold": 175.0,
39+
"low_threshold": 62.5,
40+
"median_window": 13,
41+
"outlier_detection": false,
42+
"convergence_cycles": 0,
43+
44+
"imu_angular_velocity_covariance": [1e-4, 1e-4, 1e-4],
45+
"imu_angular_velocity_bias_covariance": [1e-6, 1e-6, 1e-6],
46+
"imu_linear_acceleration_covariance": [1e-3, 1e-3, 1e-3],
47+
"imu_linear_acceleration_bias_covariance": [1e-5, 1e-5, 1e-5],
48+
49+
"use_contacts_in_base_estimation": false,
50+
"base_linear_velocity_covariance": [1e-1, 1e-1, 1e-1],
51+
"base_orientation_covariance": [1e-3, 1e-3, 1e-3],
52+
"contact_position_covariance": [1e-6, 1e-6, 1e-6],
53+
"contact_orientation_covariance": [1e-3, 1e-3, 1e-3],
54+
"contact_position_slip_covariance": [1e-4, 1e-4, 1e-4],
55+
"contact_orientation_slip_covariance": [1e-3, 1e-3, 1e-3],
56+
"com_position_process_covariance": [1e-6, 1e-6, 1e-6],
57+
"com_linear_velocity_process_covariance": [1e-4, 1e-4, 1e-4],
58+
"external_forces_process_covariance": [1e-2, 1e-2, 1e-2],
59+
"com_position_covariance": [1e-6, 1e-6, 1e-6],
60+
"com_linear_acceleration_covariance": [1e-3, 1e-3, 1e-3],
61+
"initial_base_position_covariance": [1.0, 1.0, 1.0],
62+
"initial_base_orientation_covariance": [1.0, 1.0, 1.0],
63+
"initial_base_linear_velocity_covariance": [1.0, 1.0, 1.0],
64+
65+
"initial_contact_position_covariance": [1.0, 1.0, 1.0],
66+
"initial_contact_orientation_covariance": [1.0, 1.0, 1.0],
67+
"initial_imu_linear_acceleration_bias_covariance": [1.0, 1.0, 1.0],
68+
"initial_imu_angular_velocity_bias_covariance": [1.0, 1.0, 1.0],
69+
"initial_com_position_covariance": [1.0, 1.0, 1.0],
70+
"initial_com_linear_velocity_covariance": [1.0, 1.0, 1.0],
71+
"initial_external_forces_covariance": [1.0, 1.0, 1.0],
72+
"T_base_to_odom": [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0],
73+
74+
"enable_terrain_estimation": true,
75+
"minimum_terrain_height_variance": 1e-3
76+
}

‎ros2/serow_ros2/CMakeLists.txt‎

Lines changed: 52 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -15,30 +15,63 @@ find_package(ament_cmake REQUIRED)
1515
find_package(rclcpp REQUIRED)
1616
find_package(geometry_msgs REQUIRED)
1717
find_package(sensor_msgs REQUIRED)
18+
find_package(geometry_msgs REQUIRED)
1819
find_package(serow REQUIRED)
1920
find_package(pinocchio REQUIRED)
2021
find_package(Eigen3 3.4 REQUIRED NO_MODULE)
2122
find_package(nlohmann_json REQUIRED)
23+
find_package(yaml-cpp REQUIRED)
24+
find_package(ament_index_cpp REQUIRED)
2225

26+
# Add executable
2327
add_executable(serow_ros2 src/serow_driver.cpp)
24-
ament_target_dependencies(serow_ros2 rclcpp geometry_msgs sensor_msgs)
25-
target_link_libraries(serow_ros2 serow pinocchio::pinocchio Eigen3::Eigen nlohmann_json::nlohmann_json)
26-
27-
install(TARGETS
28-
serow_ros2
29-
DESTINATION lib/${PROJECT_NAME})
30-
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
31-
32-
if(BUILD_TESTING)
33-
find_package(ament_lint_auto REQUIRED)
34-
# the following line skips the linter which checks for copyrights
35-
# comment the line when a copyright and license is added to all source files
36-
set(ament_cmake_copyright_FOUND TRUE)
37-
# the following line skips cpplint (only works in a git repo)
38-
# comment the line when this package is in a git repo and when
39-
# a copyright and license is added to all source files
40-
set(ament_cmake_cpplint_FOUND TRUE)
41-
ament_lint_auto_find_test_dependencies()
42-
endif()
28+
target_include_directories(serow_ros2 PUBLIC
29+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
30+
$<INSTALL_INTERFACE:include>
31+
${EIGEN3_INCLUDE_DIR}
32+
)
33+
34+
# Link dependencies
35+
target_link_libraries(serow_ros2
36+
serow
37+
yaml-cpp
38+
pinocchio::pinocchio
39+
Eigen3::Eigen
40+
nlohmann_json::nlohmann_json
41+
)
42+
43+
ament_target_dependencies(serow_ros2
44+
rclcpp
45+
sensor_msgs
46+
geometry_msgs
47+
ament_index_cpp
48+
)
49+
50+
# Install targets
51+
install(TARGETS serow_ros2
52+
DESTINATION lib/${PROJECT_NAME}
53+
)
54+
55+
# Install config files
56+
install(DIRECTORY
57+
config
58+
DESTINATION share/${PROJECT_NAME}
59+
)
60+
61+
# Install launch files
62+
install(DIRECTORY
63+
launch
64+
DESTINATION share/${PROJECT_NAME}
65+
)
66+
67+
# Export dependencies
68+
ament_export_dependencies(
69+
rclcpp
70+
sensor_msgs
71+
geometry_msgs
72+
ament_index_cpp
73+
yaml-cpp
74+
serow
75+
)
4376

4477
ament_package()

‎ros2/serow_ros2/config/h1_config.yaml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
robot_name: "h1"
2+
topics:
3+
joint_states: "/h1/joint_states"
4+
imu: "/h1/imu"
5+
force_torque_states:
6+
- "/h1/left_ankle/force_torque_states"
7+
- "/h1/right_ankle/force_torque_states"
8+
serow_config: "h1.json"

‎ros2/serow_ros2/launch/h1.py‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
import os
2+
3+
from launch import LaunchDescription
4+
from launch_ros.actions import Node
5+
from launch.substitutions import LaunchConfiguration
6+
from launch.actions import DeclareLaunchArgument
7+
from ament_index_python.packages import get_package_share_directory
8+
9+
10+
def generate_launch_description():
11+
# Get the path to the config file
12+
config_file = os.path.join(get_package_share_directory('serow_ros2'), 'config', 'h1_config.yaml')
13+
print("path is ",config_file)
14+
# Declare the config file argument
15+
config_arg = DeclareLaunchArgument(
16+
'config_file',
17+
default_value=config_file,
18+
description='Path to the YAML config file'
19+
)
20+
21+
return LaunchDescription([
22+
config_arg,
23+
Node(
24+
package='serow_ros2',
25+
executable='serow_ros2',
26+
name='serow_ros2',
27+
output="screen",
28+
parameters=[{
29+
'config_file': LaunchConfiguration('config_file')
30+
}]
31+
),
32+
])

‎ros2/serow_ros2/launch/serow_ros2.py‎

Lines changed: 0 additions & 12 deletions
This file was deleted.

‎ros2/serow_ros2/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
<depend>rclcpp</depend>
1414
<depend>geometry_msgs</depend>
1515
<depend>sensor_msgs</depend>
16+
<depend>yaml-cpp</depend>
1617

1718
<test_depend>ament_lint_auto</test_depend>
1819
<test_depend>ament_lint_common</test_depend>

0 commit comments

Comments
 (0)